Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
62void vpCylinder::setWorldCoordinates(const vpColVector &o_P) { this->oP = o_P; }
63
71void 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();
106}
107
117vpCylinder::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
127
152
182{
183 p_.resize(4, false);
184
185 double co, si, e, x0, y0, z0;
186 double A, B, C, X0, Y0, Z0, R;
187 double s, a, b, c, zero;
188
189 A = cP_[0];
190 B = cP_[1];
191 C = cP_[2];
192 X0 = cP_[3];
193 Y0 = cP_[4];
194 Z0 = cP_[5];
195 R = cP_[6];
196 zero = A * X0 + B * Y0 + C * Z0; // should be zero for a good reprensetation of the cylinder
197
198 s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
199 if (s < 0) {
200 throw vpException(vpException::fatalError, "The camera is inside the cylinder with s=%f!", s);
201 }
202 s = 1.0 / sqrt(s);
203 a = X0 - A * zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
204 b = Y0 - B * zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
205 c = Z0 - C * zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
206 x0 = C * Y0 - B * Z0;
207 y0 = A * Z0 - C * X0;
208 z0 = B * X0 - A * Y0;
209
210 // rho1 / theta1
211 co = R * a * s - x0;
212 si = R * b * s - y0;
213 e = sqrt(co * co + si * si);
214 p_[0] = -(R * c * s - z0) / e; // rho1
215 p_[1] = atan2(si, co); // theta 1
216
217 // rho2 / theta2
218 co = R * a * s + x0;
219 si = R * b * s + y0;
220 e = sqrt(co * co + si * si);
221 p_[2] = -(R * c * s + z0) / e; // rho2
222 p_[3] = atan2(si, co); // theta2
223}
224
235
247{
248 cP_.resize(7, false);
249
250 double X1, Y1, Z1;
251 double X2, Y2, Z2;
252 double s, a, b, c;
253
254 double oA, oB, oC, oX0, oY0, oZ0;
255 oA = oP[0];
256 oB = oP[1];
257 oC = oP[2];
258 oX0 = oP[3];
259 oY0 = oP[4];
260 oZ0 = oP[5];
261
262 X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
263 Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
264 Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
265 s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
266 a = X1 / s;
267 b = Y1 / s;
268 c = Z1 / s;
269
270 // set axis coordinates in camera frame
271 cP_[0] = a;
272 cP_[1] = b;
273 cP_[2] = c;
274
275 X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
276 Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
277 Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
278
279 // adding the constraint X0 is the nearest point to the origin (A^T . X0 =
280 // 0) using the projection operator (I - AA^T) orthogonal to A
281 cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
282 cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
283 cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
284
285 /* old version for the same onstraint
286 if ( fabs(a) > 0.25 )
287 {
288 double xx, yy, zz, xx1, yy1;
289
290 xx1 = a*Y2 - b*X2;
291 yy1 = a*Z2 - c*X2;
292 xx = -( b*xx1 + c*yy1);
293 yy = (( a*a + c*c ) * xx1 - b*c*yy1 ) /a;
294 zz = ( -b*c*xx1 + ( a*a + b*b )*yy1) /a;
295
296 // set point coordinates in camera frame
297 _cP[3] = xx ;
298 _cP[4] = yy ;
299 _cP[5] = zz ;
300 }
301 else if ( fabs(b) >0.25 )
302 {
303 double xx, yy, zz, xx1, yy1;
304
305 xx1 = a*Y2 - b*X2;
306 yy1 = c*Y2 - b*Z2;
307 xx = - (( b*b + c*c ) * xx1 - a*c*yy1 ) /b;
308 yy = a*xx1 + c*yy1;
309 zz = - ( -a*c*xx1 + (a*a + b*b) * yy1 ) /b;
310
311
312 // set point coordinates in camera frame
313 _cP[3] = xx ;
314 _cP[4] = yy ;
315 _cP[5] = zz ;
316 }
317 else
318 {
319 double xx, yy, zz, xx1, yy1;
320
321 xx1 = a*Z2 - c*X2;
322 yy1 = b*Z2 - c*Y2;
323 xx = (-( b*b + c*c ) * xx1 - a*c*yy1 ) /c;
324 yy = ( a*b*xx1 - ( a*a + c*c )*yy1) /c;
325 zz = a*xx1 + b*yy1;
326
327 // set point coordinates in camera frame
328 _cP[3] = xx ;
329 _cP[4] = yy ;
330 _cP[5] = zz ;
331 }
332 */
333 // radius
334 cP_[6] = oP[6];
335}
336
341double vpCylinder::computeZ(double x, double y) const
342{
343 double A = x * x + y * y + 1 - ((getA() * x + getB() * y + getC()) * (getA() * x + getB() * y + getC()));
344 double B = (x * getX() + y * getY() + getZ());
345 double C = getX() * getX() + getY() * getY() + getZ() * getZ() - getR() * getR();
346
347 return (B - std::sqrt(B * B - A * C)) / A;
348}
349
352{
353 vpCylinder *feature = new vpCylinder(*this);
354 return feature;
355}
356
370 const vpColor &color, unsigned int thickness)
371{
372
373 vpColVector _cP(7), _p(4);
374 changeFrame(cMo, _cP);
375 projection(_cP, _p);
376 vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
377}
378
392 const vpColor &color, unsigned int thickness)
393{
394
395 vpColVector _cP(7), _p(4);
396 changeFrame(cMo, _cP);
397 projection(_cP, _p);
398 vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
399}
400
410 unsigned int thickness)
411{
412 vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
413}
414
423void vpCylinder::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
424 unsigned int thickness)
425{
426 vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
427}
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
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:98
void init()
double getZ() const
Definition vpCylinder.h:175
double getB() const
Definition vpCylinder.h:159
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
double getX() const
Definition vpCylinder.h:167
double getY() const
Definition vpCylinder.h:171
void projection()
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
double getA() const
Definition vpCylinder.h:155
vpCylinder * duplicate() const
For memory issue (used by the vpServo class only).
double getR() const
Definition vpCylinder.h:179
double getC() const
Definition vpCylinder.h:163
double computeZ(double x, double y) const
virtual ~vpCylinder()
void setWorldCoordinates(const vpColVector &oP)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ fatalError
Fatal error.
Definition vpException.h:84
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.
Definition of the vpImage class member functions.
Definition vpImage.h:135
vpColVector cP
Definition vpTracker.h:72
vpColVector p
Definition vpTracker.h:68