ViSP  2.9.0
vpPoint.cpp
1 /****************************************************************************
2  *
3  * $Id: vpPoint.cpp 4620 2014-01-27 21:28:32Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Point feature.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
43 #include <visp/vpPoint.h>
44 #include <visp/vpDebug.h>
45 #include <visp/vpFeatureDisplay.h>
46 
47 #include <visp/vpHomography.h>
48 
56 void
58 {
59  p.resize(3) ; p = 0 ; p[2] = 1 ;
60  oP.resize(4) ; oP = 0 ; oP[3] = 1 ;
61  cP.resize(4) ; cP = 0 ; cP[3] = 1 ;
62 
63  //default value Z (1 meters)
64  set_Z(1) ;
65 }
66 
68 {
69  init() ;
70 }
71 
73 void
75  const double oy,
76  const double oz)
77 {
78  oP[0] = ox ;
79  oP[1] = oy ;
80  oP[2] = oz ;
81  oP[3] = 1 ;
82 }
83 
84 
85 void
87 {
88  oP[0] = _oP[0] ;
89  oP[1] = _oP[1] ;
90  oP[2] = _oP[2] ;
91  oP[3] = _oP[3] ;
92 
93  oP /= oP[3] ;
94 }
95 
96 void
98  double& oy,
99  double& oz)
100 {
101  ox = oP[0] ;
102  oy = oP[1] ;
103  oz = oP[2] ;
104 }
105 
106 
107 void
109 {
110  _oP[0] = oP[0] ;
111  _oP[1] = oP[1] ;
112  _oP[2] = oP[2] ;
113  _oP[3] = oP[3] ;
114 }
115 
116 
119 {
120  return this->oP;
121 }
122 
123 
124 
131 void
133 {
134  _p.resize(3) ;
135 
136  _p[0] = _cP[0]/_cP[2] ;
137  _p[1] = _cP[1]/_cP[2] ;
138  _p[2] = 1 ;
139 }
140 
149 void
151 {
152 
153  _cP.resize(4) ;
154 
155  _cP[0] = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
156  _cP[1] = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
157  _cP[2] = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
158  _cP[3] = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
159 
160  double d = 1/_cP[3] ;
161  _cP[0] *=d ;
162  _cP[1] *=d ;
163  _cP[2] *=d ;
164  _cP[3] *=d ; ;
165 }
166 
177 const vpPoint
178 operator*(const vpHomogeneousMatrix &aMb, const vpPoint& bP)
179 {
180  vpPoint aP ;
181 
182  vpColVector v(4),v1(4) ;
183 
184  v[0] = bP.get_X() ;
185  v[1] = bP.get_Y() ;
186  v[2] = bP.get_Z() ;
187  v[3] = bP.get_W() ;
188 
189  v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3] ;
190  v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3] ;
191  v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3] ;
192  v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3] ;
193 
194  v1 /= v1[3] ;
195 
196  // v1 = M*v ;
197  aP.set_X(v1[0]) ;
198  aP.set_Y(v1[1]) ;
199  aP.set_Z(v1[2]) ;
200  aP.set_W(v1[3]) ;
201 
202  aP.set_oX(v1[0]) ;
203  aP.set_oY(v1[1]) ;
204  aP.set_oZ(v1[2]) ;
205  aP.set_oW(v1[3]) ;
206 
207  return aP ;
208 }
209 
219 const vpPoint
220 operator*(const vpHomography &aHb, const vpPoint& bP)
221 {
222  vpPoint aP ;
223  vpColVector v(3),v1(3) ;
224 
225  v[0] = bP.get_x() ;
226  v[1] = bP.get_y() ;
227  v[2] = bP.get_w() ;
228 
229  v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2] ;
230  v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2] ;
231  v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2] ;
232 
233  // v1 = M*v ;
234  aP.set_x(v1[0]) ;
235  aP.set_y(v1[1]) ;
236  aP.set_w(v1[2]) ;
237 
238  return aP ;
239 }
240 
243 {
244  vpPoint *feature = new vpPoint(*this) ;
245  return feature ;
246 }
247 
251 void
253  const vpHomogeneousMatrix &cMo,
254  const vpCameraParameters &cam,
255  const vpColor &color,
256  const unsigned int thickness)
257 {
258 
259  vpColVector _cP, _p ;
260  changeFrame(cMo,_cP) ;
261 
262  if(_cP[2] < 0) // no display if point is behind the camera
263  return;
264 
265  vpPoint::projection(_cP,_p) ;
266  vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
267 }
268 
272 void
274  const vpHomogeneousMatrix &cMo,
275  const vpCameraParameters &cam,
276  const vpColor &color,
277  const unsigned int thickness)
278 {
279  vpColVector _cP, _p ;
280  changeFrame(cMo,_cP) ;
281 
282  if(_cP[2] < 0) // no display if point is behind the camera
283  return;
284 
285  vpPoint::projection(_cP,_p) ;
286  vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
287 }
288 
289 VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpPoint& /* vpp */)
290 {
291  return( os<<"vpPoint" );
292 }
293 
294 vpPoint&
296 {
297  p = vpp.p;
298  cP = vpp.cP;
299  oP = vpp.oP;
300  cPAvailable = vpp.cPAvailable;
301 
302  return *this;
303 }
304 
308 void
310  const vpCameraParameters &cam,
311  const vpColor &color,
312  const unsigned int thickness)
313 {
314  vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness) ;
315 }
316 /*
317  * Local variables:
318  * c-basic-offset: 2
319  * End:
320  */
vpPoint()
Basic constructor.
Definition: vpPoint.cpp:67
void init()
Basic construction.
Definition: vpPoint.cpp:57
vpColVector getWorldCoordinates(void)
Definition: vpPoint.cpp:118
bool cPAvailable
Definition: vpTracker.h:88
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpPoint.cpp:309
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.h:194
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.h:140
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.h:176
double get_W() const
Get the point W coordinate in the camera frame.
Definition: vpPoint.h:124
static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
void set_oX(const double X)
Set the point X coordinate in the object frame.
Definition: vpPoint.h:185
Class that defines what is a point.
Definition: vpPoint.h:65
vpColVector operator*(const double &x, const vpColVector &B)
multiplication by a scalar Ci = x*Bi
vpColVector cP
Definition: vpTracker.h:82
This class aims to compute the homography wrt.two images.
Definition: vpHomography.h:178
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.h:180
vpPoint & operator=(const vpPoint &vpp)
Definition: vpPoint.cpp:295
void set_oZ(const double Z)
Set the point Z coordinate in the object frame.
Definition: vpPoint.h:189
void projection()
Definition: vpPoint.h:167
void set_W(const double W)
Set the point W coordinate in the camera frame.
Definition: vpPoint.h:182
Generic class defining intrinsic camera parameters.
void set_oW(const double W)
Set the point W coordinate in the object frame.
Definition: vpPoint.h:191
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.h:196
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.h:178
void set_w(const double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.h:198
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
vpPoint * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpPoint.cpp:242
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.h:120
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.h:122
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Definition: vpPoint.cpp:150
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.h:118
vpColVector p
Definition: vpTracker.h:78
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74
void set_oY(const double Y)
Set the point Y coordinate in the object frame.
Definition: vpPoint.h:187
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94