Visual Servoing Platform  version 3.5.1 under development (2023-06-08)
vpPlane.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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  * Plane geometrical structure.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #include <visp3/core/vpPlane.h>
46 
47 #include <cmath> // std::fabs
48 #include <limits> // numeric_limits
49 
54 {
55  A = p.A;
56  B = p.B;
57  C = p.C;
58  D = p.D;
59 
60  return *this;
61 }
62 
66 vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {}
67 
78 vpPlane::vpPlane(double a, double b, double c, double d) : A(a), B(b), C(c), D(d) {}
79 
83 vpPlane::vpPlane(const vpPlane &P) : A(0), B(0), C(0), D(0)
84 {
85  setA(P.getA());
86  setB(P.getB());
87  setC(P.getC());
88  setD(P.getD());
89 }
90 
110 vpPlane::vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame) : A(0), B(0), C(0), D(0)
111 {
112  // Equation of the plane is given by:
113  A = n[0];
114  B = n[1];
115  C = n[2];
116 
117  if (frame == vpPlane::camera_frame)
118  D = -(A * P.get_X() + B * P.get_Y() + C * P.get_Z());
119  else
120  D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
121 }
122 
128 void vpPlane::init(const vpPlane &P)
129 {
130  setA(P.getA());
131  setB(P.getB());
132  setC(P.getC());
133  setD(P.getD());
134 }
135 
147 void vpPlane::init(const vpColVector &P, const vpColVector &n)
148 {
149  // Equation of the plane is given by:
150  A = n[0];
151  B = n[1];
152  C = n[2];
153 
154  D = -(A * P[0] + B * P[1] + C * P[2]);
155 }
156 
168 void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
169 {
170  vpColVector a(3);
171  vpColVector b(3);
172  vpColVector n(3);
173  if (frame == vpPlane::camera_frame) {
174  // Calculate vector corresponding to PQ
175  a[0] = P.get_X() - Q.get_X();
176  a[1] = P.get_Y() - Q.get_Y();
177  a[2] = P.get_Z() - Q.get_Z();
178 
179  // Calculate vector corresponding to PR
180  b[0] = P.get_X() - R.get_X();
181  b[1] = P.get_Y() - R.get_Y();
182  b[2] = P.get_Z() - R.get_Z();
183  } else {
184  // Calculate vector corresponding to PQ
185  a[0] = P.get_oX() - Q.get_oX();
186  a[1] = P.get_oY() - Q.get_oY();
187  a[2] = P.get_oZ() - Q.get_oZ();
188 
189  // Calculate vector corresponding to PR
190  b[0] = P.get_oX() - R.get_oX();
191  b[1] = P.get_oY() - R.get_oY();
192  b[2] = P.get_oZ() - R.get_oZ();
193  }
194  // Calculate normal vector to plane PQ x PR
195  n = vpColVector::cross(a, b);
196 
197  // Equation of the plane is given by:
198  A = n[0];
199  B = n[1];
200  C = n[2];
201  if (frame == vpPlane::camera_frame)
202  D = -(A * P.get_X() + B * P.get_Y() + C * P.get_Z());
203  else
204  D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
205 
206  double norm = sqrt(A * A + B * B + C * C);
207  A /= norm;
208  B /= norm;
209  C /= norm;
210  D /= norm;
211 }
212 
225 vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame) : A(0), B(0), C(0), D(0)
226 {
227  init(P, Q, R, frame);
228 }
229 
235 double vpPlane::computeZ(double x, double y) const
236 {
237  return -getD() / (getA() * x + getB() * y + getC());
238 }
239 
249 {
250  vpColVector n(3);
251  n[0] = A;
252  n[1] = B;
253  n[2] = C;
254 
255  return n;
256 }
257 
269 {
270  n.resize(3);
271  n[0] = A;
272  n[1] = B;
273  n[2] = C;
274 }
275 
282 void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
283 {
284  double x0, y0, z0;
285  double rho;
286 
287  x0 = P.get_X() / P.get_W();
288  y0 = P.get_Y() / P.get_W();
289  z0 = P.get_Z() / P.get_W();
290 
291  rho = -(A * x0 + B * y0 + C * z0 + D) / (A * A + B * B + C * C);
292 
293  Pproj.set_X(x0 + A * rho);
294  Pproj.set_Y(y0 + B * rho);
295  Pproj.set_Z(z0 + C * rho);
296  Pproj.set_W(1);
297 }
298 
299 double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
300 {
301 
302  double k, scal;
303 
304  // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
305  if (std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon() ||
306  std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon() ||
307  std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon()) {
308  double R[3];
309  R[0] = M1.get_X() - M0.get_X();
310  R[1] = M1.get_Y() - M0.get_Y();
311  R[2] = M1.get_Z() - M0.get_Z();
312 
313  scal = getA() * R[0] + getB() * R[1] + getC() * R[2];
314  // if (scal != 0)
315  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
316  k = -(getA() * M0.get_X() + getB() * M0.get_Y() + getC() * M0.get_Z() + getD()) / scal;
317  else
318  k = 0;
319 
320  H[0] = M0.get_X() + k * R[0];
321  H[1] = M0.get_Y() + k * R[1];
322  H[2] = M0.get_Z() + k * R[2];
323  } else {
324  scal = getA() * M1.get_X() + getB() * M1.get_Y() + getC() * M1.get_Z();
325  // if (scal != 0)
326  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
327  k = -getD() / scal;
328  else
329  k = 0;
330  H[0] = k * M1.get_X();
331  H[1] = k * M1.get_Y();
332  H[2] = k * M1.get_Z();
333  }
334 
335  return k;
336 }
337 
339 {
340 
341  double k, scal;
342 
343  scal = A * M1[0] + B * M1[1] + C * M1[2];
344  // if (scal != 0)
345  if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
346  k = -getD() / scal;
347  else
348  k = 0;
349  H[0] = k * M1[0];
350  H[1] = k * M1[1];
351  H[2] = k * M1[2];
352 
353  return k;
354 }
355 
365 {
366  // Save current plane parameters
367  double Ao = A, Bo = B, Co = C, Do = D;
368  A = cMo[0][0] * Ao + cMo[0][1] * Bo + cMo[0][2] * Co;
369  B = cMo[1][0] * Ao + cMo[1][1] * Bo + cMo[1][2] * Co;
370  C = cMo[2][0] * Ao + cMo[2][1] * Bo + cMo[2][2] * Co;
371  D = Do - (cMo[0][3] * A + cMo[1][3] * B + cMo[2][3] * C);
372 }
373 
380 VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpPlane &p)
381 {
382  return (os << "(" << p.getA() << "," << p.getB() << "," << p.getC() << "," << p.getD() << ") ");
383 };
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:499
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:397
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
Implementation of an homogeneous matrix and operations on such kind of matrices.
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:59
double C
Definition: vpPlane.h:67
vpPlaneFrame
Definition: vpPlane.h:70
@ camera_frame
Definition: vpPlane.h:70
double rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
Definition: vpPlane.cpp:299
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:364
void setA(double a)
Definition: vpPlane.h:85
void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
Definition: vpPlane.cpp:282
void setD(double d)
Definition: vpPlane.h:91
double A
Definition: vpPlane.h:67
double D
Definition: vpPlane.h:67
double computeZ(double x, double y) const
Definition: vpPlane.cpp:235
double getD() const
Definition: vpPlane.h:111
void setC(double c)
Definition: vpPlane.h:89
double B
Definition: vpPlane.h:67
vpColVector getNormal() const
Definition: vpPlane.cpp:248
double getA() const
Definition: vpPlane.h:105
double getC() const
Definition: vpPlane.h:109
double getB() const
Definition: vpPlane.h:107
vpPlane & operator=(const vpPlane &f)
Definition: vpPlane.cpp:53
vpPlane()
Definition: vpPlane.cpp:66
void init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame=camera_frame)
Definition: vpPlane.cpp:168
void setB(double b)
Definition: vpPlane.h:87
double getIntersection(const vpColVector &M1, vpColVector &H) const
Definition: vpPlane.cpp:338
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_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:461
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:499
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_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:493
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 set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:497
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:463
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:452