ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpFeaturePoint3D.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeaturePoint3D.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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  */