Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
vpCylinder.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpCylinder.h>
37 #include <visp3/core/vpFeatureDisplay.h>
38 
40 {
41  oP.resize(7);
42  cP.resize(7);
43 
44  p.resize(4);
45 }
46 
62 void vpCylinder::setWorldCoordinates(const vpColVector &o_P) { this->oP = o_P; }
63 
71 void vpCylinder::setWorldCoordinates(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
72 {
73  oP[0] = oA;
74  oP[1] = oB;
75  oP[2] = oC;
76  oP[3] = oX;
77  oP[4] = oY;
78  oP[5] = oZ;
79  oP[6] = R;
80 }
81 
86 
103 {
104  init();
105  setWorldCoordinates(o_P);
106 }
107 
117 vpCylinder::vpCylinder(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
118 {
119  init();
120  setWorldCoordinates(oA, oB, oC, oX, oY, oZ, R);
121 }
122 
147 
176 void vpCylinder::projection(const vpColVector &cP_, vpColVector &p_) const
177 {
178  p_.resize(4, false);
179 
180  double co, si, e, x0, y0, z0;
181  double A, B, C, X0, Y0, Z0, R;
182  double s, a, b, c, zero;
183 
184  A = cP_[0];
185  B = cP_[1];
186  C = cP_[2];
187  X0 = cP_[3];
188  Y0 = cP_[4];
189  Z0 = cP_[5];
190  R = cP_[6];
191  zero = A * X0 + B * Y0 + C * Z0; // should be zero for a good reprensetation of the cylinder
192 
193  s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
194  if (s < 0) {
195  throw vpException(vpException::fatalError, "The camera is inside the cylinder with s=%f!", s);
196  }
197  s = 1.0 / sqrt(s);
198  a = X0 - A * zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
199  b = Y0 - B * zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
200  c = Z0 - C * zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
201  x0 = C * Y0 - B * Z0;
202  y0 = A * Z0 - C * X0;
203  z0 = B * X0 - A * Y0;
204 
205  // rho1 / theta1
206  co = R * a * s - x0;
207  si = R * b * s - y0;
208  e = sqrt(co * co + si * si);
209  p_[0] = -(R * c * s - z0) / e; // rho1
210  p_[1] = atan2(si, co); // theta 1
211 
212  // rho2 / theta2
213  co = R * a * s + x0;
214  si = R * b * s + y0;
215  e = sqrt(co * co + si * si);
216  p_[2] = -(R * c * s + z0) / e; // rho2
217  p_[3] = atan2(si, co); // theta2
218 }
219 
230 
242 {
243  cP_.resize(7, false);
244 
245  double X1, Y1, Z1;
246  double X2, Y2, Z2;
247  double s, a, b, c;
248 
249  double oA, oB, oC, oX0, oY0, oZ0;
250  oA = oP[0];
251  oB = oP[1];
252  oC = oP[2];
253  oX0 = oP[3];
254  oY0 = oP[4];
255  oZ0 = oP[5];
256 
257  X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
258  Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
259  Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
260  s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
261  a = X1 / s;
262  b = Y1 / s;
263  c = Z1 / s;
264 
265  // set axis coordinates in camera frame
266  cP_[0] = a;
267  cP_[1] = b;
268  cP_[2] = c;
269 
270  X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
271  Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
272  Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
273 
274  // adding the constraint X0 is the nearest point to the origin (A^T . X0 =
275  // 0) using the projection operator (I - AA^T) orthogonal to A
276  cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
277  cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
278  cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
279 
280  /* old version for the same onstraint
281  if ( fabs(a) > 0.25 )
282  {
283  double xx, yy, zz, xx1, yy1;
284 
285  xx1 = a*Y2 - b*X2;
286  yy1 = a*Z2 - c*X2;
287  xx = -( b*xx1 + c*yy1);
288  yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
289  zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
290 
291  // set point coordinates in camera frame
292  _cP[3] = xx ;
293  _cP[4] = yy ;
294  _cP[5] = zz ;
295  }
296  else if ( fabs(b) >0.25 )
297  {
298  double xx, yy, zz, xx1, yy1;
299 
300  xx1 = a*Y2 - b*X2;
301  yy1 = c*Y2 - b*Z2;
302  xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
303  yy = a*xx1 + c*yy1;
304  zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
305 
306 
307  // set point coordinates in camera frame
308  _cP[3] = xx ;
309  _cP[4] = yy ;
310  _cP[5] = zz ;
311  }
312  else
313  {
314  double xx, yy, zz, xx1, yy1;
315 
316  xx1 = a*Z2 - c*X2;
317  yy1 = b*Z2 - c*Y2;
318  xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
319  yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
320  zz = a*xx1 + b*yy1;
321 
322  // set point coordinates in camera frame
323  _cP[3] = xx ;
324  _cP[4] = yy ;
325  _cP[5] = zz ;
326  }
327  */
328  // radius
329  cP_[6] = oP[6];
330 }
331 
336 double vpCylinder::computeZ(double x, double y) const
337 {
338  double A = x * x + y * y + 1 - ((getA() * x + getB() * y + getC()) * (getA() * x + getB() * y + getC()));
339  double B = (x * getX() + y * getY() + getZ());
340  double C = getX() * getX() + getY() * getY() + getZ() * getZ() - getR() * getR();
341 
342  return (B - std::sqrt(B * B - A * C)) / A;
343 }
344 
347 {
348  vpCylinder *feature = new vpCylinder(*this);
349  return feature;
350 }
351 
365  const vpColor &color, unsigned int thickness)
366 {
367 
368  vpColVector _cP(7), _p(4);
369  changeFrame(cMo, _cP);
370  projection(_cP, _p);
371  vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
372 }
373 
387  const vpColor &color, unsigned int thickness)
388 {
389 
390  vpColVector _cP(7), _p(4);
391  changeFrame(cMo, _cP);
392  projection(_cP, _p);
393  vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
394 }
395 
405  unsigned int thickness)
406 {
407  vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
408 }
409 
418 void vpCylinder::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
419  unsigned int thickness)
420 {
421  vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
422 }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:99
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) vp_override
Definition: vpCylinder.cpp:404
double getZ() const
Definition: vpCylinder.h:181
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpCylinder.cpp:62
double getB() const
Definition: vpCylinder.h:161
double getX() const
Definition: vpCylinder.h:171
double getY() const
Definition: vpCylinder.h:176
double getA() const
Definition: vpCylinder.h:156
void init() vp_override
Definition: vpCylinder.cpp:39
double getR() const
Definition: vpCylinder.h:186
vpCylinder * duplicate() const vp_override
For memory issue (used by the vpServo class only).
Definition: vpCylinder.cpp:346
double getC() const
Definition: vpCylinder.h:166
double computeZ(double x, double y) const
Definition: vpCylinder.cpp:336
void projection() vp_override
Definition: vpCylinder.cpp:146
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const vp_override
Definition: vpCylinder.cpp:241
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:71
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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpColVector cP
Definition: vpTracker.h:71
vpColVector p
Definition: vpTracker.h:67