Visual Servoing Platform version 3.5.0
vpCircle.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 * Visual feature circle.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpCircle.h>
40
41#include <visp3/core/vpFeatureDisplay.h>
42
44{
45 oP.resize(7);
46 cP.resize(7);
47
48 p.resize(5);
49}
50
60void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
61
72void vpCircle::setWorldCoordinates(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
73{
74 oP[0] = oA;
75 oP[1] = oB;
76 oP[2] = oC;
77 oP[3] = oX;
78 oP[4] = oY;
79 oP[5] = oZ;
80 oP[6] = R;
81}
82
87
100{
101 init();
103}
104
118vpCircle::vpCircle(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
119{
120 init();
121 setWorldCoordinates(oA, oB, oC, oX, oY, oZ, R);
122}
123
128
141
161{
162 p_.resize(5, false);
163
164 vpColVector K(6);
165 {
166 double A = cP_[0];
167 double B = cP_[1];
168 double C = cP_[2];
169
170 double X0 = cP_[3];
171 double Y0 = cP_[4];
172 double Z0 = cP_[5];
173
174 double r = cP_[6];
175
176 // projection
177 double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
178 double det = A * X0 + B * Y0 + C * Z0;
179 A = A / det;
180 B = B / det;
181 C = C / det;
182
183 K[0] = 1 - 2 * A * X0 + A * A * s;
184 K[1] = 1 - 2 * B * Y0 + B * B * s;
185 K[2] = -A * Y0 - B * X0 + A * B * s;
186 K[3] = -C * X0 - A * Z0 + A * C * s;
187 K[4] = -C * Y0 - B * Z0 + B * C * s;
188 K[5] = 1 - 2 * C * Z0 + C * C * s;
189 }
190
191 double det = K[2] * K[2] - K[0] * K[1];
192 if (fabs(det) < 1e-8) {
193 throw(vpException(vpException::divideByZeroError, "division par 0"));
194 }
195
196 double xc = (K[1] * K[3] - K[2] * K[4]) / det;
197 double yc = (K[0] * K[4] - K[2] * K[3]) / det;
198
199 double c = sqrt((K[0] - K[1]) * (K[0] - K[1]) + 4 * K[2] * K[2]);
200 double s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]);
201
202 double A, B, E;
203
204 if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
205 E = 0.0;
206 if (K[0] > K[1]) {
207 A = sqrt(s / (K[0] + K[1] + c));
208 B = sqrt(s / (K[0] + K[1] - c));
209 } else {
210 A = sqrt(s / (K[0] + K[1] - c));
211 B = sqrt(s / (K[0] + K[1] + c));
212 }
213 } else {
214 E = (K[1] - K[0] + c) / (2 * K[2]);
215 if (fabs(E) > 1.0) {
216 A = sqrt(s / (K[0] + K[1] + c));
217 B = sqrt(s / (K[0] + K[1] - c));
218 } else {
219 A = sqrt(s / (K[0] + K[1] - c));
220 B = sqrt(s / (K[0] + K[1] + c));
221 E = -1.0 / E;
222 }
223 }
224
225 // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4
226 det = 4 * (1.0 + vpMath::sqr(E));
227 double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
228 double n11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det;
229 double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
230
231 p_[0] = xc;
232 p_[1] = yc;
233 p_[2] = n20;
234 p_[3] = n11;
235 p_[4] = n02;
236}
237
249{
250 noP.resize(7, false);
251
252 double A, B, C;
253 A = noMo[0][0] * oP[0] + noMo[0][1] * oP[1] + noMo[0][2] * oP[2];
254 B = noMo[1][0] * oP[0] + noMo[1][1] * oP[1] + noMo[1][2] * oP[2];
255 C = noMo[2][0] * oP[0] + noMo[2][1] * oP[1] + noMo[2][2] * oP[2];
256
257 double X0, Y0, Z0;
258 X0 = noMo[0][3] + noMo[0][0] * oP[3] + noMo[0][1] * oP[4] + noMo[0][2] * oP[5];
259 Y0 = noMo[1][3] + noMo[1][0] * oP[3] + noMo[1][1] * oP[4] + noMo[1][2] * oP[5];
260 Z0 = noMo[2][3] + noMo[2][0] * oP[3] + noMo[2][1] * oP[4] + noMo[2][2] * oP[5];
261 double R = oP[6];
262
263 noP[0] = A;
264 noP[1] = B;
265 noP[2] = C;
266
267 noP[3] = X0;
268 noP[4] = Y0;
269 noP[5] = Z0;
270
271 noP[6] = R;
272}
273
281{
282 double A, B, C;
283 A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
284 B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
285 C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];
286
287 double X0, Y0, Z0;
288 X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
289 Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
290 Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
291 double R = oP[6];
292
293 cP[0] = A;
294 cP[1] = B;
295 cP[2] = C;
296
297 cP[3] = X0;
298 cP[4] = Y0;
299 cP[5] = Z0;
300
301 cP[6] = R;
302}
303
314 unsigned int thickness)
315{
316 vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
317}
318
328void vpCircle::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
329 unsigned int thickness)
330{
331 vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
332}
333
346 const vpColor &color, unsigned int thickness)
347{
348 vpColVector _cP, _p;
349 changeFrame(cMo, _cP);
350 projection(_cP, _p);
351 vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
352}
353
366 const vpColor &color, unsigned int thickness)
367{
368 vpColVector _cP, _p;
369 changeFrame(cMo, _cP);
370 projection(_cP, _p);
371 vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
372}
373
376{
377 vpCircle *feature = new vpCircle(*this);
378 return feature;
379}
380
398void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho,
399 const double &theta, double &i, double &j)
400{
401 // This was taken from the code of art-v1. (from the artCylinder class)
402 double px = cam.get_px();
403 double py = cam.get_py();
404 double u0 = cam.get_u0();
405 double v0 = cam.get_v0();
406
407 double n11 = circle.p[3];
408 double n02 = circle.p[4];
409 double n20 = circle.p[2];
410 double Xg = u0 + circle.p[0] * px;
411 double Yg = v0 + circle.p[1] * py;
412
413 // Find Intersection between line and ellipse in the image.
414
415 // Optimised calculation for X
416 double stheta = sin(theta);
417 double ctheta = cos(theta);
418 double sctheta = stheta * ctheta;
419 double m11yg = n11 * Yg;
420 double ctheta2 = vpMath::sqr(ctheta);
421 double m02xg = n02 * Xg;
422 double m11stheta = n11 * stheta;
423 j = ((n11 * Xg * sctheta - n20 * Yg * sctheta + n20 * rho * ctheta - m11yg + m11yg * ctheta2 + m02xg -
424 m02xg * ctheta2 + m11stheta * rho) /
425 (n20 * ctheta2 + 2.0 * m11stheta * ctheta + n02 - n02 * ctheta2));
426 // Optimised calculation for Y
427 double rhom02 = rho * n02;
428 double sctheta2 = stheta * ctheta2;
429 double ctheta3 = ctheta2 * ctheta;
430 i = (-(-rho * n11 * stheta * ctheta - rhom02 + rhom02 * ctheta2 + n11 * Xg * sctheta2 - n20 * Yg * sctheta2 -
431 ctheta * n11 * Yg + ctheta3 * n11 * Yg + ctheta * n02 * Xg - ctheta3 * n02 * Xg) /
432 (n20 * ctheta2 + 2.0 * n11 * stheta * ctheta + n02 - n02 * ctheta2) / stheta);
433}
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:92
virtual ~vpCircle()
Definition: vpCircle.cpp:127
vpCircle * duplicate() const
For memory issue (used by the vpServo class only)
Definition: vpCircle.cpp:375
vpCircle()
Definition: vpCircle.cpp:86
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
Definition: vpCircle.cpp:248
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:60
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
Definition: vpCircle.cpp:313
void projection()
Definition: vpCircle.cpp:140
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:398
void init()
Definition: vpCircle.cpp:43
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
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
static void displayEllipse(double x, double y, double n20, double n11, double n02, 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.
static double sqr(double x)
Definition: vpMath.h:116
vpColVector cP
Definition: vpTracker.h:77
vpColVector p
Definition: vpTracker.h:73