Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
vpPlane.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
41 #include <visp3/core/vpPlane.h>
42 
43 #include <cmath> // std::fabs
44 #include <limits> // numeric_limits
45 
50 {
51  A = p.A;
52  B = p.B;
53  C = p.C;
54  D = p.D;
55 
56  return *this;
57 }
58 
62 vpPlane::vpPlane() : A(0), B(0), C(0), D(0) { }
63 
74 vpPlane::vpPlane(double a, double b, double c, double d) : A(a), B(b), C(c), D(d) { }
75 
79 vpPlane::vpPlane(const vpPlane &P) : A(0), B(0), C(0), D(0)
80 {
81  setA(P.getA());
82  setB(P.getB());
83  setC(P.getC());
84  setD(P.getD());
85 }
86 
106 vpPlane::vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame) : A(0), B(0), C(0), D(0)
107 {
108  // Equation of the plane is given by:
109  A = n[0];
110  B = n[1];
111  C = n[2];
112 
113  if (frame == vpPlane::camera_frame) {
114  D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z()));
115  }
116  else {
117  D = -((A * P.get_oX()) + (B * P.get_oY()) + (C * P.get_oZ()));
118  }
119 }
120 
126 void vpPlane::init(const vpPlane &P)
127 {
128  setA(P.getA());
129  setB(P.getB());
130  setC(P.getC());
131  setD(P.getD());
132 }
133 
145 void vpPlane::init(const vpColVector &P, const vpColVector &n)
146 {
147  // Equation of the plane is given by:
148  A = n[0];
149  B = n[1];
150  C = n[2];
151 
152  D = -((A * P[0]) + (B * P[1]) + (C * P[2]));
153 }
154 
166 void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame)
167 {
168  vpColVector a(3);
169  vpColVector b(3);
170  vpColVector n(3);
171  if (frame == vpPlane::camera_frame) {
172  // Calculate vector corresponding to PQ
173  a[0] = P.get_X() - Q.get_X();
174  a[1] = P.get_Y() - Q.get_Y();
175  a[2] = P.get_Z() - Q.get_Z();
176 
177  // Calculate vector corresponding to PR
178  b[0] = P.get_X() - R.get_X();
179  b[1] = P.get_Y() - R.get_Y();
180  b[2] = P.get_Z() - R.get_Z();
181  }
182  else {
183  // Calculate vector corresponding to PQ
184  a[0] = P.get_oX() - Q.get_oX();
185  a[1] = P.get_oY() - Q.get_oY();
186  a[2] = P.get_oZ() - Q.get_oZ();
187 
188  // Calculate vector corresponding to PR
189  b[0] = P.get_oX() - R.get_oX();
190  b[1] = P.get_oY() - R.get_oY();
191  b[2] = P.get_oZ() - R.get_oZ();
192  }
193  // Calculate normal vector to plane PQ x PR
194  n = vpColVector::cross(a, b);
195 
196  // Equation of the plane is given by:
197  A = n[0];
198  B = n[1];
199  C = n[2];
200  if (frame == vpPlane::camera_frame) {
201  D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z()));
202  }
203  else {
204  D = -((A * P.get_oX()) + (B * P.get_oY()) + (C * P.get_oZ()));
205  }
206 
207  double norm = sqrt((A * A) + (B * B) + (C * C));
208  A /= norm;
209  B /= norm;
210  C /= norm;
211  D /= norm;
212 }
213 
226 vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame) : A(0), B(0), C(0), D(0)
227 {
228  init(P, Q, R, frame);
229 }
230 
236 double vpPlane::computeZ(double x, double y) const
237 {
238  return -getD() / ((getA() * x) + (getB() * y) + getC());
239 }
240 
250 {
251  vpColVector n(3);
252  n[0] = A;
253  n[1] = B;
254  n[2] = C;
255 
256  return n;
257 }
258 
270 {
271  n.resize(3);
272  n[0] = A;
273  n[1] = B;
274  n[2] = C;
275 }
276 
283 void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
284 {
285  double x0, y0, z0;
286  double rho;
287 
288  x0 = P.get_X() / P.get_W();
289  y0 = P.get_Y() / P.get_W();
290  z0 = P.get_Z() / P.get_W();
291 
292  rho = -((A * x0) + (B * y0) + (C * z0) + D) / ((A * A) + (B * B) + (C * C));
293 
294  Pproj.set_X(x0 + (A * rho));
295  Pproj.set_Y(y0 + (B * rho));
296  Pproj.set_Z(z0 + (C * rho));
297  Pproj.set_W(1);
298 }
299 
300 double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
301 {
302 
303  double k, scal;
304 
305  // --comment: if M0.get_X() diff 0 or M0.get_Y() diff 0 or M0.get_Z() diff 0
306  if ((std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()) ||
307  (std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()) ||
308  (std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())) {
309  double R[3];
310  R[0] = M1.get_X() - M0.get_X();
311  R[1] = M1.get_Y() - M0.get_Y();
312  R[2] = M1.get_Z() - M0.get_Z();
313 
314  scal = (getA() * R[0]) + (getB() * R[1]) + (getC() * R[2]);
315  // --comment: if scal != 0
316  if (std::fabs(scal) > std::numeric_limits<double>::epsilon()) {
317  k = -((getA() * M0.get_X()) + (getB() * M0.get_Y()) + (getC() * M0.get_Z()) + getD()) / scal;
318  }
319  else {
320  k = 0;
321  }
322 
323  H[0] = M0.get_X() + (k * R[0]);
324  H[1] = M0.get_Y() + (k * R[1]);
325  H[2] = M0.get_Z() + (k * R[2]);
326  }
327  else {
328  scal = (getA() * M1.get_X()) + (getB() * M1.get_Y()) + (getC() * M1.get_Z());
329  // --comment: if scal != 0
330  if (std::fabs(scal) > std::numeric_limits<double>::epsilon()) {
331  k = -getD() / scal;
332  }
333  else {
334  k = 0;
335  }
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 
345 {
346 
347  double k, scal;
348 
349  scal = (A * M1[0]) + (B * M1[1]) + (C * M1[2]);
350  // --comment: if scal != 0
351  if (std::fabs(scal) > std::numeric_limits<double>::epsilon()) {
352  k = -getD() / scal;
353  }
354  else {
355  k = 0;
356  }
357  H[0] = k * M1[0];
358  H[1] = k * M1[1];
359  H[2] = k * M1[2];
360 
361  return k;
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() << "," << p.getC() << "," << p.getD() << ") ");
391 };
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:603
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:1174
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
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:54
double C
Definition: vpPlane.h:62
vpPlaneFrame
Definition: vpPlane.h:65
@ camera_frame
Definition: vpPlane.h:65
double rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
Definition: vpPlane.cpp:300
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:372
void setA(double a)
Definition: vpPlane.h:80
void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
Definition: vpPlane.cpp:283
void setD(double d)
Definition: vpPlane.h:86
double A
Definition: vpPlane.h:62
double D
Definition: vpPlane.h:62
double computeZ(double x, double y) const
Definition: vpPlane.cpp:236
double getD() const
Definition: vpPlane.h:106
void setC(double c)
Definition: vpPlane.h:84
double B
Definition: vpPlane.h:62
vpColVector getNormal() const
Definition: vpPlane.cpp:249
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
double getB() const
Definition: vpPlane.h:102
vpPlane & operator=(const vpPlane &f)
Definition: vpPlane.cpp:49
vpPlane()
Definition: vpPlane.cpp:62
void init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame=camera_frame)
Definition: vpPlane.cpp:166
void setB(double b)
Definition: vpPlane.h:82
double getIntersection(const vpColVector &M1, vpColVector &H) const
Definition: vpPlane.cpp:344
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:454
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:492
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:447
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:458
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:486
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:451
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:488
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:449
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:490
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:456
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:445