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