Visual Servoing Platform  version 3.6.0 under development (2023-09-29)
vpPoint.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  * Point feature.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpDebug.h>
37 #include <visp3/core/vpFeatureDisplay.h>
38 #include <visp3/core/vpPoint.h>
39 
46 {
47  p.resize(3);
48  p = 0;
49  p[2] = 1;
50  oP.resize(4);
51  oP = 0;
52  oP[3] = 1;
53  cP.resize(4);
54  cP = 0;
55  cP[3] = 1;
56 
57  // default value Z (1 meters)
58  set_Z(1);
59 }
60 
62 
67 vpPoint::vpPoint(double oX, double oY, double oZ)
68 {
69  init();
70  setWorldCoordinates(oX, oY, oZ);
71 }
72 
84 {
85  init();
87 }
88 
99 vpPoint::vpPoint(const std::vector<double> &oP_)
100 {
101  init();
102  setWorldCoordinates(oP_);
103 }
104 
110 void vpPoint::setWorldCoordinates(double oX, double oY, double oZ)
111 {
112  oP[0] = oX;
113  oP[1] = oY;
114  oP[2] = oZ;
115  oP[3] = 1;
116 }
117 
129 {
130  if (oP_.size() == 3) {
131  oP[0] = oP_[0];
132  oP[1] = oP_[1];
133  oP[2] = oP_[2];
134  oP[3] = 1.;
135  } else if (oP_.size() == 4) {
136  oP[0] = oP_[0];
137  oP[1] = oP_[1];
138  oP[2] = oP_[2];
139  oP[3] = oP_[3];
140  oP /= oP[3];
141  } else {
142  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
143  }
144 }
145 
156 void vpPoint::setWorldCoordinates(const std::vector<double> &oP_)
157 {
158  if (oP_.size() == 3) {
159  oP[0] = oP_[0];
160  oP[1] = oP_[1];
161  oP[2] = oP_[2];
162  oP[3] = 1.;
163  } else if (oP_.size() == 4) {
164  oP[0] = oP_[0];
165  oP[1] = oP_[1];
166  oP[2] = oP_[2];
167  oP[3] = oP_[3];
168  oP /= oP[3];
169  } else {
170  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
171  }
172 }
173 
175 void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
176 {
177  oX = oP[0];
178  oY = oP[1];
179  oZ = oP[2];
180 }
181 
189 
196 void vpPoint::getWorldCoordinates(std::vector<double> &oP_)
197 {
198  oP_.resize(oP.size());
199  for (unsigned int i = 0; i < oP.size(); i++)
200  oP_[i] = oP[i];
201 }
202 
210 
219 void vpPoint::projection(const vpColVector &_cP, vpColVector &_p) const
220 {
221  _p.resize(3, false);
222 
223  _p[0] = _cP[0] / _cP[2];
224  _p[1] = _cP[1] / _cP[2];
225  _p[2] = 1;
226 }
227 
237 {
238  _cP.resize(4, false);
239 
240  _cP[0] = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
241  _cP[1] = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
242  _cP[2] = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
243  _cP[3] = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
244 
245  double d = 1 / _cP[3];
246  _cP[0] *= d;
247  _cP[1] *= d;
248  _cP[2] *= d;
249  _cP[3] *= d;
250 }
251 
261 {
262  double X = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
263  double Y = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
264  double Z = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
265  double W = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
266 
267  double d = 1 / W;
268  cP[0] = X * d;
269  cP[1] = Y * d;
270  cP[2] = Z * d;
271  cP[3] = 1;
272 }
273 
274 #if 0
285 const vpPoint
286 operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
287 {
288  vpPoint aP ;
289 
290  vpColVector v(4),v1(4) ;
291 
292  v[0] = bP.get_X() ;
293  v[1] = bP.get_Y() ;
294  v[2] = bP.get_Z() ;
295  v[3] = bP.get_W() ;
296 
297  v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
298  v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
299  v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
300  v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;
301 
302  v1 /= v1[3] ;
303 
304  // v1 = M*v ;
305  aP.set_X(v1[0]) ;
306  aP.set_Y(v1[1]) ;
307  aP.set_Z(v1[2]) ;
308  aP.set_W(v1[3]) ;
309 
310  aP.set_oX(v1[0]) ;
311  aP.set_oY(v1[1]) ;
312  aP.set_oZ(v1[2]) ;
313  aP.set_oW(v1[3]) ;
314 
315  return aP ;
316 }
317 
327 const vpPoint
328 operator*(const vpHomography &aHb, const vpPoint& bP)
329 {
330  vpPoint aP ;
331  vpColVector v(3),v1(3) ;
332 
333  v[0] = bP.get_x() ;
334  v[1] = bP.get_y() ;
335  v[2] = bP.get_w() ;
336 
337  v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
338  v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
339  v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;
340 
341  // v1 = M*v ;
342  aP.set_x(v1[0]) ;
343  aP.set_y(v1[1]) ;
344  aP.set_w(v1[2]) ;
345 
346  return aP ;
347 }
348 #endif
351 {
352  vpPoint *feature = new vpPoint(*this);
353  return feature;
354 }
355 
368  const vpColor &color, unsigned int thickness)
369 {
370 
371  vpColVector _cP, _p;
372  changeFrame(cMo, _cP);
373 
374  if (_cP[2] < 0) // no display if point is behind the camera
375  return;
376 
377  vpPoint::projection(_cP, _p);
378  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
379 }
380 
393  const vpColor &color, unsigned int thickness)
394 {
395  vpColVector _cP, _p;
396  changeFrame(cMo, _cP);
397 
398  if (_cP[2] < 0) // no display if point is behind the camera
399  return;
400 
401  vpPoint::projection(_cP, _p);
402  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
403 }
404 
405 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }
406 
407 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
409 {
410  p = vpp.p;
411  cP = vpp.cP;
412  oP = vpp.oP;
413  cPAvailable = vpp.cPAvailable;
414 
415  return *this;
416 }
417 #endif
418 
427 void vpPoint::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
428  unsigned int thickness)
429 {
430  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
431 }
432 
441 void vpPoint::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
442  unsigned int thickness)
443 {
444  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
445 }
446 
447 // Get coordinates
449 double vpPoint::get_X() const { return cP[0]; }
451 double vpPoint::get_Y() const { return cP[1]; }
453 double vpPoint::get_Z() const { return cP[2]; }
455 double vpPoint::get_W() const { return cP[3]; }
456 
458 double vpPoint::get_oX() const { return oP[0]; }
460 double vpPoint::get_oY() const { return oP[1]; }
462 double vpPoint::get_oZ() const { return oP[2]; }
464 double vpPoint::get_oW() const { return oP[3]; }
465 
467 double vpPoint::get_x() const { return p[0]; }
469 double vpPoint::get_y() const { return p[1]; }
471 double vpPoint::get_w() const { return p[2]; }
472 
482 {
483  double d = 1 / cP[2];
484  p[0] = cP[0] * d;
485  p[1] = cP[1] * d;
486  p[2] = 1;
487 }
488 
490 void vpPoint::set_X(double cX) { cP[0] = cX; }
492 void vpPoint::set_Y(double cY) { cP[1] = cY; }
494 void vpPoint::set_Z(double cZ) { cP[2] = cZ; }
496 void vpPoint::set_W(double cW) { cP[3] = cW; }
497 
499 void vpPoint::set_oX(double oX) { oP[0] = oX; }
501 void vpPoint::set_oY(double oY) { oP[1] = oY; }
503 void vpPoint::set_oZ(double oZ) { oP[2] = oZ; }
505 void vpPoint::set_oW(double oW) { oP[3] = oW; }
506 
508 void vpPoint::set_x(double x) { p[0] = x; }
510 void vpPoint::set_y(double y) { p[1] = y; }
512 void vpPoint::set_w(double w) { p[2] = w; }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:529
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:292
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:351
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:168
VISP_EXPORT vpImagePoint operator*(const vpImagePoint &ip1, double scale)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_oW() const
Get the point oW coordinate in the object frame.
Definition: vpPoint.cpp:464
void projection()
Definition: vpPoint.cpp:481
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:458
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:471
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:508
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:496
vpPoint & operator=(const vpPoint &vpp)=default
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:505
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:469
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:451
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:462
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:501
vpPoint * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:350
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:490
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:467
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:455
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:492
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:453
void init()
Basic construction.
Definition: vpPoint.cpp:45
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:503
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:209
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:494
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpPoint.cpp:427
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:499
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:460
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:236
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:449
vpPoint()
Basic constructor.
Definition: vpPoint.cpp:61
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:510
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:512
vpColVector cP
Definition: vpTracker.h:72
vpColVector p
Definition: vpTracker.h:68
bool cPAvailable
Definition: vpTracker.h:78