ViSP  2.9.0
vpCylinder.cpp
1 /****************************************************************************
2  *
3  * $Id: vpCylinder.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  * Cylinder feature.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
43 #include <visp/vpCylinder.h>
44 #include <visp/vpFeatureDisplay.h>
45 
46 
47 void
49 {
50 
51  oP.resize(7) ;
52  cP.resize(7) ;
53 
54  p.resize(4) ;
55 }
56 
76 void
78 {
79  this->oP = o_P ;
80 }
81 
89 void
90 vpCylinder::setWorldCoordinates(const double A, const double B,
91  const double C,
92  const double X0, const double Y0,
93  const double Z0,
94  const double R)
95 {
96  oP[0] = A ;
97  oP[1] = B ;
98  oP[2] = C ;
99  oP[3] = X0 ;
100  oP[4] = Y0 ;
101  oP[5] = Z0 ;
102  oP[6] = R ;
103 }
104 
105 
110 {
111  init() ;
112 }
113 
135 {
136  init() ;
137  setWorldCoordinates(o_P) ;
138 }
139 
148 vpCylinder::vpCylinder(const double A, const double B,
149  const double C,
150  const double X0, const double Y0,
151  const double Z0,
152  const double R)
153 {
154  init() ;
155  setWorldCoordinates(A, B, C,
156  X0, Y0, Z0,
157  R) ;
158 }
159 
164 {
165 }
166 
167 
188 void
190 {
191  projection(cP,p) ;
192 }
193 
194 
222 void
224 {
225  //calcul de la scene 2-D
226 
227  double co, si, e, x0, y0, z0;
228  double A,B,C, X0, Y0, Z0, R ;
229  double s, a, b, c, zero;
230 
231  A = cP_[0] ;
232  B = cP_[1] ;
233  C = cP_[2] ;
234  X0 = cP_[3] ;
235  Y0 = cP_[4] ;
236  Z0 = cP_[5] ;
237  R= cP_[6] ;
238  zero = A*X0 + B*Y0 + C*Z0; // should be zero for a good reprensetation of the cylinder
239 
240  s = X0*X0 + Y0*Y0 + Z0*Z0 - R*R - zero*zero;
241  if (s < 0)
242  {
243  printf("The camera is inside the cylinder!\n");
244  throw vpException(vpException::fatalError, "The camera is inside the cylinder!");
245  }
246  s = 1.0/sqrt(s);
247  a = X0 - A*zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
248  b = Y0 - B*zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
249  c = Z0 - C*zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
250  x0 = C*Y0 - B*Z0;
251  y0 = A*Z0 - C*X0;
252  z0 = B*X0 - A*Y0;
253 
254  // rho1 / theta1
255  co = R*a*s-x0;
256  si = R*b*s-y0;
257  e = sqrt(co*co + si*si);
258  p_[0] = -(R*c*s-z0)/e ; // rho1
259  p_[1] = atan2(si,co) ; // theta 1
260 
261  // while (p[1] > M_PI/2) { p[1] -= M_PI ; p[0] *= -1 ; }
262  // while (p[1] < -M_PI/2) { p[1] += M_PI ; p[0] *= -1 ; }
263 
264  // rho2 / theta2
265  co = R*a*s+x0;
266  si = R*b*s+y0;
267  e = sqrt(co*co + si*si);
268  p_[2] = -( R*c*s+z0 )/e ; //rho2
269  p_[3] = atan2( si,co ) ; //theta2
270 
271 
272  // while (p[3] > M_PI/2) { p[3] -= M_PI ; p[2] *= -1 ; }
273  // while (p[3] < -M_PI/2) { p[3] += M_PI ; p[2] *= -1 ; }
274 
275  // std::cout << p.t() << std::endl ;
276 }
277 
286 void
288 {
289  changeFrame(cMo,cP) ;
290 }
291 
301 void
303 {
304  double X1, Y1, Z1;
305  double X2, Y2, Z2;
306  double s, a, b, c;
307 
308  double oA,oB,oC, oX0, oY0, oZ0 ;
309  oA = oP[0] ;
310  oB = oP[1] ;
311  oC = oP[2] ;
312  oX0 = oP[3] ;
313  oY0 = oP[4] ;
314  oZ0 = oP[5] ;
315 
316  X1 = cMo[0][0]*oA + cMo[0][1]*oB + cMo[0][2]*oC ;
317  Y1 = cMo[1][0]*oA + cMo[1][1]*oB + cMo[1][2]*oC ;
318  Z1 = cMo[2][0]*oA + cMo[2][1]*oB + cMo[2][2]*oC ;
319  s = sqrt ( X1*X1 + Y1*Y1 + Z1*Z1 );
320  a = X1 / s;
321  b = Y1 / s;
322  c = Z1 / s;
323 
324  // set axis coordinates in camera frame
325  cP_[0] = a ;
326  cP_[1] = b ;
327  cP_[2] = c ;
328 
329  X2 = cMo[0][3] + cMo[0][0]*oX0 + cMo[0][1]*oY0 + cMo[0][2]*oZ0;
330  Y2 = cMo[1][3] + cMo[1][0]*oX0 + cMo[1][1]*oY0 + cMo[1][2]*oZ0;
331  Z2 = cMo[2][3] + cMo[2][0]*oX0 + cMo[2][1]*oY0 + cMo[2][2]*oZ0;
332 
333  // adding the constraint X0 is the nearest point to the origin (A^T . X0 = 0)
334  // using the projection operator (I - AA^T) orthogonal to A
335  cP_[3] = (1-a*a)*X2 - a*b*Y2 - a*c*Z2;
336  cP_[4] = -a*b*X2 + (1-b*b)*Y2 - b*c*Z2;
337  cP_[5] = -a*c*X2 - b*c*Y2 + (1-c*c)*Z2;
338 
339  /* old version for the same onstraint
340  if ( fabs(a) > 0.25 )
341  {
342  double xx, yy, zz, xx1, yy1;
343 
344  xx1 = a*Y2 - b*X2;
345  yy1 = a*Z2 - c*X2;
346  xx = -( b*xx1 + c*yy1);
347  yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
348  zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
349 
350  // set point coordinates in camera frame
351  _cP[3] = xx ;
352  _cP[4] = yy ;
353  _cP[5] = zz ;
354  }
355  else if ( fabs(b) >0.25 )
356  {
357  double xx, yy, zz, xx1, yy1;
358 
359  xx1 = a*Y2 - b*X2;
360  yy1 = c*Y2 - b*Z2;
361  xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
362  yy = a*xx1 + c*yy1;
363  zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
364 
365 
366  // set point coordinates in camera frame
367  _cP[3] = xx ;
368  _cP[4] = yy ;
369  _cP[5] = zz ;
370  }
371  else
372  {
373  double xx, yy, zz, xx1, yy1;
374 
375  xx1 = a*Z2 - c*X2;
376  yy1 = b*Z2 - c*Y2;
377  xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
378  yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
379  zz = a*xx1 + b*yy1;
380 
381  // set point coordinates in camera frame
382  _cP[3] = xx ;
383  _cP[4] = yy ;
384  _cP[5] = zz ;
385  }
386  */
387  //radius
388  cP_[6] = oP[6] ;
389 
390 }
391 
394 {
395  vpCylinder *feature = new vpCylinder(*this) ;
396  return feature ;
397 }
398 
402 void
404  const vpHomogeneousMatrix &cMo,
405  const vpCameraParameters &cam,
406  const vpColor &color,
407  const unsigned int thickness)
408 {
409 
410  vpColVector _cP(7), _p(4) ;
411  changeFrame(cMo,_cP) ;
412  projection(_cP,_p) ;
413  vpFeatureDisplay::displayCylinder(_p[0],_p[1], _p[2], _p[3],
414  cam, I, color, thickness) ;
415 
416 }
417 
421 void
423  const vpCameraParameters &cam,
424  const vpColor &color,
425  const unsigned int thickness)
426 {
427  vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3],
428  cam, I, color, thickness) ;
429 }
void init()
Definition: vpCylinder.cpp:48
vpCylinder * duplicate() const
for memory issue (used by the vpServo class only)
Definition: vpCylinder.cpp:393
virtual ~vpCylinder()
Definition: vpCylinder.cpp:163
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
error that can be emited by ViSP classes.
Definition: vpException.h:76
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:302
vpColVector cP
Definition: vpTracker.h:82
Generic class defining intrinsic camera parameters.
void projection()
Definition: vpCylinder.cpp:189
Class that defines what is a cylinder.
Definition: vpCylinder.h:97
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpCylinder.cpp:422
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:77
vpColVector p
Definition: vpTracker.h:78
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94