Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpPoint.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  * Point feature.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpDebug.h>
40 #include <visp3/core/vpFeatureDisplay.h>
41 #include <visp3/core/vpPoint.h>
42 
49 {
50  p.resize(3);
51  p = 0;
52  p[2] = 1;
53  oP.resize(4);
54  oP = 0;
55  oP[3] = 1;
56  cP.resize(4);
57  cP = 0;
58  cP[3] = 1;
59 
60  // default value Z (1 meters)
61  set_Z(1);
62 }
63 
65 
70 vpPoint::vpPoint(double oX, double oY, double oZ)
71 {
72  init();
73  setWorldCoordinates(oX, oY, oZ);
74 }
75 
87 {
88  init();
90 }
91 
102 vpPoint::vpPoint(const std::vector<double> &oP_)
103 {
104  init();
105  setWorldCoordinates(oP_);
106 }
107 
113 void vpPoint::setWorldCoordinates(double oX, double oY, double oZ)
114 {
115  oP[0] = oX;
116  oP[1] = oY;
117  oP[2] = oZ;
118  oP[3] = 1;
119 }
120 
132 {
133  if (oP_.size() == 3) {
134  oP[0] = oP_[0];
135  oP[1] = oP_[1];
136  oP[2] = oP_[2];
137  oP[3] = 1.;
138  } else if (oP_.size() == 4) {
139  oP[0] = oP_[0];
140  oP[1] = oP_[1];
141  oP[2] = oP_[2];
142  oP[3] = oP_[3];
143  oP /= oP[3];
144  } else {
145  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
146  }
147 }
148 
159 void vpPoint::setWorldCoordinates(const std::vector<double> &oP_)
160 {
161  if (oP_.size() == 3) {
162  oP[0] = oP_[0];
163  oP[1] = oP_[1];
164  oP[2] = oP_[2];
165  oP[3] = 1.;
166  } else if (oP_.size() == 4) {
167  oP[0] = oP_[0];
168  oP[1] = oP_[1];
169  oP[2] = oP_[2];
170  oP[3] = oP_[3];
171  oP /= oP[3];
172  } else {
173  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
174  }
175 }
176 
178 void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
179 {
180  oX = oP[0];
181  oY = oP[1];
182  oZ = oP[2];
183 }
184 
192 
199 void vpPoint::getWorldCoordinates(std::vector<double> &oP_)
200 {
201  oP_.resize(oP.size());
202  for (unsigned int i = 0; i < oP.size(); i++)
203  oP_[i] = oP[i];
204 }
205 
213 
222 void vpPoint::projection(const vpColVector &_cP, vpColVector &_p) const
223 {
224  _p.resize(3, false);
225 
226  _p[0] = _cP[0] / _cP[2];
227  _p[1] = _cP[1] / _cP[2];
228  _p[2] = 1;
229 }
230 
240 {
241  _cP.resize(4, false);
242 
243  _cP[0] = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
244  _cP[1] = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
245  _cP[2] = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
246  _cP[3] = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
247 
248  double d = 1 / _cP[3];
249  _cP[0] *= d;
250  _cP[1] *= d;
251  _cP[2] *= d;
252  _cP[3] *= d;
253 }
254 
264 {
265  double X = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
266  double Y = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
267  double Z = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
268  double W = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
269 
270  double d = 1 / W;
271  cP[0] = X * d;
272  cP[1] = Y * d;
273  cP[2] = Z * d;
274  cP[3] = 1;
275 }
276 
277 #if 0
278 
288 const vpPoint
289 operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
290 {
291  vpPoint aP ;
292 
293  vpColVector v(4),v1(4) ;
294 
295  v[0] = bP.get_X() ;
296  v[1] = bP.get_Y() ;
297  v[2] = bP.get_Z() ;
298  v[3] = bP.get_W() ;
299 
300  v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
301  v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
302  v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
303  v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;
304 
305  v1 /= v1[3] ;
306 
307  // v1 = M*v ;
308  aP.set_X(v1[0]) ;
309  aP.set_Y(v1[1]) ;
310  aP.set_Z(v1[2]) ;
311  aP.set_W(v1[3]) ;
312 
313  aP.set_oX(v1[0]) ;
314  aP.set_oY(v1[1]) ;
315  aP.set_oZ(v1[2]) ;
316  aP.set_oW(v1[3]) ;
317 
318  return aP ;
319 }
320 
330 const vpPoint
331 operator*(const vpHomography &aHb, const vpPoint& bP)
332 {
333  vpPoint aP ;
334  vpColVector v(3),v1(3) ;
335 
336  v[0] = bP.get_x() ;
337  v[1] = bP.get_y() ;
338  v[2] = bP.get_w() ;
339 
340  v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
341  v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
342  v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;
343 
344  // v1 = M*v ;
345  aP.set_x(v1[0]) ;
346  aP.set_y(v1[1]) ;
347  aP.set_w(v1[2]) ;
348 
349  return aP ;
350 }
351 #endif
354 {
355  vpPoint *feature = new vpPoint(*this);
356  return feature;
357 }
358 
371  const vpColor &color, unsigned int thickness)
372 {
373 
374  vpColVector _cP, _p;
375  changeFrame(cMo, _cP);
376 
377  if (_cP[2] < 0) // no display if point is behind the camera
378  return;
379 
380  vpPoint::projection(_cP, _p);
381  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
382 }
383 
396  const vpColor &color, unsigned int thickness)
397 {
398  vpColVector _cP, _p;
399  changeFrame(cMo, _cP);
400 
401  if (_cP[2] < 0) // no display if point is behind the camera
402  return;
403 
404  vpPoint::projection(_cP, _p);
405  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
406 }
407 
408 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }
409 
410 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
412 {
413  p = vpp.p;
414  cP = vpp.cP;
415  oP = vpp.oP;
416  cPAvailable = vpp.cPAvailable;
417 
418  return *this;
419 }
420 #endif
421 
430 void vpPoint::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
431  unsigned int thickness)
432 {
433  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
434 }
435 
444 void vpPoint::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
445  unsigned int thickness)
446 {
447  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
448 }
449 
450 // Get coordinates
452 double vpPoint::get_X() const { return cP[0]; }
454 double vpPoint::get_Y() const { return cP[1]; }
456 double vpPoint::get_Z() const { return cP[2]; }
458 double vpPoint::get_W() const { return cP[3]; }
459 
461 double vpPoint::get_oX() const { return oP[0]; }
463 double vpPoint::get_oY() const { return oP[1]; }
465 double vpPoint::get_oZ() const { return oP[2]; }
467 double vpPoint::get_oW() const { return oP[3]; }
468 
470 double vpPoint::get_x() const { return p[0]; }
472 double vpPoint::get_y() const { return p[1]; }
474 double vpPoint::get_w() const { return p[2]; }
475 
485 {
486  double d = 1 / cP[2];
487  p[0] = cP[0] * d;
488  p[1] = cP[1] * d;
489  p[2] = 1;
490 }
491 
493 void vpPoint::set_X(double cX) { cP[0] = cX; }
495 void vpPoint::set_Y(double cY) { cP[1] = cY; }
497 void vpPoint::set_Z(double cZ) { cP[2] = cZ; }
499 void vpPoint::set_W(double cW) { cP[3] = cW; }
500 
502 void vpPoint::set_oX(double oX) { oP[0] = oX; }
504 void vpPoint::set_oY(double oY) { oP[1] = oY; }
506 void vpPoint::set_oZ(double oZ) { oP[2] = oZ; }
508 void vpPoint::set_oW(double oW) { oP[3] = oW; }
509 
511 void vpPoint::set_x(double x) { p[0] = x; }
513 void vpPoint::set_y(double y) { p[1] = y; }
515 void vpPoint::set_w(double w) { p[2] = w; }
vpPoint()
Basic constructor.
Definition: vpPoint.cpp:64
vpPoint * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:353
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpPoint.cpp:430
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
void init()
Basic construction.
Definition: vpPoint.cpp:48
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:212
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
bool cPAvailable
Definition: vpTracker.h:83
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:499
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
error that can be emited by ViSP classes.
Definition: vpException.h:71
double get_oW() const
Get the point oW coordinate in the object frame.
Definition: vpPoint.cpp:467
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:497
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:458
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:493
vpColVector cP
Definition: vpTracker.h:77
vpColVector operator*(const double &x, const vpColVector &v)
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:474
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:504
void projection()
Definition: vpPoint.cpp:484
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpPoint &vpp)
Definition: vpPoint.cpp:408
Generic class defining intrinsic camera parameters.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:508
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:506
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:452
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:495
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:502
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:515
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
vpPoint & operator=(const vpPoint &vpp)=default
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:456
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:454
vpColVector p
Definition: vpTracker.h:73