Visual Servoing Platform version 3.5.0
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
70vpPoint::vpPoint(double oX, double oY, double oZ)
71{
72 init();
73 setWorldCoordinates(oX, oY, oZ);
74}
75
87{
88 init();
90}
91
102vpPoint::vpPoint(const std::vector<double> &oP_)
103{
104 init();
106}
107
113void 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
159void 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
178void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
179{
180 oX = oP[0];
181 oY = oP[1];
182 oZ = oP[2];
183}
184
192
199void 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
222void 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
288const vpPoint
289operator*(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
330const vpPoint
331operator*(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
408VISP_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;
417
418 return *this;
419}
420#endif
421
431 unsigned int thickness)
432{
433 vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
434}
435
444void 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
452double vpPoint::get_X() const { return cP[0]; }
454double vpPoint::get_Y() const { return cP[1]; }
456double vpPoint::get_Z() const { return cP[2]; }
458double vpPoint::get_W() const { return cP[3]; }
459
461double vpPoint::get_oX() const { return oP[0]; }
463double vpPoint::get_oY() const { return oP[1]; }
465double vpPoint::get_oZ() const { return oP[2]; }
467double vpPoint::get_oW() const { return oP[3]; }
468
470double vpPoint::get_x() const { return p[0]; }
472double vpPoint::get_y() const { return p[1]; }
474double 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
493void vpPoint::set_X(double cX) { cP[0] = cX; }
495void vpPoint::set_Y(double cY) { cP[1] = cY; }
497void vpPoint::set_Z(double cZ) { cP[2] = cZ; }
499void vpPoint::set_W(double cW) { cP[3] = cW; }
500
502void vpPoint::set_oX(double oX) { oP[0] = oX; }
504void vpPoint::set_oY(double oY) { oP[1] = oY; }
506void vpPoint::set_oZ(double oZ) { oP[2] = oZ; }
508void vpPoint::set_oW(double oW) { oP[3] = oW; }
509
511void vpPoint::set_x(double x) { p[0] = x; }
513void vpPoint::set_y(double y) { p[1] = y; }
515void vpPoint::set_w(double w) { p[2] = w; }
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
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:175
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:82
double get_oW() const
Get the point oW coordinate in the object frame.
Definition: vpPoint.cpp:467
void projection()
Definition: vpPoint.cpp:484
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:474
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:499
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:508
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:454
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:465
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:504
vpPoint * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:353
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:493
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:458
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:495
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:456
void init()
Basic construction.
Definition: vpPoint.cpp:48
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:506
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:212
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:497
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpPoint.cpp:430
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:502
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpPoint.cpp:239
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:452
vpPoint & operator=(const vpPoint &vpp)=default
vpPoint()
Basic constructor.
Definition: vpPoint.cpp:64
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:515
vpColVector cP
Definition: vpTracker.h:77
vpColVector p
Definition: vpTracker.h:73
bool cPAvailable
Definition: vpTracker.h:83