39 #include <visp3/core/vpLine.h>
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpFeatureDisplay.h>
86 const double &oA2,
const double &oB2,
const double &oC2,
const double &oD2)
154 for (
unsigned int i = 0; i < 4; i++) {
224 double A1, A2, B1, B2, C1, C2, D1, D2;
237 a = A2 * D1 - A1 * D2;
238 b = B2 * D1 - B1 * D2;
239 c = C2 * D1 - C1 * D2;
243 printf(
"Degenerate case: the image of the straight line is a point!\n");
249 double theta = atan2(b, a);
337 double a1, a2, b1, b2, c1, c2, d1, d2;
338 double A1, A2, B1, B2, C1, C2, D1, D2;
353 A1 = cMo[0][0] * a1 + cMo[0][1] * b1 + cMo[0][2] * c1;
354 B1 = cMo[1][0] * a1 + cMo[1][1] * b1 + cMo[1][2] * c1;
355 C1 = cMo[2][0] * a1 + cMo[2][1] * b1 + cMo[2][2] * c1;
356 D1 = d1 - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1);
358 A2 = cMo[0][0] * a2 + cMo[0][1] * b2 + cMo[0][2] * c2;
359 B2 = cMo[1][0] * a2 + cMo[1][1] * b2 + cMo[1][2] * c2;
360 C2 = cMo[2][0] * a2 + cMo[2][1] * b2 + cMo[2][2] * c2;
361 D2 = d2 - (cMo[0][3] * A2 + cMo[1][3] * B2 + cMo[2][3] * C2);
373 a2 = B1 * C2 - C1 * B2;
374 b2 = C1 * A2 - A1 * C2;
375 c2 = A1 * B2 - B1 * A2;
378 a1 = A2 * D1 - A1 * D2;
379 b1 = B2 * D1 - B1 * D2;
380 c1 = C2 * D1 - C1 * D2;
382 if (fabs(D2) < fabs(D1))
391 d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1);
392 cP_[0] = A1 = a1 * d1;
393 cP_[1] = B1 = b1 * d1;
394 cP_[2] = C1 = c1 * d1;
399 a1 = b2 * C1 - c2 * B1;
400 b1 = c2 * A1 - a2 * C1;
401 c1 = a2 * B1 - b2 * A1;
404 d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1);
410 D2 /= (A2 * a1 + B2 * b1 + C2 * c1);
469 unsigned int thickness)
497 const vpColor &color,
unsigned int thickness)
unsigned int getRows() const
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionnalities.
error that can be emited by ViSP classes.
@ dimensionError
Bad dimension.
static void displayLine(double rho, double theta, 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.
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
vpLine * duplicate() const
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)