1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Pose computation.
33 *
34 * Authors:
35 * Eric Marchand
36 * Francois Chaumette
37 *
38 *****************************************************************************/
39
40 #include <visp3/core/vpMath.h>
41 #include <visp3/vision/vpPose.h>
42
43 #define DEBUG_LEVEL1 0
44 #define DEBUG_LEVEL2 0
45 #define DEBUG_LEVEL3 0
46
47 #define SEUIL_RESIDUAL 0.0001 /* avant 0.01 dans while du calculArbreDementhon */
48 #define EPS_DEM 0.001
49
50 #define ITER_MAX 30 /* max number of iterations for Demonthon's loop */
51
calculSolutionDementhon(vpColVector & I4,vpColVector & J4,vpHomogeneousMatrix & cMo)52 static void calculSolutionDementhon(vpColVector &I4, vpColVector &J4, vpHomogeneousMatrix &cMo)
53 {
54
55 #if (DEBUG_LEVEL1)
56 std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
57 #endif
58
59 // norm of the 3 first components of I4 and J4
60 double normI3 = sqrt(I4[0]*I4[0] + I4[1]*I4[1] + I4[2]*I4[2]);
61 double normJ3 = sqrt(J4[0]*J4[0] + J4[1]*J4[1] + J4[2]*J4[2]);
62
63 if ((normI3 < 1e-10) || (normJ3 < 1e-10)) {
64 // vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
65 throw(vpException(vpException::divideByZeroError,
66 "Division by zero in Dementhon pose computation: normI or normJ = 0"));
67 }
68
69 double Z0 = 2.0 / (normI3 + normJ3);
70
71 vpColVector I3(3), J3(3), K3(3);
72 for (unsigned int i=0; i<3; i++) {
73 I3[i] = I4[i] / normI3;
74 J3[i] = J4[i] / normJ3;
75 }
76
77 K3 = vpColVector::cross(I3, J3); // k = I x J
78 K3.normalize();
79
80 J3 = vpColVector::cross(K3, I3);
81
82 // calcul de la matrice de passage
83 cMo[0][0] = I3[0];
84 cMo[0][1] = I3[1];
85 cMo[0][2] = I3[2];
86 cMo[0][3] = I4[3] * Z0;
87
88 cMo[1][0] = J3[0];
89 cMo[1][1] = J3[1];
90 cMo[1][2] = J3[2];
91 cMo[1][3] = J4[3] * Z0;
92
93 cMo[2][0] = K3[0];
94 cMo[2][1] = K3[1];
95 cMo[2][2] = K3[2];
96 cMo[2][3] = Z0;
97
98 #if (DEBUG_LEVEL1)
99 std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
100 #endif
101 }
102
103 /*!
104 Compute the pose using Dementhon approach for non planar objects.
105 This is a direct implementation of the algorithm proposed by
106 Dementhon and Davis in their 1995 paper \cite Dementhon95.
107 */
poseDementhonNonPlan(vpHomogeneousMatrix & cMo)108 void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
109 {
110 vpPoint P;
111 double cdg[3];
112 /* compute the cog of the 3D points */
113 cdg[0] = cdg[1] = cdg[2] = 0.0;
114 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
115 P = (*it);
116 cdg[0] += P.get_oX();
117 cdg[1] += P.get_oY();
118 cdg[2] += P.get_oZ();
119 }
120 for (unsigned int i=0; i<3;i++) cdg[i] /= npt;
121 // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
122
123 c3d.clear();
124 /* translate the 3D points wrt cog */
125 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
126 P = (*it);
127 P.set_oX(P.get_oX() - cdg[0]);
128 P.set_oY(P.get_oY() - cdg[1]);
129 P.set_oZ(P.get_oZ() - cdg[2]);
130 c3d.push_back(P);
131 }
132
133 vpMatrix A(npt, 4), Ap;
134
135 for (unsigned int i = 0; i < npt; i++) {
136 A[i][0] = c3d[i].get_oX();
137 A[i][1] = c3d[i].get_oY();
138 A[i][2] = c3d[i].get_oZ();
139 A[i][3] = 1.0;
140 }
141 Ap = A.pseudoInverse();
142
143 #if (DEBUG_LEVEL2)
144 {
145 std::cout << "A" << std::endl << A << std::endl;
146 std::cout << "A^+" << std::endl << Ap << std::endl;
147 }
148 #endif
149
150 vpColVector xprim(npt);
151 vpColVector yprim(npt);
152
153 for (unsigned int i = 0; i < npt; i++) {
154 xprim[i] = c3d[i].get_x();
155 yprim[i] = c3d[i].get_y();
156 }
157 vpColVector I4(4), J4(4);
158
159 I4 = Ap * xprim;
160 J4 = Ap * yprim;
161
162 calculSolutionDementhon(I4, J4, cMo);
163
164 int erreur = 0;
165 for (unsigned int i = 0; i < npt; i++) {
166 double z;
167 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];
168 if (z <= 0.0) erreur = -1;
169 }
170 if (erreur == -1) {
171 throw(vpException(vpException::fatalError, "End of Dementhon since z < 0 for both solutions at the beginning"));
172 }
173 int cpt = 0;
174 double res = sqrt(computeResidualDementhon(cMo) / npt);
175 double res_old = 2.0*res;
176
177 // In his paper, Dementhon suggests to use the difference
178 // between 2 successive norm of eps. We prefer to use the mean of the
179 // residuals in the image
180 while ((cpt < ITER_MAX) && (res > SEUIL_RESIDUAL) && (res < res_old)) {
181
182 vpHomogeneousMatrix cMo_old;
183 res_old = res;
184 cMo_old = cMo;
185
186 for (unsigned int i = 0; i < npt; i++) {
187 double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
188
189 xprim[i] = (1.0 + eps) * c3d[i].get_x();
190 yprim[i] = (1.0 + eps) * c3d[i].get_y();
191 }
192 I4 = Ap * xprim;
193 J4 = Ap * yprim;
194
195 calculSolutionDementhon(I4, J4, cMo);
196 res = sqrt(computeResidualDementhon(cMo) / npt);
197 for (unsigned int i = 0; i < npt; i++) {
198 double z;
199 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];
200 if (z <= 0.0) erreur = -1;
201 }
202 if (erreur == -1) {
203 cMo = cMo_old;
204 res = res_old; // to leave the while loop
205 #if (DEBUG_LEVEL3)
206 std::cout << "Pb z < 0 with cMo in Dementhon's loop" << std::endl;
207 #endif
208 }
209 #if (DEBUG_LEVEL3)
210 vpThetaUVector erc;
211 cMo.extract(erc);
212 std::cout << "it = " << cpt << " residu = " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
213 #endif
214 if (res > res_old) {
215 #if (DEBUG_LEVEL3)
216 std::cout << "Divergence : res = " << res << " res_old = " << res_old << std::endl;
217 #endif
218 cMo = cMo_old;
219 }
220 cpt++;
221 }
222 // go back to the initial frame
223 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
224 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
225 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
226 }
227
228
calculRTheta(double s,double c,double & r,double & theta)229 static void calculRTheta(double s, double c, double &r, double &theta)
230 {
231 if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
232 r = sqrt(sqrt(s * s + c * c));
233 theta = atan2(s, c) / 2.0;
234 } else {
235 if (fabs(c) > fabs(s)) {
236 r = fabs(c);
237 if (c >= 0.0)
238 theta = M_PI / 2;
239 else
240 theta = -M_PI / 2;
241 } else {
242 r = fabs(s);
243 if (s >= 0.0)
244 theta = M_PI / 4.0;
245 else
246 theta = -M_PI / 4.0;
247 }
248 }
249 }
250
251
calculTwoSolutionsDementhonPlan(vpColVector & I04,vpColVector & J04,vpColVector & U,vpHomogeneousMatrix & cMo1,vpHomogeneousMatrix & cMo2)252 static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, vpColVector &U, vpHomogeneousMatrix &cMo1, vpHomogeneousMatrix &cMo2)
253 {
254 vpColVector I0(3), J0(3);
255 for (unsigned int i=0; i<3; i++) {
256 I0[i] = I04[i];
257 J0[i] = J04[i];
258 }
259 double s = -2.0 * vpColVector::dotProd(I0, J0);
260 double c = J0.sumSquare() - I0.sumSquare();
261
262 double r, theta;
263 calculRTheta(s, c, r, theta);
264 double co = cos(theta);
265 double si = sin(theta);
266
267 // calcul de la premiere solution
268 vpColVector I(4),J(4);
269 I = I04 + U * r * co;
270 J = J04 + U * r * si;
271
272 #if (DEBUG_LEVEL2)
273 {
274 std::cout << "I0 " << I04.t() << std::endl;
275 std::cout << "J0 " << J04.t() << std::endl;
276 std::cout << "I1 " << I.t() << std::endl;
277 std::cout << "J1 " << J.t() << std::endl;
278 }
279 #endif
280 calculSolutionDementhon(I, J, cMo1);
281
282 // calcul de la deuxieme solution
283 I = I04 - U * r * co;
284 J = J04 - U * r * si;
285 #if (DEBUG_LEVEL2)
286 {
287 std::cout << "I2 " << I.t() << std::endl;
288 std::cout << "J2 " << J.t() << std::endl;
289 }
290 #endif
291 calculSolutionDementhon(I, J, cMo2);
292 }
293
294
295 /*!
296 Return 0 if success, -1 if failure.
297 */
calculArbreDementhon(vpMatrix & Ap,vpColVector & U,vpHomogeneousMatrix & cMo)298 int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatrix &cMo)
299 {
300 #if (DEBUG_LEVEL1)
301 std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
302 #endif
303
304 int erreur = 0;
305
306 // test if all points are in front of the camera
307 for (unsigned int i = 0; i < npt; i++) {
308 double z;
309 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];
310 if (z <= 0.0) {
311 erreur = -1;
312 return erreur;
313 }
314 }
315
316 unsigned int cpt = 0;
317 double res_min = sqrt(computeResidualDementhon(cMo)/npt);
318 double res_old = 2.0*res_min;
319
320 /* FC modif SEUIL_RESIDUAL 0.01 a 0.001 */
321 while ((cpt < ITER_MAX) && (res_min > SEUIL_RESIDUAL) && (res_min < res_old)) {
322 vpHomogeneousMatrix cMo1, cMo2, cMo_old;
323
324 res_old = res_min;
325 cMo_old = cMo;
326
327 vpColVector xprim(npt),yprim(npt);
328 for (unsigned int i = 0; i < npt; i++) {
329 double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
330
331 xprim[i] = (1.0 + eps)*c3d[i].get_x();
332 yprim[i] = (1.0 + eps)*c3d[i].get_y();
333 }
334
335 vpColVector I04(4), J04(4);
336 I04 = Ap * xprim;
337 J04 = Ap * yprim;
338
339 calculTwoSolutionsDementhonPlan(I04,J04,U,cMo1,cMo2);
340
341 // test if all points are in front of the camera for cMo1 and cMo2
342 int erreur1 = 0;
343 int erreur2 = 0;
344 for (unsigned int i = 0; i < npt; i++) {
345 double z;
346 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];
347 if (z <= 0.0) erreur1 = -1;
348 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];
349 if (z <= 0.0) 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]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
385 }
386 else std::cout << "Pb z < 0 with cMo1" << std::endl;
387
388 if (erreur2 == 0) {
389 double s = sqrt(computeResidualDementhon(cMo2)/npt);
390 vpThetaUVector erc;
391 cMo2.extract(erc);
392 std::cout << "it = " << cpt << " cMo2 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
393 }
394 else std::cout << "Pb z < 0 with cMo2" << std::endl;
395 #endif
396
397 if (res_min > res_old) {
398 #if (DEBUG_LEVEL3)
399 std::cout << "Divergence : res_min = " << res_min << " res_old = " << res_old << std::endl;
400 #endif
401 cMo = cMo_old;
402 }
403 cpt++;
404 } /* end of while */
405
406 #if (DEBUG_LEVEL1)
407 std::cout << "end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
408 #endif
409
410 return erreur;
411 }
412
413 /*!
414 \brief Compute the pose using Dementhon approach for planar objects
415 this is a direct implementation of the algorithm proposed by
416 Dementhon in his PhD.
417
418 \author Francois Chaumette (simplified by Eric Marchand)
419 */
420
poseDementhonPlan(vpHomogeneousMatrix & cMo)421 void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
422 {
423 #if (DEBUG_LEVEL1)
424 std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl;
425 #endif
426
427 vpPoint P;
428 double cdg[3];
429 /* compute the cog of the 3D points */
430 cdg[0] = cdg[1] = cdg[2] = 0.0;
431 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
432 P = (*it);
433 cdg[0] += P.get_oX();
434 cdg[1] += P.get_oY();
435 cdg[2] += P.get_oZ();
436 }
437 for (unsigned int i=0; i<3;i++) cdg[i] /= npt;
438 // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]);
439
440 c3d.clear();
441 /* translate the 3D points wrt cog */
442 for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
443 P = (*it);
444 P.set_oX(P.get_oX() - cdg[0]);
445 P.set_oY(P.get_oY() - cdg[1]);
446 P.set_oZ(P.get_oZ() - cdg[2]);
447 c3d.push_back(P);
448 }
449
450 vpMatrix A(npt,4);
451
452 for (unsigned int i = 0; i < npt; i++) {
453 A[i][0] = c3d[i].get_oX();
454 A[i][1] = c3d[i].get_oY();
455 A[i][2] = c3d[i].get_oZ();
456 A[i][3] = 1.0;
457 }
458 vpColVector sv;
459 vpMatrix Ap,imA, imAt, kAt;
460 int irank = A.pseudoInverse(Ap, sv, 1.e-6, imA, imAt, kAt);
461 if (irank != 3) {
462 throw(vpException(vpException::fatalError, "In Dementhon planar, rank is not 3"));
463 }
464 // calcul de U
465 vpColVector U(4);
466 for (unsigned int i = 0; i<4; i++) {
467 U[i] = kAt[0][i];
468 }
469 #if (DEBUG_LEVEL2)
470 {
471 std::cout << "A" << std::endl << A << std::endl;
472 std::cout << "A^+" << std::endl << Ap << std::endl;
473 std::cout << "U^T = " << U.t() << std::endl;
474 }
475 #endif
476
477 vpColVector xi(npt);
478 vpColVector yi(npt);
479
480 for (unsigned int i = 0; i < npt; i++) {
481 xi[i] = c3d[i].get_x();
482 yi[i] = c3d[i].get_y();
483 }
484
485 vpColVector I04(4), J04(4);
486 I04 = Ap * xi;
487 J04 = Ap * yi;
488
489 vpHomogeneousMatrix cMo1,cMo2;
490 calculTwoSolutionsDementhonPlan(I04,J04,U,cMo1,cMo2);
491
492 #if DEBUG_LEVEL3
493 double res = sqrt(computeResidualDementhon(cMo1)/npt);
494 vpThetaUVector erc;
495 cMo1.extract(erc);
496 std::cout << "cMo Start Tree 1 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
497 res = sqrt(computeResidualDementhon(cMo2)/npt);
498 cMo2.extract(erc);
499 std::cout << "cMo Start Tree 2 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl;
500 #endif
501
502 int erreur1 = calculArbreDementhon(Ap, U, cMo1);
503 int erreur2 = calculArbreDementhon(Ap, U, cMo2);
504
505 if ((erreur1 == -1) && (erreur2 == -1)) {
506 throw(vpException(vpException::fatalError, "Error in Dementhon planar: z < 0 with Start Tree 1 and Start Tree 2..."));
507 }
508 if ((erreur1 == 0) && (erreur2 == -1))
509 cMo = cMo1;
510 if ((erreur1 == -1) && (erreur2 == 0))
511 cMo = cMo2;
512 if ((erreur1 == 0) && (erreur2 == 0)) {
513 double s1 = computeResidualDementhon(cMo1);
514 double s2 = computeResidualDementhon(cMo2);
515
516 if (s1 <= s2)
517 cMo = cMo1;
518 else
519 cMo = cMo2;
520
521 #if DEBUG_LEVEL3
522 if (erreur1 == -1) std::cout << "Pb z < 0 with Start Tree 1" << std::endl;
523 if (erreur2 == -1) std::cout << "Pb z < 0 with Start Tree 2" << std::endl;
524 if (s1 <= s2) std::cout << " Tree 1 chosen " << std::endl;
525 else std::cout << " Tree 2 chosen " << std::endl;
526 #endif
527 }
528
529 cMo[0][3] -= (cdg[0] * cMo[0][0] + cdg[1] * cMo[0][1] + cdg[2] * cMo[0][2]);
530 cMo[1][3] -= (cdg[0] * cMo[1][0] + cdg[1] * cMo[1][1] + cdg[2] * cMo[1][2]);
531 cMo[2][3] -= (cdg[0] * cMo[2][0] + cdg[1] * cMo[2][1] + cdg[2] * cMo[2][2]);
532
533 #if (DEBUG_LEVEL1)
534 std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl;
535 #endif
536 }
537
538 /*!
539 \brief Compute and return the residual corresponding to the sum of squared residuals
540 in meter^2 for the pose matrix \e cMo.
541
542 \param cMo : the matrix that defines the pose to be tested.
543
544 \return the value of the sum of squared residuals in meter^2.
545 */
computeResidualDementhon(const vpHomogeneousMatrix & cMo)546 double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo)
547 {
548 double squared_error = 0;
549
550 // Be careful: since c3d has been translated so that point0 is at the cdg,
551 // cMo here corresponds to object frame at that point, i.e, only the one used
552 // internally to Dementhon's method
553
554 for (unsigned int i = 0; i < npt; i++) {
555
556 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];
557 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];
558 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];
559
560 double x = X / Z;
561 double y = Y / Z;
562
563 squared_error += vpMath::sqr(x - c3d[i].get_x()) + vpMath::sqr(y - c3d[i].get_y());
564 }
565 return squared_error;
566 }
567
568 #undef EPS_DEM
569 #undef SEUIL_RESIDUAL
570 #undef ITER_MAX
571 #undef DEBUG_LEVEL1
572 #undef DEBUG_LEVEL2
573 #undef DEBUG_LEVEL3
574