Visual Servoing Platform  version 3.6.1 under development (2024-06-12)
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 
42 {
43  oP.resize(7);
44  cP.resize(7);
45 
46  p.resize(5);
47 }
48 
58 void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
59 
70 void vpCircle::setWorldCoordinates(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
71 {
72  oP[0] = oA;
73  oP[1] = oB;
74  oP[2] = oC;
75  oP[3] = oX;
76  oP[4] = oY;
77  oP[5] = oZ;
78  oP[6] = R;
79 }
80 
85 
98 {
99  init();
100  setWorldCoordinates(oP_);
101 }
102 
116 vpCircle::vpCircle(double oA, double oB, double oC, double oX, double oY, double oZ, double R)
117 {
118  init();
119  setWorldCoordinates(oA, oB, oC, oX, oY, oZ, R);
120 }
121 
126 
139 
158 void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const
159 {
160  double det_threshold = 1e-10;
161  p_.resize(5, false);
162 
163  vpColVector K(6);
164  {
165  double A = cP_[0];
166  double B = cP_[1];
167  double C = cP_[2];
168 
169  double X0 = cP_[3];
170  double Y0 = cP_[4];
171  double Z0 = cP_[5];
172 
173  double r = cP_[6];
174 
175  // projection
176  double s = ((X0 * X0) + (Y0 * Y0) + (Z0 * Z0)) - (r * r);
177  double det = (A * X0) + (B * Y0) + (C * Z0);
178  A = A / det;
179  B = B / det;
180  C = C / det;
181 
182  K[0] = (1 - (2 * A * X0)) + (A * A * s);
183  K[1] = (1 - (2 * B * Y0)) + (B * B * s);
184  K[2] = ((-A * Y0) - (B * X0)) + (A * B * s);
185  K[3] = ((-C * X0) - (A * Z0)) + (A * C * s);
186  K[4] = ((-C * Y0) - (B * Z0)) + (B * C * s);
187  K[5] = (1 - (2 * C * Z0)) + (C * C * s);
188  }
189 
190  double det = (K[2] * K[2]) - (K[0] * K[1]);
191  if (fabs(det) < det_threshold) {
192  throw(vpException(vpException::divideByZeroError, "Division by 0 in vpCircle::projection."));
193  }
194 
195  double xc = ((K[1] * K[3]) - (K[2] * K[4])) / det;
196  double yc = ((K[0] * K[4]) - (K[2] * K[3])) / det;
197 
198  double c = sqrt(((K[0] - K[1]) * (K[0] - K[1])) + (4 * K[2] * K[2]));
199  double s = 2 * (((K[0] * xc * xc) + (2 * K[2] * xc * yc) + (K[1] * yc * yc)) - K[5]);
200 
201  double A, B, E;
202 
203  if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
204  E = 0.0;
205  if (K[0] > K[1]) {
206  A = sqrt(s / ((K[0] + K[1]) + c));
207  B = sqrt(s / ((K[0] + K[1]) - c));
208  }
209  else {
210  A = sqrt(s / ((K[0] + K[1]) - c));
211  B = sqrt(s / ((K[0] + K[1]) + c));
212  }
213  }
214  else {
215  E = ((K[1] - K[0]) + c) / (2 * K[2]);
216  if (fabs(E) > 1.0) {
217  A = sqrt(s / ((K[0] + K[1]) + c));
218  B = sqrt(s / ((K[0] + K[1]) - c));
219  }
220  else {
221  A = sqrt(s / ((K[0] + K[1]) - c));
222  B = sqrt(s / ((K[0] + K[1]) + c));
223  E = -1.0 / E;
224  }
225  }
226 
227  // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4
228  det = 4 * (1.0 + vpMath::sqr(E));
229  double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
230  double n11 = ((vpMath::sqr(A) - vpMath::sqr(B)) * E) / det;
231  double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
232 
233  p_[0] = xc;
234  p_[1] = yc;
235  p_[2] = n20;
236  p_[3] = n11;
237  p_[4] = n02;
238 }
239 
251 {
252  noP.resize(7, false);
253 
254  double A, B, C;
255  A = (noMo[0][0] * oP[0]) + (noMo[0][1] * oP[1]) + (noMo[0][2] * oP[2]);
256  B = (noMo[1][0] * oP[0]) + (noMo[1][1] * oP[1]) + (noMo[1][2] * oP[2]);
257  C = (noMo[2][0] * oP[0]) + (noMo[2][1] * oP[1]) + (noMo[2][2] * oP[2]);
258 
259  double X0, Y0, Z0;
260  X0 = noMo[0][3] + (noMo[0][0] * oP[3]) + (noMo[0][1] * oP[4]) + (noMo[0][2] * oP[5]);
261  Y0 = noMo[1][3] + (noMo[1][0] * oP[3]) + (noMo[1][1] * oP[4]) + (noMo[1][2] * oP[5]);
262  Z0 = noMo[2][3] + (noMo[2][0] * oP[3]) + (noMo[2][1] * oP[4]) + (noMo[2][2] * oP[5]);
263  double R = oP[6];
264 
265  noP[0] = A;
266  noP[1] = B;
267  noP[2] = C;
268 
269  noP[3] = X0;
270  noP[4] = Y0;
271  noP[5] = Z0;
272 
273  noP[6] = R;
274 }
275 
283 {
284  double A, B, C;
285  A = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]);
286  B = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]);
287  C = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]);
288 
289  double X0, Y0, Z0;
290  X0 = cMo[0][3] + (cMo[0][0] * oP[3]) + (cMo[0][1] * oP[4]) + (cMo[0][2] * oP[5]);
291  Y0 = cMo[1][3] + (cMo[1][0] * oP[3]) + (cMo[1][1] * oP[4]) + (cMo[1][2] * oP[5]);
292  Z0 = cMo[2][3] + (cMo[2][0] * oP[3]) + (cMo[2][1] * oP[4]) + (cMo[2][2] * oP[5]);
293  double R = oP[6];
294 
295  cP[0] = A;
296  cP[1] = B;
297  cP[2] = C;
298 
299  cP[3] = X0;
300  cP[4] = Y0;
301  cP[5] = Z0;
302 
303  cP[6] = R;
304 }
305 
315 void vpCircle::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
316  unsigned int thickness)
317 {
318  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
319 }
320 
330 void vpCircle::display(const vpImage<vpRGBa> &I, const vpCameraParameters &cam, const vpColor &color,
331  unsigned int thickness)
332 {
333  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
334 }
335 
348  const vpColor &color, unsigned int thickness)
349 {
350  vpColVector v_cP, v_p;
351  changeFrame(cMo, v_cP);
352  projection(v_cP, v_p);
353  vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness);
354 }
355 
368  const vpColor &color, unsigned int thickness)
369 {
370  vpColVector v_cP, v_p;
371  changeFrame(cMo, v_cP);
372  projection(v_cP, v_p);
373  vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness);
374 }
375 
378 {
379  vpCircle *feature = new vpCircle(*this);
380  return feature;
381 }
382 
400 void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho,
401  const double &theta, double &i, double &j)
402 {
403  // This was taken from the code of art-v1. (from the artCylinder class)
404  double px = cam.get_px();
405  double py = cam.get_py();
406  double u0 = cam.get_u0();
407  double v0 = cam.get_v0();
408 
409  double n11 = circle.p[3];
410  double n02 = circle.p[4];
411  double n20 = circle.p[2];
412  double Xg = u0 + (circle.p[0] * px);
413  double Yg = v0 + (circle.p[1] * py);
414 
415  // Find Intersection between line and ellipse in the image.
416 
417  // Optimised calculation for X
418  double stheta = sin(theta);
419  double ctheta = cos(theta);
420  double sctheta = stheta * ctheta;
421  double m11yg = n11 * Yg;
422  double ctheta2 = vpMath::sqr(ctheta);
423  double m02xg = n02 * Xg;
424  double m11stheta = n11 * stheta;
425  j = ((((((((n11 * Xg * sctheta) - (n20 * Yg * sctheta)) + (n20 * rho * ctheta)) - m11yg) + (m11yg * ctheta2) + m02xg) -
426  (m02xg * ctheta2)) + (m11stheta * rho)) /
427  (((n20 * ctheta2) + (2.0 * m11stheta * ctheta) + n02) - (n02 * ctheta2)));
428  // Optimised calculation for Y
429  double rhom02 = rho * n02;
430  double sctheta2 = stheta * ctheta2;
431  double ctheta3 = ctheta2 * ctheta;
432  i = (-(((((((-rho * n11 * stheta * ctheta) - rhom02) + (rhom02 * ctheta2) + (n11 * Xg * sctheta2)) - (n20 * Yg * sctheta2)) -
433  (ctheta * n11 * Yg)) + (ctheta3 * n11 * Yg) + (ctheta * n02 * Xg)) - (ctheta3 * n02 * Xg)) /
434  (((n20 * ctheta2) + (2.0 * n11 * stheta * ctheta) + n02) - (n02 * ctheta2)) / stheta);
435 }
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:88
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) vp_override
Definition: vpCircle.cpp:315
void projection() vp_override
Definition: vpCircle.cpp:138
void setWorldCoordinates(const vpColVector &oP) vp_override
Definition: vpCircle.cpp:58
virtual ~vpCircle() vp_override
Definition: vpCircle.cpp:125
vpCircle()
Definition: vpCircle.cpp:84
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const vp_override
Definition: vpCircle.cpp:250
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:400
void init() vp_override
Definition: vpCircle.cpp:41
vpCircle * duplicate() const vp_override
For memory issue (used by the vpServo class only)
Definition: vpCircle.cpp:377
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1066
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:153
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
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:203
vpColVector cP
Definition: vpTracker.h:73
vpColVector p
Definition: vpTracker.h:69