40 #include <visp3/core/vpLine.h>
42 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpFeatureDisplay.h>
55 const unsigned int val_2 = 2;
56 const unsigned int val_8 = 8;
84 const double &oA2,
const double &oB2,
const double &oC2,
const double &oD2)
86 const unsigned int index_0 = 0;
87 const unsigned int index_1 = 1;
88 const unsigned int index_2 = 2;
89 const unsigned int index_3 = 3;
90 const unsigned int index_4 = 4;
91 const unsigned int index_5 = 5;
92 const unsigned int index_6 = 6;
93 const unsigned int index_7 = 7;
125 const unsigned int val_8 = 8;
155 const unsigned int val_4 = 4;
162 for (
unsigned int i = 0; i < val_4; ++i) {
164 oP[i + val_4] = oP2[i];
223 const unsigned int val_2 = 2;
224 const unsigned int val_8 = 8;
231 double A1, A2, B1, B2, C1, C2, D1, D2;
232 const unsigned int index_0 = 0;
233 const unsigned int index_1 = 1;
234 const unsigned int index_2 = 2;
235 const unsigned int index_3 = 3;
236 const unsigned int index_4 = 4;
237 const unsigned int index_5 = 5;
238 const unsigned int index_6 = 6;
239 const unsigned int index_7 = 7;
252 a = (A2 * D1) - (A1 * D2);
253 b = (B2 * D1) - (B1 * D2);
254 c = (C2 * D1) - (C1 * D2);
255 s = (a * a) + (b * b);
258 printf(
"Degenerate case: the image of the straight line is a point!\n");
264 double theta = atan2(b, a);
350 const unsigned int val_8 = 8;
353 double a1, a2, b1, b2, c1, c2, d1, d2;
354 double A1, A2, B1, B2, C1, C2, D1, D2;
355 const unsigned int index_0 = 0;
356 const unsigned int index_1 = 1;
357 const unsigned int index_2 = 2;
358 const unsigned int index_3 = 3;
359 const unsigned int index_4 = 4;
360 const unsigned int index_5 = 5;
361 const unsigned int index_6 = 6;
362 const unsigned int index_7 = 7;
377 A1 = (cMo[index_0][0] * a1) + (cMo[index_0][1] * b1) + (cMo[index_0][index_2] * c1);
378 B1 = (cMo[index_1][0] * a1) + (cMo[index_1][1] * b1) + (cMo[index_1][index_2] * c1);
379 C1 = (cMo[index_2][0] * a1) + (cMo[index_2][1] * b1) + (cMo[index_2][index_2] * c1);
380 D1 = d1 - ((cMo[index_0][index_3] * A1) + (cMo[index_1][index_3] * B1) + (cMo[index_2][index_3] * C1));
382 A2 = (cMo[index_0][0] * a2) + (cMo[index_0][1] * b2) + (cMo[index_0][index_2] * c2);
383 B2 = (cMo[index_1][0] * a2) + (cMo[index_1][1] * b2) + (cMo[index_1][index_2] * c2);
384 C2 = (cMo[index_2][0] * a2) + (cMo[index_2][1] * b2) + (cMo[index_2][index_2] * c2);
385 D2 = d2 - ((cMo[index_0][index_3] * A2) + (cMo[index_1][index_3] * B2) + (cMo[index_2][index_3] * C2));
390 a2 = (B1 * C2) - (C1 * B2);
391 b2 = (C1 * A2) - (A1 * C2);
392 c2 = (A1 * B2) - (B1 * A2);
395 a1 = (A2 * D1) - (A1 * D2);
396 b1 = (B2 * D1) - (B1 * D2);
397 c1 = (C2 * D1) - (C1 * D2);
399 if (fabs(D2) < fabs(D1))
408 d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
420 a1 = (b2 * C1) - (c2 * B1);
421 b1 = (c2 * A1) - (a2 * C1);
422 c1 = (a2 * B1) - (b2 * A1);
425 d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1));
431 D2 /= ((A2 * a1) + (B2 * b1) + (C2 * c1));
467 unsigned int thickness)
488 unsigned int thickness)
516 const vpColor &color,
unsigned int thickness)
552 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 functionalities.
error that can be emitted 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 projection() VP_OVERRIDE
vpLine * duplicate() const VP_OVERRIDE
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) VP_OVERRIDE
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)