ViSP  2.8.0
vpFeaturePoint.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeaturePoint.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  * 2D point visual feature.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpBasicFeature.h>
50 #include <visp/vpFeaturePoint.h>
51 
52 // Exception
53 #include <visp/vpException.h>
54 #include <visp/vpMatrixException.h>
55 #include <visp/vpFeatureException.h>
56 
57 // Debug trace
58 #include <visp/vpDebug.h>
59 
60 // math
61 #include <visp/vpMath.h>
62 
63 #include <visp/vpFeatureDisplay.h>
64 
65 
66 
67 /*
68 
69 attributes and members directly related to the vpBasicFeature needs
70 other functionalities ar useful but not mandatory
71 
72 */
73 
77 void
79 {
80  //feature dimension
81  dim_s = 2 ;
82  nbParameters = 3;
83 
84  // memory allocation
85  s.resize(dim_s) ;
86  if (flags == NULL)
87  flags = new bool[nbParameters];
88  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
89 
90  //default value Z (1 meters)
91  Z = 1;
92 
93 }
94 
99 {
100  init() ;
101 }
102 
103 
109 void
110 vpFeaturePoint::set_Z(const double Z)
111 {
112  this->Z = Z ;
113  flags[2] = true;
114 }
115 
116 
122 double
124 {
125  return Z ;
126 }
127 
128 
136 void
137 vpFeaturePoint::set_x(const double x)
138 {
139  s[0] = x ;
140  flags[0] = true;
141 }
142 
143 
149 double
151 {
152  return s[0] ;
153 }
154 
155 
161 void
162 vpFeaturePoint::set_y(const double y)
163 {
164  s[1] = y ;
165  flags[1] = true;
166 }
167 
168 
174 double
176 {
177  return s[1] ;
178 }
179 
180 
192 void
193 vpFeaturePoint::set_xyZ(const double x,
194  const double y,
195  const double Z)
196 {
197  set_x(x) ;
198  set_y(y) ;
199  set_Z(Z) ;
200  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
201 }
202 
203 
247 vpMatrix
248 vpFeaturePoint::interaction(const unsigned int select)
249 {
250  vpMatrix L ;
251 
252  L.resize(0,6) ;
253 
255  {
256  for (unsigned int i = 0; i < nbParameters; i++)
257  {
258  if (flags[i] == false)
259  {
260  switch(i){
261  case 0:
262  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
263  break;
264  case 1:
265  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
266  break;
267  case 2:
268  vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
269  break;
270  default:
271  vpTRACE("Problem during the reading of the variable flags");
272  }
273  }
274  }
275  resetFlags();
276  }
277 
278  double x = get_x() ;
279  double y = get_y() ;
280  double Z = get_Z() ;
281 
282  if (Z < 0)
283  {
284  vpERROR_TRACE("Point is behind the camera ") ;
285  std::cout <<"Z = " << Z << std::endl ;
286 
288  "Point is behind the camera ")) ;
289  }
290 
291  if (fabs(Z) < 1e-6)
292  {
293  vpERROR_TRACE("Point Z coordinates is null ") ;
294  std::cout <<"Z = " << Z << std::endl ;
295 
297  "Point Z coordinates is null")) ;
298  }
299 
300  if (vpFeaturePoint::selectX() & select )
301  {
302  vpMatrix Lx(1,6) ; Lx = 0;
303 
304  Lx[0][0] = -1/Z ;
305  Lx[0][1] = 0 ;
306  Lx[0][2] = x/Z ;
307  Lx[0][3] = x*y ;
308  Lx[0][4] = -(1+x*x) ;
309  Lx[0][5] = y ;
310 
311  L = vpMatrix::stackMatrices(L,Lx) ;
312  }
313 
314  if (vpFeaturePoint::selectY() & select )
315  {
316  vpMatrix Ly(1,6) ; Ly = 0;
317 
318  Ly[0][0] = 0 ;
319  Ly[0][1] = -1/Z ;
320  Ly[0][2] = y/Z ;
321  Ly[0][3] = 1+y*y ;
322  Ly[0][4] = -x*y ;
323  Ly[0][5] = -x ;
324 
325  L = vpMatrix::stackMatrices(L,Ly) ;
326  }
327  return L ;
328 }
329 
330 
363  const unsigned int select)
364 {
365  vpColVector e(0) ;
366 
367  try{
368  if (vpFeaturePoint::selectX() & select )
369  {
370  vpColVector ex(1) ;
371  ex[0] = s[0] - s_star[0] ;
372 
373  e = vpMatrix::stackMatrices(e,ex) ;
374  }
375 
376  if (vpFeaturePoint::selectY() & select )
377  {
378  vpColVector ey(1) ;
379  ey[0] = s[1] - s_star[1] ;
380  e = vpMatrix::stackMatrices(e,ey) ;
381  }
382  }
383  catch(vpMatrixException me)
384  {
385  vpERROR_TRACE("caught a Matrix related error") ;
386  std::cout <<std::endl << me << std::endl ;
387  throw(me) ;
388  }
389  catch(vpException me)
390  {
391  vpERROR_TRACE("caught another error") ;
392  std::cout <<std::endl << me << std::endl ;
393  throw(me) ;
394  }
395 
396 
397  return e ;
398 
399 }
400 
401 
420 void
421 vpFeaturePoint::print(const unsigned int select ) const
422 {
423 
424  std::cout <<"Point: Z=" << get_Z() ;
425  if (vpFeaturePoint::selectX() & select )
426  std::cout << " x=" << get_x() ;
427  if (vpFeaturePoint::selectY() & select )
428  std::cout << " y=" << get_y() ;
429  std::cout <<std::endl ;
430 }
431 
432 
442 void
443 vpFeaturePoint::buildFrom(const double x, const double y, const double Z)
444 {
445 
446  s[0] = x ;
447  s[1] = y ;
448 
449  this->Z = Z ;
450 
451  if (Z < 0)
452  {
453  vpERROR_TRACE("Point is behind the camera ") ;
454  std::cout <<"Z = " << Z << std::endl ;
455 
457  "Point is behind the camera ")) ;
458  }
459 
460  if (fabs(Z) < 1e-6)
461  {
462  vpERROR_TRACE("Point Z coordinates is null ") ;
463  std::cout <<"Z = " << Z << std::endl ;
464 
466  "Point Z coordinates is null")) ;
467  }
468 
469  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
470 
471 }
472 
473 
484 void
486  const vpImage<unsigned char> &I,
487  const vpColor &color,
488  unsigned int thickness) const
489 {
490  try{
491  double x,y ;
492  x = get_x() ;
493  y = get_y() ;
494 
495  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
496 
497  }
498  catch(...)
499  {
500  vpERROR_TRACE("Error caught") ;
501  throw ;
502  }
503 }
504 
515 void
517  const vpImage<vpRGBa> &I,
518  const vpColor &color,
519  unsigned int thickness) const
520 {
521  try{
522  double x,y ;
523  x = get_x() ;
524  y = get_y() ;
525 
526  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
527 
528  }
529  catch(...)
530  {
531  vpERROR_TRACE("Error caught") ;
532  throw ;
533  }
534 }
535 
536 
548 {
549  vpFeaturePoint *feature = new vpFeaturePoint ;
550  return feature ;
551 }
552 
553 
554 /*
555  * Local variables:
556  * c-basic-offset: 2
557  * End:
558  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
#define vpERROR_TRACE
Definition: vpDebug.h:379
#define vpTRACE
Definition: vpDebug.h:401
void set_xyZ(const double x, const double y, const double Z)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
unsigned int dim_s
Dimension of the visual feature.
error that can be emited by ViSP classes.
Definition: vpException.h:75
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
void set_y(const double y)
void set_x(const double x)
class that defines what is a visual feature
Error that can be emited by the vpBasicFeature class and its derivates.
double get_Z() const
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 vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2263
void buildFrom(const double x, const double y, const double Z)
vpFeaturePoint * duplicate() const
vpBasicFeatureDeallocatorType deallocate
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double get_y() const
double get_x() const
error that can be emited by the vpMatrix class and its derivates
void set_Z(const double Z)
static unsigned int selectX()
static unsigned int selectY()
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
void print(const unsigned int select=FEATURE_ALL) const