Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
46  oP.resize(7);
47  cP.resize(7);
48 
49  p.resize(5);
50 }
51 
61 void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
62 
75 void vpCircle::setWorldCoordinates(const double A, const double B, const double C, const double X0, const double Y0,
76  const double Z0, const double R)
77 {
78  oP[0] = A;
79  oP[1] = B;
80  oP[2] = C;
81  oP[3] = X0;
82  oP[4] = Y0;
83  oP[5] = Z0;
84  oP[6] = R;
85 }
86 
88 
100 {
101  init();
102  setWorldCoordinates(oP_);
103 }
104 
118 vpCircle::vpCircle(const double A, const double B, const double C, const double X0, const double Y0, const double Z0,
119  const double R)
120 {
121  init();
122  setWorldCoordinates(A, B, C, X0, Y0, Z0, R);
123 }
124 
126 
139 
157 {
158  vpColVector K(6);
159  {
160  double A = cP_[0];
161  double B = cP_[1];
162  double C = cP_[2];
163 
164  double X0 = cP_[3];
165  double Y0 = cP_[4];
166  double Z0 = cP_[5];
167 
168  double r = cP_[6];
169 
170  // projection
171  double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
172  double det = A * X0 + B * Y0 + C * Z0;
173  A = A / det;
174  B = B / det;
175  C = C / det;
176 
177  K[0] = 1 - 2 * A * X0 + A * A * s;
178  K[1] = 1 - 2 * B * Y0 + B * B * s;
179  K[2] = -A * Y0 - B * X0 + A * B * s;
180  K[3] = -C * X0 - A * Z0 + A * C * s;
181  K[4] = -C * Y0 - B * Z0 + B * C * s;
182  K[5] = 1 - 2 * C * Z0 + C * C * s;
183  }
184 
185  // {
186  // std::cout << "K dans vpCircle::projection(): " << std::endl;
187  // for (unsigned int i=1; i<6; i++)
188  // std::cout << K[i]/K[0] << std::endl;
189  // }
190  double det = K[2] * K[2] - K[0] * K[1];
191  if (fabs(det) < 1e-8) {
192  vpERROR_TRACE("division par 0");
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])<1e-6)
205  if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
206  E = 0.0;
207  if (K[0] > K[1]) {
208  A = sqrt(s / (K[0] + K[1] + c));
209  B = sqrt(s / (K[0] + K[1] - c));
210  } else {
211  A = sqrt(s / (K[0] + K[1] - c));
212  B = sqrt(s / (K[0] + K[1] + c));
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  } else {
220  A = sqrt(s / (K[0] + K[1] - c));
221  B = sqrt(s / (K[0] + K[1] + c));
222  E = -1.0 / E;
223  }
224  }
225 
226  det = (1.0 + vpMath::sqr(E));
227  double mu20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
228  double mu11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det;
229  double mu02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
230 
231  p_[0] = xc;
232  p_[1] = yc;
233  p_[2] = mu20;
234  p_[3] = mu11;
235  p_[4] = mu02;
236 }
237 
240 {
241 
242  double A, B, C;
243  A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
244  B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
245  C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];
246 
247  double X0, Y0, Z0;
248  X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
249  Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
250  Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
251  double R = oP[6];
252 
253  cP_[0] = A;
254  cP_[1] = B;
255  cP_[2] = C;
256 
257  cP_[3] = X0;
258  cP_[4] = Y0;
259  cP_[5] = Z0;
260 
261  cP_[6] = R;
262 
263  // vpTRACE("_cP :") ; std::cout << _cP.t() ;
264 }
265 
268 {
269 
270  double A, B, C;
271  A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
272  B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
273  C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];
274 
275  double X0, Y0, Z0;
276  X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
277  Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
278  Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
279  double R = oP[6];
280 
281  cP[0] = A;
282  cP[1] = B;
283  cP[2] = C;
284 
285  cP[3] = X0;
286  cP[4] = Y0;
287  cP[5] = Z0;
288 
289  cP[6] = R;
290 
291  // vpTRACE("_cP :") ; std::cout << _cP.t() ;
292 }
293 
294 void vpCircle::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
295  const unsigned int thickness)
296 {
297  vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
298 }
299 
300 // non destructive wrt. cP and p
302  const vpColor &color, const unsigned int thickness)
303 {
304  vpColVector _cP, _p;
305  changeFrame(cMo, _cP);
306  projection(_cP, _p);
307  vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
308 }
311 {
312  vpCircle *feature = new vpCircle(*this);
313  return feature;
314 }
315 
332 void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho,
333  const double &theta, double &i, double &j)
334 {
335  // This was taken from the code of art-v1. (from the artCylinder class)
336  double px = cam.get_px();
337  double py = cam.get_py();
338  double u0 = cam.get_u0();
339  double v0 = cam.get_v0();
340 
341  double mu11 = circle.p[3];
342  double mu02 = circle.p[4];
343  double mu20 = circle.p[2];
344  double Xg = u0 + circle.p[0] * px;
345  double Yg = v0 + circle.p[1] * py;
346 
347  // Find Intersection between line and ellipse in the image.
348 
349  // Optimised calculation for X
350  double stheta = sin(theta);
351  double ctheta = cos(theta);
352  double sctheta = stheta * ctheta;
353  double m11yg = mu11 * Yg;
354  double ctheta2 = vpMath::sqr(ctheta);
355  double m02xg = mu02 * Xg;
356  double m11stheta = mu11 * stheta;
357  j = ((mu11 * Xg * sctheta - mu20 * Yg * sctheta + mu20 * rho * ctheta - m11yg + m11yg * ctheta2 + m02xg -
358  m02xg * ctheta2 + m11stheta * rho) /
359  (mu20 * ctheta2 + 2.0 * m11stheta * ctheta + mu02 - mu02 * ctheta2));
360  // Optimised calculation for Y
361  double rhom02 = rho * mu02;
362  double sctheta2 = stheta * ctheta2;
363  double ctheta3 = ctheta2 * ctheta;
364  i = (-(-rho * mu11 * stheta * ctheta - rhom02 + rhom02 * ctheta2 + mu11 * Xg * sctheta2 - mu20 * Yg * sctheta2 -
365  ctheta * mu11 * Yg + ctheta3 * mu11 * Yg + ctheta * mu02 * Xg - ctheta3 * mu02 * Xg) /
366  (mu20 * ctheta2 + 2.0 * mu11 * stheta * ctheta + mu02 - mu02 * ctheta2) / stheta);
367 }
void init()
Definition: vpCircle.cpp:43
double get_u0() const
static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)
perspective projection of the circle
Definition: vpCircle.cpp:239
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
error that can be emited by ViSP classes.
Definition: vpException.h:71
virtual ~vpCircle()
Definition: vpCircle.cpp:125
double get_py() const
vpColVector cP
Definition: vpTracker.h:75
void projection()
Definition: vpCircle.cpp:138
vpCircle()
Definition: vpCircle.cpp:87
double get_v0() const
static double sqr(double x)
Definition: vpMath.h:108
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:332
vpCircle * duplicate() const
for memory issue (used by the vpServo class only)
Definition: vpCircle.cpp:310
Generic class defining intrinsic camera parameters.
double get_px() const
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)
Definition: vpCircle.cpp:294
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Class that defines what is a circle.
Definition: vpCircle.h:58
vpColVector p
Definition: vpTracker.h:71
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:61
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244