Visual Servoing Platform  version 3.6.1 under development (2024-06-22)
vpPoint.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Point feature.
32  *
33 *****************************************************************************/
34 
40 #include <visp3/core/vpFeatureDisplay.h>
41 #include <visp3/core/vpPoint.h>
42 
45 {
46  const unsigned int index_2 = 2;
47  const unsigned int index_3 = 3;
48  p.resize(3);
49  p = 0;
50  p[index_2] = 1.;
51  oP.resize(4);
52  oP = 0.;
53  oP[index_3] = 1.;
54  cP.resize(4);
55  cP = 0.;
56  cP[index_3] = 1.;
57 
58  // default value Z (1 meters)
59  set_Z(1);
60 }
61 
63 
68 vpPoint::vpPoint(double oX, double oY, double oZ)
69 {
70  init();
71  setWorldCoordinates(oX, oY, oZ);
72 }
73 
85 {
86  init();
88 }
89 
100 vpPoint::vpPoint(const std::vector<double> &oP_)
101 {
102  init();
103  setWorldCoordinates(oP_);
104 }
105 
111 void vpPoint::setWorldCoordinates(double oX, double oY, double oZ)
112 {
113  const unsigned int index_0 = 0;
114  const unsigned int index_1 = 1;
115  const unsigned int index_2 = 2;
116  const unsigned int index_3 = 3;
117  oP[index_0] = oX;
118  oP[index_1] = oY;
119  oP[index_2] = oZ;
120  oP[index_3] = 1;
121 }
122 
134 {
135  const unsigned int index_0 = 0;
136  const unsigned int index_1 = 1;
137  const unsigned int index_2 = 2;
138  const unsigned int index_3 = 3;
139  const unsigned int val_3 = 3;
140  const unsigned int val_4 = 4;
141 
142  if (oP_.size() == val_3) {
143  oP[index_0] = oP_[index_0];
144  oP[index_1] = oP_[index_1];
145  oP[index_2] = oP_[index_2];
146  oP[index_3] = 1.;
147  }
148  else if (oP_.size() == val_4) {
149  oP[index_0] = oP_[index_0];
150  oP[index_1] = oP_[index_1];
151  oP[index_2] = oP_[index_2];
152  oP[index_3] = oP_[index_3];
153  oP /= oP[index_3];
154  }
155  else {
156  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
157  }
158 }
159 
170 void vpPoint::setWorldCoordinates(const std::vector<double> &oP_)
171 {
172  const unsigned int index_0 = 0;
173  const unsigned int index_1 = 1;
174  const unsigned int index_2 = 2;
175  const unsigned int index_3 = 3;
176  const unsigned int val_3 = 3;
177  const unsigned int val_4 = 4;
178 
179  if (oP_.size() == val_3) {
180  oP[index_0] = oP_[index_0];
181  oP[index_1] = oP_[index_1];
182  oP[index_2] = oP_[index_2];
183  oP[index_3] = 1.;
184  }
185  else if (oP_.size() == val_4) {
186  oP[index_0] = oP_[index_0];
187  oP[index_1] = oP_[index_1];
188  oP[index_2] = oP_[index_2];
189  oP[index_3] = oP_[index_3];
190  oP /= oP[index_3];
191  }
192  else {
193  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size()));
194  }
195 }
196 
198 void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
199 {
200  const unsigned int index_0 = 0;
201  const unsigned int index_1 = 1;
202  const unsigned int index_2 = 2;
203  oX = oP[index_0];
204  oY = oP[index_1];
205  oZ = oP[index_2];
206 }
207 
215 
222 void vpPoint::getWorldCoordinates(std::vector<double> &oP_)
223 {
224  oP_.resize(oP.size());
225  unsigned int oP_size = oP.size();
226  for (unsigned int i = 0; i < oP_size; ++i) {
227  oP_[i] = oP[i];
228  }
229 }
230 
238 
247 void vpPoint::projection(const vpColVector &v_cP, vpColVector &v_p) const
248 {
249  const unsigned int index_0 = 0;
250  const unsigned int index_1 = 1;
251  const unsigned int index_2 = 2;
252  v_p.resize(3, false);
253 
254  v_p[index_0] = v_cP[index_0] / v_cP[index_2];
255  v_p[index_1] = v_cP[index_1] / v_cP[index_2];
256  v_p[index_2] = 1;
257 }
258 
268 {
269  const unsigned int index_0 = 0;
270  const unsigned int index_1 = 1;
271  const unsigned int index_2 = 2;
272  const unsigned int index_3 = 3;
273  v_cP.resize(4, false);
274 
275  v_cP[index_0] = (cMo[index_0][index_0] * oP[index_0]) + (cMo[index_0][index_1] * oP[index_1]) + (cMo[index_0][index_2] * oP[index_2]) + (cMo[index_0][index_3] * oP[index_3]);
276  v_cP[index_1] = (cMo[index_1][index_0] * oP[index_0]) + (cMo[index_1][index_1] * oP[index_1]) + (cMo[index_1][index_2] * oP[index_2]) + (cMo[index_1][index_3] * oP[index_3]);
277  v_cP[index_2] = (cMo[index_2][index_0] * oP[index_0]) + (cMo[index_2][index_1] * oP[index_1]) + (cMo[index_2][index_2] * oP[index_2]) + (cMo[index_2][index_3] * oP[index_3]);
278  v_cP[index_3] = (cMo[index_3][index_0] * oP[index_0]) + (cMo[index_3][index_1] * oP[index_1]) + (cMo[index_3][index_2] * oP[index_2]) + (cMo[index_3][index_3] * oP[index_3]);
279 
280  double d = 1 / v_cP[index_3];
281  v_cP[index_0] *= d;
282  v_cP[index_1] *= d;
283  v_cP[index_2] *= d;
284  v_cP[index_3] *= d;
285 }
286 
296 {
297  const unsigned int index_0 = 0;
298  const unsigned int index_1 = 1;
299  const unsigned int index_2 = 2;
300  const unsigned int index_3 = 3;
301  double X = (cMo[index_0][index_0] * oP[index_0]) + (cMo[index_0][index_1] * oP[index_1]) + (cMo[index_0][index_2] * oP[index_2]) + (cMo[index_0][index_3] * oP[index_3]);
302  double Y = (cMo[index_1][index_0] * oP[index_0]) + (cMo[index_1][index_1] * oP[index_1]) + (cMo[index_1][index_2] * oP[index_2]) + (cMo[index_1][index_3] * oP[index_3]);
303  double Z = (cMo[index_2][index_0] * oP[index_0]) + (cMo[index_2][index_1] * oP[index_1]) + (cMo[index_2][index_2] * oP[index_2]) + (cMo[index_2][index_3] * oP[index_3]);
304  double W = (cMo[index_3][index_0] * oP[index_0]) + (cMo[index_3][index_1] * oP[index_1]) + (cMo[index_3][index_2] * oP[index_2]) + (cMo[index_3][index_3] * oP[index_3]);
305 
306  double d = 1. / W;
307  cP[index_0] = X * d;
308  cP[index_1] = Y * d;
309  cP[index_2] = Z * d;
310  cP[index_3] = 1.;
311 }
312 
315 {
316  vpPoint *feature = new vpPoint(*this);
317  return feature;
318 }
319 
332  const vpColor &color, unsigned int thickness)
333 {
334  vpColVector v_cP, v_p;
335  changeFrame(cMo, v_cP);
336  const unsigned int index_2 = 2;
337 
338  if (v_cP[index_2] < 0) { // no display if point is behind the camera
339  return;
340  }
341 
342  vpPoint::projection(v_cP, v_p);
343  vpFeatureDisplay::displayPoint(v_p[0], v_p[1], cam, I, color, thickness);
344 }
345 
358  const vpColor &color, unsigned int thickness)
359 {
360  vpColVector v_cP, v_p;
361  changeFrame(cMo, v_cP);
362  const unsigned int index_2 = 2;
363 
364  if (v_cP[index_2] < 0) { // no display if point is behind the camera
365  return;
366  }
367 
368  vpPoint::projection(v_cP, v_p);
369  vpFeatureDisplay::displayPoint(v_p[0], v_p[1], cam, I, color, thickness);
370 }
371 
380 void vpPoint::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
381  unsigned int thickness)
382 {
383  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
384 }
385 
394 void vpPoint::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
395  unsigned int thickness)
396 {
397  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
398 }
399 
400 // Get coordinates
402 double vpPoint::get_X() const { const unsigned int index_0 = 0; return cP[index_0]; }
404 double vpPoint::get_Y() const { const unsigned int index_1 = 1; return cP[index_1]; }
406 double vpPoint::get_Z() const { const unsigned int index_2 = 2; return cP[index_2]; }
408 double vpPoint::get_W() const { const unsigned int index_3 = 3; return cP[index_3]; }
409 
411 double vpPoint::get_oX() const { const unsigned int index_0 = 0; return oP[index_0]; }
413 double vpPoint::get_oY() const { const unsigned int index_1 = 1; return oP[index_1]; }
415 double vpPoint::get_oZ() const { const unsigned int index_2 = 2; return oP[index_2]; }
417 double vpPoint::get_oW() const { const unsigned int index_3 = 3; return oP[index_3]; }
418 
420 double vpPoint::get_x() const { const unsigned int index_0 = 0; return p[index_0]; }
422 double vpPoint::get_y() const { const unsigned int index_1 = 1; return p[index_1]; }
424 double vpPoint::get_w() const { const unsigned int index_2 = 2; return p[index_2]; }
425 
435 {
436  const unsigned int index_0 = 0;
437  const unsigned int index_1 = 1;
438  const unsigned int index_2 = 2;
439  double d = 1 / cP[index_2];
440  p[index_0] = cP[index_0] * d;
441  p[index_1] = cP[index_1] * d;
442  p[index_2] = 1;
443 }
444 
446 void vpPoint::set_X(double cX) { const unsigned int index_0 = 0; cP[index_0] = cX; }
448 void vpPoint::set_Y(double cY) { const unsigned int index_1 = 1; cP[index_1] = cY; }
450 void vpPoint::set_Z(double cZ) { const unsigned int index_2 = 2; cP[index_2] = cZ; }
452 void vpPoint::set_W(double cW) { const unsigned int index_3 = 3; cP[index_3] = cW; }
453 
455 void vpPoint::set_oX(double oX) { const unsigned int index_0 = 0; oP[index_0] = oX; }
457 void vpPoint::set_oY(double oY) { const unsigned int index_1 = 1; oP[index_1] = oY; }
459 void vpPoint::set_oZ(double oZ) { const unsigned int index_2 = 2; oP[index_2] = oZ; }
461 void vpPoint::set_oW(double oW) { const unsigned int index_3 = 3; oP[index_3] = oW; }
462 
464 void vpPoint::set_x(double x) { const unsigned int index_0 = 0; p[index_0] = x; }
466 void vpPoint::set_y(double y) { const unsigned int index_1 = 1; p[index_1] = y; }
468 void vpPoint::set_w(double w) { const unsigned int index_2 = 2; p[index_2] = w; }
469 
470 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:593
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:331
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1066
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:153
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
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.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_oW() const
Get the point oW coordinate in the object frame.
Definition: vpPoint.cpp:417
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:411
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:424
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:464
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:452
vpPoint * duplicate() const vp_override
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:314
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:422
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:404
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:415
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:457
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:446
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:420
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:408
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:448
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:406
void projection() vp_override
Definition: vpPoint.cpp:434
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:459
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:237
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:450
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:455
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:413
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:402
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) vp_override
Definition: vpPoint.cpp:380
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpPoint.cpp:267
vpPoint()
Basic constructor.
Definition: vpPoint.cpp:62
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:111
void init() vp_override
Basic construction.
Definition: vpPoint.cpp:44
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:466
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:468
vpColVector cP
Definition: vpTracker.h:73
vpColVector p
Definition: vpTracker.h:69