36 #include <visp3/core/vpCircle.h>
38 #include <visp3/core/vpFeatureDisplay.h>
159 double det_threshold = 1e-10;
175 double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
176 double det = A * X0 + B * Y0 + C * Z0;
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;
189 double det = K[2] * K[2] - K[0] * K[1];
190 if (fabs(det) < det_threshold) {
194 double xc = (K[1] * K[3] - K[2] * K[4]) / det;
195 double yc = (K[0] * K[4] - K[2] * K[3]) / det;
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]);
202 if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
205 A = sqrt(s / (K[0] + K[1] + c));
206 B = sqrt(s / (K[0] + K[1] - c));
208 A = sqrt(s / (K[0] + K[1] - c));
209 B = sqrt(s / (K[0] + K[1] + c));
212 E = (K[1] - K[0] + c) / (2 * K[2]);
214 A = sqrt(s / (K[0] + K[1] + c));
215 B = sqrt(s / (K[0] + K[1] - c));
217 A = sqrt(s / (K[0] + K[1] - c));
218 B = sqrt(s / (K[0] + K[1] + 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];
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];
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];
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];
312 unsigned int thickness)
327 unsigned int thickness)
344 const vpColor &color,
unsigned int thickness)
364 const vpColor &color,
unsigned int thickness)
397 const double &theta,
double &i,
double &j)
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;
414 double stheta = sin(theta);
415 double ctheta = cos(theta);
416 double sctheta = stheta * ctheta;
417 double m11yg = n11 * Yg;
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));
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);
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...
vpCircle * duplicate() const
For memory issue (used by the vpServo class only)
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
void setWorldCoordinates(const vpColVector &oP)
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
error that can be emitted by ViSP classes.
@ divideByZeroError
Division by zero.
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)