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