Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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)
double get_Z() const
Return the coordinate in the camera frame of the 3D point.
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
void print(const unsigned int select=FEATURE_ALL) const
#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
class that defines what is a visual feature
vpFeaturePoint3D * duplicate() const
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.
static unsigned int selectZ()
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
double get_X() const
Return the coordinate in the camera frame of the 3D point.
void set_Y(const double Y)
static unsigned int selectX()
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
double get_Y() const
Return the coordinate in the camera frame of the 3D point.