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