ViSP  2.10.0
vpFeatureDepth.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureDepth.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 
49 #include <visp/vpBasicFeature.h>
50 #include <visp/vpFeatureDepth.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 
70 
71 attributes and members directly related to the vpBasicFeature needs
72 other functionalities ar useful but not mandatory
73 
74 
75 
76 
77 
78 */
79 
83 void
85 {
86  //feature dimension
87  dim_s = 1 ;
88  nbParameters = 3;
89 
90  // memory allocation
91  s.resize(dim_s) ;
92  if (flags == NULL)
93  flags = new bool[nbParameters];
94  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
95  x = y = 0.;
96  Z = 1.;
97 }
98 
99 
103 vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.)
104 {
105  init() ;
106 }
107 
108 
114 void
115 vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar)
116 {
117  s[0] = LogZoverZstar ;
118 }
119 
120 
126 double
128 {
129  return s[0] ;
130 }
131 
132 
138 void
139 vpFeatureDepth::set_x(const double x_)
140 {
141  this->x = x_ ;
142  flags[0] = true;
143 }
144 
145 
151 double
153 {
154  return x ;
155 }
156 
157 
163 void
164 vpFeatureDepth::set_y(const double y_)
165 {
166  this->y = y_ ;
167  flags[1] = true;
168 }
169 
170 
176 double
178 {
179  return y ;
180 }
181 
187 void
188 vpFeatureDepth::set_Z(const double Z_)
189 {
190  this->Z = Z_ ;
191  flags[2] = true;
192 }
193 
194 
200 double
202 {
203  return Z ;
204 }
205 
206 
215 void
217  const double y_,
218  const double Z_,
219  const double LogZoverZstar)
220 {
221  set_x(x_) ;
222  set_y(y_) ;
223  set_Z(Z_) ;
224  set_LogZoverZstar(LogZoverZstar) ;
225  for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
226 }
227 
228 
250 vpMatrix
251 vpFeatureDepth::interaction(const unsigned int select)
252 {
253  vpMatrix L ;
254 
256  {
257  for (unsigned int i = 0; i < nbParameters; i++)
258  {
259  if (flags[i] == false)
260  {
261  switch(i){
262  case 0:
263  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
264  break;
265  case 1:
266  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
267  break;
268  case 2:
269  vpTRACE("Warning !!! The interaction matrix is computed but z was not set yet");
270  break;
271  default:
272  vpTRACE("Problem during the reading of the variable flags");
273  }
274  }
275  }
276  resetFlags();
277  }
278 
279  L.resize(1,6) ;
280 
281  double x_ = get_x();
282  double y_ = get_y();
283  double Z_ = get_Z();
284 
285  if (Z_ < 0)
286  {
287  vpERROR_TRACE("Point is behind the camera ") ;
288  std::cout <<"Z = " << Z_ << std::endl ;
289 
291  "Point is behind the camera ")) ;
292  }
293 
294  if (fabs(Z_) < 1e-6)
295  {
296  vpERROR_TRACE("Point Z coordinates is null ") ;
297  std::cout <<"Z = " << Z_ << std::endl ;
298 
300  "Point Z coordinates is null")) ;
301  }
302 
303  if (FEATURE_ALL & select)
304  {
305  L = 0;
306  L[0][0] = 0;
307  L[0][1] = 0;
308  L[0][2] = -1/Z_;
309  L[0][3] = -y_;
310  L[0][4] = x_;
311  L[0][5] = 0;
312  }
313 
314  return L ;
315 }
316 
317 
355  const unsigned int select)
356 {
357 
358  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
359  {
360  vpERROR_TRACE("s* should be zero ! ") ;
362  "s* should be zero !")) ;
363  }
364 
365  vpColVector e(1) ;
366  if(FEATURE_ALL & select)
367  {
368  e[0] = s[0];
369  }
370 
371  return e ;
372 }
373 
374 
389 void
390 vpFeatureDepth::print(const unsigned int select ) const
391 {
392  if (FEATURE_ALL & select)
393  {
394  std::cout <<"Point: x=" << get_x() ;
395  std::cout <<" Point: y=" << get_y() ;
396  std::cout <<" Point: Z=" << get_Z() ;
397 
398  std::cout << " log(Z/Z*)=" << get_LogZoverZstar() ;
399 
400  std::cout <<std::endl ;
401  }
402 }
403 
404 
413 void
414 vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar)
415 {
416 
417  s[0] = LogZoverZstar;
418 
419  this->x = x_ ;
420  this->y = y_ ;
421  this->Z = Z_ ;
422 
423  if (Z < 0)
424  {
425  vpERROR_TRACE("Point is behind the camera ") ;
426  std::cout <<"Z = " << Z << std::endl ;
427 
429  "Point is behind the camera ")) ;
430  }
431 
432  if (fabs(Z) < 1e-6)
433  {
434  vpERROR_TRACE("Point Z coordinates is null ") ;
435  std::cout <<"Z = " << Z << std::endl ;
436 
438  "Point Z coordinates is null")) ;
439  }
440 
441  for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
442 
443 }
444 
445 
446 
458 {
459  vpFeatureDepth *feature = new vpFeatureDepth;
460  return feature ;
461 }
462 
468 void
470  const vpImage<unsigned char> &/* I */,
471  const vpColor &/* color */,
472  unsigned int /* thickness */) const
473 {
474  static int firsttime =0 ;
475 
476  if (firsttime==0)
477  {
478  firsttime=1 ;
479  vpERROR_TRACE("not implemented") ;
480  // Do not throw and error since it is not subject
481  // to produce a failure
482  }
483 }
489 void
491  const vpImage<vpRGBa> &/* I */,
492  const vpColor &/* color */,
493  unsigned int /* thickness */) const
494 {
495  static int firsttime =0 ;
496 
497  if (firsttime==0)
498  {
499  firsttime=1 ;
500  vpERROR_TRACE("not implemented") ;
501  // Do not throw and error since it is not subject
502  // to produce a failure
503  }
504 }
505 
506 /*
507  * Local variables:
508  * c-basic-offset: 2
509  * End:
510  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
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:199
static const unsigned int FEATURE_ALL
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpTRACE
Definition: vpDebug.h:418
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
unsigned int dim_s
Dimension of the visual feature.
void print(const unsigned int select=FEATURE_ALL) const
void set_y(const double y)
double get_Z() const
double sumSquare() const
Definition: vpMatrix.cpp:868
void set_LogZoverZstar(const double LogZoverZstar)
class that defines what is a visual feature
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
void set_x(const double x)
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
double get_y() 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
void set_Z(const double Z)
double get_LogZoverZstar() const
void set_xyZLogZoverZstar(const double x, const double y, const double Z, const double logZZs)
vpFeatureDepth * duplicate() const
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
double get_x() const
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:98