Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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  oP.resize(7);
45  cP.resize(7);
46 
47  p.resize(4);
48 }
49 
65 void vpCylinder::setWorldCoordinates(const vpColVector &o_P) { this->oP = o_P; }
66 
74 void vpCylinder::setWorldCoordinates(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
75 {
76  oP[0] = oA;
77  oP[1] = oB;
78  oP[2] = oC;
79  oP[3] = oX;
80  oP[4] = oY;
81  oP[5] = oZ;
82  oP[6] = R;
83 }
84 
89 
106 {
107  init();
108  setWorldCoordinates(o_P);
109 }
110 
120 vpCylinder::vpCylinder(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
121 {
122  init();
123  setWorldCoordinates(oA, oB, oC, oX, oY, oZ, R);
124 }
125 
130 
155 
184 void vpCylinder::projection(const vpColVector &cP_, vpColVector &p_) const
185 {
186  p_.resize(4, false);
187 
188  double co, si, e, x0, y0, z0;
189  double A, B, C, X0, Y0, Z0, R;
190  double s, a, b, c, zero;
191 
192  A = cP_[0];
193  B = cP_[1];
194  C = cP_[2];
195  X0 = cP_[3];
196  Y0 = cP_[4];
197  Z0 = cP_[5];
198  R = cP_[6];
199  zero = A * X0 + B * Y0 + C * Z0; // should be zero for a good reprensetation of the cylinder
200 
201  s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
202  if (s < 0) {
203  throw vpException(vpException::fatalError, "The camera is inside the cylinder with s=%f!", s);
204  }
205  s = 1.0 / sqrt(s);
206  a = X0 - A * zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
207  b = Y0 - B * zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
208  c = Z0 - C * zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
209  x0 = C * Y0 - B * Z0;
210  y0 = A * Z0 - C * X0;
211  z0 = B * X0 - A * Y0;
212 
213  // rho1 / theta1
214  co = R * a * s - x0;
215  si = R * b * s - y0;
216  e = sqrt(co * co + si * si);
217  p_[0] = -(R * c * s - z0) / e; // rho1
218  p_[1] = atan2(si, co); // theta 1
219 
220  // rho2 / theta2
221  co = R * a * s + x0;
222  si = R * b * s + y0;
223  e = sqrt(co * co + si * si);
224  p_[2] = -(R * c * s + z0) / e; // rho2
225  p_[3] = atan2(si, co); // theta2
226 }
227 
238 
250 {
251  cP_.resize(7, false);
252 
253  double X1, Y1, Z1;
254  double X2, Y2, Z2;
255  double s, a, b, c;
256 
257  double oA, oB, oC, oX0, oY0, oZ0;
258  oA = oP[0];
259  oB = oP[1];
260  oC = oP[2];
261  oX0 = oP[3];
262  oY0 = oP[4];
263  oZ0 = oP[5];
264 
265  X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
266  Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
267  Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
268  s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
269  a = X1 / s;
270  b = Y1 / s;
271  c = Z1 / s;
272 
273  // set axis coordinates in camera frame
274  cP_[0] = a;
275  cP_[1] = b;
276  cP_[2] = c;
277 
278  X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
279  Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
280  Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
281 
282  // adding the constraint X0 is the nearest point to the origin (A^T . X0 =
283  // 0) using the projection operator (I - AA^T) orthogonal to A
284  cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
285  cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
286  cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
287 
288  /* old version for the same onstraint
289  if ( fabs(a) > 0.25 )
290  {
291  double xx, yy, zz, xx1, yy1;
292 
293  xx1 = a*Y2 - b*X2;
294  yy1 = a*Z2 - c*X2;
295  xx = -( b*xx1 + c*yy1);
296  yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
297  zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
298 
299  // set point coordinates in camera frame
300  _cP[3] = xx ;
301  _cP[4] = yy ;
302  _cP[5] = zz ;
303  }
304  else if ( fabs(b) >0.25 )
305  {
306  double xx, yy, zz, xx1, yy1;
307 
308  xx1 = a*Y2 - b*X2;
309  yy1 = c*Y2 - b*Z2;
310  xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
311  yy = a*xx1 + c*yy1;
312  zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
313 
314 
315  // set point coordinates in camera frame
316  _cP[3] = xx ;
317  _cP[4] = yy ;
318  _cP[5] = zz ;
319  }
320  else
321  {
322  double xx, yy, zz, xx1, yy1;
323 
324  xx1 = a*Z2 - c*X2;
325  yy1 = b*Z2 - c*Y2;
326  xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
327  yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
328  zz = a*xx1 + b*yy1;
329 
330  // set point coordinates in camera frame
331  _cP[3] = xx ;
332  _cP[4] = yy ;
333  _cP[5] = zz ;
334  }
335  */
336  // radius
337  cP_[6] = oP[6];
338 }
339 
344 double vpCylinder::computeZ(double x, double y) const
345 {
346  double A = x * x + y * y + 1 - ((getA() * x + getB() * y + getC()) * (getA() * x + getB() * y + getC()));
347  double B = (x * getX() + y * getY() + getZ());
348  double C = getX() * getX() + getY() * getY() + getZ() * getZ() - getR() * getR();
349 
350  return (B - std::sqrt(B * B - A * C)) / A;
351 }
352 
355 {
356  vpCylinder *feature = new vpCylinder(*this);
357  return feature;
358 }
359 
373  const vpColor &color, unsigned int thickness)
374 {
375 
376  vpColVector _cP(7), _p(4);
377  changeFrame(cMo, _cP);
378  projection(_cP, _p);
379  vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
380 }
381 
395  const vpColor &color, unsigned int thickness)
396 {
397 
398  vpColVector _cP(7), _p(4);
399  changeFrame(cMo, _cP);
400  projection(_cP, _p);
401  vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
402 }
403 
413  unsigned int thickness)
414 {
415  vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
416 }
417 
426 void vpCylinder::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
427  unsigned int thickness)
428 {
429  vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
430 }
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpCylinder.cpp:249
void init()
Definition: vpCylinder.cpp:42
virtual ~vpCylinder()
Definition: vpCylinder.cpp:129
double getR() const
Definition: vpCylinder.h:184
double getA() const
Definition: vpCylinder.h:160
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)
double getB() const
Definition: vpCylinder.h:164
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
double getC() const
Definition: vpCylinder.h:168
error that can be emited by ViSP classes.
Definition: vpException.h:71
double computeZ(double x, double y) const
Definition: vpCylinder.cpp:344
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpCylinder.cpp:412
vpColVector cP
Definition: vpTracker.h:77
Generic class defining intrinsic camera parameters.
double getY() const
Definition: vpCylinder.h:176
void projection()
Definition: vpCylinder.cpp:154
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:102
double getZ() const
Definition: vpCylinder.h:180
vpCylinder * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpCylinder.cpp:354
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double getX() const
Definition: vpCylinder.h:172
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:65
vpColVector p
Definition: vpTracker.h:73