Visual Servoing Platform  version 3.6.1 under development (2024-11-21)
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  if (oP_.getRows() != 8) {
126  throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
127  }
128  this->oP = oP_;
129 }
130 
153 {
154  if (oP1.getRows() != 4) {
155  throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
156  }
157  if (oP2.getRows() != 4) {
158  throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
159  }
160  const unsigned int val_4 = 4;
161  for (unsigned int i = 0; i < val_4; ++i) {
162  oP[i] = oP1[i];
163  oP[i + 4] = oP2[i];
164  }
165 }
166 
201 
220 void vpLine::projection(const vpColVector &cP_, vpColVector &p_) const
221 {
222  p_.resize(2, false);
223  // projection
224 
225  if (cP.getRows() != 8) {
226  throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
227  }
228  double A1, A2, B1, B2, C1, C2, D1, D2;
229  const unsigned int index_0 = 0;
230  const unsigned int index_1 = 1;
231  const unsigned int index_2 = 2;
232  const unsigned int index_3 = 3;
233  const unsigned int index_4 = 4;
234  const unsigned int index_5 = 5;
235  const unsigned int index_6 = 6;
236  const unsigned int index_7 = 7;
237 
238  A1 = cP_[index_0];
239  B1 = cP_[index_1];
240  C1 = cP_[index_2];
241  D1 = cP_[index_3];
242 
243  A2 = cP_[index_4];
244  B2 = cP_[index_5];
245  C2 = cP_[index_6];
246  D2 = cP_[index_7];
247 
248  double a, b, c, s;
249  a = (A2 * D1) - (A1 * D2);
250  b = (B2 * D1) - (B1 * D2);
251  c = (C2 * D1) - (C1 * D2);
252  s = (a * a) + (b * b);
253  if (s <= 1e-8) // seuil pas terrible
254  {
255  printf("Degenerate case: the image of the straight line is a point!\n");
256  throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
257  }
258  s = 1.0 / sqrt(s);
259 
260  double rho = -c * s;
261  double theta = atan2(b, a);
262 
263  p_[0] = rho;
264  p_[1] = theta;
265 }
266 
304 
346 {
347  cP_.resize(8, false);
348 
349  double a1, a2, b1, b2, c1, c2, d1, d2;
350  double A1, A2, B1, B2, C1, C2, D1, D2;
351  const unsigned int index_0 = 0;
352  const unsigned int index_1 = 1;
353  const unsigned int index_2 = 2;
354  const unsigned int index_3 = 3;
355  const unsigned int index_4 = 4;
356  const unsigned int index_5 = 5;
357  const unsigned int index_6 = 6;
358  const unsigned int index_7 = 7;
359 
360  // in case of verification
361  // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2;
362 
363  a1 = oP[index_0];
364  b1 = oP[index_1];
365  c1 = oP[index_2];
366  d1 = oP[index_3];
367 
368  a2 = oP[index_4];
369  b2 = oP[index_5];
370  c2 = oP[index_6];
371  d2 = oP[index_7];
372 
373  A1 = (cMo[index_0][0] * a1) + (cMo[index_0][1] * b1) + (cMo[index_0][index_2] * c1);
374  B1 = (cMo[index_1][0] * a1) + (cMo[index_1][1] * b1) + (cMo[index_1][index_2] * c1);
375  C1 = (cMo[index_2][0] * a1) + (cMo[index_2][1] * b1) + (cMo[index_2][index_2] * c1);
376  D1 = d1 - ((cMo[index_0][index_3] * A1) + (cMo[index_1][index_3] * B1) + (cMo[index_2][index_3] * C1));
377 
378  A2 = (cMo[index_0][0] * a2) + (cMo[index_0][1] * b2) + (cMo[index_0][index_2] * c2);
379  B2 = (cMo[index_1][0] * a2) + (cMo[index_1][1] * b2) + (cMo[index_1][index_2] * c2);
380  C2 = (cMo[index_2][0] * a2) + (cMo[index_2][1] * b2) + (cMo[index_2][index_2] * c2);
381  D2 = d2 - ((cMo[index_0][index_3] * A2) + (cMo[index_1][index_3] * B2) + (cMo[index_2][index_3] * C2));
382 
383  // Adding constraints on the straight line to have a unique representation
384 
385  // direction of the straight line = N1 x N2
386  a2 = (B1 * C2) - (C1 * B2);
387  b2 = (C1 * A2) - (A1 * C2);
388  c2 = (A1 * B2) - (B1 * A2);
389 
390  // Constraint D1 = 0 (the origin belongs to P1)
391  a1 = (A2 * D1) - (A1 * D2);
392  b1 = (B2 * D1) - (B1 * D2);
393  c1 = (C2 * D1) - (C1 * D2);
394 
395  if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
396  {
397  A2 = A1;
398  B2 = B1;
399  C2 = C1;
400  D2 = D1;
401  }
402 
403  // Constraint A1^2 + B1^2 + C1^2 = 1
404  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
405  A1 = a1 * d1;
406  B1 = b1 * d1;
407  C1 = c1 * d1;
408  cP_[index_0] = A1;
409  cP_[index_1] = B1;
410  cP_[index_2] = C1;
411 
412  cP_[index_3] = 0;
413 
414  // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
415  // N2_new = (N1 x N2) x N1_new
416  a1 = (b2 * C1) - (c2 * B1);
417  b1 = (c2 * A1) - (a2 * C1);
418  c1 = (a2 * B1) - (b2 * A1);
419 
420  // Constraint A2^2 + B2^2 + C2^2 = 1
421  d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
422  a1 *= d1;
423  b1 *= d1;
424  c1 *= d1;
425 
426  // D2_new = D2 / (N2^T . N2_new)
427  D2 /= ((A2 * a1) + (B2 * b1) + (C2 * c1));
428  A2 = a1;
429  B2 = b1;
430  C2 = c1;
431 
432  // Constraint D2 < 0
433  if (D2 > 0) {
434  A2 = -A2;
435  B2 = -B2;
436  C2 = -C2;
437  D2 = -D2;
438  }
439 
440  cP_[4] = A2;
441  cP_[5] = B2;
442  cP_[6] = C2;
443  cP_[7] = D2;
444 }
445 
462 void 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 
483 void 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 v_cP, v_p;
515  changeFrame(cMo, v_cP);
516  try {
517  projection(v_cP, v_p);
518  vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
519  }
520  catch (...) {
521  // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
522  }
523 }
524 
548  const vpColor &color, unsigned int thickness)
549 {
550  vpColVector v_cP, v_p;
551  changeFrame(cMo, v_cP);
552  try {
553  projection(v_cP, v_p);
554  vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness);
555  }
556  catch (...) {
557  // Skip potential exception: due to a degenerate case: the image of the straight line is a point!
558  }
559 }
560 
572 {
573  vpLine *feature = new vpLine(*this);
574  return feature;
575 }
576 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:200
vpLine * duplicate() const VP_OVERRIDE
Definition: vpLine.cpp:571
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
Definition: vpLine.cpp:345
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:462
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