Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpPoint.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
39 #include <visp3/core/vpPoint.h>
40 #include <visp3/core/vpDebug.h>
41 #include <visp3/core/vpFeatureDisplay.h>
42 
50 void
52 {
53  p.resize(3) ; p = 0 ; p[2] = 1 ;
54  oP.resize(4) ; oP = 0 ; oP[3] = 1 ;
55  cP.resize(4) ; cP = 0 ; cP[3] = 1 ;
56 
57  //default value Z (1 meters)
58  set_Z(1) ;
59 }
60 
62 {
63  init();
64 }
65 
70 vpPoint::vpPoint(double oX, double oY, double oZ)
71 {
72  init();
73  setWorldCoordinates(oX, oY, oZ);
74 }
75 
86 {
87  init();
89 }
90 
100 vpPoint::vpPoint(const std::vector<double> &P)
101 {
102  init();
104 }
105 
110 void
111 vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ)
112 {
113  oP[0] = oX;
114  oP[1] = oY;
115  oP[2] = oZ;
116  oP[3] = 1;
117 }
118 
128 void
130 {
131  if (P.size() == 3) {
132  oP[0] = P[0];
133  oP[1] = P[1];
134  oP[2] = P[2];
135  oP[3] = 1.;
136  }
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  }
144  else {
146  "Cannot initialize vpPoint from vector with size %d", P.size()));
147  }
148 }
149 
159 void
160 vpPoint::setWorldCoordinates(const std::vector<double> &P)
161 {
162  if (P.size() == 3) {
163  oP[0] = P[0];
164  oP[1] = P[1];
165  oP[2] = P[2];
166  oP[3] = 1.;
167  }
168  else if (P.size() == 4) {
169  oP[0] = P[0];
170  oP[1] = P[1];
171  oP[2] = P[2];
172  oP[3] = P[3];
173  oP /= oP[3] ;
174  }
175  else {
177  "Cannot initialize vpPoint from vector with size %d", P.size()));
178  }
179 }
180 
182 void
183 vpPoint::getWorldCoordinates(double& oX, double& oY, double& oZ)
184 {
185  oX = oP[0];
186  oY = oP[1];
187  oZ = oP[2];
188 }
189 
190 
195 void
197 {
198  P = oP;
199 }
204 void
205 vpPoint::getWorldCoordinates(std::vector<double> &P)
206 {
207  P.resize(oP.size());
208  for(unsigned int i = 0; i < oP.size(); i++)
209  P[i] = oP[i];
210 }
211 
218 {
219  return this->oP;
220 }
221 
228 void
230 {
231  _p.resize(3) ;
232 
233  _p[0] = _cP[0]/_cP[2] ;
234  _p[1] = _cP[1]/_cP[2] ;
235  _p[2] = 1 ;
236 }
237 
246 void
248 {
249 
250  _cP.resize(4) ;
251 
252  _cP[0] = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
253  _cP[1] = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
254  _cP[2] = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
255  _cP[3] = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
256 
257  double d = 1/_cP[3] ;
258  _cP[0] *=d ;
259  _cP[1] *=d ;
260  _cP[2] *=d ;
261  _cP[3] *=d ; ;
262 }
263 
273  double X = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
274  double Y = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
275  double Z = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
276  double W = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
277 
278  double d = 1/W ;
279  cP[0] =X*d ;
280  cP[1] =Y*d ;
281  cP[2] =Z*d ;
282  cP[3] =1 ;
283 }
284 
285 #if 0
286 
296 const vpPoint
297 operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
298 {
299  vpPoint aP ;
300 
301  vpColVector v(4),v1(4) ;
302 
303  v[0] = bP.get_X() ;
304  v[1] = bP.get_Y() ;
305  v[2] = bP.get_Z() ;
306  v[3] = bP.get_W() ;
307 
308  v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
309  v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
310  v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
311  v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;
312 
313  v1 /= v1[3] ;
314 
315  // v1 = M*v ;
316  aP.set_X(v1[0]) ;
317  aP.set_Y(v1[1]) ;
318  aP.set_Z(v1[2]) ;
319  aP.set_W(v1[3]) ;
320 
321  aP.set_oX(v1[0]) ;
322  aP.set_oY(v1[1]) ;
323  aP.set_oZ(v1[2]) ;
324  aP.set_oW(v1[3]) ;
325 
326  return aP ;
327 }
328 
338 const vpPoint
339 operator*(const vpHomography &aHb, const vpPoint& bP)
340 {
341  vpPoint aP ;
342  vpColVector v(3),v1(3) ;
343 
344  v[0] = bP.get_x() ;
345  v[1] = bP.get_y() ;
346  v[2] = bP.get_w() ;
347 
348  v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
349  v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
350  v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;
351 
352  // v1 = M*v ;
353  aP.set_x(v1[0]) ;
354  aP.set_y(v1[1]) ;
355  aP.set_w(v1[2]) ;
356 
357  return aP ;
358 }
359 #endif
362 {
363  vpPoint *feature = new vpPoint(*this) ;
364  return feature ;
365 }
366 
370 void
372  const vpHomogeneousMatrix &cMo,
373  const vpCameraParameters &cam,
374  const vpColor &color,
375  const unsigned int thickness)
376 {
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 
391 void
393  const vpHomogeneousMatrix &cMo,
394  const vpCameraParameters &cam,
395  const vpColor &color,
396  const 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 
408 VISP_EXPORT std::ostream& operator<<(std::ostream& os, const vpPoint& /* vpp */)
409 {
410  return( os<<"vpPoint" );
411 }
412 
413 vpPoint&
415 {
416  p = vpp.p;
417  cP = vpp.cP;
418  oP = vpp.oP;
419  cPAvailable = vpp.cPAvailable;
420 
421  return *this;
422 }
423 
427 void
429  const vpCameraParameters &cam,
430  const vpColor &color,
431  const unsigned int thickness)
432 {
433  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness) ;
434 }
435 
436 // Get coordinates
438 double vpPoint::get_X() const { return cP[0] ; }
440 double vpPoint::get_Y() const { return cP[1] ; }
442 double vpPoint::get_Z() const { return cP[2] ; }
444 double vpPoint::get_W() const { return cP[3] ; }
445 
447 double vpPoint::get_oX() const { return oP[0] ; }
449 double vpPoint::get_oY() const { return oP[1] ; }
451 double vpPoint::get_oZ() const { return oP[2] ; }
453 double vpPoint::get_oW() const { return oP[3] ; }
454 
456 double vpPoint::get_x() const { return p[0] ; }
458 double vpPoint::get_y() const { return p[1] ; }
460 double vpPoint::get_w() const { return p[2] ; }
461 
471  double d = 1/cP[2] ;
472  p[0] = cP[0]*d ;
473  p[1] = cP[1]*d ;
474  p[2] = 1 ;
475 }
476 
478 void vpPoint::set_X(const double X) { cP[0] = X ; }
480 void vpPoint::set_Y(const double Y) { cP[1] = Y ; }
482 void vpPoint::set_Z(const double Z) { cP[2] = Z ; }
484 void vpPoint::set_W(const double W) { cP[3] = W ; }
485 
487 void vpPoint::set_oX(const double oX) { oP[0] = oX ; }
489 void vpPoint::set_oY(const double oY) { oP[1] = oY ; }
491 void vpPoint::set_oZ(const double oZ) { oP[2] = oZ ; }
493 void vpPoint::set_oW(const double oW) { oP[3] = oW ; }
494 
496 void vpPoint::set_x(const double x) { p[0] = x ; }
498 void vpPoint::set_y(const double y) { p[1] = y ; }
500 void vpPoint::set_w(const double w) { p[2] = w ; }
vpPoint()
Basic constructor.
Definition: vpPoint.cpp:61
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:491
void init()
Basic construction.
Definition: vpPoint.cpp:51
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:217
bool cPAvailable
Definition: vpTracker.h:83
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpColVector operator*(const double &x, const vpColVector &v)
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpPoint.cpp:428
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:449
error that can be emited by ViSP classes.
Definition: vpException.h:73
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:496
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:156
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:460
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:478
double get_W() const
Get the point W coordinate in the camera frame.
Definition: vpPoint.cpp:444
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:59
vpColVector cP
Definition: vpTracker.h:77
void set_oW(const double oW)
Set the point W coordinate in the object frame.
Definition: vpPoint.cpp:493
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:179
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:482
vpPoint & operator=(const vpPoint &vpp)
Definition: vpPoint.cpp:414
void projection()
Definition: vpPoint.cpp:470
void set_W(const double W)
Set the point W coordinate in the camera frame.
Definition: vpPoint.cpp:484
Generic class defining intrinsic camera parameters.
double get_oZ() const
Get the point Z coordinate in the object frame.
Definition: vpPoint.cpp:451
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:498
void set_oX(const double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:487
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:480
void set_w(const double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:500
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:267
vpPoint * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:361
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:440
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:442
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:447
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:111
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:489
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:247
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:438
double get_oW() const
Get the point W coordinate in the object frame.
Definition: vpPoint.cpp:453
vpColVector p
Definition: vpTracker.h:73
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225