Visual Servoing Platform  version 3.6.1 under development (2024-04-15)
vpFeatureDepth.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * 2D point visual feature.
33  *
34 *****************************************************************************/
35 
41 #include <visp3/visual_features/vpBasicFeature.h>
42 #include <visp3/visual_features/vpFeatureDepth.h>
43 
44 // Exception
45 #include <visp3/core/vpException.h>
46 #include <visp3/visual_features/vpFeatureException.h>
47 
48 // Debug trace
49 #include <visp3/core/vpDebug.h>
50 
51 // math
52 #include <visp3/core/vpMath.h>
53 
54 #include <visp3/core/vpFeatureDisplay.h>
55 
56 /*
57 
58 
59 
60 attributes and members directly related to the vpBasicFeature needs
61 other functionalities ar useful but not mandatory
62 
63 
64 
65 
66 
67 */
68 
73 {
74  // feature dimension
75  dim_s = 1;
76  nbParameters = 3;
77 
78  // memory allocation
79  s.resize(dim_s);
80  if (flags == nullptr)
81  flags = new bool[nbParameters];
82  for (unsigned int i = 0; i < nbParameters; i++)
83  flags[i] = false;
84  x = y = 0.;
85  Z = 1.;
86 }
87 
91 vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.) { init(); }
92 
99 void vpFeatureDepth::set_LogZoverZstar(double LogZoverZstar) { s[0] = LogZoverZstar; }
100 
107 double vpFeatureDepth::get_LogZoverZstar() const { return s[0]; }
108 
115 void vpFeatureDepth::set_x(double x_)
116 {
117  this->x = x_;
118  flags[0] = true;
119 }
120 
127 double vpFeatureDepth::get_x() const { return x; }
128 
135 void vpFeatureDepth::set_y(double y_)
136 {
137  this->y = y_;
138  flags[1] = true;
139 }
140 
147 double vpFeatureDepth::get_y() const { return y; }
148 
155 void vpFeatureDepth::set_Z(double Z_)
156 {
157  this->Z = Z_;
158  flags[2] = true;
159 }
160 
167 double vpFeatureDepth::get_Z() const { return Z; }
168 
181 void vpFeatureDepth::set_xyZLogZoverZstar(double x_, double y_, double Z_, double LogZoverZstar)
182 {
183  set_x(x_);
184  set_y(y_);
185  set_Z(Z_);
186  set_LogZoverZstar(LogZoverZstar);
187  for (unsigned int i = 0; i < nbParameters; i++)
188  flags[i] = true;
189 }
190 
217 {
218  vpMatrix L;
219 
221  for (unsigned int i = 0; i < nbParameters; i++) {
222  if (flags[i] == false) {
223  switch (i) {
224  case 0:
225  vpTRACE("Warning !!! The interaction matrix is computed but x was "
226  "not set yet");
227  break;
228  case 1:
229  vpTRACE("Warning !!! The interaction matrix is computed but y was "
230  "not set yet");
231  break;
232  case 2:
233  vpTRACE("Warning !!! The interaction matrix is computed but z was "
234  "not set yet");
235  break;
236  default:
237  vpTRACE("Problem during the reading of the variable flags");
238  }
239  }
240  }
241  resetFlags();
242  }
243 
244  L.resize(1, 6);
245 
246  double x_ = get_x();
247  double y_ = get_y();
248  double Z_ = get_Z();
249 
250  if (Z_ < 0) {
251  vpERROR_TRACE("Point is behind the camera ");
252  std::cout << "Z = " << Z_ << std::endl;
253 
254  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
255  }
256 
257  if (fabs(Z_) < 1e-6) {
258  vpERROR_TRACE("Point Z coordinates is null ");
259  std::cout << "Z = " << Z_ << std::endl;
260 
261  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
262  }
263 
264  if (FEATURE_ALL & select) {
265  L = 0;
266  L[0][0] = 0;
267  L[0][1] = 0;
268  L[0][2] = -1 / Z_;
269  L[0][3] = -y_;
270  L[0][4] = x_;
271  L[0][5] = 0;
272  }
273 
274  return L;
275 }
276 
313 vpColVector vpFeatureDepth::error(const vpBasicFeature &s_star, unsigned int select)
314 {
315 
316  if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
317  vpERROR_TRACE("s* should be zero ! ");
318  throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
319  }
320 
321  vpColVector e(1);
322  if (FEATURE_ALL & select) {
323  e[0] = s[0];
324  }
325 
326  return e;
327 }
328 
344 void vpFeatureDepth::print(unsigned int select) const
345 {
346  if (FEATURE_ALL & select) {
347  std::cout << "Point: x=" << get_x();
348  std::cout << " Point: y=" << get_y();
349  std::cout << " Point: Z=" << get_Z();
350 
351  std::cout << " log(Z/Z*)=" << get_LogZoverZstar();
352 
353  std::cout << std::endl;
354  }
355 }
356 
368 void vpFeatureDepth::buildFrom(double x_, double y_, double Z_, double LogZoverZstar)
369 {
370 
371  s[0] = LogZoverZstar;
372 
373  this->x = x_;
374  this->y = y_;
375  this->Z = Z_;
376 
377  if (Z < 0) {
378  vpERROR_TRACE("Point is behind the camera ");
379  std::cout << "Z = " << Z << std::endl;
380 
381  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
382  }
383 
384  if (fabs(Z) < 1e-6) {
385  vpERROR_TRACE("Point Z coordinates is null ");
386  std::cout << "Z = " << Z << std::endl;
387 
388  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
389  }
390 
391  for (unsigned int i = 0; i < nbParameters; i++)
392  flags[i] = true;
393 }
394 
406 {
407  vpFeatureDepth *feature = new vpFeatureDepth;
408  return feature;
409 }
410 
416 void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage<unsigned char> & /* I */,
417  const vpColor & /* color */, unsigned int /* thickness */) const
418 {
419  static int firsttime = 0;
420 
421  if (firsttime == 0) {
422  firsttime = 1;
423  vpERROR_TRACE("not implemented");
424  // Do not throw and error since it is not subject
425  // to produce a failure
426  }
427 }
433 void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
434  const vpColor & /* color */, unsigned int /* thickness */) const
435 {
436  static int firsttime = 0;
437 
438  if (firsttime == 0) {
439  firsttime = 1;
440  vpERROR_TRACE("not implemented");
441  // Do not throw and error since it is not subject
442  // to produce a failure
443  }
444 }
445 
446 /*
447  * Local variables:
448  * c-basic-offset: 2
449  * End:
450  */
class that defines what is a visual feature
vpColVector s
State of the visual feature.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void print(unsigned int select=FEATURE_ALL) const vp_override
void set_x(double x)
void init() vp_override
void buildFrom(double x, double y, double Z, double LogZoverZstar)
double get_y() const
void set_y(double y)
double get_LogZoverZstar() const
void set_LogZoverZstar(double LogZoverZstar)
void set_xyZLogZoverZstar(double x, double y, double Z, double logZZs)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
double get_Z() const
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
void set_Z(double Z)
double get_x() const
vpFeatureDepth * duplicate() const vp_override
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
#define vpTRACE
Definition: vpDebug.h:405
#define vpERROR_TRACE
Definition: vpDebug.h:382