ViSP  2.9.0
vpLine.cpp
1 /****************************************************************************
2  *
3  * $Id: vpLine.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Line feature.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
43 #include <visp/vpLine.h>
44 
45 #include <visp/vpDebug.h>
46 #include <visp/vpMath.h>
47 
48 #include <visp/vpFeatureDisplay.h>
49 
63 void
65 {
66  oP.resize(8) ;
67  cP.resize(8) ;
68  p.resize(2) ;
69 }
70 
75 {
76  init() ;
77 }
78 
79 
80 
97 void
98 vpLine::setWorldCoordinates(const double &A1, const double &B1,
99  const double &C1, const double &D1,
100  const double &A2, const double &B2,
101  const double &C2, const double &D2)
102 {
103  oP[0] = A1 ;
104  oP[1] = B1 ;
105  oP[2] = C1 ;
106  oP[3] = D1 ;
107 
108  oP[4] = A2 ;
109  oP[5] = B2 ;
110  oP[6] = C2 ;
111  oP[7] = D2 ;
112 }
113 
114 
133 void
135 {
136  if (oP_.getRows() != 8)
137  throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
138 
139  this->oP = oP_ ;
140 }
141 
142 
164 void
166  const vpColVector &oP2)
167 {
168  if (oP1.getRows() != 4)
169  throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
170 
171  if (oP2.getRows() != 4)
172  throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
173 
174  for (unsigned int i=0 ; i < 4 ; i++)
175  {
176  oP[i] = oP1[i] ;
177  oP[i+4] = oP2[i] ;
178  }
179 
180 }
181 
182 
215 void
217 {
218  projection(cP,p) ;
219 }
220 
221 
239 void
241 {
242  //projection
243 
244  if (cP.getRows() != 8)
245  throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
246 
247  double A1, A2, B1, B2, C1, C2, D1, D2;
248 
249  A1=cP_[0] ;
250  B1=cP_[1] ;
251  C1=cP_[2] ;
252  D1=cP_[3] ;
253 
254  A2=cP_[4] ;
255  B2=cP_[5] ;
256  C2=cP_[6] ;
257  D2=cP_[7] ;
258 
259  double a, b, c, s;
260  a = A2*D1 - A1*D2;
261  b = B2*D1 - B1*D2;
262  c = C2*D1 - C1*D2;
263  s = a*a+b*b;
264  if (s <= 1e-8) // seuil pas terrible
265  {
266  printf("Degenerate case: the image of the straight line is a point!\n");
267  throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
268  }
269  s = 1.0/sqrt(s);
270 
271  double rho = -c*s ;
272  double theta = atan2( b, a);
273 
274  if (p.getRows() != 2)
275  p.resize(2);
276 
277  p_[0] = rho ;
278  p_[1] = theta ;
279 }
280 
281 
317 void
319 {
320  changeFrame(cMo,cP) ;
321 }
322 
323 
365 void
367 {
368 
369  double a1, a2, b1, b2, c1, c2, d1, d2;
370  double A1, A2, B1, B2, C1, C2, D1, D2;
371 
372  // in case of verification
373  // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2;
374 
375  if (cP.getRows() != 8)
376  cP.resize(8);
377 
378  a1=oP[0] ;
379  b1=oP[1] ;
380  c1=oP[2] ;
381  d1=oP[3] ;
382 
383  a2=oP[4] ;
384  b2=oP[5] ;
385  c2=oP[6] ;
386  d2=oP[7] ;
387 
388  A1 = cMo[0][0]*a1 + cMo[0][1]*b1 + cMo[0][2]*c1;
389  B1 = cMo[1][0]*a1 + cMo[1][1]*b1 + cMo[1][2]*c1;
390  C1 = cMo[2][0]*a1 + cMo[2][1]*b1 + cMo[2][2]*c1;
391  D1 = d1 - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
392 
393  A2 = cMo[0][0]*a2 + cMo[0][1]*b2 + cMo[0][2]*c2;
394  B2 = cMo[1][0]*a2 + cMo[1][1]*b2 + cMo[1][2]*c2;
395  C2 = cMo[2][0]*a2 + cMo[2][1]*b2 + cMo[2][2]*c2;
396  D2 = d2 - (cMo[0][3]*A2 + cMo[1][3]*B2 + cMo[2][3]*C2);
397 
398  // in case of verification
399  // ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1;
400  // ap2 = A2; bp2 = B2; cp2 = C2; dp2 = D2;
401 
402  // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
403  // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
404 
405  // Adding constraints on the straight line to have a unique representation
406 
407  // direction of the straight line = N1 x N2
408  a2 = B1*C2 - C1*B2;
409  b2 = C1*A2 - A1*C2;
410  c2 = A1*B2 - B1*A2;
411 
412  // Constraint D1 = 0 (the origin belongs to P1)
413  a1 = A2*D1 - A1*D2;
414  b1 = B2*D1 - B1*D2;
415  c1 = C2*D1 - C1*D2;
416 
417  if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
418  {
419  A2 = A1;
420  B2 = B1;
421  C2 = C1;
422  D2 = D1;
423  }
424 
425  // Constraint A1^2 + B1^2 + C1^2 = 1
426  d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
427  cP_[0] = A1 = a1*d1 ;
428  cP_[1] = B1 = b1*d1 ;
429  cP_[2] = C1 = c1*d1 ;
430  cP_[3] = D1 = 0 ;
431 
432  // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
433  // N2_new = (N1 x N2) x N1_new
434  a1 = b2*C1 - c2*B1;
435  b1 = c2*A1 - a2*C1;
436  c1 = a2*B1 - b2*A1;
437 
438  // Constraint A2^2 + B2^2 + C2^2 = 1
439  d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
440  a1 *= d1 ;
441  b1 *= d1 ;
442  c1 *= d1 ;
443 
444  // D2_new = D2 / (N2^T . N2_new)
445  D2 /= (A2*a1 + B2*b1 + C2*c1);
446  A2 = a1;
447  B2 = b1;
448  C2 = c1;
449 
450  // Constraint D2 < 0
451  if (D2 > 0)
452  {
453  A2 = -A2;
454  B2 = -B2;
455  C2 = -C2;
456  D2 = -D2;
457  }
458  // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
459  // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
460 
461  cP_[4] = A2;
462  cP_[5] = B2;
463  cP_[6] = C2;
464  cP_[7] = D2;
465 
466  // in case of verification
467  /*
468  x = -A2*D2;
469  y = -B2*D2;
470  z = -C2*D2;
471  d1 = ap1*x+bp1*y+cp1*z+dp1;
472  d2 = ap2*x+bp2*y+cp2*z+dp2;
473  if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
474  {
475  printf("PB in VPline: P1 : 0 = %lf, P2: 0 = %lf\n",d1,d2);
476  exit(-1);
477  }
478  d1 = A1*x+B1*y+C1*z+D1;
479  d2 = A2*x+B2*y+C2*z+D2;
480  if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
481  {
482  printf("PB in VPline: Pn1 : 0 = %lf, Pn2: 0 = %lf\n",d1,d2);
483  exit(-1);
484  }
485  */
486 
487 }
488 
489 
490 
508  const vpCameraParameters &cam,
509  const vpColor &color,
510  const unsigned int thickness)
511 {
512  vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness) ;
513 }
514 
515 
536 // non destructive wrt. cP and p
538  const vpHomogeneousMatrix &cMo,
539  const vpCameraParameters &cam,
540  const vpColor &color,
541  const unsigned int thickness)
542 {
543  vpColVector _cP, _p ;
544  changeFrame(cMo,_cP) ;
545  projection(_cP,_p) ;
546  vpFeatureDisplay::displayLine(_p[0],_p[1],
547  cam, I, color, thickness) ;
548 
549 }
550 
551 
563 {
564  vpLine *feature = new vpLine(*this) ;
565  return feature ;
566 }
567 
568 /*
569  * Local variables:
570  * c-basic-offset: 2
571  * End:
572  */
void init()
Definition: vpLine.cpp:64
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2, const double &B2, const double &C2, const double &D2)
Definition: vpLine.cpp:98
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
error that can be emited by ViSP classes.
Definition: vpException.h:76
vpColVector cP
Definition: vpTracker.h:82
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:124
Generic class defining intrinsic camera parameters.
vpLine()
Definition: vpLine.cpp:74
vpLine * duplicate() const
Definition: vpLine.cpp:562
void projection()
Definition: vpLine.cpp:216
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpLine.cpp:366
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.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)
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpLine.cpp:507
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
vpColVector p
Definition: vpTracker.h:78
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94