Visual Servoing Platform  version 3.4.1 under development (2021-10-26)
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 
85 void 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 
213 void 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 
465 void 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 
486 void 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 }
void init()
Definition: vpLine.cpp:57
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpLine.cpp:330
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
vpLine * duplicate() const
Definition: vpLine.cpp:572
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
vpColVector cP
Definition: vpTracker.h:77
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:104
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
Generic class defining intrinsic camera parameters.
vpLine()
Definition: vpLine.cpp:67
void projection()
Definition: vpLine.cpp:193
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpLine.cpp:465
vpColVector p
Definition: vpTracker.h:73