Visual Servoing Platform  version 3.6.1 under development (2024-05-26)
vpFeaturePoint3D.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  * 3D point visual feature.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/visual_features/vpBasicFeature.h>
37 #include <visp3/visual_features/vpFeaturePoint3D.h>
38 
39 // Exception
40 #include <visp3/core/vpException.h>
41 #include <visp3/visual_features/vpFeatureException.h>
42 
43 // Debug trace
44 #include <visp3/core/vpDebug.h>
45 
46 /*
47 
48  attributes and members directly related to the vpBasicFeature needs
49  other functionalities are useful but not mandatory
50 
51 */
52 
61 {
62  // feature dimension
63  dim_s = 3;
64  nbParameters = 3;
65 
66  // memory allocation
67  s.resize(dim_s);
68  if (flags == nullptr)
69  flags = new bool[nbParameters];
70  for (unsigned int i = 0; i < nbParameters; i++)
71  flags[i] = false;
72 
73  // default value XYZ
74  s[0] = 0;
75  s[1] = 0;
76  s[2] = 1;
77 }
78 
86 
97 {
98  s[0] = X;
99  flags[0] = true;
100 }
101 
112 {
113  s[1] = Y;
114  flags[1] = true;
115 }
116 
127 {
128  s[2] = Z;
129  flags[2] = true;
130 }
131 
140 void vpFeaturePoint3D::set_XYZ(double X, double Y, double Z)
141 {
142  set_X(X);
143  set_Y(Y);
144  set_Z(Z);
145 
146  for (unsigned int i = 0; i < nbParameters; i++)
147  flags[i] = true;
148 }
149 
151 double vpFeaturePoint3D::get_X() const { return s[0]; }
152 
154 double vpFeaturePoint3D::get_Y() const { return s[1]; }
155 
157 double vpFeaturePoint3D::get_Z() const { return s[2]; }
158 
227 {
228  vpMatrix L;
229 
230  L.resize(0, 6);
231 
233  for (unsigned int i = 0; i < nbParameters; i++) {
234  if (flags[i] == false) {
235  switch (i) {
236  case 0:
237  vpTRACE("Warning !!! The interaction matrix is computed but X was "
238  "not set yet");
239  break;
240  case 1:
241  vpTRACE("Warning !!! The interaction matrix is computed but Y was "
242  "not set yet");
243  break;
244  case 2:
245  vpTRACE("Warning !!! The interaction matrix is computed but Z was "
246  "not set yet");
247  break;
248  default:
249  vpTRACE("Problem during the reading of the variable flags");
250  }
251  }
252  }
253  resetFlags();
254  }
255 
256  double X = get_X();
257  double Y = get_Y();
258  double Z = get_Z();
259 
260  if (vpFeaturePoint3D::selectX() & select) {
261  vpMatrix Lx(1, 6);
262  Lx = 0;
263 
264  Lx[0][0] = -1;
265  Lx[0][1] = 0;
266  Lx[0][2] = 0;
267  Lx[0][3] = 0;
268  Lx[0][4] = -Z;
269  Lx[0][5] = Y;
270 
271  L = vpMatrix::stack(L, Lx);
272  }
273 
274  if (vpFeaturePoint3D::selectY() & select) {
275  vpMatrix Ly(1, 6);
276  Ly = 0;
277 
278  Ly[0][0] = 0;
279  Ly[0][1] = -1;
280  Ly[0][2] = 0;
281  Ly[0][3] = Z;
282  Ly[0][4] = 0;
283  Ly[0][5] = -X;
284 
285  L = vpMatrix::stack(L, Ly);
286  }
287  if (vpFeaturePoint3D::selectZ() & select) {
288  vpMatrix Lz(1, 6);
289  Lz = 0;
290 
291  Lz[0][0] = 0;
292  Lz[0][1] = 0;
293  Lz[0][2] = -1;
294  Lz[0][3] = -Y;
295  Lz[0][4] = X;
296  Lz[0][5] = 0;
297 
298  L = vpMatrix::stack(L, Lz);
299  }
300  return L;
301 }
302 
354 vpColVector vpFeaturePoint3D::error(const vpBasicFeature &s_star, unsigned int select)
355 {
356  vpColVector e(0);
357 
358  try {
359  if (vpFeaturePoint3D::selectX() & select) {
360  vpColVector ex(1);
361  ex[0] = s[0] - s_star[0];
362 
363  e = vpColVector::stack(e, ex);
364  }
365 
366  if (vpFeaturePoint3D::selectY() & select) {
367  vpColVector ey(1);
368  ey[0] = s[1] - s_star[1];
369  e = vpColVector::stack(e, ey);
370  }
371 
372  if (vpFeaturePoint3D::selectZ() & select) {
373  vpColVector ez(1);
374  ez[0] = s[2] - s_star[2];
375  e = vpColVector::stack(e, ez);
376  }
377  }
378  catch (...) {
379  throw;
380  }
381 
382  return e;
383 }
384 
385 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
403 {
404  build(p);
405 }
406 
423 void vpFeaturePoint3D::buildFrom(double X, double Y, double Z)
424 {
425  build(X, Y, Z);
426 }
427 #endif
428 
446 {
447 
448  // cP is expressed in homogeneous coordinates
449  // we devide by the fourth coordinate
450  s[0] = p.cP[0] / p.cP[3];
451  s[1] = p.cP[1] / p.cP[3];
452  s[2] = p.cP[2] / p.cP[3];
453 
454  double Z = s[2];
455  if (Z < 0) {
456  vpERROR_TRACE("Point is behind the camera ");
457  std::cout << "Z = " << Z << std::endl;
458 
459  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
460  }
461 
462  if (fabs(Z) < 1e-6) {
463  vpERROR_TRACE("Point Z coordinates is null ");
464  std::cout << "Z = " << Z << std::endl;
465 
466  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
467  }
468 
469  for (unsigned int i = 0; i < nbParameters; ++i) {
470  flags[i] = true;
471  }
472  return *this;
473 }
474 
491 vpFeaturePoint3D &vpFeaturePoint3D::build(const double &X, const double &Y, const double &Z)
492 {
493  s[0] = X;
494  s[1] = Y;
495  s[2] = Z;
496 
497  if (Z < 0) {
498  vpERROR_TRACE("Point is behind the camera ");
499  std::cout << "Z = " << Z << std::endl;
500 
501  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
502  }
503 
504  if (fabs(Z) < 1e-6) {
505  vpERROR_TRACE("Point Z coordinates is null ");
506  std::cout << "Z = " << Z << std::endl;
507 
508  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
509  }
510 
511  for (unsigned int i = 0; i < nbParameters; ++i) {
512  flags[i] = true;
513  }
514  return *this;
515 }
516 
540 void vpFeaturePoint3D::print(unsigned int select) const
541 {
542 
543  std::cout << "Point3D: ";
544  if (vpFeaturePoint3D::selectX() & select)
545  std::cout << " X=" << get_X();
546  if (vpFeaturePoint3D::selectY() & select)
547  std::cout << " Y=" << get_Y();
548  if (vpFeaturePoint3D::selectZ() & select)
549  std::cout << " Z=" << get_Z();
550  std::cout << std::endl;
551 }
552 
565 {
566  vpFeaturePoint3D *feature = new vpFeaturePoint3D;
567  return feature;
568 }
569 
575  const vpColor & /* color */, unsigned int /* thickness */) const
576 {
577  static int firsttime = 0;
578 
579  if (firsttime == 0) {
580  firsttime = 1;
581  vpERROR_TRACE("not implemented");
582  // Do not throw and error since it is not subject
583  // to produce a failure
584  }
585 }
586 
591 void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage<vpRGBa> & /* I */,
592  const vpColor & /* color */, unsigned int /* thickness */) const
593 {
594  static int firsttime = 0;
595 
596  if (firsttime == 0) {
597  firsttime = 1;
598  vpERROR_TRACE("not implemented");
599  // Do not throw and error since it is not subject
600  // to produce a failure
601  }
602 }
628 unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0]; }
629 
655 unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1]; }
656 
681 unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2]; }
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:1058
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
Error that can be emitted by the vpBasicFeature class and its derivates.
@ badInitializationError
Wrong feature initialization.
Class that defines the 3D point visual feature.
void print(unsigned int select=FEATURE_ALL) const 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
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
double get_X() const
Return the coordinate in the camera frame of the 3D point.
vpFeaturePoint3D & build(const vpPoint &p)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
void set_XYZ(double X, double Y, double Z)
double get_Y() const
Return the coordinate in the camera frame of the 3D point.
vpFeaturePoint3D * duplicate() const vp_override
static unsigned int selectZ()
void buildFrom(const vpPoint &p)
void set_Y(double Y)
static unsigned int selectY()
void init() vp_override
void set_Z(double Z)
void set_X(double X)
double get_Z() const
Return the coordinate in the camera frame of the 3D point.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5669
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
vpColVector cP
Definition: vpTracker.h:71
#define vpTRACE
Definition: vpDebug.h:407
#define vpERROR_TRACE
Definition: vpDebug.h:384