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 * Homography transformation.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39 //#include <computeHomography.h>
40 //#include <utilsHomography.h>
41 #include <iostream>
42 #include <visp3/core/vpExponentialMap.h>
43 #include <visp3/core/vpHomogeneousMatrix.h>
44 #include <visp3/core/vpMath.h>
45 #include <visp3/core/vpMatrix.h>
46 #include <visp3/core/vpPlane.h>
47 #include <visp3/core/vpPoint.h>
48 #include <visp3/core/vpRobust.h>
49 #include <visp3/vision/vpHomography.h>
50
51 const double vpHomography::threshold_rotation = 1e-7;
52 const double vpHomography::threshold_displacement = 1e-18;
53
54 #ifndef DOXYGEN_SHOULD_SKIP_THIS
55
updatePoseRotation(vpColVector & dx,vpHomogeneousMatrix & mati)56 static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati)
57 {
58 vpRotationMatrix rd;
59
60 double s = sqrt(dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2]);
61 if (s > 1.e-25) {
62 double u[3];
63
64 for (unsigned int i = 0; i < 3; i++)
65 u[i] = dx[i] / s;
66 double sinu = sin(s);
67 double cosi = cos(s);
68 double mcosi = 1 - cosi;
69 rd[0][0] = cosi + mcosi * u[0] * u[0];
70 rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
71 rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
72 rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
73 rd[1][1] = cosi + mcosi * u[1] * u[1];
74 rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
75 rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
76 rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
77 rd[2][2] = cosi + mcosi * u[2] * u[2];
78 } else {
79 for (unsigned int i = 0; i < 3; i++) {
80 for (unsigned int j = 0; j < 3; j++)
81 rd[i][j] = 0.0;
82 rd[i][i] = 1.0;
83 }
84 }
85
86 vpHomogeneousMatrix Delta;
87 Delta.insert(rd);
88
89 mati = Delta.inverse() * mati;
90 }
91
computeRotation(unsigned int nbpoint,vpPoint * c1P,vpPoint * c2P,vpHomogeneousMatrix & c2Mc1,int userobust)92 double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpHomogeneousMatrix &c2Mc1,
93 int userobust)
94 {
95 vpColVector e(2);
96 double r_1 = -1;
97
98 vpColVector p2(3);
99 vpColVector p1(3);
100 vpColVector Hp2(3);
101 vpColVector Hp1(3);
102
103 vpMatrix H2(2, 3);
104 vpColVector e2(2);
105 vpMatrix H1(2, 3);
106 vpColVector e1(2);
107
108 bool only_1 = false;
109 bool only_2 = false;
110 int iter = 0;
111
112 unsigned int n = 0;
113 for (unsigned int i = 0; i < nbpoint; i++) {
114 // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
115 if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.))) &&
116 (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.)))) {
117 n++;
118 }
119 }
120 if ((!only_1) && (!only_2))
121 n *= 2;
122
123 vpRobust robust;
124 vpColVector res(n);
125 vpColVector w(n);
126 w = 1;
127 robust.setMinMedianAbsoluteDeviation(0.00001);
128 vpMatrix W(2 * n, 2 * n);
129 W = 0;
130 vpMatrix c2Rc1(3, 3);
131 double r = 0;
132 while (vpMath::equal(r_1, r, threshold_rotation) == false) {
133
134 r_1 = r;
135 // compute current position
136
137 // Change frame (current)
138 for (unsigned int i = 0; i < 3; i++)
139 for (unsigned int j = 0; j < 3; j++)
140 c2Rc1[i][j] = c2Mc1[i][j];
141
142 vpMatrix L(2, 3), Lp;
143 int k = 0;
144 for (unsigned int i = 0; i < nbpoint; i++) {
145 // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
146 if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.))) &&
147 (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.)))) {
148 p2[0] = c2P[i].get_x();
149 p2[1] = c2P[i].get_y();
150 p2[2] = 1.0;
151 p1[0] = c1P[i].get_x();
152 p1[1] = c1P[i].get_y();
153 p1[2] = 1.0;
154
155 Hp2 = c2Rc1.t() * p2; // p2 = Hp1
156 Hp1 = c2Rc1 * p1; // p1 = Hp2
157
158 Hp2 /= Hp2[2]; // normalisation
159 Hp1 /= Hp1[2];
160
161 // set up the interaction matrix
162 double x = Hp2[0];
163 double y = Hp2[1];
164
165 H2[0][0] = x * y;
166 H2[0][1] = -(1 + x * x);
167 H2[0][2] = y;
168 H2[1][0] = 1 + y * y;
169 H2[1][1] = -x * y;
170 H2[1][2] = -x;
171 H2 *= -1;
172 H2 = H2 * c2Rc1.t();
173
174 // Set up the error vector
175 e2[0] = Hp2[0] - c1P[i].get_x();
176 e2[1] = Hp2[1] - c1P[i].get_y();
177
178 // set up the interaction matrix
179 x = Hp1[0];
180 y = Hp1[1];
181
182 H1[0][0] = x * y;
183 H1[0][1] = -(1 + x * x);
184 H1[0][2] = y;
185 H1[1][0] = 1 + y * y;
186 H1[1][1] = -x * y;
187 H1[1][2] = -x;
188
189 // Set up the error vector
190 e1[0] = Hp1[0] - c2P[i].get_x();
191 e1[1] = Hp1[1] - c2P[i].get_y();
192
193 if (only_2) {
194 if (k == 0) {
195 L = H2;
196 e = e2;
197 } else {
198 L = vpMatrix::stack(L, H2);
199 e = vpColVector::stack(e, e2);
200 }
201 } else if (only_1) {
202 if (k == 0) {
203 L = H1;
204 e = e1;
205 } else {
206 L = vpMatrix::stack(L, H1);
207 e = vpColVector::stack(e, e1);
208 }
209 } else {
210 if (k == 0) {
211 L = H2;
212 e = e2;
213 } else {
214 L = vpMatrix::stack(L, H2);
215 e = vpColVector::stack(e, e2);
216 }
217 L = vpMatrix::stack(L, H1);
218 e = vpColVector::stack(e, e1);
219 }
220
221 k++;
222 }
223 }
224
225 if (userobust) {
226 for (unsigned int l = 0; l < n; l++) {
227 res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]);
228 }
229 robust.MEstimator(vpRobust::TUKEY, res, w);
230
231 // compute the pseudo inverse of the interaction matrix
232 for (unsigned int l = 0; l < n; l++) {
233 W[2 * l][2 * l] = w[l];
234 W[2 * l + 1][2 * l + 1] = w[l];
235 }
236 } else {
237 for (unsigned int l = 0; l < 2 * n; l++)
238 W[l][l] = 1;
239 }
240 // CreateDiagonalMatrix(w, W) ;
241 (L).pseudoInverse(Lp, 1e-6);
242 // Compute the camera velocity
243 vpColVector c2rc1, v(6);
244
245 c2rc1 = -2 * Lp * W * e;
246 for (unsigned int i = 0; i < 3; i++)
247 v[i + 3] = c2rc1[i];
248
249 // only for simulation
250
251 updatePoseRotation(c2rc1, c2Mc1);
252 r = e.sumSquare();
253
254 if ((W * e).sumSquare() < 1e-10)
255 break;
256 if (iter > 25)
257 break;
258 iter++; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e="
259 // <<(W*e).sumSquare() <<std::endl ;
260 }
261
262 // std::cout << c2Mc1 <<std::endl ;
263 return (W * e).sumSquare();
264 }
265
getPlaneInfo(vpPlane & oN,vpHomogeneousMatrix & cMo,vpColVector & cN,double & cd)266 static void getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, vpColVector &cN, double &cd)
267 {
268 double A1 = cMo[0][0] * oN.getA() + cMo[0][1] * oN.getB() + cMo[0][2] * oN.getC();
269 double B1 = cMo[1][0] * oN.getA() + cMo[1][1] * oN.getB() + cMo[1][2] * oN.getC();
270 double C1 = cMo[2][0] * oN.getA() + cMo[2][1] * oN.getB() + cMo[2][2] * oN.getC();
271 double D1 = oN.getD() - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1);
272
273 cN.resize(3);
274 cN[0] = A1;
275 cN[1] = B1;
276 cN[2] = C1;
277 cd = -D1;
278 }
279
computeDisplacement(unsigned int nbpoint,vpPoint * c1P,vpPoint * c2P,vpPlane & oN,vpHomogeneousMatrix & c2Mc1,vpHomogeneousMatrix & c1Mo,int userobust)280 double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane &oN,
281 vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust)
282 {
283 vpColVector e(2);
284 double r_1 = -1;
285
286 vpColVector p2(3);
287 vpColVector p1(3);
288 vpColVector Hp2(3);
289 vpColVector Hp1(3);
290
291 vpMatrix H2(2, 6);
292 vpColVector e2(2);
293 vpMatrix H1(2, 6);
294 vpColVector e1(2);
295
296 bool only_1 = true;
297 bool only_2 = false;
298 int iter = 0;
299 unsigned int n = 0;
300 n = nbpoint;
301
302 // next 2 lines are useless (detected by Coverity Scan)
303 // if ( (! only_1) && (! only_2) )
304 // n *=2 ;
305
306 vpRobust robust;
307 vpColVector res(n);
308 vpColVector w(n);
309 w = 1;
310 robust.setMinMedianAbsoluteDeviation(0.00001);
311 vpMatrix W(2 * n, 2 * n);
312 W = 0;
313
314 vpColVector N1(3), N2(3);
315 double d1, d2;
316
317 double r = 1e10;
318 iter = 0;
319 while (vpMath::equal(r_1, r, threshold_displacement) == false) {
320 r_1 = r;
321 // compute current position
322
323 // Change frame (current)
324 vpHomogeneousMatrix c1Mc2, c2Mo;
325 vpRotationMatrix c1Rc2, c2Rc1;
326 vpTranslationVector c1Tc2, c2Tc1;
327 c1Mc2 = c2Mc1.inverse();
328 c2Mc1.extract(c2Rc1);
329 c2Mc1.extract(c2Tc1);
330 c2Mc1.extract(c1Rc2);
331 c1Mc2.extract(c1Tc2);
332
333 c2Mo = c2Mc1 * c1Mo;
334
335 getPlaneInfo(oN, c1Mo, N1, d1);
336 getPlaneInfo(oN, c2Mo, N2, d2);
337
338 vpMatrix L(2, 3), Lp;
339 int k = 0;
340 for (unsigned int i = 0; i < nbpoint; i++) {
341 p2[0] = c2P[i].get_x();
342 p2[1] = c2P[i].get_y();
343 p2[2] = 1.0;
344 p1[0] = c1P[i].get_x();
345 p1[1] = c1P[i].get_y();
346 p1[2] = 1.0;
347
348 vpMatrix H(3, 3);
349
350 Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2 * N2.t()) / d2) * p2; // p2 = Hp1
351 Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1 * N1.t()) / d1) * p1; // p1 = Hp2
352
353 Hp2 /= Hp2[2]; // normalisation
354 Hp1 /= Hp1[2];
355
356 // set up the interaction matrix
357 double x = Hp2[0];
358 double y = Hp2[1];
359 double Z1;
360
361 Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z
362
363 H2[0][0] = -Z1;
364 H2[0][1] = 0;
365 H2[0][2] = x * Z1;
366 H2[1][0] = 0;
367 H2[1][1] = -Z1;
368 H2[1][2] = y * Z1;
369 H2[0][3] = x * y;
370 H2[0][4] = -(1 + x * x);
371 H2[0][5] = y;
372 H2[1][3] = 1 + y * y;
373 H2[1][4] = -x * y;
374 H2[1][5] = -x;
375 H2 *= -1;
376
377 vpMatrix c1CFc2(6, 6);
378 {
379 vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2;
380 for (unsigned int k_ = 0; k_ < 3; k_++)
381 for (unsigned int l = 0; l < 3; l++) {
382 c1CFc2[k_][l] = c1Rc2[k_][l];
383 c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
384 c1CFc2[k_][l + 3] = sTR[k_][l];
385 }
386 }
387 H2 = H2 * c1CFc2;
388
389 // Set up the error vector
390 e2[0] = Hp2[0] - c1P[i].get_x();
391 e2[1] = Hp2[1] - c1P[i].get_y();
392
393 x = Hp1[0];
394 y = Hp1[1];
395
396 Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z
397
398 H1[0][0] = -Z1;
399 H1[0][1] = 0;
400 H1[0][2] = x * Z1;
401 H1[1][0] = 0;
402 H1[1][1] = -Z1;
403 H1[1][2] = y * Z1;
404 H1[0][3] = x * y;
405 H1[0][4] = -(1 + x * x);
406 H1[0][5] = y;
407 H1[1][3] = 1 + y * y;
408 H1[1][4] = -x * y;
409 H1[1][5] = -x;
410
411 // Set up the error vector
412 e1[0] = Hp1[0] - c2P[i].get_x();
413 e1[1] = Hp1[1] - c2P[i].get_y();
414
415 if (only_2) {
416 if (k == 0) {
417 L = H2;
418 e = e2;
419 } else {
420 L = vpMatrix::stack(L, H2);
421 e = vpColVector::stack(e, e2);
422 }
423 } else if (only_1) {
424 if (k == 0) {
425 L = H1;
426 e = e1;
427 } else {
428 L = vpMatrix::stack(L, H1);
429 e = vpColVector::stack(e, e1);
430 }
431 } else {
432 if (k == 0) {
433 L = H2;
434 e = e2;
435 } else {
436 L = vpMatrix::stack(L, H2);
437 e = vpColVector::stack(e, e2);
438 }
439 L = vpMatrix::stack(L, H1);
440 e = vpColVector::stack(e, e1);
441 }
442
443 k++;
444 }
445
446 if (userobust) {
447 for (unsigned int l = 0; l < n; l++) {
448 res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]);
449 }
450 robust.MEstimator(vpRobust::TUKEY, res, w);
451
452 // compute the pseudo inverse of the interaction matrix
453 for (unsigned int l = 0; l < n; l++) {
454 W[2 * l][2 * l] = w[l];
455 W[2 * l + 1][2 * l + 1] = w[l];
456 }
457 } else {
458 for (unsigned int l = 0; l < 2 * n; l++)
459 W[l][l] = 1;
460 }
461 (W * L).pseudoInverse(Lp, 1e-16);
462 // Compute the camera velocity
463 vpColVector c2Tcc1;
464
465 c2Tcc1 = -1 * Lp * W * e;
466
467 // only for simulation
468
469 c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1;
470 // UpdatePose2(c2Tcc1, c2Mc1) ;
471 r = (W * e).sumSquare();
472
473 if (r < 1e-15) {
474 break;
475 }
476 if (iter > 1000) {
477 break;
478 }
479 if (r > r_1) {
480 break;
481 }
482 iter++;
483 }
484
485 return (W * e).sumSquare();
486 }
487
computeDisplacement(unsigned int nbpoint,vpPoint * c1P,vpPoint * c2P,vpPlane * oN,vpHomogeneousMatrix & c2Mc1,vpHomogeneousMatrix & c1Mo,int userobust)488 double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane *oN,
489 vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust)
490 {
491
492 vpColVector e(2);
493 double r_1 = -1;
494
495 vpColVector p2(3);
496 vpColVector p1(3);
497 vpColVector Hp2(3);
498 vpColVector Hp1(3);
499
500 vpMatrix H2(2, 6);
501 vpColVector e2(2);
502 vpMatrix H1(2, 6);
503 vpColVector e1(2);
504
505 bool only_1 = true;
506 bool only_2 = false;
507 int iter = 0;
508 unsigned int i;
509 unsigned int n = 0;
510 n = nbpoint;
511
512 // next 2 lines are useless (detected by Coverity Scan)
513 // if ( (! only_1) && (! only_2) )
514 // n *=2 ;
515
516 vpRobust robust;
517 vpColVector res(n);
518 vpColVector w(n);
519 w = 1;
520 robust.setMinMedianAbsoluteDeviation(0.00001);
521 vpMatrix W(2 * n, 2 * n);
522 W = 0;
523
524 vpColVector N1(3), N2(3);
525 double d1, d2;
526
527 double r = 1e10;
528 iter = 0;
529 while (vpMath::equal(r_1, r, threshold_displacement) == false) {
530 r_1 = r;
531 // compute current position
532
533 // Change frame (current)
534 vpHomogeneousMatrix c1Mc2, c2Mo;
535 vpRotationMatrix c1Rc2, c2Rc1;
536 vpTranslationVector c1Tc2, c2Tc1;
537 c1Mc2 = c2Mc1.inverse();
538 c2Mc1.extract(c2Rc1);
539 c2Mc1.extract(c2Tc1);
540 c2Mc1.extract(c1Rc2);
541 c1Mc2.extract(c1Tc2);
542
543 c2Mo = c2Mc1 * c1Mo;
544
545 vpMatrix L(2, 3), Lp;
546 int k = 0;
547 for (i = 0; i < nbpoint; i++) {
548 getPlaneInfo(oN[i], c1Mo, N1, d1);
549 getPlaneInfo(oN[i], c2Mo, N2, d2);
550 p2[0] = c2P[i].get_x();
551 p2[1] = c2P[i].get_y();
552 p2[2] = 1.0;
553 p1[0] = c1P[i].get_x();
554 p1[1] = c1P[i].get_y();
555 p1[2] = 1.0;
556
557 vpMatrix H(3, 3);
558
559 Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2 * N2.t()) / d2) * p2; // p2 = Hp1
560 Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1 * N1.t()) / d1) * p1; // p1 = Hp2
561
562 Hp2 /= Hp2[2]; // normalisation
563 Hp1 /= Hp1[2];
564
565 // set up the interaction matrix
566 double x = Hp2[0];
567 double y = Hp2[1];
568 double Z1;
569
570 Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z
571
572 H2[0][0] = -Z1;
573 H2[0][1] = 0;
574 H2[0][2] = x * Z1;
575 H2[1][0] = 0;
576 H2[1][1] = -Z1;
577 H2[1][2] = y * Z1;
578 H2[0][3] = x * y;
579 H2[0][4] = -(1 + x * x);
580 H2[0][5] = y;
581 H2[1][3] = 1 + y * y;
582 H2[1][4] = -x * y;
583 H2[1][5] = -x;
584 H2 *= -1;
585
586 vpMatrix c1CFc2(6, 6);
587 {
588 vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2;
589 for (unsigned int k_ = 0; k_ < 3; k_++)
590 for (unsigned int l = 0; l < 3; l++) {
591 c1CFc2[k_][l] = c1Rc2[k_][l];
592 c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
593 c1CFc2[k_][l + 3] = sTR[k_][l];
594 }
595 }
596 H2 = H2 * c1CFc2;
597
598 // Set up the error vector
599 e2[0] = Hp2[0] - c1P[i].get_x();
600 e2[1] = Hp2[1] - c1P[i].get_y();
601
602 x = Hp1[0];
603 y = Hp1[1];
604
605 Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z
606
607 H1[0][0] = -Z1;
608 H1[0][1] = 0;
609 H1[0][2] = x * Z1;
610 H1[1][0] = 0;
611 H1[1][1] = -Z1;
612 H1[1][2] = y * Z1;
613 H1[0][3] = x * y;
614 H1[0][4] = -(1 + x * x);
615 H1[0][5] = y;
616 H1[1][3] = 1 + y * y;
617 H1[1][4] = -x * y;
618 H1[1][5] = -x;
619
620 // Set up the error vector
621 e1[0] = Hp1[0] - c2P[i].get_x();
622 e1[1] = Hp1[1] - c2P[i].get_y();
623
624 if (only_2) {
625 if (k == 0) {
626 L = H2;
627 e = e2;
628 } else {
629 L = vpMatrix::stack(L, H2);
630 e = vpColVector::stack(e, e2);
631 }
632 } else if (only_1) {
633 if (k == 0) {
634 L = H1;
635 e = e1;
636 } else {
637 L = vpMatrix::stack(L, H1);
638 e = vpColVector::stack(e, e1);
639 }
640 } else {
641 if (k == 0) {
642 L = H2;
643 e = e2;
644 } else {
645 L = vpMatrix::stack(L, H2);
646 e = vpColVector::stack(e, e2);
647 }
648 L = vpMatrix::stack(L, H1);
649 e = vpColVector::stack(e, e1);
650 }
651
652 k++;
653 }
654
655 if (userobust) {
656 for (unsigned int k_ = 0; k_ < n; k_++) {
657 res[k_] = vpMath::sqr(e[2 * k_]) + vpMath::sqr(e[2 * k_ + 1]);
658 }
659 robust.MEstimator(vpRobust::TUKEY, res, w);
660
661 // compute the pseudo inverse of the interaction matrix
662 for (unsigned int k_ = 0; k_ < n; k_++) {
663 W[2 * k_][2 * k_] = w[k_];
664 W[2 * k_ + 1][2 * k_ + 1] = w[k_];
665 }
666 } else {
667 for (unsigned int k_ = 0; k_ < 2 * n; k_++)
668 W[k_][k_] = 1;
669 }
670 (W * L).pseudoInverse(Lp, 1e-16);
671 // Compute the camera velocity
672 vpColVector c2Tcc1;
673
674 c2Tcc1 = -1 * Lp * W * e;
675
676 // only for simulation
677
678 c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1;
679 // UpdatePose2(c2Tcc1, c2Mc1) ;
680 r = (W * e).sumSquare();
681
682 if (r < 1e-15) {
683 break;
684 }
685 if (iter > 1000) {
686 break;
687 }
688 if (r > r_1) {
689 break;
690 }
691 iter++;
692 }
693
694 return (W * e).sumSquare();
695 }
696
697 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
698