Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureDepth.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  * 2D point visual feature.
32  *
33  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
38 
45 #include <visp3/visual_features/vpBasicFeature.h>
46 #include <visp3/visual_features/vpFeatureDepth.h>
47 
48 // Exception
49 #include <visp3/core/vpException.h>
50 #include <visp3/visual_features/vpFeatureException.h>
51 
52 // Debug trace
53 #include <visp3/core/vpDebug.h>
54 
55 // math
56 #include <visp3/core/vpMath.h>
57 
58 #include <visp3/core/vpFeatureDisplay.h>
59 
60 
61 
62 /*
63 
64 
65 
66 attributes and members directly related to the vpBasicFeature needs
67 other functionalities ar useful but not mandatory
68 
69 
70 
71 
72 
73 */
74 
78 void
80 {
81  //feature dimension
82  dim_s = 1 ;
83  nbParameters = 3;
84 
85  // memory allocation
86  s.resize(dim_s) ;
87  if (flags == NULL)
88  flags = new bool[nbParameters];
89  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
90  x = y = 0.;
91  Z = 1.;
92 }
93 
94 
98 vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.)
99 {
100  init() ;
101 }
102 
103 
109 void
110 vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar)
111 {
112  s[0] = LogZoverZstar ;
113 }
114 
115 
121 double
123 {
124  return s[0] ;
125 }
126 
127 
133 void
134 vpFeatureDepth::set_x(const double x_)
135 {
136  this->x = x_ ;
137  flags[0] = true;
138 }
139 
140 
146 double
148 {
149  return x ;
150 }
151 
152 
158 void
159 vpFeatureDepth::set_y(const double y_)
160 {
161  this->y = y_ ;
162  flags[1] = true;
163 }
164 
165 
171 double
173 {
174  return y ;
175 }
176 
182 void
183 vpFeatureDepth::set_Z(const double Z_)
184 {
185  this->Z = Z_ ;
186  flags[2] = true;
187 }
188 
189 
195 double
197 {
198  return Z ;
199 }
200 
201 
210 void
212  const double y_,
213  const double Z_,
214  const double LogZoverZstar)
215 {
216  set_x(x_) ;
217  set_y(y_) ;
218  set_Z(Z_) ;
219  set_LogZoverZstar(LogZoverZstar) ;
220  for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
221 }
222 
223 
245 vpMatrix
246 vpFeatureDepth::interaction(const unsigned int select)
247 {
248  vpMatrix L ;
249 
251  {
252  for (unsigned int i = 0; i < nbParameters; i++)
253  {
254  if (flags[i] == false)
255  {
256  switch(i){
257  case 0:
258  vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
259  break;
260  case 1:
261  vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
262  break;
263  case 2:
264  vpTRACE("Warning !!! The interaction matrix is computed but z was not set yet");
265  break;
266  default:
267  vpTRACE("Problem during the reading of the variable flags");
268  }
269  }
270  }
271  resetFlags();
272  }
273 
274  L.resize(1,6) ;
275 
276  double x_ = get_x();
277  double y_ = get_y();
278  double Z_ = get_Z();
279 
280  if (Z_ < 0)
281  {
282  vpERROR_TRACE("Point is behind the camera ") ;
283  std::cout <<"Z = " << Z_ << std::endl ;
284 
286  "Point is behind the camera ")) ;
287  }
288 
289  if (fabs(Z_) < 1e-6)
290  {
291  vpERROR_TRACE("Point Z coordinates is null ") ;
292  std::cout <<"Z = " << Z_ << std::endl ;
293 
295  "Point Z coordinates is null")) ;
296  }
297 
298  if (FEATURE_ALL & select)
299  {
300  L = 0;
301  L[0][0] = 0;
302  L[0][1] = 0;
303  L[0][2] = -1/Z_;
304  L[0][3] = -y_;
305  L[0][4] = x_;
306  L[0][5] = 0;
307  }
308 
309  return L ;
310 }
311 
312 
350  const unsigned int select)
351 {
352 
353  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
354  {
355  vpERROR_TRACE("s* should be zero ! ") ;
357  "s* should be zero !")) ;
358  }
359 
360  vpColVector e(1) ;
361  if(FEATURE_ALL & select)
362  {
363  e[0] = s[0];
364  }
365 
366  return e ;
367 }
368 
369 
384 void
385 vpFeatureDepth::print(const unsigned int select ) const
386 {
387  if (FEATURE_ALL & select)
388  {
389  std::cout <<"Point: x=" << get_x() ;
390  std::cout <<" Point: y=" << get_y() ;
391  std::cout <<" Point: Z=" << get_Z() ;
392 
393  std::cout << " log(Z/Z*)=" << get_LogZoverZstar() ;
394 
395  std::cout <<std::endl ;
396  }
397 }
398 
399 
408 void
409 vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar)
410 {
411 
412  s[0] = LogZoverZstar;
413 
414  this->x = x_ ;
415  this->y = y_ ;
416  this->Z = Z_ ;
417 
418  if (Z < 0)
419  {
420  vpERROR_TRACE("Point is behind the camera ") ;
421  std::cout <<"Z = " << Z << std::endl ;
422 
424  "Point is behind the camera ")) ;
425  }
426 
427  if (fabs(Z) < 1e-6)
428  {
429  vpERROR_TRACE("Point Z coordinates is null ") ;
430  std::cout <<"Z = " << Z << std::endl ;
431 
433  "Point Z coordinates is null")) ;
434  }
435 
436  for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
437 
438 }
439 
440 
441 
453 {
454  vpFeatureDepth *feature = new vpFeatureDepth;
455  return feature ;
456 }
457 
463 void
465  const vpImage<unsigned char> &/* I */,
466  const vpColor &/* color */,
467  unsigned int /* thickness */) const
468 {
469  static int firsttime =0 ;
470 
471  if (firsttime==0)
472  {
473  firsttime=1 ;
474  vpERROR_TRACE("not implemented") ;
475  // Do not throw and error since it is not subject
476  // to produce a failure
477  }
478 }
484 void
486  const vpImage<vpRGBa> &/* I */,
487  const vpColor &/* color */,
488  unsigned int /* thickness */) const
489 {
490  static int firsttime =0 ;
491 
492  if (firsttime==0)
493  {
494  firsttime=1 ;
495  vpERROR_TRACE("not implemented") ;
496  // Do not throw and error since it is not subject
497  // to produce a failure
498  }
499 }
500 
501 /*
502  * Local variables:
503  * c-basic-offset: 2
504  * End:
505  */
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
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 flagNullify=true)
Definition: vpArray2D.h:167
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:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
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
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)
#define vpTRACE
Definition: vpDebug.h:414
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
double get_y() const
double sumSquare() const
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
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:225