Visual Servoing Platform  version 3.0.0
vpCircle.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Visual feature circle.
32  *
33  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
39 #include <visp3/core/vpCircle.h>
40 
41 #include <visp3/core/vpFeatureDisplay.h>
42 
43 void
45 {
46 
47  oP.resize(7) ;
48  cP.resize(7) ;
49 
50  p.resize(5) ;
51 }
52 
61 void
63 {
64  this->oP = oP_ ;
65 }
66 
79 void
80 vpCircle::setWorldCoordinates(const double A, const double B, const double C,
81  const double X0, const double Y0, const double Z0,
82  const double R)
83 {
84  oP[0] = A ;
85  oP[1] = B ;
86  oP[2] = C ;
87  oP[3] = X0 ;
88  oP[4] = Y0 ;
89  oP[5] = Z0 ;
90  oP[6] = R ;
91 }
92 
93 
94 
96 {
97  init() ;
98 }
99 
110 {
111  init() ;
112  setWorldCoordinates(oP_) ;
113 }
114 
128 vpCircle::vpCircle(const double A, const double B,
129  const double C,
130  const double X0, const double Y0,
131  const double Z0,
132  const double R)
133 {
134  init() ;
135  setWorldCoordinates(A, B, C,
136  X0, Y0, Z0,
137  R) ;
138 }
139 
141 {
142 }
143 
144 
145 
146 
154 void
156 {
157  projection(cP,p) ;
158 }
159 
172 void
174 {
175  vpColVector K(6) ;
176  {
177  double A = cP_[0] ;
178  double B = cP_[1] ;
179  double C = cP_[2] ;
180 
181  double X0 = cP_[3] ;
182  double Y0 = cP_[4] ;
183  double Z0 = cP_[5] ;
184 
185  double r = cP_[6];
186 
187  // projection
188  double s = X0*X0 + Y0*Y0 + Z0*Z0 - r*r ;
189  double det = A*X0+B*Y0+C*Z0;
190  A = A/det ;
191  B = B/det ;
192  C = C/det ;
193 
194  K[0] = 1 - 2*A*X0 + A*A*s;
195  K[1] = 1 - 2*B*Y0 + B*B*s;
196  K[2] = -A*Y0 - B*X0 + A*B*s;
197  K[3] = -C*X0 - A*Z0 + A*C*s;
198  K[4] = -C*Y0 - B*Z0 + B*C*s;
199  K[5] = 1 - 2*C*Z0 + C*C*s;
200 
201  }
202 
203 // {
204 // std::cout << "K dans vpCircle::projection(): " << std::endl;
205 // for (unsigned int i=1; i<6; i++)
206 // std::cout << K[i]/K[0] << std::endl;
207 // }
208  double det = K[2]*K[2] -K[0]*K[1];
209  if (fabs(det) < 1e-8)
210  {
211  vpERROR_TRACE("division par 0") ;
213  "division par 0")) ;
214  }
215 
216  double xc = (K[1]*K[3]-K[2]*K[4])/det;
217  double yc = (K[0]*K[4]-K[2]*K[3])/det;
218 
219  double c = sqrt( (K[0]-K[1])*(K[0]-K[1]) + 4*K[2]*K[2] );
220  double s = 2*(K[0]*xc*xc + 2*K[2]*xc*yc + K[1]*yc*yc - K[5]);
221 
222  double A,B,E ;
223 
224  //if (fabs(K[2])<1e-6)
225  if (fabs(K[2])<std::numeric_limits<double>::epsilon())
226  {
227  E = 0.0;
228  if (K[0] > K[1])
229  {
230  A = sqrt(s/(K[0] + K[1] + c));
231  B = sqrt(s/(K[0] + K[1] - c));
232  }
233  else
234  {
235  A = sqrt(s/(K[0] + K[1] - c));
236  B = sqrt(s/(K[0] + K[1] + c));
237  }
238  }
239  else
240  {
241  E = (K[1] - K[0] + c)/(2*K[2]);
242  if ( fabs(E) > 1.0)
243  {
244  A = sqrt(s/(K[0] + K[1] + c));
245  B = sqrt(s/(K[0] + K[1] - c));
246  }
247  else
248  {
249  A = sqrt(s/(K[0] + K[1] - c));
250  B = sqrt(s/(K[0] + K[1] + c));
251  E = -1.0/E;
252  }
253  }
254 
255  det = (1.0 + vpMath::sqr(E));
256  double mu20 = (vpMath::sqr(A) + vpMath::sqr(B*E)) /det ;
257  double mu11 = (vpMath::sqr(A) - vpMath::sqr(B)) *E / det ;
258  double mu02 = (vpMath::sqr(B) + vpMath::sqr(A*E)) / det ;
259 
260  p_[0] = xc ;
261  p_[1] = yc ;
262  p_[2] = mu20 ;
263  p_[3] = mu11 ;
264  p_[4] = mu02 ;
265 }
266 
268 void
270 {
271 
272  double A,B,C ;
273  A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
274  B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
275  C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
276 
277  double X0,Y0,Z0 ;
278  X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
279  Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
280  Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
281  double R = oP[6] ;
282 
283  cP_[0] = A ;
284  cP_[1] = B ;
285  cP_[2] = C ;
286 
287  cP_[3] = X0 ;
288  cP_[4] = Y0 ;
289  cP_[5] = Z0 ;
290 
291  cP_[6] = R ;
292 
293  // vpTRACE("_cP :") ; std::cout << _cP.t() ;
294 
295 }
296 
298 void
300 {
301 
302  double A,B,C ;
303  A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
304  B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
305  C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
306 
307  double X0,Y0,Z0 ;
308  X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
309  Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
310  Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
311  double R = oP[6] ;
312 
313  cP[0] = A ;
314  cP[1] = B ;
315  cP[2] = C ;
316 
317  cP[3] = X0 ;
318  cP[4] = Y0 ;
319  cP[5] = Z0 ;
320 
321  cP[6] = R ;
322 
323  // vpTRACE("_cP :") ; std::cout << _cP.t() ;
324 
325 }
326 
328  const vpCameraParameters &cam,
329  const vpColor &color,
330  const unsigned int thickness)
331 {
332  vpFeatureDisplay::displayEllipse(p[0],p[1],p[2],p[3], p[4],
333  cam, I, color, thickness) ;
334 }
335 
336 // non destructive wrt. cP and p
338  const vpHomogeneousMatrix &cMo,
339  const vpCameraParameters &cam,
340  const vpColor &color,
341  const unsigned int thickness)
342 {
343  vpColVector _cP, _p ;
344  changeFrame(cMo,_cP) ;
345  projection(_cP,_p) ;
346  vpFeatureDisplay::displayEllipse(_p[0],_p[1],_p[2],_p[3], _p[4],
347  cam, I, color, thickness) ;
348 
349 }
352 {
353  vpCircle *feature = new vpCircle(*this) ;
354  return feature ;
355 }
356 
372 void
373 vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
374 {
375  // This was taken from the code of art-v1. (from the artCylinder class)
376  double px = cam.get_px() ;
377  double py = cam.get_py() ;
378  double u0 = cam.get_u0() ;
379  double v0 = cam.get_v0() ;
380 
381  double mu11 = circle.p[3];
382  double mu02 = circle.p[4];
383  double mu20 = circle.p[2];
384  double Xg = u0 + circle.p[0]*px;
385  double Yg = v0 + circle.p[1]*py;
386 
387  // Find Intersection between line and ellipse in the image.
388 
389  // Optimised calculation for X
390  double stheta = sin(theta);
391  double ctheta = cos(theta);
392  double sctheta = stheta*ctheta;
393  double m11yg = mu11*Yg;
394  double ctheta2 = vpMath::sqr(ctheta);
395  double m02xg = mu02*Xg;
396  double m11stheta = mu11*stheta;
397  j = ((mu11*Xg*sctheta-mu20*Yg*sctheta+mu20*rho*ctheta
398  -m11yg+m11yg*ctheta2+m02xg-m02xg*ctheta2+
399  m11stheta*rho)/(mu20*ctheta2+2.0*m11stheta*ctheta
400  +mu02-mu02*ctheta2));
401  //Optimised calculation for Y
402  double rhom02 = rho*mu02;
403  double sctheta2 = stheta*ctheta2;
404  double ctheta3 = ctheta2*ctheta;
405  i = (-(-rho*mu11*stheta*ctheta-rhom02+rhom02*ctheta2
406  +mu11*Xg*sctheta2-mu20*Yg*sctheta2-ctheta*mu11*Yg
407  +ctheta3*mu11*Yg+ctheta*mu02*Xg-ctheta3*mu02*Xg)/
408  (mu20*ctheta2+2.0*mu11*stheta*ctheta+mu02-
409  mu02*ctheta2)/stheta);
410 }
void init()
Definition: vpCircle.cpp:44
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:269
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
error that can be emited by ViSP classes.
Definition: vpException.h:73
virtual ~vpCircle()
Definition: vpCircle.cpp:140
double get_py() const
vpColVector cP
Definition: vpTracker.h:77
void projection()
Definition: vpCircle.cpp:155
vpCircle()
Definition: vpCircle.cpp:95
double get_v0() const
static double sqr(double x)
Definition: vpMath.h:110
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Definition: vpCircle.cpp:373
vpCircle * duplicate() const
for memory issue (used by the vpServo class only)
Definition: vpCircle.cpp:351
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:327
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Class that defines what is a circle.
Definition: vpCircle.h:57
vpColVector p
Definition: vpTracker.h:73
void setWorldCoordinates(const vpColVector &oP)
Definition: vpCircle.cpp:62
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217