34 #include <visp3/vision/vpPose.h>
45 static void calculTranslation(
vpMatrix &a,
vpMatrix &b,
unsigned int nl,
unsigned int nc1,
unsigned int nc3,
49 const unsigned int nbRows = 3;
51 for (i = 0; i < nbRows; ++i) {
52 for (j = 0; j < nl; ++j) {
53 ct[i][j] = b[j][i + nc3];
73 for (i = 0; i < nc1; ++i) {
74 for (j = 0; j < nc3; ++j) {
75 CTB[i][j] = ctb[i][j];
79 for (j = 0; j < nc3; ++j) {
84 sv = (cta * x1) + (CTB * X2);
89 for (i = 0; i < nc1; ++i) {
103 unsigned int i, imin;
151 e = -(a.
t() * b) * r;
159 unsigned int v_x1_rows = x1.
getRows();
160 for (i = 0; i < v_x1_rows; ++i) {
161 if (x1[i] < x1[imin]) {
166 unsigned int x1_rows = x1.
getRows();
167 for (i = 0; i < x1_rows; ++i) {
168 x1[i] = ata[i][imin];
176 const unsigned int index_0 = 0;
177 const unsigned int index_1 = 1;
178 const unsigned int index_2 = 2;
179 const unsigned int index_3 = 3;
180 const unsigned int index_4 = 4;
181 const unsigned int index_5 = 5;
187 bool p_isplan_and_p_a_no_null = (p_isPlan !=
nullptr) && (p_a !=
nullptr);
188 bool p_b_p_c_p_d_no_null = (p_b !=
nullptr) && (p_c !=
nullptr) && (p_d !=
nullptr);
189 if (p_isplan_and_p_a_no_null && p_b_p_c_p_d_no_null) {
199 throw vpException(
vpException::fatalError,
"Called vpPose::poseLagrangePlan but the call to vpPose::coplanar done outside the method indicated that the points are not coplanar");
205 bool areCoplanar =
coplanar(coplanarType, &a, &b, &c, &d);
218 double n = 1.0 / sqrt((a * a) + (b * b) + (c * c));
224 const unsigned int size = 3;
231 double n1 = sqrt(1.0 - (a * a));
232 double n2 = sqrt(1.0 - (b * b));
235 r1[index_1] = (-a * b) / n1;
236 r1[index_2] = (-a * c) / n1;
239 r1[index_0] = (-a * b) / n2;
241 r1[index_2] = (-b * c) / n2;
247 const unsigned int sizeRotation = 3;
248 const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3;
249 for (
unsigned int i = 0; i < sizeRotation; ++i) {
254 fMo[idX][idTranslation] = 0.0;
255 fMo[idY][idTranslation] = 0.0;
256 fMo[idZ][idTranslation] = d;
260 unsigned int nl =
npt * 2;
262 const unsigned int nbColsA = 3U, nbColsB = 6U;
267 std::list<vpPoint>::const_iterator listp_end =
listP.end();
268 const unsigned int idHomogeneous = 3U, sizeHomogeneous = 4U;
269 const unsigned int offsetk = 2U;
270 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
278 X[idHomogeneous] = 1.0;
281 A[k][index_0] = -Xf[0];
283 A[k][index_2] = Xf[0] * P.
get_x();
285 A[k + 1][index_0] = 0.0;
286 A[k + 1][index_1] = -Xf[0];
287 A[k + 1][index_2] = Xf[0] * P.
get_y();
289 B[k][index_0] = -Xf[1];
291 B[k][index_2] = Xf[1] * P.
get_x();
292 B[k][index_3] = -1.0;
294 B[k][index_5] = P.
get_x();
296 B[k + 1][index_0] = 0.0;
297 B[k + 1][index_1] = -Xf[1];
298 B[k + 1][index_2] = Xf[1] * P.
get_y();
299 B[k + 1][index_3] = 0.0;
300 B[k + 1][index_4] = -1.0;
301 B[k + 1][index_5] = P.
get_y();
305 const unsigned int sizeX1 = nbColsA, sizeX2 = nbColsB, lastX2 = sizeX2 - 1;
309 lagrange(A, B, X1, X2);
311 if (X2[lastX2] < 0.0) {
312 for (
unsigned int i = 0; i < sizeX1; ++i) {
316 for (
unsigned int i = 0; i < sizeX2; ++i) {
321 for (
unsigned int i = 0; i < sizeX1; ++i) {
322 s += (X1[i] * X2[i]);
324 for (
unsigned int i = 0; i < sizeX1; ++i) {
325 X2[i] -= (s * X1[i]);
329 s = (X2[index_0] * X2[index_0]) + (X2[index_1] * X2[index_1]) + (X2[index_2] * X2[index_2]);
333 "(planar plane case)"));
337 const unsigned int val_3 = 3, nc1 = 3, nc3 = 3;
338 for (
unsigned int i = 0; i < val_3; ++i) {
342 calculTranslation(A, B, nl, nc1, nc3, X1, X2);
346 cMf[index_0][index_2] = (X1[index_1] * X2[index_2]) - (X1[index_2] * X2[index_1]);
347 cMf[index_1][index_2] = (X1[index_2] * X2[index_0]) - (X1[index_0] * X2[index_2]);
348 cMf[index_2][index_2] = (X1[index_0] * X2[index_1]) - (X1[index_1] * X2[index_0]);
350 for (
unsigned int i = 0; i < val_3; ++i) {
351 cMf[i][index_0] = X1[i];
352 cMf[i][index_1] = X2[i];
353 cMf[i][index_3] = X2[i + index_3];
367 const unsigned int twice = 2;
368 unsigned int nl =
npt * twice;
369 const unsigned int npt_min = 6;
373 "Lagrange, non planar case, insufficient number of points %d < 6\n",
npt));
376 const unsigned int nbColsA = 3, nbColsB = 9;
383 std::list<vpPoint>::const_iterator listp_end =
listP.end();
384 const unsigned int id0 = 0, id1 = 1, id2 = 2;
385 const unsigned int id3 = 3, id4 = 4, id5 = 5;
386 const unsigned int id6 = 6, id7 = 7, id8 = 8;
387 const unsigned int offsetk = 2U;
388 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
395 a[k + 1][id1] = -P.
get_oX();
408 b[k][id8] = P.
get_x();
411 b[k + 1][id1] = -P.
get_oY();
415 b[k + 1][id4] = -P.
get_oZ();
419 b[k + 1][id7] = -1.0;
420 b[k + 1][id8] = P.
get_y();
427 lagrange(a, b, X1, X2);
434 for (i = 0; i < nbColsA; ++i) {
435 s += (X1[i] * X2[i]);
437 for (i = 0; i < nbColsA; ++i) {
438 X2[i] -= (s * X1[i]);
441 s = (X2[id0] * X2[id0]) + (X2[id1] * X2[id1]) + (X2[id2] * X2[id2]);
445 "planar plane case)"));
449 const unsigned int istop = 3;
450 for (i = 0; i < istop; ++i) {
454 X2[id3] = (X1[id1] * X2[id2]) - (X1[id2] * X2[id1]);
455 X2[id4] = (X1[id2] * X2[id0]) - (X1[id0] * X2[id2]);
456 X2[id5] = (X1[id0] * X2[id1]) - (X1[id1] * X2[id0]);
458 const unsigned int nc1 = 3, nc3 = 6;
459 calculTranslation(a, b, nl, nc1, nc3, X1, X2);
461 for (i = 0; i < id3; ++i) {
464 cMo[i][id2] = X2[i + id3];
465 cMo[i][id3] = X2[i + id6];
unsigned int getCols() const
unsigned int getRows() const
Implementation of column vector and the associated operations.
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
@ divideByZeroError
Division by zero.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
vpMatrix inverseByLU() const
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
double get_y() const
Get the point y coordinate in the image plane.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
double get_oY() const
Get the point oY coordinate in the object frame.
unsigned int npt
Number of point used in pose computation.
void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan=nullptr, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
bool coplanar(int &coplanar_plane_type, double *p_a=nullptr, double *p_b=nullptr, double *p_c=nullptr, double *p_d=nullptr)