Visual Servoing Platform  version 3.4.0 under development (2021-04-14)
vpHomographyVVS.cpp
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 
56 static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati)
57 {
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 
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 
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 
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 
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 
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 
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 
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
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRowVector t() const
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:293
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
void insert(const vpRotationMatrix &R)
double getD() const
Definition: vpPlane.h:108
static double sqr(double x)
Definition: vpMath.h:116
double getB() const
Definition: vpPlane.h:104
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 > &inlier, double &residual, double weights_threshold=0.4, unsigned int niter=4, bool normalization=true)
double getA() const
Definition: vpPlane.h:102
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
static vpHomogeneousMatrix direct(const vpColVector &v)
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
Tukey influence function.
Definition: vpRobust.h:93
double getC() const
Definition: vpPlane.h:106
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Class that consider the case of a translation vector.