Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
67vpPoint::vpPoint(double oX, double oY, double oZ)
68{
69 init();
70 setWorldCoordinates(oX, oY, oZ);
71}
72
84{
85 init();
87}
88
99vpPoint::vpPoint(const std::vector<double> &oP_)
100{
101 init();
103}
104
110void 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
156void 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
175void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
176{
177 oX = oP[0];
178 oY = oP[1];
179 oZ = oP[2];
180}
181
189
196void 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
219void 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
285const vpPoint
286operator*(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
327const vpPoint
328operator*(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
405VISP_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;
414
415 return *this;
416}
417#endif
418
428 unsigned int thickness)
429{
430 vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
431}
432
441void 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
449double vpPoint::get_X() const { return cP[0]; }
451double vpPoint::get_Y() const { return cP[1]; }
453double vpPoint::get_Z() const { return cP[2]; }
455double vpPoint::get_W() const { return cP[3]; }
456
458double vpPoint::get_oX() const { return oP[0]; }
460double vpPoint::get_oY() const { return oP[1]; }
462double vpPoint::get_oZ() const { return oP[2]; }
464double vpPoint::get_oW() const { return oP[3]; }
465
467double vpPoint::get_x() const { return p[0]; }
469double vpPoint::get_y() const { return p[1]; }
471double 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
490void vpPoint::set_X(double cX) { cP[0] = cX; }
492void vpPoint::set_Y(double cY) { cP[1] = cY; }
494void vpPoint::set_Z(double cZ) { cP[2] = cZ; }
496void vpPoint::set_W(double cW) { cP[3] = cW; }
497
499void vpPoint::set_oX(double oX) { oP[0] = oX; }
501void vpPoint::set_oY(double oY) { oP[1] = oY; }
503void vpPoint::set_oZ(double oZ) { oP[2] = oZ; }
505void vpPoint::set_oW(double oW) { oP[3] = oW; }
506
508void vpPoint::set_x(double x) { p[0] = x; }
510void vpPoint::set_y(double y) { p[1] = y; }
512void vpPoint::set_w(double w) { p[2] = w; }
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.
void resize(unsigned int i, bool flagNullify=true)
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 of the vpImage class member functions.
Definition vpImage.h:135
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
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 & operator=(const vpPoint &vpp)=default
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
vpColVector operator*(const double &x, const vpColVector &v)