Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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  }
136  else if (oP_.size() == 4) {
137  oP[0] = oP_[0];
138  oP[1] = oP_[1];
139  oP[2] = oP_[2];
140  oP[3] = oP_[3];
141  oP /= oP[3];
142  }
143  else {
144  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
145  }
146 }
147 
158 void vpPoint::setWorldCoordinates(const std::vector<double> &oP_)
159 {
160  if (oP_.size() == 3) {
161  oP[0] = oP_[0];
162  oP[1] = oP_[1];
163  oP[2] = oP_[2];
164  oP[3] = 1.;
165  }
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  }
173  else {
174  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
175  }
176 }
177 
179 void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
180 {
181  oX = oP[0];
182  oY = oP[1];
183  oZ = oP[2];
184 }
185 
193 
200 void vpPoint::getWorldCoordinates(std::vector<double> &oP_)
201 {
202  oP_.resize(oP.size());
203  for (unsigned int i = 0; i < oP.size(); i++)
204  oP_[i] = oP[i];
205 }
206 
214 
223 void vpPoint::projection(const vpColVector &_cP, vpColVector &_p) const
224 {
225  _p.resize(3, false);
226 
227  _p[0] = _cP[0] / _cP[2];
228  _p[1] = _cP[1] / _cP[2];
229  _p[2] = 1;
230 }
231 
241 {
242  _cP.resize(4, false);
243 
244  _cP[0] = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
245  _cP[1] = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
246  _cP[2] = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
247  _cP[3] = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
248 
249  double d = 1 / _cP[3];
250  _cP[0] *= d;
251  _cP[1] *= d;
252  _cP[2] *= d;
253  _cP[3] *= d;
254 }
255 
265 {
266  double X = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
267  double Y = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
268  double Z = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
269  double W = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
270 
271  double d = 1 / W;
272  cP[0] = X * d;
273  cP[1] = Y * d;
274  cP[2] = Z * d;
275  cP[3] = 1;
276 }
277 
278 #if 0
289 const vpPoint
290 operator*(const vpHomogeneousMatrix &aMb, const vpPoint &bP)
291 {
292  vpPoint aP;
293 
294  vpColVector v(4), v1(4);
295 
296  v[0] = bP.get_X();
297  v[1] = bP.get_Y();
298  v[2] = bP.get_Z();
299  v[3] = bP.get_W();
300 
301  v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3];
302  v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3];
303  v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3];
304  v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3];
305 
306  v1 /= v1[3];
307 
308  // v1 = M*v ;
309  aP.set_X(v1[0]);
310  aP.set_Y(v1[1]);
311  aP.set_Z(v1[2]);
312  aP.set_W(v1[3]);
313 
314  aP.set_oX(v1[0]);
315  aP.set_oY(v1[1]);
316  aP.set_oZ(v1[2]);
317  aP.set_oW(v1[3]);
318 
319  return aP;
320 }
321 
331 const vpPoint
332 operator*(const vpHomography &aHb, const vpPoint &bP)
333 {
334  vpPoint aP;
335  vpColVector v(3), v1(3);
336 
337  v[0] = bP.get_x();
338  v[1] = bP.get_y();
339  v[2] = bP.get_w();
340 
341  v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2];
342  v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2];
343  v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2];
344 
345  // v1 = M*v ;
346  aP.set_x(v1[0]);
347  aP.set_y(v1[1]);
348  aP.set_w(v1[2]);
349 
350  return aP;
351 }
352 #endif
355 {
356  vpPoint *feature = new vpPoint(*this);
357  return feature;
358 }
359 
372  const vpColor &color, unsigned int thickness)
373 {
374 
375  vpColVector _cP, _p;
376  changeFrame(cMo, _cP);
377 
378  if (_cP[2] < 0) // no display if point is behind the camera
379  return;
380 
381  vpPoint::projection(_cP, _p);
382  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
383 }
384 
397  const vpColor &color, unsigned int thickness)
398 {
399  vpColVector _cP, _p;
400  changeFrame(cMo, _cP);
401 
402  if (_cP[2] < 0) // no display if point is behind the camera
403  return;
404 
405  vpPoint::projection(_cP, _p);
406  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
407 }
408 
409 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }
410 
419 void vpPoint::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
420  unsigned int thickness)
421 {
422  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
423 }
424 
433 void vpPoint::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
434  unsigned int thickness)
435 {
436  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
437 }
438 
439 // Get coordinates
441 double vpPoint::get_X() const { return cP[0]; }
443 double vpPoint::get_Y() const { return cP[1]; }
445 double vpPoint::get_Z() const { return cP[2]; }
447 double vpPoint::get_W() const { return cP[3]; }
448 
450 double vpPoint::get_oX() const { return oP[0]; }
452 double vpPoint::get_oY() const { return oP[1]; }
454 double vpPoint::get_oZ() const { return oP[2]; }
456 double vpPoint::get_oW() const { return oP[3]; }
457 
459 double vpPoint::get_x() const { return p[0]; }
461 double vpPoint::get_y() const { return p[1]; }
463 double vpPoint::get_w() const { return p[2]; }
464 
474 {
475  double d = 1 / cP[2];
476  p[0] = cP[0] * d;
477  p[1] = cP[1] * d;
478  p[2] = 1;
479 }
480 
482 void vpPoint::set_X(double cX) { cP[0] = cX; }
484 void vpPoint::set_Y(double cY) { cP[1] = cY; }
486 void vpPoint::set_Z(double cZ) { cP[2] = cZ; }
488 void vpPoint::set_W(double cW) { cP[3] = cW; }
489 
491 void vpPoint::set_oX(double oX) { oP[0] = oX; }
493 void vpPoint::set_oY(double oY) { oP[1] = oY; }
495 void vpPoint::set_oZ(double oZ) { oP[2] = oZ; }
497 void vpPoint::set_oW(double oW) { oP[3] = oW; }
498 
500 void vpPoint::set_x(double x) { p[0] = x; }
502 void vpPoint::set_y(double y) { p[1] = y; }
504 void vpPoint::set_w(double w) { p[2] = w; }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:523
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector operator*(const double &x, const vpColVector &v)
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
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
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:456
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:450
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:463
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:500
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:488
vpPoint * duplicate() const vp_override
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:354
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:497
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:461
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:443
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:454
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:493
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:482
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:459
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:447
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:484
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:445
void projection() vp_override
Definition: vpPoint.cpp:473
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:495
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:213
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:486
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:491
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:452
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:441
void init() vp_override
Basic construction.
Definition: vpPoint.cpp:45
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) vp_override
Definition: vpPoint.cpp:419
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpPoint.cpp:240
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:502
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:504
vpColVector cP
Definition: vpTracker.h:71
vpColVector p
Definition: vpTracker.h:67