Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
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 attributes and members directly related to the vpBasicFeature needs
58 other functionalities ar useful but not mandatory
59 */
60 
61 BEGIN_VISP_NAMESPACE
66 {
67  // feature dimension
68  dim_s = 1;
69  nbParameters = 3;
70 
71  // memory allocation
72  s.resize(dim_s);
73  if (flags == nullptr)
74  flags = new bool[nbParameters];
75  for (unsigned int i = 0; i < nbParameters; i++)
76  flags[i] = false;
77  x = y = 0.;
78  Z = 1.;
79 }
80 
84 vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.) { init(); }
85 
92 void vpFeatureDepth::set_LogZoverZstar(double LogZoverZstar) { s[0] = LogZoverZstar; }
93 
100 double vpFeatureDepth::get_LogZoverZstar() const { return s[0]; }
101 
108 void vpFeatureDepth::set_x(double x_)
109 {
110  this->x = x_;
111  flags[0] = true;
112 }
113 
120 double vpFeatureDepth::get_x() const { return x; }
121 
128 void vpFeatureDepth::set_y(double y_)
129 {
130  this->y = y_;
131  flags[1] = true;
132 }
133 
140 double vpFeatureDepth::get_y() const { return y; }
141 
148 void vpFeatureDepth::set_Z(double Z_)
149 {
150  this->Z = Z_;
151  flags[2] = true;
152 }
153 
160 double vpFeatureDepth::get_Z() const { return Z; }
161 
174 void vpFeatureDepth::set_xyZLogZoverZstar(double x_, double y_, double Z_, double LogZoverZstar)
175 {
176  set_x(x_);
177  set_y(y_);
178  set_Z(Z_);
179  set_LogZoverZstar(LogZoverZstar);
180  for (unsigned int i = 0; i < nbParameters; i++)
181  flags[i] = true;
182 }
183 
210 {
211  vpMatrix L;
212 
214  for (unsigned int i = 0; i < nbParameters; i++) {
215  if (flags[i] == false) {
216  switch (i) {
217  case 0:
218  vpTRACE("Warning !!! The interaction matrix is computed but x was "
219  "not set yet");
220  break;
221  case 1:
222  vpTRACE("Warning !!! The interaction matrix is computed but y was "
223  "not set yet");
224  break;
225  case 2:
226  vpTRACE("Warning !!! The interaction matrix is computed but z was "
227  "not set yet");
228  break;
229  default:
230  vpTRACE("Problem during the reading of the variable flags");
231  }
232  }
233  }
234  resetFlags();
235  }
236 
237  L.resize(1, 6);
238 
239  double x_ = get_x();
240  double y_ = get_y();
241  double Z_ = get_Z();
242 
243  if (Z_ < 0) {
244  vpERROR_TRACE("Point is behind the camera ");
245  std::cout << "Z = " << Z_ << std::endl;
246 
247  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
248  }
249 
250  if (fabs(Z_) < 1e-6) {
251  vpERROR_TRACE("Point Z coordinates is null ");
252  std::cout << "Z = " << Z_ << std::endl;
253 
254  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
255  }
256 
257  if (FEATURE_ALL & select) {
258  L = 0;
259  L[0][0] = 0;
260  L[0][1] = 0;
261  L[0][2] = -1 / Z_;
262  L[0][3] = -y_;
263  L[0][4] = x_;
264  L[0][5] = 0;
265  }
266 
267  return L;
268 }
269 
306 vpColVector vpFeatureDepth::error(const vpBasicFeature &s_star, unsigned int select)
307 {
308 
309  if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
310  vpERROR_TRACE("s* should be zero ! ");
311  throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
312  }
313 
314  vpColVector e(1);
315  if (FEATURE_ALL & select) {
316  e[0] = s[0];
317  }
318 
319  return e;
320 }
321 
337 void vpFeatureDepth::print(unsigned int select) const
338 {
339  if (FEATURE_ALL & select) {
340  std::cout << "Point: x=" << get_x();
341  std::cout << " Point: y=" << get_y();
342  std::cout << " Point: Z=" << get_Z();
343 
344  std::cout << " log(Z/Z*)=" << get_LogZoverZstar();
345 
346  std::cout << std::endl;
347  }
348 }
349 
361 vpFeatureDepth &vpFeatureDepth::buildFrom(const double &x_, const double &y_, const double &Z_, const double &LogZoverZstar)
362 {
363 
364  s[0] = LogZoverZstar;
365 
366  this->x = x_;
367  this->y = y_;
368  this->Z = Z_;
369 
370  if (Z < 0) {
371  vpERROR_TRACE("Point is behind the camera ");
372  std::cout << "Z = " << Z << std::endl;
373 
374  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
375  }
376 
377  if (fabs(Z) < 1e-6) {
378  vpERROR_TRACE("Point Z coordinates is null ");
379  std::cout << "Z = " << Z << std::endl;
380 
381  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
382  }
383 
384  for (unsigned int i = 0; i < nbParameters; ++i) {
385  flags[i] = true;
386  }
387  return *this;
388 }
389 
401 {
402  vpFeatureDepth *feature = new vpFeatureDepth;
403  return feature;
404 }
405 
411 void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage<unsigned char> & /* I */,
412  const vpColor & /* color */, unsigned int /* thickness */) const
413 {
414  static int firsttime = 0;
415 
416  if (firsttime == 0) {
417  firsttime = 1;
418  vpERROR_TRACE("not implemented");
419  // Do not throw and error since it is not subject
420  // to produce a failure
421  }
422 }
428 void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
429  const vpColor & /* color */, unsigned int /* thickness */) const
430 {
431  static int firsttime = 0;
432 
433  if (firsttime == 0) {
434  firsttime = 1;
435  vpERROR_TRACE("not implemented");
436  // Do not throw and error since it is not subject
437  // to produce a failure
438  }
439 }
440 END_VISP_NAMESPACE
441 /*
442  * Local variables:
443  * c-basic-offset: 2
444  * End:
445  */
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:191
double sumSquare() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void set_x(double x)
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
double get_y() const
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
vpFeatureDepth & buildFrom(const double &x, const double &y, const double &Z, const double &LogZoverZstar)
void set_y(double y)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const VP_OVERRIDE
void init() VP_OVERRIDE
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) VP_OVERRIDE
double get_LogZoverZstar() const
void set_LogZoverZstar(double LogZoverZstar)
void set_xyZLogZoverZstar(double x, double y, double Z, double logZZs)
vpFeatureDepth * duplicate() const VP_OVERRIDE
double get_Z() const
void set_Z(double Z)
double get_x() const
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:169