Visual Servoing Platform version 3.5.0
vpLine.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 * Line feature.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpLine.h>
40
41#include <visp3/core/vpDebug.h>
42#include <visp3/core/vpMath.h>
43
44#include <visp3/core/vpFeatureDisplay.h>
45
58{
59 oP.resize(8);
60 cP.resize(8);
61 p.resize(2);
62}
63
68
85void vpLine::setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1,
86 const double &oA2, const double &oB2, const double &oC2, const double &oD2)
87{
88 oP[0] = oA1;
89 oP[1] = oB1;
90 oP[2] = oC1;
91 oP[3] = oD1;
92
93 oP[4] = oA2;
94 oP[5] = oB2;
95 oP[6] = oC2;
96 oP[7] = oD2;
97}
98
118{
119 if (oP_.getRows() != 8)
120 throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
121
122 this->oP = oP_;
123}
124
147{
148 if (oP1.getRows() != 4)
149 throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
150
151 if (oP2.getRows() != 4)
152 throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
153
154 for (unsigned int i = 0; i < 4; i++) {
155 oP[i] = oP1[i];
156 oP[i + 4] = oP2[i];
157 }
158}
159
194
213void vpLine::projection(const vpColVector &cP_, vpColVector &p_) const
214{
215 p_.resize(2, false);
216 // projection
217
218 if (cP.getRows() != 8)
219 throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
220
221 double A1, A2, B1, B2, C1, C2, D1, D2;
222
223 A1 = cP_[0];
224 B1 = cP_[1];
225 C1 = cP_[2];
226 D1 = cP_[3];
227
228 A2 = cP_[4];
229 B2 = cP_[5];
230 C2 = cP_[6];
231 D2 = cP_[7];
232
233 double a, b, c, s;
234 a = A2 * D1 - A1 * D2;
235 b = B2 * D1 - B1 * D2;
236 c = C2 * D1 - C1 * D2;
237 s = a * a + b * b;
238 if (s <= 1e-8) // seuil pas terrible
239 {
240 printf("Degenerate case: the image of the straight line is a point!\n");
241 throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
242 }
243 s = 1.0 / sqrt(s);
244
245 double rho = -c * s;
246 double theta = atan2(b, a);
247
248 p_[0] = rho;
249 p_[1] = theta;
250}
251
289
331{
332 cP_.resize(8, false);
333
334 double a1, a2, b1, b2, c1, c2, d1, d2;
335 double A1, A2, B1, B2, C1, C2, D1, D2;
336
337 // in case of verification
338 // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2;
339
340 a1 = oP[0];
341 b1 = oP[1];
342 c1 = oP[2];
343 d1 = oP[3];
344
345 a2 = oP[4];
346 b2 = oP[5];
347 c2 = oP[6];
348 d2 = oP[7];
349
350 A1 = cMo[0][0] * a1 + cMo[0][1] * b1 + cMo[0][2] * c1;
351 B1 = cMo[1][0] * a1 + cMo[1][1] * b1 + cMo[1][2] * c1;
352 C1 = cMo[2][0] * a1 + cMo[2][1] * b1 + cMo[2][2] * c1;
353 D1 = d1 - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1);
354
355 A2 = cMo[0][0] * a2 + cMo[0][1] * b2 + cMo[0][2] * c2;
356 B2 = cMo[1][0] * a2 + cMo[1][1] * b2 + cMo[1][2] * c2;
357 C2 = cMo[2][0] * a2 + cMo[2][1] * b2 + cMo[2][2] * c2;
358 D2 = d2 - (cMo[0][3] * A2 + cMo[1][3] * B2 + cMo[2][3] * C2);
359
360 // in case of verification
361 // ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1;
362 // ap2 = A2; bp2 = B2; cp2 = C2; dp2 = D2;
363
364 // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
365 // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
366
367 // Adding constraints on the straight line to have a unique representation
368
369 // direction of the straight line = N1 x N2
370 a2 = B1 * C2 - C1 * B2;
371 b2 = C1 * A2 - A1 * C2;
372 c2 = A1 * B2 - B1 * A2;
373
374 // Constraint D1 = 0 (the origin belongs to P1)
375 a1 = A2 * D1 - A1 * D2;
376 b1 = B2 * D1 - B1 * D2;
377 c1 = C2 * D1 - C1 * D2;
378
379 if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
380 {
381 A2 = A1;
382 B2 = B1;
383 C2 = C1;
384 D2 = D1;
385 }
386
387 // Constraint A1^2 + B1^2 + C1^2 = 1
388 d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1);
389 cP_[0] = A1 = a1 * d1;
390 cP_[1] = B1 = b1 * d1;
391 cP_[2] = C1 = c1 * d1;
392 cP_[3] = 0;
393
394 // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
395 // N2_new = (N1 x N2) x N1_new
396 a1 = b2 * C1 - c2 * B1;
397 b1 = c2 * A1 - a2 * C1;
398 c1 = a2 * B1 - b2 * A1;
399
400 // Constraint A2^2 + B2^2 + C2^2 = 1
401 d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1);
402 a1 *= d1;
403 b1 *= d1;
404 c1 *= d1;
405
406 // D2_new = D2 / (N2^T . N2_new)
407 D2 /= (A2 * a1 + B2 * b1 + C2 * c1);
408 A2 = a1;
409 B2 = b1;
410 C2 = c1;
411
412 // Constraint D2 < 0
413 if (D2 > 0) {
414 A2 = -A2;
415 B2 = -B2;
416 C2 = -C2;
417 D2 = -D2;
418 }
419 // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
420 // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
421
422 cP_[4] = A2;
423 cP_[5] = B2;
424 cP_[6] = C2;
425 cP_[7] = D2;
426
427 // in case of verification
428 /*
429 x = -A2*D2;
430 y = -B2*D2;
431 z = -C2*D2;
432 d1 = ap1*x+bp1*y+cp1*z+dp1;
433 d2 = ap2*x+bp2*y+cp2*z+dp2;
434 if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
435 {
436 printf("PB in VPline: P1 : 0 = %lf, P2: 0 = %lf\n",d1,d2);
437 exit(-1);
438 }
439 d1 = A1*x+B1*y+C1*z+D1;
440 d2 = A2*x+B2*y+C2*z+D2;
441 if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
442 {
443 printf("PB in VPline: Pn1 : 0 = %lf, Pn2: 0 = %lf\n",d1,d2);
444 exit(-1);
445 }
446 */
447}
448
465void vpLine::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
466 unsigned int thickness)
467{
468 vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
469}
470
486void vpLine::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
487 unsigned int thickness)
488{
489 vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
490}
491
515 const vpColor &color, unsigned int thickness)
516{
517 vpColVector _cP, _p;
518 changeFrame(cMo, _cP);
519 try {
520 projection(_cP, _p);
521 vpFeatureDisplay::displayLine(_p[0], _p[1], cam, I, color, thickness);
522 } catch (...) {
523 // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
524 }
525}
526
550 const vpColor &color, unsigned int thickness)
551{
552 vpColVector _cP, _p;
553 changeFrame(cMo, _cP);
554 try {
555 projection(_cP, _p);
556 vpFeatureDisplay::displayLine(_p[0], _p[1], cam, I, color, thickness);
557 } catch (...) {
558 // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
559 }
560}
561
573{
574 vpLine *feature = new vpLine(*this);
575 return feature;
576}
unsigned int getRows() const
Definition: vpArray2D.h:289
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:105
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpLine.cpp:330
vpLine * duplicate() const
Definition: vpLine.cpp:572
void projection()
Definition: vpLine.cpp:193
vpLine()
Definition: vpLine.cpp:67
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpLine.cpp:465
void init()
Definition: vpLine.cpp:57
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
Definition: vpLine.cpp:85
vpColVector cP
Definition: vpTracker.h:77
vpColVector p
Definition: vpTracker.h:73