ViSP  2.6.2
vpFeaturePoint3D.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeaturePoint3D.cpp 3709 2012-05-07 18:42:16Z okermorg $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * 3D point visual feature.
36  *
37  * Authors:
38  * Eric Marchand
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 
44 #include <visp/vpBasicFeature.h>
45 #include <visp/vpFeaturePoint3D.h>
46 
47 // Exception
48 #include <visp/vpException.h>
49 #include <visp/vpMatrixException.h>
50 #include <visp/vpFeatureException.h>
51 
52 // Debug trace
53 #include <visp/vpDebug.h>
54 
55 
56 
57 
58 
59 /*
60 
61  attributes and members directly related to the vpBasicFeature needs
62  other functionalities are useful but not mandatory
63 
64 */
65 
73 void
75 {
76  //feature dimension
77  dim_s = 3 ;
78  nbParameters = 3;
79 
80  // memory allocation
81  s.resize(dim_s) ;
82  if (flags == NULL)
83  flags = new bool[nbParameters];
84  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
85 
86  //default value XYZ
87  s[0] = 0;
88  s[1] = 0;
89  s[2] = 1;
90 
91 }
92 
100 {
101  init() ;
102 }
103 
104 
114 void
115 vpFeaturePoint3D::set_X(const double X)
116 {
117  s[0] = X ;
118  flags[0] = true;
119 }
120 
130 void
131 vpFeaturePoint3D::set_Y(const double Y)
132 {
133  s[1] = Y ;
134  flags[1] = true;
135 }
136 
146 void
147 vpFeaturePoint3D::set_Z(const double Z)
148 {
149  s[2] = Z ;
150  flags[2] = true;
151 }
152 
161 void
163  const double Y,
164  const double Z)
165 {
166  set_X(X) ;
167  set_Y(Y) ;
168  set_Z(Z) ;
169 
170  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
171 }
172 
174 double
176 {
177  return s[0] ;
178 }
179 
181 double
183 {
184  return s[1] ;
185 }
186 
188 double
190 {
191  return s[2] ;
192 }
193 
261 vpMatrix
262 vpFeaturePoint3D::interaction(const unsigned int select)
263 {
264  vpMatrix L ;
265 
266  L.resize(0,6) ;
267 
269  {
270  for (unsigned int i = 0; i < nbParameters; i++)
271  {
272  if (flags[i] == false)
273  {
274  switch(i){
275  case 0:
276  vpTRACE("Warning !!! The interaction matrix is computed but X was not set yet");
277  break;
278  case 1:
279  vpTRACE("Warning !!! The interaction matrix is computed but Y was not set yet");
280  break;
281  case 2:
282  vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
283  break;
284  default:
285  vpTRACE("Problem during the reading of the variable flags");
286  }
287  }
288  }
289  resetFlags();
290  }
291 
292  double X = get_X() ;
293  double Y = get_Y() ;
294  double Z = get_Z() ;
295 
296  if (vpFeaturePoint3D::selectX() & select )
297  {
298  vpMatrix Lx(1,6) ; Lx = 0;
299 
300  Lx[0][0] = -1 ;
301  Lx[0][1] = 0 ;
302  Lx[0][2] = 0 ;
303  Lx[0][3] = 0 ;
304  Lx[0][4] = -Z ;
305  Lx[0][5] = Y ;
306 
307  L = vpMatrix::stackMatrices(L,Lx) ;
308  }
309 
310  if (vpFeaturePoint3D::selectY() & select )
311  {
312  vpMatrix Ly(1,6) ; Ly = 0;
313 
314  Ly[0][0] = 0 ;
315  Ly[0][1] = -1 ;
316  Ly[0][2] = 0 ;
317  Ly[0][3] = Z ;
318  Ly[0][4] = 0 ;
319  Ly[0][5] = -X ;
320 
321  L = vpMatrix::stackMatrices(L,Ly) ;
322  }
323  if (vpFeaturePoint3D::selectZ() & select )
324  {
325  vpMatrix Lz(1,6) ; Lz = 0;
326 
327  Lz[0][0] = 0 ;
328  Lz[0][1] = 0 ;
329  Lz[0][2] = -1 ;
330  Lz[0][3] = -Y ;
331  Lz[0][4] = X ;
332  Lz[0][5] = 0 ;
333 
334  L = vpMatrix::stackMatrices(L,Lz) ;
335  }
336  return L ;
337 }
338 
390  const unsigned int select)
391 {
392  vpColVector e(0) ;
393 
394  try{
395  if (vpFeaturePoint3D::selectX() & select )
396  {
397  vpColVector ex(1) ;
398  ex[0] = s[0] - s_star[0] ;
399 
400  e = vpMatrix::stackMatrices(e,ex) ;
401  }
402 
403  if (vpFeaturePoint3D::selectY() & select )
404  {
405  vpColVector ey(1) ;
406  ey[0] = s[1] - s_star[1] ;
407  e = vpMatrix::stackMatrices(e,ey) ;
408  }
409 
410  if (vpFeaturePoint3D::selectZ() & select )
411  {
412  vpColVector ez(1) ;
413  ez[0] = s[2] - s_star[2] ;
414  e = vpMatrix::stackMatrices(e,ez) ;
415  }
416  }
417  catch(vpMatrixException me)
418  {
419  vpERROR_TRACE("caught a Matrix related error") ;
420  std::cout <<std::endl << me << std::endl ;
421  throw(me) ;
422  }
423  catch(vpException me)
424  {
425  vpERROR_TRACE("caught another error") ;
426  std::cout <<std::endl << me << std::endl ;
427  throw(me) ;
428  }
429 
430  return e ;
431 
432 }
433 
450 void
452 {
453 
454  // cP is expressed in homogeneous coordinates
455  // we devide by the fourth coordinate
456  s[0] = p.cP[0]/p.cP[3] ;
457  s[1] = p.cP[1]/p.cP[3] ;
458  s[2] = p.cP[2]/p.cP[3] ;
459 
460  double Z = s[2] ;
461  if (Z < 0)
462  {
463  vpERROR_TRACE("Point is behind the camera ") ;
464  std::cout <<"Z = " << Z << std::endl ;
465 
467  "Point is behind the camera ")) ;
468  }
469 
470  if (fabs(Z) < 1e-6)
471  {
472  vpERROR_TRACE("Point Z coordinates is null ") ;
473  std::cout <<"Z = " << Z << std::endl ;
474 
476  "Point Z coordinates is null")) ;
477  }
478 
479  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
480 
481 }
482 
499 void
500 vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z)
501 {
502 
503  s[0] = X ;
504  s[1] = Y ;
505  s[2] = Z ;
506 
507  if (Z < 0)
508  {
509  vpERROR_TRACE("Point is behind the camera ") ;
510  std::cout <<"Z = " << Z << std::endl ;
511 
513  "Point is behind the camera ")) ;
514  }
515 
516  if (fabs(Z) < 1e-6)
517  {
518  vpERROR_TRACE("Point Z coordinates is null ") ;
519  std::cout <<"Z = " << Z << std::endl ;
520 
522  "Point Z coordinates is null")) ;
523  }
524 
525  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
526 
527 }
528 
529 
553 void
554 vpFeaturePoint3D::print(const unsigned int select ) const
555 {
556 
557  std::cout <<"Point3D: " ;
558  if (vpFeaturePoint3D::selectX() & select )
559  std::cout << " X=" << get_X() ;
560  if (vpFeaturePoint3D::selectY() & select )
561  std::cout << " Y=" << get_Y() ;
562  if (vpFeaturePoint3D::selectZ() & select )
563  std::cout << " Z=" << get_Z() ;
564  std::cout <<std::endl ;
565 }
566 
567 
580 {
581  vpFeaturePoint3D *feature = new vpFeaturePoint3D ;
582  return feature ;
583 }
584 
589 void
591  const vpImage<unsigned char> &/* I */,
592  const vpColor &/* color */,
593  unsigned int /* thickness */) const
594 {
595  static int firsttime =0 ;
596 
597  if (firsttime==0)
598  {
599  firsttime=1 ;
600  vpERROR_TRACE("not implemented") ;
601  // Do not throw and error since it is not subject
602  // to produce a failure
603  }
604 }
605 
610 void
612  const vpImage<vpRGBa> &/* I */,
613  const vpColor &/* color */,
614  unsigned int /* thickness */) const
615 {
616  static int firsttime =0 ;
617 
618  if (firsttime==0)
619  {
620  firsttime=1 ;
621  vpERROR_TRACE("not implemented") ;
622  // Do not throw and error since it is not subject
623  // to produce a failure
624  }
625 }
626 
627 /*
628  * Local variables:
629  * c-basic-offset: 2
630  * End:
631  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
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 resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
double get_Z() const
Return the coordinate in the camera frame of the 3D point.
void set_Z(const double Z)
#define vpERROR_TRACE
Definition: vpDebug.h:379
void print(const unsigned int select=FEATURE_ALL) const
#define vpTRACE
Definition: vpDebug.h:401
Class to define colors available for display functionnalities.
Definition: vpColor.h:123
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:65
vpColVector cP
Definition: vpTracker.h:82
class that defines what is a visual feature
vpFeaturePoint3D * duplicate() const
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
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 vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2261
vpBasicFeatureDeallocatorType deallocate
static unsigned int selectX()
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
static unsigned int selectY()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
error that can be emited by the vpMatrix class and its derivates
double get_X() const
Return the coordinate in the camera frame of the 3D point.
void set_Y(const double Y)
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:94
double get_Y() const
Return the coordinate in the camera frame of the 3D point.