Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
70 vpPoint::vpPoint(double oX, double oY, double oZ)
71 {
72  init();
73  setWorldCoordinates(oX, oY, oZ);
74 }
75 
87 {
88  init();
90 }
91 
102 vpPoint::vpPoint(const std::vector<double> &P)
103 {
104  init();
106 }
107 
113 void vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ)
114 {
115  oP[0] = oX;
116  oP[1] = oY;
117  oP[2] = oZ;
118  oP[3] = 1;
119 }
120 
131 {
132  if (P.size() == 3) {
133  oP[0] = P[0];
134  oP[1] = P[1];
135  oP[2] = P[2];
136  oP[3] = 1.;
137  } else if (P.size() == 4) {
138  oP[0] = P[0];
139  oP[1] = P[1];
140  oP[2] = P[2];
141  oP[3] = P[3];
142  oP /= oP[3];
143  } else {
144  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size()));
145  }
146 }
147 
157 void vpPoint::setWorldCoordinates(const std::vector<double> &P)
158 {
159  if (P.size() == 3) {
160  oP[0] = P[0];
161  oP[1] = P[1];
162  oP[2] = P[2];
163  oP[3] = 1.;
164  } else if (P.size() == 4) {
165  oP[0] = P[0];
166  oP[1] = P[1];
167  oP[2] = P[2];
168  oP[3] = P[3];
169  oP /= oP[3];
170  } else {
171  throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size()));
172  }
173 }
174 
177 void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
178 {
179  oX = oP[0];
180  oY = oP[1];
181  oZ = oP[2];
182 }
183 
195 void vpPoint::getWorldCoordinates(std::vector<double> &P)
196 {
197  P.resize(oP.size());
198  for (unsigned int i = 0; i < oP.size(); i++)
199  P[i] = oP[i];
200 }
201 
208 
217 {
218  _p.resize(3);
219 
220  _p[0] = _cP[0] / _cP[2];
221  _p[1] = _cP[1] / _cP[2];
222  _p[2] = 1;
223 }
224 
234 {
235 
236  _cP.resize(4);
237 
238  _cP[0] = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
239  _cP[1] = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
240  _cP[2] = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
241  _cP[3] = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
242 
243  double d = 1 / _cP[3];
244  _cP[0] *= d;
245  _cP[1] *= d;
246  _cP[2] *= d;
247  _cP[3] *= d;
248  ;
249 }
250 
260 {
261  double X = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
262  double Y = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
263  double Z = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
264  double W = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
265 
266  double d = 1 / W;
267  cP[0] = X * d;
268  cP[1] = Y * d;
269  cP[2] = Z * d;
270  cP[3] = 1;
271 }
272 
273 #if 0
274 
284 const vpPoint
285 operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
286 {
287  vpPoint aP ;
288 
289  vpColVector v(4),v1(4) ;
290 
291  v[0] = bP.get_X() ;
292  v[1] = bP.get_Y() ;
293  v[2] = bP.get_Z() ;
294  v[3] = bP.get_W() ;
295 
296  v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
297  v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
298  v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
299  v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;
300 
301  v1 /= v1[3] ;
302 
303  // v1 = M*v ;
304  aP.set_X(v1[0]) ;
305  aP.set_Y(v1[1]) ;
306  aP.set_Z(v1[2]) ;
307  aP.set_W(v1[3]) ;
308 
309  aP.set_oX(v1[0]) ;
310  aP.set_oY(v1[1]) ;
311  aP.set_oZ(v1[2]) ;
312  aP.set_oW(v1[3]) ;
313 
314  return aP ;
315 }
316 
326 const vpPoint
327 operator*(const vpHomography &aHb, const vpPoint& bP)
328 {
329  vpPoint aP ;
330  vpColVector v(3),v1(3) ;
331 
332  v[0] = bP.get_x() ;
333  v[1] = bP.get_y() ;
334  v[2] = bP.get_w() ;
335 
336  v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
337  v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
338  v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;
339 
340  // v1 = M*v ;
341  aP.set_x(v1[0]) ;
342  aP.set_y(v1[1]) ;
343  aP.set_w(v1[2]) ;
344 
345  return aP ;
346 }
347 #endif
350 {
351  vpPoint *feature = new vpPoint(*this);
352  return feature;
353 }
354 
359  const vpColor &color, const unsigned int thickness)
360 {
361 
362  vpColVector _cP, _p;
363  changeFrame(cMo, _cP);
364 
365  if (_cP[2] < 0) // no display if point is behind the camera
366  return;
367 
368  vpPoint::projection(_cP, _p);
369  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
370 }
371 
376  const vpColor &color, const unsigned int thickness)
377 {
378  vpColVector _cP, _p;
379  changeFrame(cMo, _cP);
380 
381  if (_cP[2] < 0) // no display if point is behind the camera
382  return;
383 
384  vpPoint::projection(_cP, _p);
385  vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
386 }
387 
388 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }
389 
391 {
392  p = vpp.p;
393  cP = vpp.cP;
394  oP = vpp.oP;
395  cPAvailable = vpp.cPAvailable;
396 
397  return *this;
398 }
399 
403 void vpPoint::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
404  const unsigned int thickness)
405 {
406  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
407 }
408 
409 // Get coordinates
411 double vpPoint::get_X() const { return cP[0]; }
413 double vpPoint::get_Y() const { return cP[1]; }
415 double vpPoint::get_Z() const { return cP[2]; }
417 double vpPoint::get_W() const { return cP[3]; }
418 
420 double vpPoint::get_oX() const { return oP[0]; }
422 double vpPoint::get_oY() const { return oP[1]; }
424 double vpPoint::get_oZ() const { return oP[2]; }
426 double vpPoint::get_oW() const { return oP[3]; }
427 
429 double vpPoint::get_x() const { return p[0]; }
431 double vpPoint::get_y() const { return p[1]; }
433 double vpPoint::get_w() const { return p[2]; }
434 
444 {
445  double d = 1 / cP[2];
446  p[0] = cP[0] * d;
447  p[1] = cP[1] * d;
448  p[2] = 1;
449 }
450 
452 void vpPoint::set_X(const double X) { cP[0] = X; }
454 void vpPoint::set_Y(const double Y) { cP[1] = Y; }
456 void vpPoint::set_Z(const double Z) { cP[2] = Z; }
458 void vpPoint::set_W(const double W) { cP[3] = W; }
459 
461 void vpPoint::set_oX(const double oX) { oP[0] = oX; }
463 void vpPoint::set_oY(const double oY) { oP[1] = oY; }
465 void vpPoint::set_oZ(const double oZ) { oP[2] = oZ; }
467 void vpPoint::set_oW(const double oW) { oP[3] = oW; }
468 
470 void vpPoint::set_x(const double x) { p[0] = x; }
472 void vpPoint::set_y(const double y) { p[1] = y; }
474 void vpPoint::set_w(const double w) { p[2] = w; }
vpPoint()
Basic constructor.
Definition: vpPoint.cpp:64
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:465
void init()
Basic construction.
Definition: vpPoint.cpp:48
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:207
bool cPAvailable
Definition: vpTracker.h:81
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpPoint.cpp:403
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:422
error that can be emited by ViSP classes.
Definition: vpException.h:71
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:431
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:433
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:452
double get_W() const
Get the point W coordinate in the camera frame.
Definition: vpPoint.cpp:417
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Class that defines what is a point.
Definition: vpPoint.h:58
vpColVector cP
Definition: vpTracker.h:75
vpColVector operator*(const double &x, const vpColVector &v)
void set_oW(const double oW)
Set the point W coordinate in the object frame.
Definition: vpPoint.cpp:467
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:456
vpPoint & operator=(const vpPoint &vpp)
Definition: vpPoint.cpp:390
void projection()
Definition: vpPoint.cpp:443
void set_W(const double W)
Set the point W coordinate in the camera frame.
Definition: vpPoint.cpp:458
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpPoint &vpp)
Definition: vpPoint.cpp:388
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:424
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
void set_oX(const double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:461
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:454
void set_w(const double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:474
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:429
vpPoint * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:349
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:413
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:415
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:420
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
Definition: vpPoint.cpp:463
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:233
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:411
double get_oW() const
Get the point W coordinate in the object frame.
Definition: vpPoint.cpp:426
vpColVector p
Definition: vpTracker.h:71
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244