Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
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 
82 void 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 
210 void 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  // Adding constraints on the straight line to have a unique representation
358 
359  // direction of the straight line = N1 x N2
360  a2 = (B1 * C2) - (C1 * B2);
361  b2 = (C1 * A2) - (A1 * C2);
362  c2 = (A1 * B2) - (B1 * A2);
363 
364  // Constraint D1 = 0 (the origin belongs to P1)
365  a1 = (A2 * D1) - (A1 * D2);
366  b1 = (B2 * D1) - (B1 * D2);
367  c1 = (C2 * D1) - (C1 * D2);
368 
369  if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
370  {
371  A2 = A1;
372  B2 = B1;
373  C2 = C1;
374  D2 = D1;
375  }
376 
377  // Constraint A1^2 + B1^2 + C1^2 = 1
378  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
379  A1 = a1 * d1;
380  B1 = b1 * d1;
381  C1 = c1 * d1;
382  cP_[0] = A1;
383  cP_[1] = B1;
384  cP_[2] = C1;
385 
386  cP_[3] = 0;
387 
388  // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
389  // N2_new = (N1 x N2) x N1_new
390  a1 = (b2 * C1) - (c2 * B1);
391  b1 = (c2 * A1) - (a2 * C1);
392  c1 = (a2 * B1) - (b2 * A1);
393 
394  // Constraint A2^2 + B2^2 + C2^2 = 1
395  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
396  a1 *= d1;
397  b1 *= d1;
398  c1 *= d1;
399 
400  // D2_new = D2 / (N2^T . N2_new)
401  D2 /= ((A2 * a1) + (B2 * b1) + (C2 * c1));
402  A2 = a1;
403  B2 = b1;
404  C2 = c1;
405 
406  // Constraint D2 < 0
407  if (D2 > 0) {
408  A2 = -A2;
409  B2 = -B2;
410  C2 = -C2;
411  D2 = -D2;
412  }
413  // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
414  // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
415 
416  cP_[4] = A2;
417  cP_[5] = B2;
418  cP_[6] = C2;
419  cP_[7] = D2;
420 }
421 
438 void vpLine::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
439  unsigned int thickness)
440 {
441  vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
442 }
443 
459 void vpLine::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
460  unsigned int thickness)
461 {
462  vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
463 }
464 
488  const vpColor &color, unsigned int thickness)
489 {
490  vpColVector v_cP, v_p;
491  changeFrame(cMo, v_cP);
492  try {
493  projection(v_cP, v_p);
494  vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
495  }
496  catch (...) {
497  // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
498  }
499 }
500 
524  const vpColor &color, unsigned int thickness)
525 {
526  vpColVector v_cP, v_p;
527  changeFrame(cMo, v_cP);
528  try {
529  projection(v_cP, v_p);
530  vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
531  }
532  catch (...) {
533  // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
534  }
535 }
536 
548 {
549  vpLine *feature = new vpLine(*this);
550  return feature;
551 }
unsigned int getRows() const
Definition: vpArray2D.h:339
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
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:70
@ fatalError
Fatal error.
Definition: vpException.h:71
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:101
vpLine()
Definition: vpLine.cpp:64
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpLine.cpp:327
vpLine * duplicate() const vp_override
Definition: vpLine.cpp:547
void init() vp_override
Definition: vpLine.cpp:54
void projection() vp_override
Definition: vpLine.cpp:190
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
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) vp_override
Definition: vpLine.cpp:438
vpColVector cP
Definition: vpTracker.h:71
vpColVector p
Definition: vpTracker.h:67