Visual Servoing Platform  version 3.0.1
vpFeaturePoint3D.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * 3D point visual feature.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 
40 #include <visp3/visual_features/vpBasicFeature.h>
41 #include <visp3/visual_features/vpFeaturePoint3D.h>
42 
43 // Exception
44 #include <visp3/core/vpException.h>
45 #include <visp3/visual_features/vpFeatureException.h>
46 
47 // Debug trace
48 #include <visp3/core/vpDebug.h>
49 
50 
51 
52 
53 
54 /*
55 
56  attributes and members directly related to the vpBasicFeature needs
57  other functionalities are useful but not mandatory
58 
59 */
60 
68 void
70 {
71  //feature dimension
72  dim_s = 3 ;
73  nbParameters = 3;
74 
75  // memory allocation
76  s.resize(dim_s) ;
77  if (flags == NULL)
78  flags = new bool[nbParameters];
79  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
80 
81  //default value XYZ
82  s[0] = 0;
83  s[1] = 0;
84  s[2] = 1;
85 
86 }
87 
95 {
96  init() ;
97 }
98 
99 
109 void
110 vpFeaturePoint3D::set_X(const double X)
111 {
112  s[0] = X ;
113  flags[0] = true;
114 }
115 
125 void
126 vpFeaturePoint3D::set_Y(const double Y)
127 {
128  s[1] = Y ;
129  flags[1] = true;
130 }
131 
141 void
142 vpFeaturePoint3D::set_Z(const double Z)
143 {
144  s[2] = Z ;
145  flags[2] = true;
146 }
147 
156 void
158  const double Y,
159  const double Z)
160 {
161  set_X(X) ;
162  set_Y(Y) ;
163  set_Z(Z) ;
164 
165  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
166 }
167 
169 double
171 {
172  return s[0] ;
173 }
174 
176 double
178 {
179  return s[1] ;
180 }
181 
183 double
185 {
186  return s[2] ;
187 }
188 
256 vpMatrix
257 vpFeaturePoint3D::interaction(const unsigned int select)
258 {
259  vpMatrix L ;
260 
261  L.resize(0,6) ;
262 
264  {
265  for (unsigned int i = 0; i < nbParameters; i++)
266  {
267  if (flags[i] == false)
268  {
269  switch(i){
270  case 0:
271  vpTRACE("Warning !!! The interaction matrix is computed but X was not set yet");
272  break;
273  case 1:
274  vpTRACE("Warning !!! The interaction matrix is computed but Y was not set yet");
275  break;
276  case 2:
277  vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
278  break;
279  default:
280  vpTRACE("Problem during the reading of the variable flags");
281  }
282  }
283  }
284  resetFlags();
285  }
286 
287  double X = get_X() ;
288  double Y = get_Y() ;
289  double Z = get_Z() ;
290 
291  if (vpFeaturePoint3D::selectX() & select )
292  {
293  vpMatrix Lx(1,6) ; Lx = 0;
294 
295  Lx[0][0] = -1 ;
296  Lx[0][1] = 0 ;
297  Lx[0][2] = 0 ;
298  Lx[0][3] = 0 ;
299  Lx[0][4] = -Z ;
300  Lx[0][5] = Y ;
301 
302  L = vpMatrix::stack(L,Lx) ;
303  }
304 
305  if (vpFeaturePoint3D::selectY() & select )
306  {
307  vpMatrix Ly(1,6) ; Ly = 0;
308 
309  Ly[0][0] = 0 ;
310  Ly[0][1] = -1 ;
311  Ly[0][2] = 0 ;
312  Ly[0][3] = Z ;
313  Ly[0][4] = 0 ;
314  Ly[0][5] = -X ;
315 
316  L = vpMatrix::stack(L,Ly) ;
317  }
318  if (vpFeaturePoint3D::selectZ() & select )
319  {
320  vpMatrix Lz(1,6) ; Lz = 0;
321 
322  Lz[0][0] = 0 ;
323  Lz[0][1] = 0 ;
324  Lz[0][2] = -1 ;
325  Lz[0][3] = -Y ;
326  Lz[0][4] = X ;
327  Lz[0][5] = 0 ;
328 
329  L = vpMatrix::stack(L,Lz) ;
330  }
331  return L ;
332 }
333 
385  const unsigned int select)
386 {
387  vpColVector e(0) ;
388 
389  try{
390  if (vpFeaturePoint3D::selectX() & select )
391  {
392  vpColVector ex(1) ;
393  ex[0] = s[0] - s_star[0] ;
394 
395  e = vpColVector::stack(e,ex) ;
396  }
397 
398  if (vpFeaturePoint3D::selectY() & select )
399  {
400  vpColVector ey(1) ;
401  ey[0] = s[1] - s_star[1] ;
402  e = vpColVector::stack(e,ey) ;
403  }
404 
405  if (vpFeaturePoint3D::selectZ() & select )
406  {
407  vpColVector ez(1) ;
408  ez[0] = s[2] - s_star[2] ;
409  e = vpColVector::stack(e,ez) ;
410  }
411  }
412  catch(...) {
413  throw ;
414  }
415 
416  return e ;
417 
418 }
419 
436 void
438 {
439 
440  // cP is expressed in homogeneous coordinates
441  // we devide by the fourth coordinate
442  s[0] = p.cP[0]/p.cP[3] ;
443  s[1] = p.cP[1]/p.cP[3] ;
444  s[2] = p.cP[2]/p.cP[3] ;
445 
446  double Z = s[2] ;
447  if (Z < 0)
448  {
449  vpERROR_TRACE("Point is behind the camera ") ;
450  std::cout <<"Z = " << Z << std::endl ;
451 
453  "Point is behind the camera ")) ;
454  }
455 
456  if (fabs(Z) < 1e-6)
457  {
458  vpERROR_TRACE("Point Z coordinates is null ") ;
459  std::cout <<"Z = " << Z << std::endl ;
460 
462  "Point Z coordinates is null")) ;
463  }
464 
465  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
466 
467 }
468 
485 void
486 vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z)
487 {
488 
489  s[0] = X ;
490  s[1] = Y ;
491  s[2] = Z ;
492 
493  if (Z < 0)
494  {
495  vpERROR_TRACE("Point is behind the camera ") ;
496  std::cout <<"Z = " << Z << std::endl ;
497 
499  "Point is behind the camera ")) ;
500  }
501 
502  if (fabs(Z) < 1e-6)
503  {
504  vpERROR_TRACE("Point Z coordinates is null ") ;
505  std::cout <<"Z = " << Z << std::endl ;
506 
508  "Point Z coordinates is null")) ;
509  }
510 
511  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
512 
513 }
514 
515 
539 void
540 vpFeaturePoint3D::print(const unsigned int select ) const
541 {
542 
543  std::cout <<"Point3D: " ;
544  if (vpFeaturePoint3D::selectX() & select )
545  std::cout << " X=" << get_X() ;
546  if (vpFeaturePoint3D::selectY() & select )
547  std::cout << " Y=" << get_Y() ;
548  if (vpFeaturePoint3D::selectZ() & select )
549  std::cout << " Z=" << get_Z() ;
550  std::cout <<std::endl ;
551 }
552 
553 
566 {
567  vpFeaturePoint3D *feature = new vpFeaturePoint3D ;
568  return feature ;
569 }
570 
575 void
577  const vpImage<unsigned char> &/* I */,
578  const vpColor &/* color */,
579  unsigned int /* thickness */) const
580 {
581  static int firsttime =0 ;
582 
583  if (firsttime==0)
584  {
585  firsttime=1 ;
586  vpERROR_TRACE("not implemented") ;
587  // Do not throw and error since it is not subject
588  // to produce a failure
589  }
590 }
591 
596 void
598  const vpImage<vpRGBa> &/* I */,
599  const vpColor &/* color */,
600  unsigned int /* thickness */) const
601 {
602  static int firsttime =0 ;
603 
604  if (firsttime==0)
605  {
606  firsttime=1 ;
607  vpERROR_TRACE("not implemented") ;
608  // Do not throw and error since it is not subject
609  // to produce a failure
610  }
611 }
636 unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0] ; }
637 
662 unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1] ; }
663 
688 unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2] ; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void set_X(const double X)
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void set_XYZ(const double X, const double Y, const double Z)
void set_Z(const double Z)
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
unsigned int dim_s
Dimension of the visual feature.
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
void buildFrom(const vpPoint &p)
Class that defines what is a point.
Definition: vpPoint.h:59
static unsigned int selectY()
vpColVector cP
Definition: vpTracker.h:77
double get_Z() const
Return the coordinate in the camera frame of the 3D point.
class that defines what is a visual feature
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
#define vpTRACE
Definition: vpDebug.h:414
Error that can be emited by the vpBasicFeature class and its derivates.
Class that defines the 3D point visual feature.
Generic class defining intrinsic camera parameters.
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
static unsigned int selectZ()
vpFeaturePoint3D * duplicate() const
static const unsigned int FEATURE_LINE[32]
void print(const unsigned int select=FEATURE_ALL) const
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_Y(const double Y)
static unsigned int selectX()
double get_Y() const
Return the coordinate in the camera frame of the 3D point.
double get_X() const
Return the coordinate in the camera frame of the 3D point.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225