Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpCylinder.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Cylinder feature.
32  *
33  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
39 #include <visp3/core/vpCylinder.h>
40 #include <visp3/core/vpFeatureDisplay.h>
41 
42 
43 void
45 {
46 
47  oP.resize(7) ;
48  cP.resize(7) ;
49 
50  p.resize(4) ;
51 }
52 
72 void
74 {
75  this->oP = o_P ;
76 }
77 
85 void
86 vpCylinder::setWorldCoordinates(const double A, const double B,
87  const double C,
88  const double X0, const double Y0,
89  const double Z0,
90  const double R)
91 {
92  oP[0] = A ;
93  oP[1] = B ;
94  oP[2] = C ;
95  oP[3] = X0 ;
96  oP[4] = Y0 ;
97  oP[5] = Z0 ;
98  oP[6] = R ;
99 }
100 
101 
106 {
107  init() ;
108 }
109 
131 {
132  init() ;
133  setWorldCoordinates(o_P) ;
134 }
135 
144 vpCylinder::vpCylinder(const double A, const double B,
145  const double C,
146  const double X0, const double Y0,
147  const double Z0,
148  const double R)
149 {
150  init() ;
151  setWorldCoordinates(A, B, C,
152  X0, Y0, Z0,
153  R) ;
154 }
155 
160 {
161 }
162 
163 
184 void
186 {
187  projection(cP,p) ;
188 }
189 
190 
218 void
220 {
221  //calcul de la scene 2-D
222 
223  double co, si, e, x0, y0, z0;
224  double A,B,C, X0, Y0, Z0, R ;
225  double s, a, b, c, zero;
226 
227  A = cP_[0] ;
228  B = cP_[1] ;
229  C = cP_[2] ;
230  X0 = cP_[3] ;
231  Y0 = cP_[4] ;
232  Z0 = cP_[5] ;
233  R= cP_[6] ;
234  zero = A*X0 + B*Y0 + C*Z0; // should be zero for a good reprensetation of the cylinder
235 
236  s = X0*X0 + Y0*Y0 + Z0*Z0 - R*R - zero*zero;
237  if (s < 0)
238  {
239  printf("The camera is inside the cylinder with s=%f !\n", s);
240  throw vpException(vpException::fatalError, "The camera is inside the cylinder!");
241  }
242  s = 1.0/sqrt(s);
243  a = X0 - A*zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
244  b = Y0 - B*zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
245  c = Z0 - C*zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
246  x0 = C*Y0 - B*Z0;
247  y0 = A*Z0 - C*X0;
248  z0 = B*X0 - A*Y0;
249 
250  // rho1 / theta1
251  co = R*a*s-x0;
252  si = R*b*s-y0;
253  e = sqrt(co*co + si*si);
254  p_[0] = -(R*c*s-z0)/e ; // rho1
255  p_[1] = atan2(si,co) ; // theta 1
256 
257  // while (p[1] > M_PI/2) { p[1] -= M_PI ; p[0] *= -1 ; }
258  // while (p[1] < -M_PI/2) { p[1] += M_PI ; p[0] *= -1 ; }
259 
260  // rho2 / theta2
261  co = R*a*s+x0;
262  si = R*b*s+y0;
263  e = sqrt(co*co + si*si);
264  p_[2] = -( R*c*s+z0 )/e ; //rho2
265  p_[3] = atan2( si,co ) ; //theta2
266 
267 
268  // while (p[3] > M_PI/2) { p[3] -= M_PI ; p[2] *= -1 ; }
269  // while (p[3] < -M_PI/2) { p[3] += M_PI ; p[2] *= -1 ; }
270 
271  // std::cout << p.t() << std::endl ;
272 }
273 
282 void
284 {
285  changeFrame(cMo,cP) ;
286 }
287 
297 void
299 {
300  double X1, Y1, Z1;
301  double X2, Y2, Z2;
302  double s, a, b, c;
303 
304  double oA,oB,oC, oX0, oY0, oZ0 ;
305  oA = oP[0] ;
306  oB = oP[1] ;
307  oC = oP[2] ;
308  oX0 = oP[3] ;
309  oY0 = oP[4] ;
310  oZ0 = oP[5] ;
311 
312  X1 = cMo[0][0]*oA + cMo[0][1]*oB + cMo[0][2]*oC ;
313  Y1 = cMo[1][0]*oA + cMo[1][1]*oB + cMo[1][2]*oC ;
314  Z1 = cMo[2][0]*oA + cMo[2][1]*oB + cMo[2][2]*oC ;
315  s = sqrt ( X1*X1 + Y1*Y1 + Z1*Z1 );
316  a = X1 / s;
317  b = Y1 / s;
318  c = Z1 / s;
319 
320  // set axis coordinates in camera frame
321  cP_[0] = a ;
322  cP_[1] = b ;
323  cP_[2] = c ;
324 
325  X2 = cMo[0][3] + cMo[0][0]*oX0 + cMo[0][1]*oY0 + cMo[0][2]*oZ0;
326  Y2 = cMo[1][3] + cMo[1][0]*oX0 + cMo[1][1]*oY0 + cMo[1][2]*oZ0;
327  Z2 = cMo[2][3] + cMo[2][0]*oX0 + cMo[2][1]*oY0 + cMo[2][2]*oZ0;
328 
329  // adding the constraint X0 is the nearest point to the origin (A^T . X0 = 0)
330  // using the projection operator (I - AA^T) orthogonal to A
331  cP_[3] = (1-a*a)*X2 - a*b*Y2 - a*c*Z2;
332  cP_[4] = -a*b*X2 + (1-b*b)*Y2 - b*c*Z2;
333  cP_[5] = -a*c*X2 - b*c*Y2 + (1-c*c)*Z2;
334 
335  /* old version for the same onstraint
336  if ( fabs(a) > 0.25 )
337  {
338  double xx, yy, zz, xx1, yy1;
339 
340  xx1 = a*Y2 - b*X2;
341  yy1 = a*Z2 - c*X2;
342  xx = -( b*xx1 + c*yy1);
343  yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
344  zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
345 
346  // set point coordinates in camera frame
347  _cP[3] = xx ;
348  _cP[4] = yy ;
349  _cP[5] = zz ;
350  }
351  else if ( fabs(b) >0.25 )
352  {
353  double xx, yy, zz, xx1, yy1;
354 
355  xx1 = a*Y2 - b*X2;
356  yy1 = c*Y2 - b*Z2;
357  xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
358  yy = a*xx1 + c*yy1;
359  zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
360 
361 
362  // set point coordinates in camera frame
363  _cP[3] = xx ;
364  _cP[4] = yy ;
365  _cP[5] = zz ;
366  }
367  else
368  {
369  double xx, yy, zz, xx1, yy1;
370 
371  xx1 = a*Z2 - c*X2;
372  yy1 = b*Z2 - c*Y2;
373  xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
374  yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
375  zz = a*xx1 + b*yy1;
376 
377  // set point coordinates in camera frame
378  _cP[3] = xx ;
379  _cP[4] = yy ;
380  _cP[5] = zz ;
381  }
382  */
383  //radius
384  cP_[6] = oP[6] ;
385 
386 }
387 
391 double vpCylinder::computeZ(const double x, const double y) const {
392  double A = x * x + y * y + 1 - ((getA() * x + getB() * y + getC())
393  * (getA() * x + getB() * y + getC()));
394  double B = (x * getX() + y * getY() + getZ());
395  double C = getX() * getX() + getY() * getY() + getZ() * getZ()
396  - getR() * getR();
397 
398  return (B - std::sqrt(B * B - A * C)) / A;
399 }
400 
403 {
404  vpCylinder *feature = new vpCylinder(*this) ;
405  return feature ;
406 }
407 
411 void
413  const vpHomogeneousMatrix &cMo,
414  const vpCameraParameters &cam,
415  const vpColor &color,
416  const unsigned int thickness)
417 {
418 
419  vpColVector _cP(7), _p(4) ;
420  changeFrame(cMo,_cP) ;
421  projection(_cP,_p) ;
422  vpFeatureDisplay::displayCylinder(_p[0],_p[1], _p[2], _p[3],
423  cam, I, color, thickness) ;
424 
425 }
426 
430 void
432  const vpCameraParameters &cam,
433  const vpColor &color,
434  const unsigned int thickness)
435 {
436  vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3],
437  cam, I, color, thickness) ;
438 }
void init()
Definition: vpCylinder.cpp:44
vpCylinder * duplicate() const
for memory issue (used by the vpServo class only)
Definition: vpCylinder.cpp:402
virtual ~vpCylinder()
Definition: vpCylinder.cpp:159
double getY() const
Definition: vpCylinder.h:173
Implementation of an homogeneous matrix and operations on such kind of matrices.
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:121
double getZ() const
Definition: vpCylinder.h:177
error that can be emited by ViSP classes.
Definition: vpException.h:73
double computeZ(const double x, const double y) const
Definition: vpCylinder.cpp:391
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:298
vpColVector cP
Definition: vpTracker.h:77
double getC() const
Definition: vpCylinder.h:165
double getB() const
Definition: vpCylinder.h:161
Generic class defining intrinsic camera parameters.
void projection()
Definition: vpCylinder.cpp:185
Class that defines what is a cylinder.
Definition: vpCylinder.h:93
double getA() const
Definition: vpCylinder.h:157
Implementation of column vector and the associated operations.
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:431
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:73
vpColVector p
Definition: vpTracker.h:73
double getX() const
Definition: vpCylinder.h:169
double getR() const
Definition: vpCylinder.h:181
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225