Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpPoseLagrange.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 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 #define DEBUG_LEVEL1 0
37 #define DEBUG_LEVEL2 0
38 
39 /**********************************************************************/
40 /* FONCTION : CalculTranslation */
41 /* ROLE : Calcul de la translation entre la */
42 /* camera et l'outil connaissant la */
43 /* rotation */
44 /**********************************************************************/
45 
46 static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1, unsigned int nc3,
47  vpColVector &x1, vpColVector &x2)
48 {
49 
50  try {
51  unsigned int i, j;
52 
53  vpMatrix ct(3, nl);
54  for (i = 0; i < 3; i++) {
55  for (j = 0; j < nl; j++)
56  ct[i][j] = b[j][i + nc3];
57  }
58 
59  vpMatrix c;
60  c = ct.t();
61 
62  vpMatrix ctc;
63  ctc = ct * c;
64 
65  vpMatrix ctc1; // (C^T C)^(-1)
66  ctc1 = ctc.inverseByLU();
67 
68  vpMatrix cta;
69  vpMatrix ctb;
70  cta = ct * a; /* C^T A */
71  ctb = ct * b; /* C^T B */
72 
73 #if (DEBUG_LEVEL2)
74  {
75  std::cout << "ctc " << std::endl << ctc;
76  std::cout << "cta " << std::endl << cta;
77  std::cout << "ctb " << std::endl << ctb;
78  }
79 #endif
80 
81  vpColVector X2(nc3);
82  vpMatrix CTB(nc1, nc3);
83  for (i = 0; i < nc1; i++) {
84  for (j = 0; j < nc3; j++)
85  CTB[i][j] = ctb[i][j];
86  }
87 
88  for (j = 0; j < nc3; j++)
89  X2[j] = x2[j];
90 
91  vpColVector sv; // C^T A X1 + C^T B X2)
92  sv = cta * x1 + CTB * X2; // C^T A X1 + C^T B X2)
93 
94 #if (DEBUG_LEVEL2)
95  std::cout << "sv " << sv.t();
96 #endif
97 
98  vpColVector X3; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
99  X3 = -ctc1 * sv;
100 
101 #if (DEBUG_LEVEL2)
102  std::cout << "x3 " << X3.t();
103 #endif
104 
105  for (i = 0; i < nc1; i++)
106  x2[i + nc3] = X3[i];
107  }
108  catch (...) {
109 
110  // en fait il y a des dizaines de raisons qui font que cette fonction
111  // rende une erreur (matrice pas inversible, pb de memoire etc...)
112  vpERROR_TRACE(" ");
113  throw;
114  }
115 }
116 
117 //*********************************************************************
118 // FONCTION LAGRANGE :
119 // -------------------
120 // Resolution d'un systeme lineaire de la forme A x1 + B x2 = 0
121 // sous la contrainte || x1 || = 1
122 // ou A est de dimension nl x nc1 et B nl x nc2
123 //*********************************************************************
124 
125 //#define EPS 1.e-5
126 
127 static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
128 {
129 #if (DEBUG_LEVEL1)
130  std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;
131 #endif
132 
133  try {
134  unsigned int i, imin;
135 
136  vpMatrix ata; // A^T A
137  ata = a.t() * a;
138  vpMatrix btb; // B^T B
139  btb = b.t() * b;
140 
141  vpMatrix bta; // B^T A
142  bta = b.t() * a;
143 
144  vpMatrix btb1; // (B^T B)^(-1)
145 
146  /* Warning:
147  when using btb.inverseByLU() that call cv::inv(cv::DECOMP_LU) with
148  OpenCV 3.1.0 and 3.2.0 we notice that OpenCV is not able to compute the
149  inverse of the following matrix:
150 
151  btb[9,9]=
152  0.015925 0.0 0.0030866 0.00035 0.0 0.000041 0.105
153  0.0 0.0346242 0.0 0.015925 -0.0050979 0.0 0.00035
154  -0.000063 0.0 0.105 -0.0637464 0.0030866 -0.0050979 0.0032301
155  0.000041 -0.000063 0.000016 0.0346242 -0.0637464 0.0311185 0.00035
156  0.0 0.000041 0.0001 0.0 0.000012 0.01 0.0
157  0.0011594 0.0 0.00035 -0.000063 0.0 0.0001 -0.000018
158  0.0 0.01 -0.0018040 0.000041 -0.000063 0.000016 0.000012
159  -0.000018 0.000005 0.0011594 -0.0018040 0.0004599 0.105 0.0
160  0.0346242 0.01 0.0 0.0011594 5.0 0.0 0.13287
161  0.0 0.105 -0.0637464 0.0 0.01 -0.0018040 0.0
162  5.0 -0.731499 0.0346242 -0.0637464 0.0311185 0.0011594 -0.0018040
163  0.0004599 0.13287 -0.731499 0.454006
164 
165  That's why instead of using inverseByLU() we are now using pseudoInverse()
166  */
167 #if 0
168  if (b.getRows() >= b.getCols()) {
169  btb1 = btb.inverseByLU();
170  }
171  else {
172  btb1 = btb.pseudoInverse();
173  }
174 #else
175  btb1 = btb.pseudoInverse();
176 #endif
177 
178 #if (DEBUG_LEVEL1)
179  {
180  std::cout << " BTB1 * BTB : " << std::endl << btb1 * btb << std::endl;
181  std::cout << " BTB * BTB1 : " << std::endl << btb * btb1 << std::endl;
182  }
183 #endif
184 
185  vpMatrix r; // (B^T B)^(-1) B^T A
186  r = btb1 * bta;
187 
188  vpMatrix e; // - A^T B (B^T B)^(-1) B^T A
189  e = -(a.t() * b) * r;
190 
191  e += ata; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
192 
193 #if (DEBUG_LEVEL1)
194  {
195  std::cout << " E :" << std::endl << e << std::endl;
196  }
197 #endif
198 
199  // vpColVector sv ;
200  // vpMatrix v ;
201  e.svd(x1, ata); // destructif sur e
202  // calcul du vecteur propre de E correspondant a la valeur propre min.
203  /* calcul de SVmax */
204  imin = 0;
205  // FC : Pourquoi calculer SVmax ??????
206  // double svm = 0.0;
207  // for (i=0;i<x1.getRows();i++)
208  // {
209  // if (x1[i] > svm) { svm = x1[i]; imin = i; }
210  // }
211  // svm *= EPS; /* pour le rang */
212 
213  for (i = 0; i < x1.getRows(); i++)
214  if (x1[i] < x1[imin])
215  imin = i;
216 
217 #if (DEBUG_LEVEL1)
218  {
219  printf("SV(E) : %.15lf %.15lf %.15lf\n", x1[0], x1[1], x1[2]);
220  std::cout << " i_min " << imin << std::endl;
221  }
222 #endif
223  for (i = 0; i < x1.getRows(); i++)
224  x1[i] = ata[i][imin];
225 
226  x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1
227 
228 #if (DEBUG_LEVEL1)
229  {
230  std::cout << " X1 : " << x1.t() << std::endl;
231  std::cout << " V : " << std::endl << ata << std::endl;
232  }
233 #endif
234  }
235  catch (...) {
236  vpERROR_TRACE(" ");
237  throw;
238  }
239 #if (DEBUG_LEVEL1)
240  std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
241 #endif
242 }
243 
244 //#undef EPS
245 
246 void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double *p_a, double *p_b, double *p_c, double *p_d)
247 {
248 #if (DEBUG_LEVEL1)
249  std::cout << "begin vpPose::PoseLagrangePlan(...) " << std::endl;
250 #endif
251  // determination of the plane equation a X + b Y + c Z + d = 0
252  double a, b, c, d;
253 
254  // Checking if coplanar has already been called and if the plan coefficients have been given
255  if ((p_isPlan != nullptr) && (p_a != nullptr) && (p_b != nullptr) && (p_c != nullptr) && (p_d != nullptr)) {
256  if (*p_isPlan) {
257  // All the pointers towards the plan coefficients are different from nullptr => using them in the rest of the method
258  a = *p_a;
259  b = *p_b;
260  c = *p_c;
261  d = *p_d;
262  }
263  else {
264  // The call to coplanar that was performed outside vpPose::poseLagrangePlan indicated that the points are not coplanar.
265  throw vpException(vpException::fatalError, "Called vpPose::poseLagrangePlan but the call to vpPose::coplanar done outside the method indicated that the points are not coplanar");
266  }
267  }
268  else {
269  // At least one of the coefficient is a nullptr pointer => calling coplanar by ourselves
270  int coplanarType;
271  bool areCoplanar = coplanar(coplanarType, &a, &b, &c, &d);
272  if (!areCoplanar) {
273  throw vpException(vpException::fatalError, "Called vpPose::poseLagrangePlan but call to vpPose::coplanar indicates that the points are not coplanar");
274  }
275  }
276 
277  if (c < 0.0) { // imposing c >= 0
278  a = -a;
279  b = -b;
280  c = -c;
281  d = -d;
282  }
283  // to have (a,b,c) as a unit vector if it was not the case
284  double n = 1.0 / sqrt(a * a + b * b + c * c); // Not possible to have a NaN...
285  a *= n;
286  b *= n;
287  c *= n;
288  d *= n;
289  // printf("a = %lf, b = %lf, c = %lf, d = %lf\n",a,b,c,d);
290  // transformation to have object plane with equation Z = 0
291  vpColVector r1(3), r2(3), r3(3);
292 
293  r3[0] = a;
294  r3[1] = b;
295  r3[2] = c;
296  // build r1 as a unit vector orthogonal to r3
297  double n1 = sqrt(1.0 - a * a);
298  double n2 = sqrt(1.0 - b * b);
299  if (n1 >= n2) {
300  r1[0] = n1;
301  r1[1] = -a * b / n1;
302  r1[2] = -a * c / n1;
303  }
304  else {
305  r1[0] = -a * b / n2;
306  r1[1] = n2;
307  r1[2] = -b * c / n2;
308  }
309  // double norm = r1[0]*r1[0] + r1[1]*r1[1] + r1[2]*r1[2];
310  // double crossprod = r1[0]*r3[0] + r1[1]*r3[1] + r1[2]*r3[2];
311  // printf("r1 norm = 1 ? %lf, r1^T r3 = 0 ? %lf\n",norm, crossprod);
312  // r2 unit vector orthogonal to r3 and r1
313  r2 = vpColVector::crossProd(r3, r1);
314 
316  for (unsigned int i = 0; i < 3; i++) {
317  fMo[0][i] = r1[i];
318  fMo[1][i] = r2[i];
319  fMo[2][i] = r3[i];
320  }
321  fMo[0][3] = fMo[1][3] = 0.0;
322  fMo[2][3] = d;
323 
324  // std::cout << "fMo : " << std::endl << fMo << std::endl;
325  // Build and solve the system
326  unsigned int k = 0;
327  unsigned int nl = npt * 2;
328 
329  vpMatrix A(nl, 3);
330  vpMatrix B(nl, 6);
331  vpPoint P;
332 
333  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
334  P = *it;
335 
336  // Transform each point in plane Z = 0
337  vpColVector Xf, X(4);
338  X[0] = P.get_oX();
339  X[1] = P.get_oY();
340  X[2] = P.get_oZ();
341  X[3] = 1.0;
342  Xf = fMo * X;
343  // printf("Z = 0 = %lf\n",Xf[2]);
344  // build the system
345  A[k][0] = -Xf[0];
346  A[k][1] = 0.0;
347  A[k][2] = Xf[0] * P.get_x();
348 
349  A[k + 1][0] = 0.0;
350  A[k + 1][1] = -Xf[0];
351  A[k + 1][2] = Xf[0] * P.get_y();
352 
353  B[k][0] = -Xf[1];
354  B[k][1] = 0.0;
355  B[k][2] = Xf[1] * P.get_x();
356  B[k][3] = -1.0;
357  B[k][4] = 0.0;
358  B[k][5] = P.get_x();
359 
360  B[k + 1][0] = 0.0;
361  B[k + 1][1] = -Xf[1];
362  B[k + 1][2] = Xf[1] * P.get_y();
363  B[k + 1][3] = 0.0;
364  B[k + 1][4] = -1.0;
365  B[k + 1][5] = P.get_y();
366 
367  k += 2;
368  }
369  vpColVector X1(3);
370  vpColVector X2(6);
371 
372 #if (DEBUG_LEVEL2)
373  {
374  std::cout << "A " << std::endl << A << std::endl;
375  std::cout << "B " << std::endl << B << std::endl;
376  }
377 #endif
378 
379  lagrange(A, B, X1, X2);
380 
381 #if (DEBUG_LEVEL2)
382  {
383  std::cout << "A X1+B X2 (should be 0): " << (A * X1 + B * X2).t() << std::endl;
384  std::cout << " X1 norm: " << X1.sumSquare() << std::endl;
385  }
386 #endif
387 
388  if (X2[5] < 0.0) { /* to obtain Zo > 0 */
389  for (unsigned int i = 0; i < 3; i++)
390  X1[i] = -X1[i];
391  for (unsigned int i = 0; i < 6; i++)
392  X2[i] = -X2[i];
393  }
394  double s = 0.0;
395  for (unsigned int i = 0; i < 3; i++) {
396  s += (X1[i] * X2[i]);
397  }
398  for (unsigned int i = 0; i < 3; i++) {
399  X2[i] -= (s * X1[i]);
400  } /* X1^T X2 = 0 */
401 
402  // s = 0.0;
403  // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
404  s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error
405 
406  if (s < 1e-10) {
407  // std::cout << "Points that produce an error: " << std::endl;
408  // for (std::list<vpPoint>::const_iterator it = listP.begin(); it
409  // != listP.end(); ++it)
410  // {
411  // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() <<
412  // " "
413  // << (*it).get_oX() << " " << (*it).get_oY() << " " <<
414  // (*it).get_oZ() << std::endl;
415  // }
416  throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation "
417  "(planar plane case)"));
418  }
419 
420  s = 1.0 / sqrt(s);
421  for (unsigned int i = 0; i < 3; i++) {
422  X2[i] *= s;
423  } /* X2^T X2 = 1 */
424 
425  calculTranslation(A, B, nl, 3, 3, X1, X2);
426 
427  // if (err != OK)
428  {
429  // std::cout << "in (vpCalculPose_plan.cc)CalculTranslation returns " ;
430  // PrintError(err) ;
431  // return err ;
432  }
434  /* X1 x X2 */
435  cMf[0][2] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
436  cMf[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
437  cMf[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
438  /* calcul de la matrice de passage */
439  for (unsigned int i = 0; i < 3; i++) {
440  cMf[i][0] = X1[i];
441  cMf[i][1] = X2[i];
442  cMf[i][3] = X2[i + 3];
443  }
444  // std::cout << "cMf : " << std::endl << cMf << std::endl;
445 
446  // Apply the transform to go back to object frame
447  cMo = cMf * fMo;
448 
449 #if (DEBUG_LEVEL1)
450  std::cout << "end vpCalculPose::PoseLagrangePlan(...) " << std::endl;
451 #endif
452  // return(OK);
453 }
454 
456 {
457 
458 #if (DEBUG_LEVEL1)
459  std::cout << "begin CPose::PoseLagrangeNonPlan(...) " << std::endl;
460 #endif
461  try {
462  double s;
463  unsigned int i;
464 
465  unsigned int k = 0;
466  unsigned int nl = npt * 2;
467 
468  if (npt < 6) {
470  "Lagrange, non planar case, insufficient number of points %d < 6\n", npt));
471  }
472 
473  vpMatrix a(nl, 3);
474  vpMatrix b(nl, 9);
475  b = 0;
476 
477  vpPoint P;
478  i = 0;
479  for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
480  P = *it;
481  a[k][0] = -P.get_oX();
482  a[k][1] = 0.0;
483  a[k][2] = P.get_oX() * P.get_x();
484 
485  a[k + 1][0] = 0.0;
486  a[k + 1][1] = -P.get_oX();
487  a[k + 1][2] = P.get_oX() * P.get_y();
488 
489  b[k][0] = -P.get_oY();
490  b[k][1] = 0.0;
491  b[k][2] = P.get_oY() * P.get_x();
492 
493  b[k][3] = -P.get_oZ();
494  b[k][4] = 0.0;
495  b[k][5] = P.get_oZ() * P.get_x();
496 
497  b[k][6] = -1.0;
498  b[k][7] = 0.0;
499  b[k][8] = P.get_x();
500 
501  b[k + 1][0] = 0.0;
502  b[k + 1][1] = -P.get_oY();
503  b[k + 1][2] = P.get_oY() * P.get_y();
504 
505  b[k + 1][3] = 0.0;
506  b[k + 1][4] = -P.get_oZ();
507  b[k + 1][5] = P.get_oZ() * P.get_y();
508 
509  b[k + 1][6] = 0.0;
510  b[k + 1][7] = -1.0;
511  b[k + 1][8] = P.get_y();
512 
513  k += 2;
514  }
515  vpColVector X1(3);
516  vpColVector X2(9);
517 
518 #if (DEBUG_LEVEL2)
519  {
520  std::cout << "a " << a << std::endl;
521  std::cout << "b " << b << std::endl;
522  }
523 #endif
524 
525  lagrange(a, b, X1, X2);
526  // if (err != OK)
527  {
528  // std::cout << "in (CLagrange.cc)Lagrange returns " ;
529  // PrintError(err) ;
530  // return err ;
531  }
532 
533 #if (DEBUG_LEVEL2)
534  {
535  std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl;
536  std::cout << "norme X1 " << X1.sumSquare() << std::endl;
537  }
538 #endif
539 
540  if (X2[8] < 0.0) { /* car Zo > 0 */
541  X1 *= -1;
542  X2 *= -1;
543  }
544  s = 0.0;
545  for (i = 0; i < 3; i++) {
546  s += (X1[i] * X2[i]);
547  }
548  for (i = 0; i < 3; i++) {
549  X2[i] -= (s * X1[i]);
550  } /* X1^T X2 = 0 */
551 
552  // s = 0.0;
553  // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
554  s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error
555 
556  if (s < 1e-10) {
557  // std::cout << "Points that produce an error: " << std::endl;
558  // for (std::list<vpPoint>::const_iterator it = listP.begin(); it
559  // != listP.end(); ++it)
560  // {
561  // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() <<
562  // " "
563  // << (*it).get_oX() << " " << (*it).get_oY() << " " <<
564  // (*it).get_oZ() << std::endl;
565  // }
566  // vpERROR_TRACE(" division par zero " ) ;
567  throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation (non "
568  "planar plane case)"));
569  }
570 
571  s = 1.0 / sqrt(s);
572  for (i = 0; i < 3; i++) {
573  X2[i] *= s;
574  } /* X2^T X2 = 1 */
575 
576  X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
577  X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
578  X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
579 
580  calculTranslation(a, b, nl, 3, 6, X1, X2);
581 
582  for (i = 0; i < 3; i++) {
583  cMo[i][0] = X1[i];
584  cMo[i][1] = X2[i];
585  cMo[i][2] = X2[i + 3];
586  cMo[i][3] = X2[i + 6];
587  }
588 
589  }
590  catch (...) {
591  throw; // throw the original exception
592  }
593 
594 #if (DEBUG_LEVEL1)
595  std::cout << "end vpCalculPose::PoseLagrangeNonPlan(...) " << std::endl;
596 #endif
597 }
598 
599 #undef DEBUG_LEVEL1
600 #undef DEBUG_LEVEL2
unsigned int getCols() const
Definition: vpArray2D.h:274
unsigned int getRows() const
Definition: vpArray2D.h:284
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
vpRowVector t() const
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ fatalError
Fatal error.
Definition: vpException.h:84
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix inverseByLU() const
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2075
vpMatrix t() const
Definition: vpMatrix.cpp:457
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2286
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:450
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:461
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:454
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:459
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:452
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:114
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:115
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:117
#define vpERROR_TRACE
Definition: vpDebug.h:382