Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPoseDementhon.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/core/vpMath.h>
35#include <visp3/vision/vpPose.h>
36
37#define DEBUG_LEVEL1 0
38#define DEBUG_LEVEL2 0
39#define DEBUG_LEVEL3 0
40
41#define SEUIL_RESIDUAL 0.0001 /* avant 0.01 dans while du calculArbreDementhon */
42#define EPS_DEM 0.001
43
44#define ITER_MAX 30 /* max number of iterations for Demonthon's loop */
45
46static void calculSolutionDementhon(vpColVector &I4, vpColVector &J4, vpHomogeneousMatrix &cMo)
47{
48
49#if (DEBUG_LEVEL1)
50 std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
51#endif
52
53 // norm of the 3 first components of I4 and J4
54 double normI3 = sqrt(I4[0] * I4[0] + I4[1] * I4[1] + I4[2] * I4[2]);
55 double normJ3 = sqrt(J4[0] * J4[0] + J4[1] * J4[1] + J4[2] * J4[2]);
56
57 if ((normI3 < 1e-10) || (normJ3 < 1e-10)) {
58 // vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
60 "Division by zero in Dementhon pose computation: normI or normJ = 0"));
61 }
62
63 double Z0 = 2.0 / (normI3 + normJ3);
64
65 vpColVector I3(3), J3(3), K3(3);
66 for (unsigned int i = 0; i < 3; i++) {
67 I3[i] = I4[i] / normI3;
68 J3[i] = J4[i] / normJ3;
69 }
70
71 K3 = vpColVector::cross(I3, J3); // k = I x J
72 K3.normalize();
73
74 J3 = vpColVector::cross(K3, I3);
75
76 // calcul de la matrice de passage
77 cMo[0][0] = I3[0];
78 cMo[0][1] = I3[1];
79 cMo[0][2] = I3[2];
80 cMo[0][3] = I4[3] * Z0;
81
82 cMo[1][0] = J3[0];
83 cMo[1][1] = J3[1];
84 cMo[1][2] = J3[2];
85 cMo[1][3] = J4[3] * Z0;
86
87 cMo[2][0] = K3[0];
88 cMo[2][1] = K3[1];
89 cMo[2][2] = K3[2];
90 cMo[2][3] = Z0;
91
92#if (DEBUG_LEVEL1)
93 std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
94#endif
95}
96
103{
104 vpPoint P;
105 double cdg[3];
106 /* compute the cog of the 3D points */
107 cdg[0] = cdg[1] = cdg[2] = 0.0;
108 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
109 P = (*it);
110 cdg[0] += P.get_oX();
111 cdg[1] += P.get_oY();
112 cdg[2] += P.get_oZ();
113 }
114 for (unsigned int i = 0; i < 3; i++)
115 cdg[i] /= npt;
116 // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
117
118 c3d.clear();
119 /* translate the 3D points wrt cog */
120 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
121 P = (*it);
122 P.set_oX(P.get_oX() - cdg[0]);
123 P.set_oY(P.get_oY() - cdg[1]);
124 P.set_oZ(P.get_oZ() - cdg[2]);
125 c3d.push_back(P);
126 }
127
128 vpMatrix A(npt, 4), Ap;
129
130 for (unsigned int i = 0; i < npt; i++) {
131 A[i][0] = c3d[i].get_oX();
132 A[i][1] = c3d[i].get_oY();
133 A[i][2] = c3d[i].get_oZ();
134 A[i][3] = 1.0;
135 }
137
138#if (DEBUG_LEVEL2)
139 {
140 std::cout << "A" << std::endl << A << std::endl;
141 std::cout << "A^+" << std::endl << Ap << std::endl;
142 }
143#endif
144
145 vpColVector xprim(npt);
146 vpColVector yprim(npt);
147
148 for (unsigned int i = 0; i < npt; i++) {
149 xprim[i] = c3d[i].get_x();
150 yprim[i] = c3d[i].get_y();
151 }
152 vpColVector I4(4), J4(4);
153
154 I4 = Ap * xprim;
155 J4 = Ap * yprim;
156
157 calculSolutionDementhon(I4, J4, cMo);
158
159 int erreur = 0;
160 for (unsigned int i = 0; i < npt; i++) {
161 double z;
162 z = cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ() + cMo[2][3];
163 if (z <= 0.0)
164 erreur = -1;
165 }
166 if (erreur == -1) {
167 throw(vpException(vpException::fatalError, "End of Dementhon since z < 0 for both solutions at the beginning"));
168 }
169 int cpt = 0;
170 double res = sqrt(computeResidualDementhon(cMo) / npt);
171 double res_old = 2.0 * res;
172
173 // In his paper, Dementhon suggests to use the difference
174 // between 2 successive norm of eps. We prefer to use the mean of the
175 // residuals in the image
176 while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
177
178 vpHomogeneousMatrix cMo_old;
179 res_old = res;
180 cMo_old = cMo;
181
182 for (unsigned int i = 0; i < npt; i++) {
183 double eps =
184 (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
185
186 xprim[i] = (1.0 + eps) * c3d[i].get_x();
187 yprim[i] = (1.0 + eps) * c3d[i].get_y();
188 }
189 I4 = Ap * xprim;
190 J4 = Ap * yprim;
191
192 calculSolutionDementhon(I4, J4, cMo);
193 res = sqrt(computeResidualDementhon(cMo) / npt);
194 for (unsigned int i = 0; i < npt; i++) {
195 double z;
196 z = cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ() + cMo[2][3];
197 if (z <= 0.0)
198 erreur = -1;
199 }
200 if (erreur == -1) {
201 cMo = cMo_old;
202 res = res_old; // to leave the while loop
203#if (DEBUG_LEVEL3)
204 std::cout << "Pb z < 0 with cMo in Dementhon's loop" << std::endl;
205#endif
206 }
207#if (DEBUG_LEVEL3)
208 vpThetaUVector erc;
209 cMo.extract(erc);
210 std::cout << "it = " << cpt << " residu = " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
211 << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
212#endif
213 if (res > res_old) {
214#if (DEBUG_LEVEL3)
215 std::cout << "Divergence : res = " << res << " res_old = " << res_old << std::endl;
216#endif
217 cMo = cMo_old;
218 }
219 cpt++;
220 }
221 // go back to the initial frame
222 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
223 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
224 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
225}
226
227static void calculRTheta(double s, double c, double &r, double &theta)
228{
229 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
230 r = sqrt(sqrt(s * s + c * c));
231 theta = atan2(s, c) / 2.0;
232 } else {
233 if (fabs(c) > fabs(s)) {
234 r = fabs(c);
235 if (c >= 0.0)
236 theta = M_PI / 2;
237 else
238 theta = -M_PI / 2;
239 } else {
240 r = fabs(s);
241 if (s >= 0.0)
242 theta = M_PI / 4.0;
243 else
244 theta = -M_PI / 4.0;
245 }
246 }
247}
248
249static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, vpColVector &U,
251{
252 vpColVector I0(3), J0(3);
253 for (unsigned int i = 0; i < 3; i++) {
254 I0[i] = I04[i];
255 J0[i] = J04[i];
256 }
257 double s = -2.0 * vpColVector::dotProd(I0, J0);
258 double c = J0.sumSquare() - I0.sumSquare();
259
260 double r, theta;
261 calculRTheta(s, c, r, theta);
262 double co = cos(theta);
263 double si = sin(theta);
264
265 // calcul de la premiere solution
266 vpColVector I(4), J(4);
267 I = I04 + U * r * co;
268 J = J04 + U * r * si;
269
270#if (DEBUG_LEVEL2)
271 {
272 std::cout << "I0 " << I04.t() << std::endl;
273 std::cout << "J0 " << J04.t() << std::endl;
274 std::cout << "I1 " << I.t() << std::endl;
275 std::cout << "J1 " << J.t() << std::endl;
276 }
277#endif
278 calculSolutionDementhon(I, J, cMo1);
279
280 // calcul de la deuxieme solution
281 I = I04 - U * r * co;
282 J = J04 - U * r * si;
283#if (DEBUG_LEVEL2)
284 {
285 std::cout << "I2 " << I.t() << std::endl;
286 std::cout << "J2 " << J.t() << std::endl;
287 }
288#endif
289 calculSolutionDementhon(I, J, cMo2);
290}
291
296{
297#if (DEBUG_LEVEL1)
298 std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
299#endif
300
301 int erreur = 0;
302
303 // test if all points are in front of the camera
304 for (unsigned int i = 0; i < npt; i++) {
305 double z;
306 z = cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ() + cMo[2][3];
307 if (z <= 0.0) {
308 erreur = -1;
309 return erreur;
310 }
311 }
312
313 unsigned int cpt = 0;
314 double res_min = sqrt(computeResidualDementhon(cMo) / npt);
315 double res_old = 2.0 * res_min;
316
317 /* FC modif SEUIL_RESIDUAL 0.01 a 0.001 */
318 while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
319 vpHomogeneousMatrix cMo1, cMo2, cMo_old;
320
321 res_old = res_min;
322 cMo_old = cMo;
323
324 vpColVector xprim(npt), yprim(npt);
325 for (unsigned int i = 0; i < npt; i++) {
326 double eps =
327 (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
328
329 xprim[i] = (1.0 + eps) * c3d[i].get_x();
330 yprim[i] = (1.0 + eps) * c3d[i].get_y();
331 }
332
333 vpColVector I04(4), J04(4);
334 I04 = Ap * xprim;
335 J04 = Ap * yprim;
336
337 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
338
339 // test if all points are in front of the camera for cMo1 and cMo2
340 int erreur1 = 0;
341 int erreur2 = 0;
342 for (unsigned int i = 0; i < npt; i++) {
343 double z;
344 z = cMo1[2][0] * c3d[i].get_oX() + cMo1[2][1] * c3d[i].get_oY() + cMo1[2][2] * c3d[i].get_oZ() + cMo1[2][3];
345 if (z <= 0.0)
346 erreur1 = -1;
347 z = cMo2[2][0] * c3d[i].get_oX() + cMo2[2][1] * c3d[i].get_oY() + cMo2[2][2] * c3d[i].get_oZ() + cMo2[2][3];
348 if (z <= 0.0)
349 erreur2 = -1;
350 }
351
352 if ((erreur1 == -1) && (erreur2 == -1)) {
353 cMo = cMo_old;
354#if (DEBUG_LEVEL3)
355 std::cout << " End of loop since z < 0 for both solutions" << std::endl;
356#endif
357 break; // outside of while due to z < 0
358 }
359 if ((erreur1 == 0) && (erreur2 == -1)) {
360 cMo = cMo1;
361 res_min = sqrt(computeResidualDementhon(cMo) / npt);
362 }
363 if ((erreur1 == -1) && (erreur2 == 0)) {
364 cMo = cMo2;
365 res_min = sqrt(computeResidualDementhon(cMo) / npt);
366 }
367 if ((erreur1 == 0) && (erreur2 == 0)) {
368 double res1 = sqrt(computeResidualDementhon(cMo1) / npt);
369 double res2 = sqrt(computeResidualDementhon(cMo2) / npt);
370 if (res1 <= res2) {
371 res_min = res1;
372 cMo = cMo1;
373 } else {
374 res_min = res2;
375 cMo = cMo2;
376 }
377 }
378
379#if (DEBUG_LEVEL3)
380 if (erreur1 == 0) {
381 double s = sqrt(computeResidualDementhon(cMo1) / npt);
382 vpThetaUVector erc;
383 cMo1.extract(erc);
384 std::cout << "it = " << cpt << " cMo1 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
385 << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
386 } else
387 std::cout << "Pb z < 0 with cMo1" << std::endl;
388
389 if (erreur2 == 0) {
390 double s = sqrt(computeResidualDementhon(cMo2) / npt);
391 vpThetaUVector erc;
392 cMo2.extract(erc);
393 std::cout << "it = " << cpt << " cMo2 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
394 << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
395 } else
396 std::cout << "Pb z < 0 with cMo2" << std::endl;
397#endif
398
399 if (res_min > res_old) {
400#if (DEBUG_LEVEL3)
401 std::cout << "Divergence : res_min = " << res_min << " res_old = " << res_old << std::endl;
402#endif
403 cMo = cMo_old;
404 }
405 cpt++;
406 } /* end of while */
407
408#if (DEBUG_LEVEL1)
409 std::cout << "end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
410#endif
411
412 return erreur;
413}
414
424{
425#if (DEBUG_LEVEL1)
426 std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl;
427#endif
428 const double svdFactorUsedWhenFailure = 10.; // Factor by which is multipled dementhonSvThresh each time the svdDecomposition fails
429 const double svdThresholdLimit = 1e-2; // The svd decomposition will be tested with a threshold up to this value. If with this threshold, the rank of A is still !=3, an exception is thrown
430 const double lnOfSvdFactorUsed = std::log(svdFactorUsedWhenFailure);
431 const double logNOfSvdThresholdLimit = std::log(svdThresholdLimit)/lnOfSvdFactorUsed;
432 vpPoint P;
433 double cdg[3];
434 /* compute the cog of the 3D points */
435 cdg[0] = cdg[1] = cdg[2] = 0.0;
436 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
437 P = (*it);
438 cdg[0] += P.get_oX();
439 cdg[1] += P.get_oY();
440 cdg[2] += P.get_oZ();
441 }
442 for (unsigned int i = 0; i < 3; i++)
443 cdg[i] /= npt;
444 // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
445
446 c3d.clear();
447 /* translate the 3D points wrt cog */
448 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
449 P = (*it);
450 P.set_oX(P.get_oX() - cdg[0]);
451 P.set_oY(P.get_oY() - cdg[1]);
452 P.set_oZ(P.get_oZ() - cdg[2]);
453 c3d.push_back(P);
454 }
455
456 vpMatrix A(npt, 4);
457
458 for (unsigned int i = 0; i < npt; i++) {
459 A[i][0] = c3d[i].get_oX();
460 A[i][1] = c3d[i].get_oY();
461 A[i][2] = c3d[i].get_oZ();
462 A[i][3] = 1.0;
463 }
464 vpColVector sv;
465 vpMatrix Ap, imA, imAt, kAt;
466 bool isRankEqualTo3 = false; // Indicates if the rank of A is the expected one
467 double logNofSvdThresh = std::log(dementhonSvThresh)/lnOfSvdFactorUsed; // Get the log_n(dementhonSvThresh), where n is the factor by which we will multiply it if the svd decomposition fails.
468 int nbMaxIter = static_cast<int>(std::max(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.)); // Ensure that if the user chose a threshold > svdThresholdLimit, at least 1 iteration of svd decomposition is performed
469 double svdThreshold = dementhonSvThresh;
470 int irank = 0;
471 for(int i = 0; i < nbMaxIter && !isRankEqualTo3; i++)
472 {
473 irank = A.pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt);
474 if(irank == 3)
475 {
476 isRankEqualTo3 = true;
477 }
478 else
479 {
480 isRankEqualTo3 = false;
481 svdThreshold *= svdFactorUsedWhenFailure;
482 }
483 }
484
485 if (!isRankEqualTo3) {
486 std::stringstream errorMsg;
487 errorMsg << "In Dementhon planar, after ";
488 errorMsg << nbMaxIter;
489 errorMsg << " trials multiplying the svd threshold by ";
490 errorMsg << svdFactorUsedWhenFailure;
491 errorMsg << ", rank (";
492 errorMsg << irank;
493 errorMsg << ") is still not 3";
494 throw(vpException(vpException::fatalError, errorMsg.str()));
495 }
496 // calcul de U
497 vpColVector U(4);
498 for (unsigned int i = 0; i < 4; i++) {
499 U[i] = kAt[0][i];
500 }
501#if (DEBUG_LEVEL2)
502 {
503 std::cout << "A" << std::endl << A << std::endl;
504 std::cout << "A^+" << std::endl << Ap << std::endl;
505 std::cout << "U^T = " << U.t() << std::endl;
506 }
507#endif
508
509 vpColVector xi(npt);
510 vpColVector yi(npt);
511
512 for (unsigned int i = 0; i < npt; i++) {
513 xi[i] = c3d[i].get_x();
514 yi[i] = c3d[i].get_y();
515 }
516
517 vpColVector I04(4), J04(4);
518 I04 = Ap * xi;
519 J04 = Ap * yi;
520
521 vpHomogeneousMatrix cMo1, cMo2;
522 calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2);
523
524#if DEBUG_LEVEL3
525 double res = sqrt(computeResidualDementhon(cMo1) / npt);
526 vpThetaUVector erc;
527 cMo1.extract(erc);
528 std::cout << "cMo Start Tree 1 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
529 << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
530 res = sqrt(computeResidualDementhon(cMo2) / npt);
531 cMo2.extract(erc);
532 std::cout << "cMo Start Tree 2 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " "
533 << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
534#endif
535
536 int erreur1 = calculArbreDementhon(Ap, U, cMo1);
537 int erreur2 = calculArbreDementhon(Ap, U, cMo2);
538
539 if ((erreur1 == -1) && (erreur2 == -1)) {
540 throw(
541 vpException(vpException::fatalError, "Error in Dementhon planar: z < 0 with Start Tree 1 and Start Tree 2..."));
542 }
543 if ((erreur1 == 0) && (erreur2 == -1))
544 cMo = cMo1;
545 if ((erreur1 == -1) && (erreur2 == 0))
546 cMo = cMo2;
547 if ((erreur1 == 0) && (erreur2 == 0)) {
548 double s1 = computeResidualDementhon(cMo1);
549 double s2 = computeResidualDementhon(cMo2);
550
551 if (s1 <= s2)
552 cMo = cMo1;
553 else
554 cMo = cMo2;
555
556#if DEBUG_LEVEL3
557 if (erreur1 == -1)
558 std::cout << "Pb z < 0 with Start Tree 1" << std::endl;
559 if (erreur2 == -1)
560 std::cout << "Pb z < 0 with Start Tree 2" << std::endl;
561 if (s1 <= s2)
562 std::cout << " Tree 1 chosen " << std::endl;
563 else
564 std::cout << " Tree 2 chosen " << std::endl;
565#endif
566 }
567
568 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
569 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
570 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
571
572#if (DEBUG_LEVEL1)
573 std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl;
574#endif
575}
576
586{
587 double squared_error = 0;
588
589 // Be careful: since c3d has been translated so that point0 is at the cdg,
590 // cMo here corresponds to object frame at that point, i.e, only the one used
591 // internally to Dementhon's method
592
593 for (unsigned int i = 0; i < npt; i++) {
594
595 double X = c3d[i].get_oX() * cMo[0][0] + c3d[i].get_oY() * cMo[0][1] + c3d[i].get_oZ() * cMo[0][2] + cMo[0][3];
596 double Y = c3d[i].get_oX() * cMo[1][0] + c3d[i].get_oY() * cMo[1][1] + c3d[i].get_oZ() * cMo[1][2] + cMo[1][3];
597 double Z = c3d[i].get_oX() * cMo[2][0] + c3d[i].get_oY() * cMo[2][1] + c3d[i].get_oZ() * cMo[2][2] + cMo[2][3];
598
599 double x = X / Z;
600 double y = Y / Z;
601
602 squared_error += vpMath::sqr(x - c3d[i].get_x()) + vpMath::sqr(y - c3d[i].get_y());
603 }
604 return squared_error;
605}
606
607#undef EPS_DEM
608#undef SEUIL_RESIDUAL
609#undef ITER_MAX
610#undef DEBUG_LEVEL1
611#undef DEBUG_LEVEL2
612#undef DEBUG_LEVEL3
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
vpRowVector t() const
static vpColVector cross(const vpColVector &a, const vpColVector &b)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ 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.
void extract(vpRotationMatrix &R) const
static double sqr(double x)
Definition vpMath.h:124
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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 ...
Definition vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition vpPoint.cpp:501
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition vpPoint.cpp:503
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition vpPoint.cpp:499
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
double computeResidualDementhon(const vpHomogeneousMatrix &cMo)
Compute and return the residual corresponding to the sum of squared residuals in meter^2 for the pose...
unsigned int npt
Number of point used in pose computation.
Definition vpPose.h:114
double dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition vpPose.h:199
void poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition vpPose.h:115
int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
void poseDementhonPlan(vpHomogeneousMatrix &cMo)
Compute the pose using Dementhon approach for planar objects this is a direct implementation of the a...
Implementation of a rotation vector as axis-angle minimal representation.