Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpPlane.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  * Plane geometrical structure.
32  *
33  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
45 #include <visp3/core/vpPlane.h>
46 
47 #include <cmath> // std::fabs
48 #include <limits> // numeric_limits
49 
53 vpPlane&
55 {
56  A = p.A ;
57  B = p.B ;
58  C = p.C ;
59  D = p.D ;
60 
61  return *this ;
62 }
63 
67 vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {}
68 
79 vpPlane::vpPlane(const double a,const double b,const double c, const double d)
80  : A(a), B(b), C(c), D(d) {}
81 
86  : A(0), B(0), C(0), D(0)
87 {
88  setA(P.getA()) ;
89  setB(P.getB()) ;
90  setC(P.getC()) ;
91  setD(P.getD()) ;
92 }
93 
113  : A(0), B(0), C(0), D(0)
114 {
115  //Equation of the plane is given by:
116  A = n[0];
117  B = n[1];
118  C = n[2];
119 
120  if (frame == vpPlane::camera_frame)
121  D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
122  else
123  D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
124 
125 }
126 
132 void vpPlane::init(const vpPlane& P)
133 {
134  setA(P.getA()) ;
135  setB(P.getB()) ;
136  setC(P.getC()) ;
137  setD(P.getD()) ;
138 }
139 
151 void vpPlane::init(const vpColVector & P, const vpColVector &n)
152 {
153  //Equation of the plane is given by:
154  A = n[0];
155  B = n[1];
156  C = n[2];
157 
158  D=-(A*P[0]+B*P[1]+C*P[2]);
159 }
160 
172 void
173 vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
174 {
175  vpColVector a(3);
176  vpColVector b(3);
177  vpColVector n(3);
178  if (frame == vpPlane::camera_frame) {
179  //Calculate vector corresponding to PQ
180  a[0]=P.get_X()-Q.get_X();
181  a[1]=P.get_Y()-Q.get_Y();
182  a[2]=P.get_Z()-Q.get_Z();
183 
184  //Calculate vector corresponding to PR
185  b[0]=P.get_X()-R.get_X();
186  b[1]=P.get_Y()-R.get_Y();
187  b[2]=P.get_Z()-R.get_Z();
188  }
189  else {
190  //Calculate vector corresponding to PQ
191  a[0]=P.get_oX()-Q.get_oX();
192  a[1]=P.get_oY()-Q.get_oY();
193  a[2]=P.get_oZ()-Q.get_oZ();
194 
195  //Calculate vector corresponding to PR
196  b[0]=P.get_oX()-R.get_oX();
197  b[1]=P.get_oY()-R.get_oY();
198  b[2]=P.get_oZ()-R.get_oZ();
199  }
200  //Calculate normal vector to plane PQ x PR
201  n=vpColVector::cross(a,b);
202 
203  //Equation of the plane is given by:
204  A = n[0];
205  B = n[1];
206  C = n[2];
207  if (frame == vpPlane::camera_frame)
208  D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
209  else
210  D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
211 
212  double norm = sqrt(A*A+B*B+C*C) ;
213  A /= norm ;
214  B /= norm ;
215  C /= norm ;
216  D /= norm ;
217 }
218 
219 
232 vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
233  : A(0), B(0), C(0), D(0)
234 {
235  init(P, Q, R, frame) ;
236 }
237 
247 {
248  vpColVector n(3);
249  n[0] = A ;
250  n[1] = B ;
251  n[2] = C ;
252 
253  return n ;
254 }
255 
267 {
268  n.resize(3) ;
269  n[0] = A ;
270  n[1] = B ;
271  n[2] = C ;
272 }
273 
280 void
282 {
283  double x0,y0,z0 ;
284  double rho ;
285 
286  x0 = P.get_X()/P.get_W() ;
287  y0 = P.get_Y()/P.get_W() ;
288  z0 = P.get_Z()/P.get_W() ;
289 
290  rho = - (A*x0+B*y0+C*z0+D)/(A*A+B*B+C*C) ;
291 
292  Pproj.set_X(x0+A*rho) ;
293  Pproj.set_Y(y0+B*rho) ;
294  Pproj.set_Z(z0+C*rho) ;
295  Pproj.set_W(1) ;
296 }
297 
298 
299 double
301  const vpPoint &M1,
302  vpColVector &H ) const
303 {
304 
305  double k,scal;
306 
307  // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
308  if(std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()
309  || std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()
310  || std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())
311  {
312  double R[3];
313  R[0]= M1.get_X() - M0.get_X();
314  R[1]= M1.get_Y() - M0.get_Y();
315  R[2]= M1.get_Z() - M0.get_Z();
316 
317  scal = getA()*R[0] + getB()*R[1] + getC()*R[2];
318  //if (scal != 0)
319  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
320  k = -( getA()*M0.get_X() + getB()*M0.get_Y() + getC()*M0.get_Z() + getD())/scal;
321  else
322  k = 0;
323 
324  H[0] = M0.get_X()+ k*R[0];
325  H[1] = M0.get_Y()+ k*R[1];
326  H[2] = M0.get_Z()+ k*R[2];
327  }
328  else
329  {
330  scal = getA()*M1.get_X() + getB()*M1.get_Y() + getC()*M1.get_Z();
331  //if (scal != 0)
332  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
333  k = -getD()/scal;
334  else
335  k=0;
336  H[0] = k*M1.get_X();
337  H[1] = k*M1.get_Y();
338  H[2] = k*M1.get_Z();
339  }
340 
341  return k;
342 
343 }
344 
346 {
347 
348  double k,scal;
349 
350  scal = A*M1[0] + B*M1[1] + C*M1[2];
351  //if (scal != 0)
352  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
353  k = -getD()/scal;
354  else
355  k=0;
356  H[0] = k*M1[0];
357  H[1] = k*M1[1];
358  H[2] = k*M1[2];
359 
360  return k;
361 
362 }
363 
373 {
374  // Save current plane parameters
375  double Ao = A, Bo = B, Co = C, Do =D ;
376  A = cMo[0][0]*Ao + cMo[0][1]*Bo + cMo[0][2]*Co;
377  B = cMo[1][0]*Ao + cMo[1][1]*Bo + cMo[1][2]*Co;
378  C = cMo[2][0]*Ao + cMo[2][1]*Bo + cMo[2][2]*Co;
379  D = Do - (cMo[0][3]*A + cMo[1][3]*B + cMo[2][3]*C);
380 }
381 
388 VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p)
389 {
390  return (os << "("<<p.getA() << ","<<p.getB()
391  << ","<<p.getC()<< ","<<p.getD() <<") ") ;
392 } ;
double B
Definition: vpPlane.h:67
void setD(const double d)
Definition: vpPlane.h:91
vpPlane & operator=(const vpPlane &f)
Definition: vpPlane.cpp:54
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:266
Implementation of an homogeneous matrix and operations on such kind of matrices.
double get_oY() const
Get the point Y coordinate in the object frame.
Definition: vpPoint.cpp:449
double C
Definition: vpPlane.h:67
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
Class that defines what is a point.
Definition: vpPoint.h:59
double rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
Definition: vpPlane.cpp:300
double D
Definition: vpPlane.h:67
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:482
void set_W(const double W)
Set the point W coordinate in the camera frame.
Definition: vpPoint.cpp:484
void init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame=camera_frame)
Definition: vpPlane.cpp:173
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:372
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 camera frame.
Definition: vpPoint.cpp:480
void setA(const double a)
Definition: vpPlane.h:85
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:267
void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
Definition: vpPlane.cpp:281
vpPlane()
Definition: vpPlane.cpp:67
vpColVector getNormal() const
Definition: vpPlane.cpp:246
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:440
double getIntersection(const vpColVector &M1, vpColVector &H) const
Definition: vpPlane.cpp:345
void setC(const double c)
Definition: vpPlane.h:89
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:442
double A
Definition: vpPlane.h:67
double get_oX() const
Get the point X coordinate in the object frame.
Definition: vpPoint.cpp:447
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
double getB() const
Definition: vpPlane.h:108
void setB(const double b)
Definition: vpPlane.h:87
double getA() const
Definition: vpPlane.h:106
double getC() const
Definition: vpPlane.h:110
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:438
double getD() const
Definition: vpPlane.h:112
vpPlaneFrame
Definition: vpPlane.h:70
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225