Visual Servoing Platform  version 3.6.1 under development (2024-11-15)
vpPoseLagrange.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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  * Pose computation.
32  */
33 
34 #include <visp3/vision/vpPose.h>
35 
36 BEGIN_VISP_NAMESPACE
37 
38 /**********************************************************************/
39 /* FONCTION : CalculTranslation */
40 /* ROLE : Calcul de la translation entre la */
41 /* camera et l'outil connaissant la */
42 /* rotation */
43 /**********************************************************************/
44 
45 static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1, unsigned int nc3,
46  vpColVector &x1, vpColVector &x2)
47 {
48  unsigned int i, j;
49  const unsigned int nbRows = 3;
50  vpMatrix ct(nbRows, nl);
51  for (i = 0; i < nbRows; ++i) {
52  for (j = 0; j < nl; ++j) {
53  ct[i][j] = b[j][i + nc3];
54  }
55  }
56 
57  vpMatrix c;
58  c = ct.t();
59 
60  vpMatrix ctc;
61  ctc = ct * c;
62 
63  vpMatrix ctc1; // (C^T C)^(-1)
64  ctc1 = ctc.inverseByLU();
65 
66  vpMatrix cta;
67  vpMatrix ctb;
68  cta = ct * a; /* C^T A */
69  ctb = ct * b; /* C^T B */
70 
71  vpColVector X2(nc3);
72  vpMatrix CTB(nc1, nc3);
73  for (i = 0; i < nc1; ++i) {
74  for (j = 0; j < nc3; ++j) {
75  CTB[i][j] = ctb[i][j];
76  }
77  }
78 
79  for (j = 0; j < nc3; ++j) {
80  X2[j] = x2[j];
81  }
82 
83  vpColVector sv; // C^T A X1 + C^T B X2)
84  sv = (cta * x1) + (CTB * X2); // C^T A X1 + C^T B X2)
85 
86  vpColVector X3; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
87  X3 = -ctc1 * sv;
88 
89  for (i = 0; i < nc1; ++i) {
90  x2[i + nc3] = X3[i];
91  }
92 }
93 
94 //*********************************************************************
95 // FONCTION LAGRANGE :
96 // -------------------
97 // Resolution d'un systeme lineaire de la forme A x1 + B x2 = 0
98 // sous la contrainte || x1 || = 1
99 // ou A est de dimension nl x nc1 et B nl x nc2
100 //*********************************************************************
101 static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
102 {
103  unsigned int i, imin;
104 
105  vpMatrix ata; // A^T A
106  ata = a.t() * a;
107  vpMatrix btb; // B^T B
108  btb = b.t() * b;
109 
110  vpMatrix bta; // B^T A
111  bta = b.t() * a;
112 
113  vpMatrix btb1; // (B^T B)^(-1)
114 
115  /* Warning:
116  when using btb.inverseByLU() that call cv::inv(cv::DECOMP_LU) with
117  OpenCV 3.1.0 and 3.2.0 we notice that OpenCV is not able to compute the
118  inverse of the following matrix:
119 
120  btb[9,9]=
121  0.015925 0.0 0.0030866 0.00035 0.0 0.000041 0.105
122  0.0 0.0346242 0.0 0.015925 -0.0050979 0.0 0.00035
123  -0.000063 0.0 0.105 -0.0637464 0.0030866 -0.0050979 0.0032301
124  0.000041 -0.000063 0.000016 0.0346242 -0.0637464 0.0311185 0.00035
125  0.0 0.000041 0.0001 0.0 0.000012 0.01 0.0
126  0.0011594 0.0 0.00035 -0.000063 0.0 0.0001 -0.000018
127  0.0 0.01 -0.0018040 0.000041 -0.000063 0.000016 0.000012
128  -0.000018 0.000005 0.0011594 -0.0018040 0.0004599 0.105 0.0
129  0.0346242 0.01 0.0 0.0011594 5.0 0.0 0.13287
130  0.0 0.105 -0.0637464 0.0 0.01 -0.0018040 0.0
131  5.0 -0.731499 0.0346242 -0.0637464 0.0311185 0.0011594 -0.0018040
132  0.0004599 0.13287 -0.731499 0.454006
133 
134  That's why instead of using inverseByLU() we are now using pseudoInverse()
135  */
136 #if 0
137  if (b.getRows() >= b.getCols()) {
138  btb1 = btb.inverseByLU();
139  }
140  else {
141  btb1 = btb.pseudoInverse();
142  }
143 #else
144  btb1 = btb.pseudoInverse();
145 #endif
146 
147  vpMatrix r; // (B^T B)^(-1) B^T A
148  r = btb1 * bta;
149 
150  vpMatrix e; // - A^T B (B^T B)^(-1) B^T A
151  e = -(a.t() * b) * r;
152 
153  e += ata; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
154 
155  e.svd(x1, ata); // destructif sur e
156  // calcul du vecteur propre de E correspondant a la valeur propre min.
157  imin = 0;
158 
159  unsigned int v_x1_rows = x1.getRows();
160  for (i = 0; i < v_x1_rows; ++i) {
161  if (x1[i] < x1[imin]) {
162  imin = i;
163  }
164  }
165 
166  unsigned int x1_rows = x1.getRows();
167  for (i = 0; i < x1_rows; ++i) {
168  x1[i] = ata[i][imin];
169  }
170 
171  x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1
172 }
173 
174 void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double *p_a, double *p_b, double *p_c, double *p_d)
175 {
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;
182 
183  // determination of the plane equation a X + b Y + c Z + d = 0
184  double a, b, c, d;
185 
186  // Checking if coplanar has already been called and if the plan coefficients have been given
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) {
190  if (*p_isPlan) {
191  // All the pointers towards the plan coefficients are different from nullptr => using them in the rest of the method
192  a = *p_a;
193  b = *p_b;
194  c = *p_c;
195  d = *p_d;
196  }
197  else {
198  // The call to coplanar that was performed outside vpPose::poseLagrangePlan indicated that the points are not coplanar.
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");
200  }
201  }
202  else {
203  // At least one of the coefficient is a nullptr pointer => calling coplanar by ourselves
204  int coplanarType;
205  bool areCoplanar = coplanar(coplanarType, &a, &b, &c, &d);
206  if (!areCoplanar) {
207  throw vpException(vpException::fatalError, "Called vpPose::poseLagrangePlan but call to vpPose::coplanar indicates that the points are not coplanar");
208  }
209  }
210 
211  if (c < 0.0) { // imposing c greater than or equal to 0
212  a = -a;
213  b = -b;
214  c = -c;
215  d = -d;
216  }
217  // to have (a,b,c) as a unit vector if it was not the case
218  double n = 1.0 / sqrt((a * a) + (b * b) + (c * c)); // Not possible to have a NaN...
219  a *= n;
220  b *= n;
221  c *= n;
222  d *= n;
223  // transformation to have object plane with equation Z = 0
224  const unsigned int size = 3;
225  vpColVector r1(size), r2(size), r3(size);
226 
227  r3[index_0] = a;
228  r3[index_1] = b;
229  r3[index_2] = c;
230  // build r1 as a unit vector orthogonal to r3
231  double n1 = sqrt(1.0 - (a * a));
232  double n2 = sqrt(1.0 - (b * b));
233  if (n1 >= n2) {
234  r1[index_0] = n1;
235  r1[index_1] = (-a * b) / n1;
236  r1[index_2] = (-a * c) / n1;
237  }
238  else {
239  r1[index_0] = (-a * b) / n2;
240  r1[index_1] = n2;
241  r1[index_2] = (-b * c) / n2;
242  }
243 
244  r2 = vpColVector::crossProd(r3, r1);
245 
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) {
250  fMo[idX][i] = r1[i];
251  fMo[idY][i] = r2[i];
252  fMo[idZ][i] = r3[i];
253  }
254  fMo[idX][idTranslation] = 0.0;
255  fMo[idY][idTranslation] = 0.0;
256  fMo[idZ][idTranslation] = d;
257 
258  // Build and solve the system
259  unsigned int k = 0;
260  unsigned int nl = npt * 2;
261 
262  const unsigned int nbColsA = 3U, nbColsB = 6U;
263  vpMatrix A(nl, nbColsA);
264  vpMatrix B(nl, nbColsB);
265  vpPoint P;
266 
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) {
271  P = *it;
272 
273  // Transform each point in plane Z = 0
274  vpColVector Xf, X(sizeHomogeneous);
275  X[idX] = P.get_oX();
276  X[idY] = P.get_oY();
277  X[idZ] = P.get_oZ();
278  X[idHomogeneous] = 1.0;
279  Xf = fMo * X;
280  // build the system
281  A[k][index_0] = -Xf[0];
282  A[k][index_1] = 0.0;
283  A[k][index_2] = Xf[0] * P.get_x();
284 
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();
288 
289  B[k][index_0] = -Xf[1];
290  B[k][index_1] = 0.0;
291  B[k][index_2] = Xf[1] * P.get_x();
292  B[k][index_3] = -1.0;
293  B[k][index_4] = 0.0;
294  B[k][index_5] = P.get_x();
295 
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();
302 
303  k += offsetk;
304  }
305  const unsigned int sizeX1 = nbColsA, sizeX2 = nbColsB, lastX2 = sizeX2 - 1; // X1 is of the size of A^T A and X2 of B^T B
306  vpColVector X1(sizeX1);
307  vpColVector X2(sizeX2);
308 
309  lagrange(A, B, X1, X2);
310 
311  if (X2[lastX2] < 0.0) { /* to obtain Zo > 0 */
312  for (unsigned int i = 0; i < sizeX1; ++i) {
313  X1[i] = -X1[i];
314  }
315 
316  for (unsigned int i = 0; i < sizeX2; ++i) {
317  X2[i] = -X2[i];
318  }
319  }
320  double s = 0.0;
321  for (unsigned int i = 0; i < sizeX1; ++i) {
322  s += (X1[i] * X2[i]);
323  }
324  for (unsigned int i = 0; i < sizeX1; ++i) {
325  X2[i] -= (s * X1[i]);
326  } /* X1^T X2 = 0 */
327 
328  // --comment: s equals 0.0
329  s = (X2[index_0] * X2[index_0]) + (X2[index_1] * X2[index_1]) + (X2[index_2] * X2[index_2]); // To avoid a Coverity copy/past error
330 
331  if (s < 1e-10) {
332  throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation "
333  "(planar plane case)"));
334  }
335 
336  s = 1.0 / sqrt(s);
337  const unsigned int val_3 = 3, nc1 = 3, nc3 = 3;
338  for (unsigned int i = 0; i < val_3; ++i) {
339  X2[i] *= s;
340  } /* X2^T X2 is equal to 1 */
341 
342  calculTranslation(A, B, nl, nc1, nc3, X1, X2);
343 
345  /* X1 x 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]);
349  /* calcul de la matrice de passage */
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];
354  }
355 
356  // Apply the transform to go back to object frame
357  cMo = cMf * fMo;
358 }
359 
361 {
362  try {
363  double s;
364  unsigned int i;
365 
366  unsigned int k = 0;
367  const unsigned int twice = 2;
368  unsigned int nl = npt * twice;
369  const unsigned int npt_min = 6;
370 
371  if (npt < npt_min) {
373  "Lagrange, non planar case, insufficient number of points %d < 6\n", npt));
374  }
375 
376  const unsigned int nbColsA = 3, nbColsB = 9;
377  vpMatrix a(nl, nbColsA);
378  vpMatrix b(nl, nbColsB);
379  b = 0;
380 
381  vpPoint P;
382  i = 0;
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) {
389  P = *it;
390  a[k][id0] = -P.get_oX();
391  a[k][id1] = 0.0;
392  a[k][id2] = P.get_oX() * P.get_x();
393 
394  a[k + 1][id0] = 0.0;
395  a[k + 1][id1] = -P.get_oX();
396  a[k + 1][id2] = P.get_oX() * P.get_y();
397 
398  b[k][id0] = -P.get_oY();
399  b[k][id1] = 0.0;
400  b[k][id2] = P.get_oY() * P.get_x();
401 
402  b[k][id3] = -P.get_oZ();
403  b[k][id4] = 0.0;
404  b[k][id5] = P.get_oZ() * P.get_x();
405 
406  b[k][id6] = -1.0;
407  b[k][id7] = 0.0;
408  b[k][id8] = P.get_x();
409 
410  b[k + 1][id0] = 0.0;
411  b[k + 1][id1] = -P.get_oY();
412  b[k + 1][id2] = P.get_oY() * P.get_y();
413 
414  b[k + 1][id3] = 0.0;
415  b[k + 1][id4] = -P.get_oZ();
416  b[k + 1][id5] = P.get_oZ() * P.get_y();
417 
418  b[k + 1][id6] = 0.0;
419  b[k + 1][id7] = -1.0;
420  b[k + 1][id8] = P.get_y();
421 
422  k += offsetk;
423  }
424  vpColVector X1(nbColsA); // X1 is of size A^T A
425  vpColVector X2(nbColsB); // X2 is of size B^T B
426 
427  lagrange(a, b, X1, X2);
428 
429  if (X2[id8] < 0.0) { /* because Zo greater than 0 */
430  X1 *= -1;
431  X2 *= -1;
432  }
433  s = 0.0;
434  for (i = 0; i < nbColsA; ++i) {
435  s += (X1[i] * X2[i]);
436  }
437  for (i = 0; i < nbColsA; ++i) {
438  X2[i] -= (s * X1[i]);
439  } /* X1^T X2 is null */
440 
441  s = (X2[id0] * X2[id0]) + (X2[id1] * X2[id1]) + (X2[id2] * X2[id2]); // To avoid a Coverity copy/past error
442 
443  if (s < 1e-10) {
444  throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation (non "
445  "planar plane case)"));
446  }
447 
448  s = 1.0 / sqrt(s);
449  const unsigned int istop = 3;
450  for (i = 0; i < istop; ++i) {
451  X2[i] *= s;
452  } /* X2^T X2 = 1 */
453 
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]);
457 
458  const unsigned int nc1 = 3, nc3 = 6;
459  calculTranslation(a, b, nl, nc1, nc3, X1, X2);
460 
461  for (i = 0; i < id3; ++i) {
462  cMo[i][id0] = X1[i];
463  cMo[i][id1] = X2[i];
464  cMo[i][id2] = X2[i + id3];
465  cMo[i][id3] = X2[i + id6];
466  }
467 
468  }
469  catch (...) {
470  throw; // throw the original exception
471  }
472 }
473 
474 END_VISP_NAMESPACE
unsigned int getCols() const
Definition: vpArray2D.h:337
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix inverseByLU() const
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpMatrix t() const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:411
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:422
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:415
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:420
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:413
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:113
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)
Definition: vpPose.h:114
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)
Definition: vpPose.cpp:120