Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpCircle.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  * Visual feature circle.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpCircle.h>
37 
38 #include <visp3/core/vpFeatureDisplay.h>
39 
41 {
42  oP.resize(7);
43  cP.resize(7);
44 
45  p.resize(5);
46 }
47 
57 void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
58 
69 void vpCircle::setWorldCoordinates(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
70 {
71  oP[0] = oA;
72  oP[1] = oB;
73  oP[2] = oC;
74  oP[3] = oX;
75  oP[4] = oY;
76  oP[5] = oZ;
77  oP[6] = R;
78 }
79 
84 
97 {
98  init();
100 }
101 
115 vpCircle::vpCircle(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
116 {
117  init();
118  setWorldCoordinates(oA, oB, oC, oX, oY, oZ, R);
119 }
120 
125 
138 
157 void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const
158 {
159  double det_threshold = 1e-10;
160  p_.resize(5, false);
161 
162  vpColVector K(6);
163  {
164  double A = cP_[0];
165  double B = cP_[1];
166  double C = cP_[2];
167 
168  double X0 = cP_[3];
169  double Y0 = cP_[4];
170  double Z0 = cP_[5];
171 
172  double r = cP_[6];
173 
174  // projection
175  double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
176  double det = A * X0 + B * Y0 + C * Z0;
177  A = A / det;
178  B = B / det;
179  C = C / det;
180 
181  K[0] = 1 - 2 * A * X0 + A * A * s;
182  K[1] = 1 - 2 * B * Y0 + B * B * s;
183  K[2] = -A * Y0 - B * X0 + A * B * s;
184  K[3] = -C * X0 - A * Z0 + A * C * s;
185  K[4] = -C * Y0 - B * Z0 + B * C * s;
186  K[5] = 1 - 2 * C * Z0 + C * C * s;
187  }
188 
189  double det = K[2] * K[2] - K[0] * K[1];
190  if (fabs(det) < det_threshold) {
191  throw(vpException(vpException::divideByZeroError, "Division by 0 in vpCircle::projection."));
192  }
193 
194  double xc = (K[1] * K[3] - K[2] * K[4]) / det;
195  double yc = (K[0] * K[4] - K[2] * K[3]) / det;
196 
197  double c = sqrt((K[0] - K[1]) * (K[0] - K[1]) + 4 * K[2] * K[2]);
198  double s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]);
199 
200  double A, B, E;
201 
202  if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
203  E = 0.0;
204  if (K[0] > K[1]) {
205  A = sqrt(s / (K[0] + K[1] + c));
206  B = sqrt(s / (K[0] + K[1] - c));
207  } else {
208  A = sqrt(s / (K[0] + K[1] - c));
209  B = sqrt(s / (K[0] + K[1] + c));
210  }
211  } else {
212  E = (K[1] - K[0] + c) / (2 * K[2]);
213  if (fabs(E) > 1.0) {
214  A = sqrt(s / (K[0] + K[1] + c));
215  B = sqrt(s / (K[0] + K[1] - c));
216  } else {
217  A = sqrt(s / (K[0] + K[1] - c));
218  B = sqrt(s / (K[0] + K[1] + c));
219  E = -1.0 / E;
220  }
221  }
222 
223  // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4
224  det = 4 * (1.0 + vpMath::sqr(E));
225  double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
226  double n11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det;
227  double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
228 
229  p_[0] = xc;
230  p_[1] = yc;
231  p_[2] = n20;
232  p_[3] = n11;
233  p_[4] = n02;
234 }
235 
247 {
248  noP.resize(7, false);
249 
250  double A, B, C;
251  A = noMo[0][0] * oP[0] + noMo[0][1] * oP[1] + noMo[0][2] * oP[2];
252  B = noMo[1][0] * oP[0] + noMo[1][1] * oP[1] + noMo[1][2] * oP[2];
253  C = noMo[2][0] * oP[0] + noMo[2][1] * oP[1] + noMo[2][2] * oP[2];
254 
255  double X0, Y0, Z0;
256  X0 = noMo[0][3] + noMo[0][0] * oP[3] + noMo[0][1] * oP[4] + noMo[0][2] * oP[5];
257  Y0 = noMo[1][3] + noMo[1][0] * oP[3] + noMo[1][1] * oP[4] + noMo[1][2] * oP[5];
258  Z0 = noMo[2][3] + noMo[2][0] * oP[3] + noMo[2][1] * oP[4] + noMo[2][2] * oP[5];
259  double R = oP[6];
260 
261  noP[0] = A;
262  noP[1] = B;
263  noP[2] = C;
264 
265  noP[3] = X0;
266  noP[4] = Y0;
267  noP[5] = Z0;
268 
269  noP[6] = R;
270 }
271 
279 {
280  double A, B, C;
281  A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
282  B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
283  C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];
284 
285  double X0, Y0, Z0;
286  X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
287  Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
288  Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
289  double R = oP[6];
290 
291  cP[0] = A;
292  cP[1] = B;
293  cP[2] = C;
294 
295  cP[3] = X0;
296  cP[4] = Y0;
297  cP[5] = Z0;
298 
299  cP[6] = R;
300 }
301 
311 void vpCircle::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
312  unsigned int thickness)
313 {
314  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
315 }
316 
326 void vpCircle::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
327  unsigned int thickness)
328 {
329  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
330 }
331 
344  const vpColor &color, unsigned int thickness)
345 {
346  vpColVector _cP, _p;
347  changeFrame(cMo, _cP);
348  projection(_cP, _p);
349  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
350 }
351 
364  const vpColor &color, unsigned int thickness)
365 {
366  vpColVector _cP, _p;
367  changeFrame(cMo, _cP);
368  projection(_cP, _p);
369  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
370 }
371 
374 {
375  vpCircle *feature = new vpCircle(*this);
376  return feature;
377 }
378 
396 void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho,
397  const double &theta, double &i, double &j)
398 {
399  // This was taken from the code of art-v1. (from the artCylinder class)
400  double px = cam.get_px();
401  double py = cam.get_py();
402  double u0 = cam.get_u0();
403  double v0 = cam.get_v0();
404 
405  double n11 = circle.p[3];
406  double n02 = circle.p[4];
407  double n20 = circle.p[2];
408  double Xg = u0 + circle.p[0] * px;
409  double Yg = v0 + circle.p[1] * py;
410 
411  // Find Intersection between line and ellipse in the image.
412 
413  // Optimised calculation for X
414  double stheta = sin(theta);
415  double ctheta = cos(theta);
416  double sctheta = stheta * ctheta;
417  double m11yg = n11 * Yg;
418  double ctheta2 = vpMath::sqr(ctheta);
419  double m02xg = n02 * Xg;
420  double m11stheta = n11 * stheta;
421  j = ((n11 * Xg * sctheta - n20 * Yg * sctheta + n20 * rho * ctheta - m11yg + m11yg * ctheta2 + m02xg -
422  m02xg * ctheta2 + m11stheta * rho) /
423  (n20 * ctheta2 + 2.0 * m11stheta * ctheta + n02 - n02 * ctheta2));
424  // Optimised calculation for Y
425  double rhom02 = rho * n02;
426  double sctheta2 = stheta * ctheta2;
427  double ctheta3 = ctheta2 * ctheta;
428  i = (-(-rho * n11 * stheta * ctheta - rhom02 + rhom02 * ctheta2 + n11 * Xg * sctheta2 - n20 * Yg * sctheta2 -
429  ctheta * n11 * Yg + ctheta3 * n11 * Yg + ctheta * n02 * Xg - ctheta3 * n02 * Xg) /
430  (n20 * ctheta2 + 2.0 * n11 * stheta * ctheta + n02 - n02 * ctheta2) / stheta);
431 }
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:86
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) vp_override
Definition: vpCircle.cpp:311
void projection() vp_override
Definition: vpCircle.cpp:137
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpCircle.cpp:57
virtual ~vpCircle() vp_override
Definition: vpCircle.cpp:124
vpCircle()
Definition: vpCircle.cpp:83
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const vp_override
Definition: vpCircle.cpp:246
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:396
vpCircle * duplicate() const vp_override
For memory issue (used by the vpServo class only)
Definition: vpCircle.cpp:373
void init() vp_override
Definition: vpCircle.cpp:40
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
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:201
vpColVector cP
Definition: vpTracker.h:71
vpColVector p
Definition: vpTracker.h:67