Visual Servoing Platform version 3.5.0
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
65void vpCylinder::setWorldCoordinates(const vpColVector &o_P) { this->oP = o_P; }
66
74void 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();
109}
110
120vpCylinder::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
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
344double 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
426void 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}
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:103
void init()
Definition: vpCylinder.cpp:42
double getZ() const
Definition: vpCylinder.h:180
double getB() const
Definition: vpCylinder.h:164
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Definition: vpCylinder.cpp:249
double getX() const
Definition: vpCylinder.h:172
double getY() const
Definition: vpCylinder.h:176
void projection()
Definition: vpCylinder.cpp:154
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpCylinder.cpp:412
double getA() const
Definition: vpCylinder.h:160
vpCylinder * duplicate() const
For memory issue (used by the vpServo class only).
Definition: vpCylinder.cpp:354
double getR() const
Definition: vpCylinder.h:184
double getC() const
Definition: vpCylinder.h:168
double computeZ(double x, double y) const
Definition: vpCylinder.cpp:344
virtual ~vpCylinder()
Definition: vpCylinder.cpp:129
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCylinder.cpp:65
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
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:77
vpColVector p
Definition: vpTracker.h:73