Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
vpLine.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Line feature.
32  *
33 *****************************************************************************/
34 
40 #include <visp3/core/vpLine.h>
41 
42 #include <visp3/core/vpMath.h>
43 
44 #include <visp3/core/vpFeatureDisplay.h>
45 
46 BEGIN_VISP_NAMESPACE
54 {
55  const unsigned int val_2 = 2;
56  const unsigned int val_8 = 8;
57  oP.resize(val_8);
58  cP.resize(val_8);
59  p.resize(val_2);
60 }
61 
66 
83 void vpLine::setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1,
84  const double &oA2, const double &oB2, const double &oC2, const double &oD2)
85 {
86  const unsigned int index_0 = 0;
87  const unsigned int index_1 = 1;
88  const unsigned int index_2 = 2;
89  const unsigned int index_3 = 3;
90  const unsigned int index_4 = 4;
91  const unsigned int index_5 = 5;
92  const unsigned int index_6 = 6;
93  const unsigned int index_7 = 7;
94  oP[index_0] = oA1;
95  oP[index_1] = oB1;
96  oP[index_2] = oC1;
97  oP[index_3] = oD1;
98 
99  oP[index_4] = oA2;
100  oP[index_5] = oB2;
101  oP[index_6] = oC2;
102  oP[index_7] = oD2;
103 }
104 
124 {
125  const unsigned int val_8 = 8;
126  if (oP_.getRows() != val_8) {
127  throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
128  }
129  this->oP = oP_;
130 }
131 
154 {
155  const unsigned int val_4 = 4;
156  if (oP1.getRows() != val_4) {
157  throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
158  }
159  if (oP2.getRows() != val_4) {
160  throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
161  }
162  for (unsigned int i = 0; i < val_4; ++i) {
163  oP[i] = oP1[i];
164  oP[i + val_4] = oP2[i];
165  }
166 }
167 
202 
221 void vpLine::projection(const vpColVector &cP_, vpColVector &p_) const
222 {
223  const unsigned int val_2 = 2;
224  const unsigned int val_8 = 8;
225  p_.resize(val_2, false);
226  // projection
227 
228  if (cP.getRows() != val_8) {
229  throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
230  }
231  double A1, A2, B1, B2, C1, C2, D1, D2;
232  const unsigned int index_0 = 0;
233  const unsigned int index_1 = 1;
234  const unsigned int index_2 = 2;
235  const unsigned int index_3 = 3;
236  const unsigned int index_4 = 4;
237  const unsigned int index_5 = 5;
238  const unsigned int index_6 = 6;
239  const unsigned int index_7 = 7;
240 
241  A1 = cP_[index_0];
242  B1 = cP_[index_1];
243  C1 = cP_[index_2];
244  D1 = cP_[index_3];
245 
246  A2 = cP_[index_4];
247  B2 = cP_[index_5];
248  C2 = cP_[index_6];
249  D2 = cP_[index_7];
250 
251  double a, b, c, s;
252  a = (A2 * D1) - (A1 * D2);
253  b = (B2 * D1) - (B1 * D2);
254  c = (C2 * D1) - (C1 * D2);
255  s = (a * a) + (b * b);
256  if (s <= 1e-8) // seuil pas terrible
257  {
258  printf("Degenerate case: the image of the straight line is a point!\n");
259  throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
260  }
261  s = 1.0 / sqrt(s);
262 
263  double rho = -c * s;
264  double theta = atan2(b, a);
265 
266  p_[0] = rho;
267  p_[1] = theta;
268 }
269 
307 
349 {
350  const unsigned int val_8 = 8;
351  cP_.resize(val_8, false);
352 
353  double a1, a2, b1, b2, c1, c2, d1, d2;
354  double A1, A2, B1, B2, C1, C2, D1, D2;
355  const unsigned int index_0 = 0;
356  const unsigned int index_1 = 1;
357  const unsigned int index_2 = 2;
358  const unsigned int index_3 = 3;
359  const unsigned int index_4 = 4;
360  const unsigned int index_5 = 5;
361  const unsigned int index_6 = 6;
362  const unsigned int index_7 = 7;
363 
364  // in case of verification
365  // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2;
366 
367  a1 = oP[index_0];
368  b1 = oP[index_1];
369  c1 = oP[index_2];
370  d1 = oP[index_3];
371 
372  a2 = oP[index_4];
373  b2 = oP[index_5];
374  c2 = oP[index_6];
375  d2 = oP[index_7];
376 
377  A1 = (cMo[index_0][0] * a1) + (cMo[index_0][1] * b1) + (cMo[index_0][index_2] * c1);
378  B1 = (cMo[index_1][0] * a1) + (cMo[index_1][1] * b1) + (cMo[index_1][index_2] * c1);
379  C1 = (cMo[index_2][0] * a1) + (cMo[index_2][1] * b1) + (cMo[index_2][index_2] * c1);
380  D1 = d1 - ((cMo[index_0][index_3] * A1) + (cMo[index_1][index_3] * B1) + (cMo[index_2][index_3] * C1));
381 
382  A2 = (cMo[index_0][0] * a2) + (cMo[index_0][1] * b2) + (cMo[index_0][index_2] * c2);
383  B2 = (cMo[index_1][0] * a2) + (cMo[index_1][1] * b2) + (cMo[index_1][index_2] * c2);
384  C2 = (cMo[index_2][0] * a2) + (cMo[index_2][1] * b2) + (cMo[index_2][index_2] * c2);
385  D2 = d2 - ((cMo[index_0][index_3] * A2) + (cMo[index_1][index_3] * B2) + (cMo[index_2][index_3] * C2));
386 
387  // Adding constraints on the straight line to have a unique representation
388 
389  // direction of the straight line = N1 x N2
390  a2 = (B1 * C2) - (C1 * B2);
391  b2 = (C1 * A2) - (A1 * C2);
392  c2 = (A1 * B2) - (B1 * A2);
393 
394  // Constraint D1 = 0 (the origin belongs to P1)
395  a1 = (A2 * D1) - (A1 * D2);
396  b1 = (B2 * D1) - (B1 * D2);
397  c1 = (C2 * D1) - (C1 * D2);
398 
399  if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
400  {
401  A2 = A1;
402  B2 = B1;
403  C2 = C1;
404  D2 = D1;
405  }
406 
407  // Constraint A1^2 + B1^2 + C1^2 = 1
408  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
409  A1 = a1 * d1;
410  B1 = b1 * d1;
411  C1 = c1 * d1;
412  cP_[index_0] = A1;
413  cP_[index_1] = B1;
414  cP_[index_2] = C1;
415 
416  cP_[index_3] = 0;
417 
418  // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
419  // N2_new = (N1 x N2) x N1_new
420  a1 = (b2 * C1) - (c2 * B1);
421  b1 = (c2 * A1) - (a2 * C1);
422  c1 = (a2 * B1) - (b2 * A1);
423 
424  // Constraint A2^2 + B2^2 + C2^2 = 1
425  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
426  a1 *= d1;
427  b1 *= d1;
428  c1 *= d1;
429 
430  // D2_new = D2 / (N2^T . N2_new)
431  D2 /= ((A2 * a1) + (B2 * b1) + (C2 * c1));
432  A2 = a1;
433  B2 = b1;
434  C2 = c1;
435 
436  // Constraint D2 < 0
437  if (D2 > 0) {
438  A2 = -A2;
439  B2 = -B2;
440  C2 = -C2;
441  D2 = -D2;
442  }
443 
444  cP_[index_4] = A2;
445  cP_[index_5] = B2;
446  cP_[index_6] = C2;
447  cP_[index_7] = D2;
448 }
449 
466 void vpLine::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
467  unsigned int thickness)
468 {
469  vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
470 }
471 
487 void vpLine::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
488  unsigned int thickness)
489 {
490  vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
491 }
492 
516  const vpColor &color, unsigned int thickness)
517 {
518  vpColVector v_cP, v_p;
519  changeFrame(cMo, v_cP);
520  try {
521  projection(v_cP, v_p);
522  vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
523  }
524  catch (...) {
525  // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
526  }
527 }
528 
552  const vpColor &color, unsigned int thickness)
553 {
554  vpColVector v_cP, v_p;
555  changeFrame(cMo, v_cP);
556  try {
557  projection(v_cP, v_p);
558  vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
559  }
560  catch (...) {
561  // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
562  }
563 }
564 
576 {
577  vpLine *feature = new vpLine(*this);
578  return feature;
579 }
580 END_VISP_NAMESPACE
unsigned int getRows() const
Definition: vpArray2D.h:347
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
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:103
void projection() VP_OVERRIDE
Definition: vpLine.cpp:201
vpLine * duplicate() const VP_OVERRIDE
Definition: vpLine.cpp:575
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
Definition: vpLine.cpp:348
vpLine()
Definition: vpLine.cpp:65
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) VP_OVERRIDE
Definition: vpLine.cpp:466
void init() VP_OVERRIDE
Definition: vpLine.cpp:53
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:83
vpColVector cP
Definition: vpTracker.h:73
vpColVector p
Definition: vpTracker.h:69