Visual Servoing Platform  version 3.2.1 under development (2019-07-16)
vpFeaturePoint3D.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <visp3/visual_features/vpBasicFeature.h>
41 #include <visp3/visual_features/vpFeaturePoint3D.h>
42 
43 // Exception
44 #include <visp3/core/vpException.h>
45 #include <visp3/visual_features/vpFeatureException.h>
46 
47 // Debug trace
48 #include <visp3/core/vpDebug.h>
49 
50 /*
51 
52  attributes and members directly related to the vpBasicFeature needs
53  other functionalities are useful but not mandatory
54 
55 */
56 
65 {
66  // feature dimension
67  dim_s = 3;
68  nbParameters = 3;
69 
70  // memory allocation
71  s.resize(dim_s);
72  if (flags == NULL)
73  flags = new bool[nbParameters];
74  for (unsigned int i = 0; i < nbParameters; i++)
75  flags[i] = false;
76 
77  // default value XYZ
78  s[0] = 0;
79  s[1] = 0;
80  s[2] = 1;
81 }
82 
90 
100 void vpFeaturePoint3D::set_X(const double X)
101 {
102  s[0] = X;
103  flags[0] = true;
104 }
105 
115 void vpFeaturePoint3D::set_Y(const double Y)
116 {
117  s[1] = Y;
118  flags[1] = true;
119 }
120 
130 void vpFeaturePoint3D::set_Z(const double Z)
131 {
132  s[2] = Z;
133  flags[2] = true;
134 }
135 
144 void vpFeaturePoint3D::set_XYZ(const double X, const double Y, const double Z)
145 {
146  set_X(X);
147  set_Y(Y);
148  set_Z(Z);
149 
150  for (unsigned int i = 0; i < nbParameters; i++)
151  flags[i] = true;
152 }
153 
155 double vpFeaturePoint3D::get_X() const { return s[0]; }
156 
158 double vpFeaturePoint3D::get_Y() const { return s[1]; }
159 
161 double vpFeaturePoint3D::get_Z() const { return s[2]; }
162 
230 vpMatrix vpFeaturePoint3D::interaction(const unsigned int select)
231 {
232  vpMatrix L;
233 
234  L.resize(0, 6);
235 
237  for (unsigned int i = 0; i < nbParameters; i++) {
238  if (flags[i] == false) {
239  switch (i) {
240  case 0:
241  vpTRACE("Warning !!! The interaction matrix is computed but X was "
242  "not set yet");
243  break;
244  case 1:
245  vpTRACE("Warning !!! The interaction matrix is computed but Y was "
246  "not set yet");
247  break;
248  case 2:
249  vpTRACE("Warning !!! The interaction matrix is computed but Z was "
250  "not set yet");
251  break;
252  default:
253  vpTRACE("Problem during the reading of the variable flags");
254  }
255  }
256  }
257  resetFlags();
258  }
259 
260  double X = get_X();
261  double Y = get_Y();
262  double Z = get_Z();
263 
264  if (vpFeaturePoint3D::selectX() & select) {
265  vpMatrix Lx(1, 6);
266  Lx = 0;
267 
268  Lx[0][0] = -1;
269  Lx[0][1] = 0;
270  Lx[0][2] = 0;
271  Lx[0][3] = 0;
272  Lx[0][4] = -Z;
273  Lx[0][5] = Y;
274 
275  L = vpMatrix::stack(L, Lx);
276  }
277 
278  if (vpFeaturePoint3D::selectY() & select) {
279  vpMatrix Ly(1, 6);
280  Ly = 0;
281 
282  Ly[0][0] = 0;
283  Ly[0][1] = -1;
284  Ly[0][2] = 0;
285  Ly[0][3] = Z;
286  Ly[0][4] = 0;
287  Ly[0][5] = -X;
288 
289  L = vpMatrix::stack(L, Ly);
290  }
291  if (vpFeaturePoint3D::selectZ() & select) {
292  vpMatrix Lz(1, 6);
293  Lz = 0;
294 
295  Lz[0][0] = 0;
296  Lz[0][1] = 0;
297  Lz[0][2] = -1;
298  Lz[0][3] = -Y;
299  Lz[0][4] = X;
300  Lz[0][5] = 0;
301 
302  L = vpMatrix::stack(L, Lz);
303  }
304  return L;
305 }
306 
358 vpColVector vpFeaturePoint3D::error(const vpBasicFeature &s_star, const unsigned int select)
359 {
360  vpColVector e(0);
361 
362  try {
363  if (vpFeaturePoint3D::selectX() & select) {
364  vpColVector ex(1);
365  ex[0] = s[0] - s_star[0];
366 
367  e = vpColVector::stack(e, ex);
368  }
369 
370  if (vpFeaturePoint3D::selectY() & select) {
371  vpColVector ey(1);
372  ey[0] = s[1] - s_star[1];
373  e = vpColVector::stack(e, ey);
374  }
375 
376  if (vpFeaturePoint3D::selectZ() & select) {
377  vpColVector ez(1);
378  ez[0] = s[2] - s_star[2];
379  e = vpColVector::stack(e, ez);
380  }
381  } catch (...) {
382  throw;
383  }
384 
385  return e;
386 }
387 
405 {
406 
407  // cP is expressed in homogeneous coordinates
408  // we devide by the fourth coordinate
409  s[0] = p.cP[0] / p.cP[3];
410  s[1] = p.cP[1] / p.cP[3];
411  s[2] = p.cP[2] / p.cP[3];
412 
413  double Z = s[2];
414  if (Z < 0) {
415  vpERROR_TRACE("Point is behind the camera ");
416  std::cout << "Z = " << Z << std::endl;
417 
418  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
419  }
420 
421  if (fabs(Z) < 1e-6) {
422  vpERROR_TRACE("Point Z coordinates is null ");
423  std::cout << "Z = " << Z << std::endl;
424 
425  throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
426  }
427 
428  for (unsigned int i = 0; i < nbParameters; i++)
429  flags[i] = true;
430 }
431 
448 void vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z)
449 {
450 
451  s[0] = X;
452  s[1] = Y;
453  s[2] = Z;
454 
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 
496 void vpFeaturePoint3D::print(const unsigned int select) const
497 {
498 
499  std::cout << "Point3D: ";
500  if (vpFeaturePoint3D::selectX() & select)
501  std::cout << " X=" << get_X();
502  if (vpFeaturePoint3D::selectY() & select)
503  std::cout << " Y=" << get_Y();
504  if (vpFeaturePoint3D::selectZ() & select)
505  std::cout << " Z=" << get_Z();
506  std::cout << std::endl;
507 }
508 
521 {
522  vpFeaturePoint3D *feature = new vpFeaturePoint3D;
523  return feature;
524 }
525 
531  const vpColor & /* color */, unsigned int /* thickness */) const
532 {
533  static int firsttime = 0;
534 
535  if (firsttime == 0) {
536  firsttime = 1;
537  vpERROR_TRACE("not implemented");
538  // Do not throw and error since it is not subject
539  // to produce a failure
540  }
541 }
542 
547 void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage<vpRGBa> & /* I */,
548  const vpColor & /* color */, unsigned int /* thickness */) const
549 {
550  static int firsttime = 0;
551 
552  if (firsttime == 0) {
553  firsttime = 1;
554  vpERROR_TRACE("not implemented");
555  // Do not throw and error since it is not subject
556  // to produce a failure
557  }
558 }
584 unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0]; }
585 
611 unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1]; }
612 
637 unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2]; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void set_X(const double X)
void set_XYZ(const double X, const double Y, const double Z)
double get_Z() const
Return the coordinate in the camera frame of the 3D point.
void set_Z(const double Z)
void print(const unsigned int select=FEATURE_ALL) const
#define vpERROR_TRACE
Definition: vpDebug.h:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4551
unsigned int dim_s
Dimension of the visual feature.
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
void buildFrom(const vpPoint &p)
Class that defines what is a point.
Definition: vpPoint.h:58
static unsigned int selectY()
vpColVector cP
Definition: vpTracker.h:75
class that defines what is a visual feature
vpFeaturePoint3D * duplicate() const
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
#define vpTRACE
Definition: vpDebug.h:416
Error that can be emited by the vpBasicFeature class and its derivates.
Class that defines the 3D point visual feature.
Generic class defining intrinsic camera parameters.
static unsigned int selectZ()
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
double get_X() const
Return the coordinate in the camera frame of the 3D point.
void set_Y(const double Y)
static unsigned int selectX()
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:310
double get_Y() const
Return the coordinate in the camera frame of the 3D point.