Visual Servoing Platform  version 3.2.1 under development (2019-08-21) under development (2019-08-21)
vpCylinder.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Cylinder feature.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpCylinder.h>
40 #include <visp3/core/vpFeatureDisplay.h>
41 
43 {
44 
45  oP.resize(7);
46  cP.resize(7);
47 
48  p.resize(4);
49 }
50 
71 void vpCylinder::setWorldCoordinates(const vpColVector &o_P) { this->oP = o_P; }
72 
81 void vpCylinder::setWorldCoordinates(const double A, const double B, const double C, const double X0, const double Y0,
82  const double Z0, const double R)
83 {
84  oP[0] = A;
85  oP[1] = B;
86  oP[2] = C;
87  oP[3] = X0;
88  oP[4] = Y0;
89  oP[5] = Z0;
90  oP[6] = R;
91 }
92 
97 
120 {
121  init();
122  setWorldCoordinates(o_P);
123 }
124 
135 vpCylinder::vpCylinder(const double A, const double B, const double C, const double X0, const double Y0,
136  const double Z0, const double R)
137 {
138  init();
139  setWorldCoordinates(A, B, C, X0, Y0, Z0, R);
140 }
141 
146 
168 
198 {
199  // calcul de la scene 2-D
200 
201  double co, si, e, x0, y0, z0;
202  double A, B, C, X0, Y0, Z0, R;
203  double s, a, b, c, zero;
204 
205  A = cP_[0];
206  B = cP_[1];
207  C = cP_[2];
208  X0 = cP_[3];
209  Y0 = cP_[4];
210  Z0 = cP_[5];
211  R = cP_[6];
212  zero = A * X0 + B * Y0 + C * Z0; // should be zero for a good reprensetation of the cylinder
213 
214  s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
215  if (s < 0) {
216  printf("The camera is inside the cylinder with s=%f !\n", s);
217  throw vpException(vpException::fatalError, "The camera is inside the cylinder!");
218  }
219  s = 1.0 / sqrt(s);
220  a = X0 - A * zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
221  b = Y0 - B * zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
222  c = Z0 - C * zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
223  x0 = C * Y0 - B * Z0;
224  y0 = A * Z0 - C * X0;
225  z0 = B * X0 - A * Y0;
226 
227  // rho1 / theta1
228  co = R * a * s - x0;
229  si = R * b * s - y0;
230  e = sqrt(co * co + si * si);
231  p_[0] = -(R * c * s - z0) / e; // rho1
232  p_[1] = atan2(si, co); // theta 1
233 
234  // while (p[1] > M_PI/2) { p[1] -= M_PI ; p[0] *= -1 ; }
235  // while (p[1] < -M_PI/2) { p[1] += M_PI ; p[0] *= -1 ; }
236 
237  // rho2 / theta2
238  co = R * a * s + x0;
239  si = R * b * s + y0;
240  e = sqrt(co * co + si * si);
241  p_[2] = -(R * c * s + z0) / e; // rho2
242  p_[3] = atan2(si, co); // theta2
243 
244  // while (p[3] > M_PI/2) { p[3] -= M_PI ; p[2] *= -1 ; }
245  // while (p[3] < -M_PI/2) { p[3] += M_PI ; p[2] *= -1 ; }
246 
247  // std::cout << p.t() << std::endl ;
248 }
249 
260 
273 {
274  double X1, Y1, Z1;
275  double X2, Y2, Z2;
276  double s, a, b, c;
277 
278  double oA, oB, oC, oX0, oY0, oZ0;
279  oA = oP[0];
280  oB = oP[1];
281  oC = oP[2];
282  oX0 = oP[3];
283  oY0 = oP[4];
284  oZ0 = oP[5];
285 
286  X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
287  Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
288  Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
289  s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
290  a = X1 / s;
291  b = Y1 / s;
292  c = Z1 / s;
293 
294  // set axis coordinates in camera frame
295  cP_[0] = a;
296  cP_[1] = b;
297  cP_[2] = c;
298 
299  X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
300  Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
301  Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
302 
303  // adding the constraint X0 is the nearest point to the origin (A^T . X0 =
304  // 0) using the projection operator (I - AA^T) orthogonal to A
305  cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
306  cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
307  cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
308 
309  /* old version for the same onstraint
310  if ( fabs(a) > 0.25 )
311  {
312  double xx, yy, zz, xx1, yy1;
313 
314  xx1 = a*Y2 - b*X2;
315  yy1 = a*Z2 - c*X2;
316  xx = -( b*xx1 + c*yy1);
317  yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
318  zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
319 
320  // set point coordinates in camera frame
321  _cP[3] = xx ;
322  _cP[4] = yy ;
323  _cP[5] = zz ;
324  }
325  else if ( fabs(b) >0.25 )
326  {
327  double xx, yy, zz, xx1, yy1;
328 
329  xx1 = a*Y2 - b*X2;
330  yy1 = c*Y2 - b*Z2;
331  xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
332  yy = a*xx1 + c*yy1;
333  zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
334 
335 
336  // set point coordinates in camera frame
337  _cP[3] = xx ;
338  _cP[4] = yy ;
339  _cP[5] = zz ;
340  }
341  else
342  {
343  double xx, yy, zz, xx1, yy1;
344 
345  xx1 = a*Z2 - c*X2;
346  yy1 = b*Z2 - c*Y2;
347  xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
348  yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
349  zz = a*xx1 + b*yy1;
350 
351  // set point coordinates in camera frame
352  _cP[3] = xx ;
353  _cP[4] = yy ;
354  _cP[5] = zz ;
355  }
356  */
357  // radius
358  cP_[6] = oP[6];
359 }
360 
365 double vpCylinder::computeZ(const double x, const double y) const
366 {
367  double A = x * x + y * y + 1 - ((getA() * x + getB() * y + getC()) * (getA() * x + getB() * y + getC()));
368  double B = (x * getX() + y * getY() + getZ());
369  double C = getX() * getX() + getY() * getY() + getZ() * getZ() - getR() * getR();
370 
371  return (B - std::sqrt(B * B - A * C)) / A;
372 }
373 
376 {
377  vpCylinder *feature = new vpCylinder(*this);
378  return feature;
379 }
380 
385  const vpColor &color, const unsigned int thickness)
386 {
387 
388  vpColVector _cP(7), _p(4);
389  changeFrame(cMo, _cP);
390  projection(_cP, _p);
391  vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
392 }
393 
398  const unsigned int thickness)
399 {
400  vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
401 }
void init()
Definition: vpCylinder.cpp:42
virtual ~vpCylinder()
Definition: vpCylinder.cpp:145
double getR() const
Definition: vpCylinder.h:175
double getA() const
Definition: vpCylinder.h:151
Implementation of an homogeneous matrix and operations on such kind of matrices.
double computeZ(const double x, const double y) const
Definition: vpCylinder.cpp:365
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)
double getB() const
Definition: vpCylinder.h:155
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
double getC() const
Definition: vpCylinder.h:159
error that can be emited by ViSP classes.
Definition: vpException.h:71
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
Definition: vpCylinder.cpp:272
vpColVector cP
Definition: vpTracker.h:75
Generic class defining intrinsic camera parameters.
double getY() const
Definition: vpCylinder.h:167
void projection()
Definition: vpCylinder.cpp:167
Class that defines what is a cylinder.
Definition: vpCylinder.h:96
double getZ() const
Definition: vpCylinder.h:171
vpCylinder * duplicate() const
for memory issue (used by the vpServo class only)
Definition: vpCylinder.cpp:375
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpCylinder.cpp:397
double getX() const
Definition: vpCylinder.h:163
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:71
vpColVector p
Definition: vpTracker.h:71
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310