Visual Servoing Platform version 3.5.0
vpPlane.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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
66vpPlane::vpPlane() : A(0), B(0), C(0), D(0) {}
67
78vpPlane::vpPlane(double a, double b, double c, double d) : A(a), B(b), C(c), D(d) {}
79
83vpPlane::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
110vpPlane::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
128void vpPlane::init(const vpPlane &P)
129{
130 setA(P.getA());
131 setB(P.getB());
132 setC(P.getC());
133 setD(P.getD());
134}
135
147void 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
168void 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
225vpPlane::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
239{
240 vpColVector n(3);
241 n[0] = A;
242 n[1] = B;
243 n[2] = C;
244
245 return n;
246}
247
259{
260 n.resize(3);
261 n[0] = A;
262 n[1] = B;
263 n[2] = C;
264}
265
273{
274 double x0, y0, z0;
275 double rho;
276
277 x0 = P.get_X() / P.get_W();
278 y0 = P.get_Y() / P.get_W();
279 z0 = P.get_Z() / P.get_W();
280
281 rho = -(A * x0 + B * y0 + C * z0 + D) / (A * A + B * B + C * C);
282
283 Pproj.set_X(x0 + A * rho);
284 Pproj.set_Y(y0 + B * rho);
285 Pproj.set_Z(z0 + C * rho);
286 Pproj.set_W(1);
287}
288
289double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
290{
291
292 double k, scal;
293
294 // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
295 if (std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon() ||
296 std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon() ||
297 std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon()) {
298 double R[3];
299 R[0] = M1.get_X() - M0.get_X();
300 R[1] = M1.get_Y() - M0.get_Y();
301 R[2] = M1.get_Z() - M0.get_Z();
302
303 scal = getA() * R[0] + getB() * R[1] + getC() * R[2];
304 // if (scal != 0)
305 if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
306 k = -(getA() * M0.get_X() + getB() * M0.get_Y() + getC() * M0.get_Z() + getD()) / scal;
307 else
308 k = 0;
309
310 H[0] = M0.get_X() + k * R[0];
311 H[1] = M0.get_Y() + k * R[1];
312 H[2] = M0.get_Z() + k * R[2];
313 } else {
314 scal = getA() * M1.get_X() + getB() * M1.get_Y() + getC() * M1.get_Z();
315 // if (scal != 0)
316 if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
317 k = -getD() / scal;
318 else
319 k = 0;
320 H[0] = k * M1.get_X();
321 H[1] = k * M1.get_Y();
322 H[2] = k * M1.get_Z();
323 }
324
325 return k;
326}
327
329{
330
331 double k, scal;
332
333 scal = A * M1[0] + B * M1[1] + C * M1[2];
334 // if (scal != 0)
335 if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
336 k = -getD() / scal;
337 else
338 k = 0;
339 H[0] = k * M1[0];
340 H[1] = k * M1[1];
341 H[2] = k * M1[2];
342
343 return k;
344}
345
355{
356 // Save current plane parameters
357 double Ao = A, Bo = B, Co = C, Do = D;
358 A = cMo[0][0] * Ao + cMo[0][1] * Bo + cMo[0][2] * Co;
359 B = cMo[1][0] * Ao + cMo[1][1] * Bo + cMo[1][2] * Co;
360 C = cMo[2][0] * Ao + cMo[2][1] * Bo + cMo[2][2] * Co;
361 D = Do - (cMo[0][3] * A + cMo[1][3] * B + cMo[2][3] * C);
362}
363
370VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpPlane &p)
371{
372 return (os << "(" << p.getA() << "," << p.getB() << "," << p.getC() << "," << p.getD() << ") ");
373};
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static vpColVector cross(const vpColVector &a, const vpColVector &b)
Definition: vpColVector.h:351
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:289
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
void setA(double a)
Definition: vpPlane.h:82
void projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
Definition: vpPlane.cpp:272
void setD(double d)
Definition: vpPlane.h:88
double A
Definition: vpPlane.h:67
double D
Definition: vpPlane.h:67
double getD() const
Definition: vpPlane.h:108
void setC(double c)
Definition: vpPlane.h:86
double B
Definition: vpPlane.h:67
vpColVector getNormal() const
Definition: vpPlane.cpp:238
double getA() const
Definition: vpPlane.h:102
double getC() const
Definition: vpPlane.h:106
double getB() const
Definition: vpPlane.h:104
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:84
double getIntersection(const vpColVector &M1, vpColVector &H) const
Definition: vpPlane.cpp:328
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