Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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 
123  vpRobust robust(n);
124  vpColVector res(n);
125  vpColVector w(n);
126  w = 1;
127  robust.setThreshold(0.0000);
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  robust.setIteration(0);
227 
228  for (unsigned int l = 0; l < n; l++) {
229  res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]);
230  }
231  robust.MEstimator(vpRobust::TUKEY, res, w);
232 
233  // compute the pseudo inverse of the interaction matrix
234  for (unsigned int l = 0; l < n; l++) {
235  W[2 * l][2 * l] = w[l];
236  W[2 * l + 1][2 * l + 1] = w[l];
237  }
238  } else {
239  for (unsigned int l = 0; l < 2 * n; l++)
240  W[l][l] = 1;
241  }
242  // CreateDiagonalMatrix(w, W) ;
243  (L).pseudoInverse(Lp, 1e-6);
244  // Compute the camera velocity
245  vpColVector c2rc1, v(6);
246 
247  c2rc1 = -2 * Lp * W * e;
248  for (unsigned int i = 0; i < 3; i++)
249  v[i + 3] = c2rc1[i];
250 
251  // only for simulation
252 
253  updatePoseRotation(c2rc1, c2Mc1);
254  r = e.sumSquare();
255 
256  if ((W * e).sumSquare() < 1e-10)
257  break;
258  if (iter > 25)
259  break;
260  iter++; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e="
261  // <<(W*e).sumSquare() <<std::endl ;
262  }
263 
264  // std::cout << c2Mc1 <<std::endl ;
265  return (W * e).sumSquare();
266 }
267 
268 static void getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, vpColVector &cN, double &cd)
269 {
270  double A1 = cMo[0][0] * oN.getA() + cMo[0][1] * oN.getB() + cMo[0][2] * oN.getC();
271  double B1 = cMo[1][0] * oN.getA() + cMo[1][1] * oN.getB() + cMo[1][2] * oN.getC();
272  double C1 = cMo[2][0] * oN.getA() + cMo[2][1] * oN.getB() + cMo[2][2] * oN.getC();
273  double D1 = oN.getD() - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1);
274 
275  cN.resize(3);
276  cN[0] = A1;
277  cN[1] = B1;
278  cN[2] = C1;
279  cd = -D1;
280 }
281 
282 double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane &oN,
283  vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust)
284 {
285  vpColVector e(2);
286  double r_1 = -1;
287 
288  vpColVector p2(3);
289  vpColVector p1(3);
290  vpColVector Hp2(3);
291  vpColVector Hp1(3);
292 
293  vpMatrix H2(2, 6);
294  vpColVector e2(2);
295  vpMatrix H1(2, 6);
296  vpColVector e1(2);
297 
298  bool only_1 = true;
299  bool only_2 = false;
300  int iter = 0;
301  unsigned int n = 0;
302  n = nbpoint;
303 
304  // next 2 lines are useless (detected by Coverity Scan)
305  // if ( (! only_1) && (! only_2) )
306  // n *=2 ;
307 
308  vpRobust robust(n);
309  vpColVector res(n);
310  vpColVector w(n);
311  w = 1;
312  robust.setThreshold(0.0000);
313  vpMatrix W(2 * n, 2 * n);
314  W = 0;
315 
316  vpColVector N1(3), N2(3);
317  double d1, d2;
318 
319  double r = 1e10;
320  iter = 0;
321  while (vpMath::equal(r_1, r, threshold_displacement) == false) {
322  r_1 = r;
323  // compute current position
324 
325  // Change frame (current)
326  vpHomogeneousMatrix c1Mc2, c2Mo;
327  vpRotationMatrix c1Rc2, c2Rc1;
328  vpTranslationVector c1Tc2, c2Tc1;
329  c1Mc2 = c2Mc1.inverse();
330  c2Mc1.extract(c2Rc1);
331  c2Mc1.extract(c2Tc1);
332  c2Mc1.extract(c1Rc2);
333  c1Mc2.extract(c1Tc2);
334 
335  c2Mo = c2Mc1 * c1Mo;
336 
337  getPlaneInfo(oN, c1Mo, N1, d1);
338  getPlaneInfo(oN, c2Mo, N2, d2);
339 
340  vpMatrix L(2, 3), Lp;
341  int k = 0;
342  for (unsigned int i = 0; i < nbpoint; i++) {
343  p2[0] = c2P[i].get_x();
344  p2[1] = c2P[i].get_y();
345  p2[2] = 1.0;
346  p1[0] = c1P[i].get_x();
347  p1[1] = c1P[i].get_y();
348  p1[2] = 1.0;
349 
350  vpMatrix H(3, 3);
351 
352  Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2 * N2.t()) / d2) * p2; // p2 = Hp1
353  Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1 * N1.t()) / d1) * p1; // p1 = Hp2
354 
355  Hp2 /= Hp2[2]; // normalisation
356  Hp1 /= Hp1[2];
357 
358  // set up the interaction matrix
359  double x = Hp2[0];
360  double y = Hp2[1];
361  double Z1;
362 
363  Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z
364 
365  H2[0][0] = -Z1;
366  H2[0][1] = 0;
367  H2[0][2] = x * Z1;
368  H2[1][0] = 0;
369  H2[1][1] = -Z1;
370  H2[1][2] = y * Z1;
371  H2[0][3] = x * y;
372  H2[0][4] = -(1 + x * x);
373  H2[0][5] = y;
374  H2[1][3] = 1 + y * y;
375  H2[1][4] = -x * y;
376  H2[1][5] = -x;
377  H2 *= -1;
378 
379  vpMatrix c1CFc2(6, 6);
380  {
381  vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2;
382  for (unsigned int k_ = 0; k_ < 3; k_++)
383  for (unsigned int l = 0; l < 3; l++) {
384  c1CFc2[k_][l] = c1Rc2[k_][l];
385  c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
386  c1CFc2[k_][l + 3] = sTR[k_][l];
387  }
388  }
389  H2 = H2 * c1CFc2;
390 
391  // Set up the error vector
392  e2[0] = Hp2[0] - c1P[i].get_x();
393  e2[1] = Hp2[1] - c1P[i].get_y();
394 
395  x = Hp1[0];
396  y = Hp1[1];
397 
398  Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z
399 
400  H1[0][0] = -Z1;
401  H1[0][1] = 0;
402  H1[0][2] = x * Z1;
403  H1[1][0] = 0;
404  H1[1][1] = -Z1;
405  H1[1][2] = y * Z1;
406  H1[0][3] = x * y;
407  H1[0][4] = -(1 + x * x);
408  H1[0][5] = y;
409  H1[1][3] = 1 + y * y;
410  H1[1][4] = -x * y;
411  H1[1][5] = -x;
412 
413  // Set up the error vector
414  e1[0] = Hp1[0] - c2P[i].get_x();
415  e1[1] = Hp1[1] - c2P[i].get_y();
416 
417  if (only_2) {
418  if (k == 0) {
419  L = H2;
420  e = e2;
421  } else {
422  L = vpMatrix::stack(L, H2);
423  e = vpColVector::stack(e, e2);
424  }
425  } else if (only_1) {
426  if (k == 0) {
427  L = H1;
428  e = e1;
429  } else {
430  L = vpMatrix::stack(L, H1);
431  e = vpColVector::stack(e, e1);
432  }
433  } else {
434  if (k == 0) {
435  L = H2;
436  e = e2;
437  } else {
438  L = vpMatrix::stack(L, H2);
439  e = vpColVector::stack(e, e2);
440  }
441  L = vpMatrix::stack(L, H1);
442  e = vpColVector::stack(e, e1);
443  }
444 
445  k++;
446  }
447 
448  if (userobust) {
449  robust.setIteration(0);
450  for (unsigned int l = 0; l < n; l++) {
451  res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]);
452  }
453  robust.MEstimator(vpRobust::TUKEY, res, w);
454 
455  // compute the pseudo inverse of the interaction matrix
456  for (unsigned int l = 0; l < n; l++) {
457  W[2 * l][2 * l] = w[l];
458  W[2 * l + 1][2 * l + 1] = w[l];
459  }
460  } else {
461  for (unsigned int l = 0; l < 2 * n; l++)
462  W[l][l] = 1;
463  }
464  (W * L).pseudoInverse(Lp, 1e-16);
465  // Compute the camera velocity
466  vpColVector c2Tcc1;
467 
468  c2Tcc1 = -1 * Lp * W * e;
469 
470  // only for simulation
471 
472  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1;
473  ;
474  // UpdatePose2(c2Tcc1, c2Mc1) ;
475  r = (W * e).sumSquare();
476 
477  if (r < 1e-15) {
478  break;
479  }
480  if (iter > 1000) {
481  break;
482  }
483  if (r > r_1) {
484  break;
485  }
486  iter++;
487  }
488 
489  return (W * e).sumSquare();
490 }
491 
492 double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane *oN,
493  vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust)
494 {
495 
496  vpColVector e(2);
497  double r_1 = -1;
498 
499  vpColVector p2(3);
500  vpColVector p1(3);
501  vpColVector Hp2(3);
502  vpColVector Hp1(3);
503 
504  vpMatrix H2(2, 6);
505  vpColVector e2(2);
506  vpMatrix H1(2, 6);
507  vpColVector e1(2);
508 
509  bool only_1 = true;
510  bool only_2 = false;
511  int iter = 0;
512  unsigned int i;
513  unsigned int n = 0;
514  n = nbpoint;
515 
516  // next 2 lines are useless (detected by Coverity Scan)
517  // if ( (! only_1) && (! only_2) )
518  // n *=2 ;
519 
520  vpRobust robust(n);
521  vpColVector res(n);
522  vpColVector w(n);
523  w = 1;
524  robust.setThreshold(0.0000);
525  vpMatrix W(2 * n, 2 * n);
526  W = 0;
527 
528  vpColVector N1(3), N2(3);
529  double d1, d2;
530 
531  double r = 1e10;
532  iter = 0;
533  while (vpMath::equal(r_1, r, threshold_displacement) == false) {
534  r_1 = r;
535  // compute current position
536 
537  // Change frame (current)
538  vpHomogeneousMatrix c1Mc2, c2Mo;
539  vpRotationMatrix c1Rc2, c2Rc1;
540  vpTranslationVector c1Tc2, c2Tc1;
541  c1Mc2 = c2Mc1.inverse();
542  c2Mc1.extract(c2Rc1);
543  c2Mc1.extract(c2Tc1);
544  c2Mc1.extract(c1Rc2);
545  c1Mc2.extract(c1Tc2);
546 
547  c2Mo = c2Mc1 * c1Mo;
548 
549  vpMatrix L(2, 3), Lp;
550  int k = 0;
551  for (i = 0; i < nbpoint; i++) {
552  getPlaneInfo(oN[i], c1Mo, N1, d1);
553  getPlaneInfo(oN[i], c2Mo, N2, d2);
554  p2[0] = c2P[i].get_x();
555  p2[1] = c2P[i].get_y();
556  p2[2] = 1.0;
557  p1[0] = c1P[i].get_x();
558  p1[1] = c1P[i].get_y();
559  p1[2] = 1.0;
560 
561  vpMatrix H(3, 3);
562 
563  Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2 * N2.t()) / d2) * p2; // p2 = Hp1
564  Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1 * N1.t()) / d1) * p1; // p1 = Hp2
565 
566  Hp2 /= Hp2[2]; // normalisation
567  Hp1 /= Hp1[2];
568 
569  // set up the interaction matrix
570  double x = Hp2[0];
571  double y = Hp2[1];
572  double Z1;
573 
574  Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z
575 
576  H2[0][0] = -Z1;
577  H2[0][1] = 0;
578  H2[0][2] = x * Z1;
579  H2[1][0] = 0;
580  H2[1][1] = -Z1;
581  H2[1][2] = y * Z1;
582  H2[0][3] = x * y;
583  H2[0][4] = -(1 + x * x);
584  H2[0][5] = y;
585  H2[1][3] = 1 + y * y;
586  H2[1][4] = -x * y;
587  H2[1][5] = -x;
588  H2 *= -1;
589 
590  vpMatrix c1CFc2(6, 6);
591  {
592  vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2;
593  for (unsigned int k_ = 0; k_ < 3; k_++)
594  for (unsigned int l = 0; l < 3; l++) {
595  c1CFc2[k_][l] = c1Rc2[k_][l];
596  c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
597  c1CFc2[k_][l + 3] = sTR[k_][l];
598  }
599  }
600  H2 = H2 * c1CFc2;
601 
602  // Set up the error vector
603  e2[0] = Hp2[0] - c1P[i].get_x();
604  e2[1] = Hp2[1] - c1P[i].get_y();
605 
606  x = Hp1[0];
607  y = Hp1[1];
608 
609  Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z
610 
611  H1[0][0] = -Z1;
612  H1[0][1] = 0;
613  H1[0][2] = x * Z1;
614  H1[1][0] = 0;
615  H1[1][1] = -Z1;
616  H1[1][2] = y * Z1;
617  H1[0][3] = x * y;
618  H1[0][4] = -(1 + x * x);
619  H1[0][5] = y;
620  H1[1][3] = 1 + y * y;
621  H1[1][4] = -x * y;
622  H1[1][5] = -x;
623 
624  // Set up the error vector
625  e1[0] = Hp1[0] - c2P[i].get_x();
626  e1[1] = Hp1[1] - c2P[i].get_y();
627 
628  if (only_2) {
629  if (k == 0) {
630  L = H2;
631  e = e2;
632  } else {
633  L = vpMatrix::stack(L, H2);
634  e = vpColVector::stack(e, e2);
635  }
636  } else if (only_1) {
637  if (k == 0) {
638  L = H1;
639  e = e1;
640  } else {
641  L = vpMatrix::stack(L, H1);
642  e = vpColVector::stack(e, e1);
643  }
644  } else {
645  if (k == 0) {
646  L = H2;
647  e = e2;
648  } else {
649  L = vpMatrix::stack(L, H2);
650  e = vpColVector::stack(e, e2);
651  }
652  L = vpMatrix::stack(L, H1);
653  e = vpColVector::stack(e, e1);
654  }
655 
656  k++;
657  }
658 
659  if (userobust) {
660  robust.setIteration(0);
661  for (unsigned int k_ = 0; k_ < n; k_++) {
662  res[k_] = vpMath::sqr(e[2 * k_]) + vpMath::sqr(e[2 * k_ + 1]);
663  }
664  robust.MEstimator(vpRobust::TUKEY, res, w);
665 
666  // compute the pseudo inverse of the interaction matrix
667  for (unsigned int k_ = 0; k_ < n; k_++) {
668  W[2 * k_][2 * k_] = w[k_];
669  W[2 * k_ + 1][2 * k_ + 1] = w[k_];
670  }
671  } else {
672  for (unsigned int k_ = 0; k_ < 2 * n; k_++)
673  W[k_][k_] = 1;
674  }
675  (W * L).pseudoInverse(Lp, 1e-16);
676  // Compute the camera velocity
677  vpColVector c2Tcc1;
678 
679  c2Tcc1 = -1 * Lp * W * e;
680 
681  // only for simulation
682 
683  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1;
684  ;
685  // UpdatePose2(c2Tcc1, c2Mc1) ;
686  r = (W * e).sumSquare();
687 
688  if (r < 1e-15) {
689  break;
690  }
691  if (iter > 1000) {
692  break;
693  }
694  if (r > r_1) {
695  break;
696  }
697  iter++;
698  }
699 
700  return (W * e).sumSquare();
701 }
702 
703 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
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:296
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4800
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Class that defines what is a point.
Definition: vpPoint.h:58
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
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:114
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:431
static vpHomogeneousMatrix direct(const vpColVector &v)
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:433
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
double getC() const
Definition: vpPlane.h:106
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
Class that consider the case of a translation vector.