Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpFeaturePoint.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/vpFeaturePoint.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 attributes and members directly related to the vpBasicFeature needs
59 other functionalities ar useful but not mandatory
60 
61 */
62 
67 {
68  // feature dimension
69  dim_s = 2;
70  nbParameters = 3;
71 
72  // memory allocation
73  s.resize(dim_s);
74  if (flags == nullptr)
75  flags = new bool[nbParameters];
76  for (unsigned int i = 0; i < nbParameters; i++)
77  flags[i] = false;
78 
79  // default value Z (1 meters)
80  Z = 1;
81 }
82 
87 
94 void vpFeaturePoint::set_Z(double Z_)
95 {
96  this->Z = Z_;
97  flags[2] = true;
98 }
99 
106 double vpFeaturePoint::get_Z() const { return Z; }
107 
115 void vpFeaturePoint::set_x(double x)
116 {
117  s[0] = x;
118  flags[0] = true;
119 }
120 
127 double vpFeaturePoint::get_x() const { return s[0]; }
128 
135 void vpFeaturePoint::set_y(double y)
136 {
137  s[1] = y;
138  flags[1] = true;
139 }
140 
147 double vpFeaturePoint::get_y() const { return s[1]; }
148 
160 void vpFeaturePoint::set_xyZ(double x_, double y_, double Z_)
161 {
162  set_x(x_);
163  set_y(y_);
164  set_Z(Z_);
165  for (unsigned int i = 0; i < nbParameters; i++)
166  flags[i] = true;
167 }
168 
213 {
214  vpMatrix L;
215 
216  L.resize(0, 6);
217 
219  for (unsigned int i = 0; i < nbParameters; i++) {
220  if (flags[i] == false) {
221  switch (i) {
222  case 0:
223  vpTRACE("Warning !!! The interaction matrix is computed but x was "
224  "not set yet");
225  break;
226  case 1:
227  vpTRACE("Warning !!! The interaction matrix is computed but y was "
228  "not set yet");
229  break;
230  case 2:
231  vpTRACE("Warning !!! The interaction matrix is computed but Z was "
232  "not set yet");
233  break;
234  default:
235  vpTRACE("Problem during the reading of the variable flags");
236  }
237  }
238  }
239  resetFlags();
240  }
241 
242  double x_ = get_x();
243  double y_ = get_y();
244  double Z_ = get_Z();
245 
246  if (Z_ < 0) {
247  vpERROR_TRACE("Point is behind the camera ");
248  std::cout << "Z = " << Z_ << std::endl;
249 
250  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
251  }
252 
253  if (fabs(Z_) < 1e-6) {
254  vpERROR_TRACE("Point Z coordinates is null ");
255  std::cout << "Z = " << Z_ << std::endl;
256 
257  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
258  }
259 
260  if (vpFeaturePoint::selectX() & select) {
261  vpMatrix Lx(1, 6);
262  Lx = 0;
263 
264  Lx[0][0] = -1 / Z_;
265  Lx[0][1] = 0;
266  Lx[0][2] = x_ / Z_;
267  Lx[0][3] = x_ * y_;
268  Lx[0][4] = -(1 + x_ * x_);
269  Lx[0][5] = y_;
270 
271  L = vpMatrix::stack(L, Lx);
272  }
273 
274  if (vpFeaturePoint::selectY() & select) {
275  vpMatrix Ly(1, 6);
276  Ly = 0;
277 
278  Ly[0][0] = 0;
279  Ly[0][1] = -1 / Z_;
280  Ly[0][2] = y_ / Z_;
281  Ly[0][3] = 1 + y_ * y_;
282  Ly[0][4] = -x_ * y_;
283  Ly[0][5] = -x_;
284 
285  L = vpMatrix::stack(L, Ly);
286  }
287  return L;
288 }
289 
326 vpColVector vpFeaturePoint::error(const vpBasicFeature &s_star, unsigned int select)
327 {
328  vpColVector e(0);
329 
330  try {
331  if (vpFeaturePoint::selectX() & select) {
332  vpColVector ex(1);
333  ex[0] = s[0] - s_star[0];
334 
335  e = vpColVector::stack(e, ex);
336  }
337 
338  if (vpFeaturePoint::selectY() & select) {
339  vpColVector ey(1);
340  ey[0] = s[1] - s_star[1];
341  e = vpColVector::stack(e, ey);
342  }
343  } catch (...) {
344  throw;
345  }
346 
347  return e;
348 }
349 
369 void vpFeaturePoint::print(unsigned int select) const
370 {
371 
372  std::cout << "Point: Z=" << get_Z();
373  if (vpFeaturePoint::selectX() & select)
374  std::cout << " x=" << get_x();
375  if (vpFeaturePoint::selectY() & select)
376  std::cout << " y=" << get_y();
377  std::cout << std::endl;
378 }
379 
392 void vpFeaturePoint::buildFrom(double x_, double y_, double Z_)
393 {
394 
395  s[0] = x_;
396  s[1] = y_;
397 
398  this->Z = Z_;
399 
400  if (Z_ < 0) {
401  vpERROR_TRACE("Point is behind the camera ");
402  std::cout << "Z = " << Z_ << std::endl;
403 
404  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
405  }
406 
407  if (fabs(Z_) < 1e-6) {
408  vpERROR_TRACE("Point Z coordinates is null ");
409  std::cout << "Z = " << Z_ << std::endl;
410 
411  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
412  }
413 
414  for (unsigned int i = 0; i < nbParameters; i++)
415  flags[i] = true;
416 }
417 
429  unsigned int thickness) const
430 {
431  try {
432  double x, y;
433  x = get_x();
434  y = get_y();
435 
436  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
437 
438  } catch (...) {
439  vpERROR_TRACE("Error caught");
440  throw;
441  }
442 }
443 
454 void vpFeaturePoint::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
455  unsigned int thickness) const
456 {
457  try {
458  double x, y;
459  x = get_x();
460  y = get_y();
461 
462  vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
463 
464  } catch (...) {
465  vpERROR_TRACE("Error caught");
466  throw;
467  }
468 }
469 
481 {
482  vpFeaturePoint *feature = new vpFeaturePoint;
483  return feature;
484 }
485 
504 unsigned int vpFeaturePoint::selectX() { return FEATURE_LINE[0]; }
505 
524 unsigned int vpFeaturePoint::selectY() { return FEATURE_LINE[1]; }
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
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
void stack(double d)
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
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
static unsigned int selectX()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
void set_xyZ(double x, double y, double Z)
static unsigned int selectY()
void set_y(double y)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
double get_y() const
void init() vp_override
double get_x() const
void set_x(double x)
void print(unsigned int select=FEATURE_ALL) const vp_override
double get_Z() const
void set_Z(double Z)
vpFeaturePoint * duplicate() const vp_override
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5601
#define vpTRACE
Definition: vpDebug.h:405
#define vpERROR_TRACE
Definition: vpDebug.h:382