ViSP  2.8.0
vpHomographyVVS.cpp
1 /****************************************************************************
2  *
3  * $Id: vpHomographyVVS.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Homography transformation.
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 //#include <computeHomography.h>
43 //#include <utilsHomography.h>
44 #include <visp/vpRobust.h>
45 #include <visp/vpHomogeneousMatrix.h>
46 #include <visp/vpHomography.h>
47 #include <visp/vpMath.h>
48 #include <visp/vpMatrix.h>
49 #include <visp/vpPoint.h>
50 #include <visp/vpPlane.h>
51 #include <iostream>
52 #include <visp/vpExponentialMap.h>
53 
54 
55 const double vpHomography::threshold_rotation = 1e-7;
56 const double vpHomography::threshold_displacement = 1e-18;
57 
58 static void
59 updatePoseRotation(vpColVector& dx,vpHomogeneousMatrix& mati)
60 {
61  unsigned int i,j;
62 
63  double sinu,cosi,mcosi,u[3], s;
64  vpRotationMatrix rd ;
65 
66 
67  s = sqrt(dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2]);
68  if (s > 1.e-25)
69  {
70 
71  for (i=0;i<3;i++) u[i] = dx[i]/s;
72  sinu = sin(s);
73  cosi = cos(s);
74  mcosi = 1-cosi;
75  rd[0][0] = cosi + mcosi*u[0]*u[0];
76  rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
77  rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
78  rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
79  rd[1][1] = cosi + mcosi*u[1]*u[1];
80  rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
81  rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
82  rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
83  rd[2][2] = cosi + mcosi*u[2]*u[2];
84  }
85  else
86  {
87  for (i=0;i<3;i++)
88  {
89  for (j=0;j<3;j++) rd[i][j] = 0.0;
90  rd[i][i] = 1.0;
91  }
92 
93  }
94 
95  vpHomogeneousMatrix Delta ;
96  Delta.insert(rd) ;
97 
98  mati = Delta.inverse() * mati ;
99 }
100 
101 double
102 vpHomography::computeRotation(unsigned int nbpoint,
103  vpPoint *c1P,
104  vpPoint *c2P,
105  vpHomogeneousMatrix &c2Mc1,
106  int userobust
107  )
108 {
109  vpColVector e(2) ;
110  double r_1 = -1 ;
111 
112  vpColVector p2(3) ;
113  vpColVector p1(3) ;
114  vpColVector Hp2(3) ;
115  vpColVector Hp1(3) ;
116 
117  vpMatrix H2(2,3) ;
118  vpColVector e2(2) ;
119  vpMatrix H1(2,3) ;
120  vpColVector e1(2) ;
121 
122  int only_1 = 0 ;
123  int only_2 = 0 ;
124  int iter = 0 ;
125 
126  unsigned int n=0 ;
127  for (unsigned int i=0 ; i < nbpoint ; i++) {
128  // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
129  if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
130  &&
131  (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
132  {
133  n++ ;
134  }
135  }
136  if ((only_1==1) || (only_2==1)) ; else n *=2 ;
137 
138  vpRobust robust(n);
139  vpColVector res(n) ;
140  vpColVector w(n) ;
141  w =1 ;
142  robust.setThreshold(0.0000) ;
143  vpMatrix W(2*n,2*n) ;
144  W = 0 ;
145  vpMatrix c2Rc1(3,3) ;
146  double r =0 ;
147  while (vpMath::equal(r_1,r,threshold_rotation) == false )
148  {
149 
150  r_1 =r ;
151  // compute current position
152 
153  //Change frame (current)
154  for (unsigned int i=0 ; i < 3 ; i++)
155  for (unsigned int j=0 ; j < 3 ; j++)
156  c2Rc1[i][j] = c2Mc1[i][j] ;
157 
158  vpMatrix L(2,3), Lp ;
159  int k =0 ;
160  for (unsigned int i=0 ; i < nbpoint ; i++) {
161  //if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
162  if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
163  &&
164  (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
165  {
166  p2[0] = c2P[i].get_x() ;
167  p2[1] = c2P[i].get_y() ;
168  p2[2] = 1.0 ;
169  p1[0] = c1P[i].get_x() ;
170  p1[1] = c1P[i].get_y() ;
171  p1[2] = 1.0 ;
172 
173  Hp2 = c2Rc1.t()*p2 ; // p2 = Hp1
174  Hp1 = c2Rc1*p1 ; // p1 = Hp2
175 
176  Hp2 /= Hp2[2] ; // normalisation
177  Hp1 /= Hp1[2] ;
178 
179 
180  // set up the interaction matrix
181  double x = Hp2[0] ;
182  double y = Hp2[1] ;
183 
184  H2[0][0] = x*y ; H2[0][1] = -(1+x*x) ; H2[0][2] = y ;
185  H2[1][0] = 1+y*y ; H2[1][1] = -x*y ; H2[1][2] = -x ;
186  H2 *=-1 ;
187  H2 = H2*c2Rc1.t() ;
188 
189  // Set up the error vector
190  e2[0] = Hp2[0] - c1P[i].get_x() ;
191  e2[1] = Hp2[1] - c1P[i].get_y() ;
192 
193  // set up the interaction matrix
194  x = Hp1[0] ;
195  y = Hp1[1] ;
196 
197  H1[0][0] = x*y ; H1[0][1] = -(1+x*x) ; H1[0][2] = y ;
198  H1[1][0] = 1+y*y ; H1[1][1] = -x*y ; H1[1][2] = -x ;
199 
200  // Set up the error vector
201  e1[0] = Hp1[0] - c2P[i].get_x() ;
202  e1[1] = Hp1[1] - c2P[i].get_y() ;
203 
204  if (only_2==1)
205  {
206  if (k == 0) { L = H2 ; e = e2 ; }
207  else
208  {
209  L = vpMatrix::stackMatrices(L,H2) ;
210  e = vpMatrix::stackMatrices(e,e2) ;
211  }
212  }
213  else
214  if (only_1==1)
215  {
216  if (k == 0) { L = H1 ; e= e1 ; }
217  else
218  {
219  L = vpMatrix::stackMatrices(L,H1) ;
220  e = vpMatrix::stackMatrices(e,e1) ;
221  }
222  }
223  else
224  {
225  if (k == 0) {L = H2 ; e = e2 ; }
226  else
227  {
228  L = vpMatrix::stackMatrices(L,H2) ;
229  e = vpMatrix::stackMatrices(e,e2) ;
230  }
231  L = vpMatrix::stackMatrices(L,H1) ;
232  e = vpMatrix::stackMatrices(e,e1) ;
233  }
234 
235  k++ ;
236  }
237  }
238 
239  if (userobust)
240  {
241  robust.setIteration(0);
242 
243  for (unsigned int k=0 ; k < n ; k++)
244  {
245  res[k] = vpMath::sqr(e[2*k]) + vpMath::sqr(e[2*k+1]) ;
246  }
247  robust.MEstimator(vpRobust::TUKEY, res, w);
248 
249 
250  // compute the pseudo inverse of the interaction matrix
251  for (unsigned int k=0 ; k < n ; k++)
252  {
253  W[2*k][2*k] = w[k] ;
254  W[2*k+1][2*k+1] = w[k] ;
255  }
256  }
257  else
258  {
259  for (unsigned int k=0 ; k < 2*n ; k++) W[k][k] = 1 ;
260  }
261  // CreateDiagonalMatrix(w, W) ;
262  (L).pseudoInverse(Lp, 1e-6) ;
263  // Compute the camera velocity
264  vpColVector c2Rc1, v(6) ;
265 
266  c2Rc1 = -2*Lp*W*e ;
267  for (unsigned int i=0 ; i < 3 ; i++) v[i+3] = c2Rc1[i] ;
268 
269  // only for simulation
270 
271  updatePoseRotation(c2Rc1, c2Mc1) ;
272  r =e.sumSquare() ;
273 
274  if ((W*e).sumSquare() < 1e-10) break ;
275  if (iter>25) break ;
276  iter++ ; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e=" <<(W*e).sumSquare() <<std::endl ;
277 
278  }
279 
280  // std::cout << c2Mc1 <<std::endl ;
281  return (W*e).sumSquare() ;
282 }
283 
284 
285 static void
286 getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, vpColVector &cN, double &cd)
287 {
288  double A1 = cMo[0][0]*oN.getA()+ cMo[0][1]*oN.getB() + cMo[0][2]*oN.getC();
289  double B1 = cMo[1][0]*oN.getA()+ cMo[1][1]*oN.getB() + cMo[1][2]*oN.getC();
290  double C1 = cMo[2][0]*oN.getA()+ cMo[2][1]*oN.getB() + cMo[2][2]*oN.getC();
291  double D1 = oN.getD() - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
292 
293  cN.resize(3) ;
294  cN[0] = A1 ;
295  cN[1] = B1 ;
296  cN[2] = C1 ;
297  cd = -D1 ;
298 }
299 
300 double
301 vpHomography::computeDisplacement(unsigned int nbpoint,
302  vpPoint *c1P,
303  vpPoint *c2P,
304  vpPlane &oN,
305  vpHomogeneousMatrix &c2Mc1,
306  vpHomogeneousMatrix &c1Mo,
307  int userobust
308  )
309 {
310 
311 
312  vpColVector e(2) ;
313  double r_1 = -1 ;
314 
315 
316 
317  vpColVector p2(3) ;
318  vpColVector p1(3) ;
319  vpColVector Hp2(3) ;
320  vpColVector Hp1(3) ;
321 
322  vpMatrix H2(2,6) ;
323  vpColVector e2(2) ;
324  vpMatrix H1(2,6) ;
325  vpColVector e1(2) ;
326 
327  int only_1 = 1 ;
328  int only_2 = 0 ;
329  int iter = 0 ;
330  unsigned int i ;
331  unsigned int n=0 ;
332  n = nbpoint ;
333  if ((only_1==1) || (only_2==1)) ; else n *=2 ;
334 
335  vpRobust robust(n);
336  vpColVector res(n) ;
337  vpColVector w(n) ;
338  w =1 ;
339  robust.setThreshold(0.0000) ;
340  vpMatrix W(2*n,2*n) ;
341  W = 0 ;
342 
343  vpColVector N1(3), N2(3) ;
344  double d1, d2 ;
345 
346  double r =1e10 ;
347  iter =0 ;
348  while (vpMath::equal(r_1,r,threshold_displacement) == false )
349  {
350 
351  r_1 =r ;
352  // compute current position
353 
354 
355  //Change frame (current)
356  vpHomogeneousMatrix c1Mc2, c2Mo ;
357  vpRotationMatrix c1Rc2, c2Rc1 ;
358  vpTranslationVector c1Tc2, c2Tc1 ;
359  c1Mc2 = c2Mc1.inverse() ;
360  c2Mc1.extract(c2Rc1) ;
361  c2Mc1.extract(c2Tc1) ;
362  c2Mc1.extract(c1Rc2) ;
363  c1Mc2.extract(c1Tc2) ;
364 
365  c2Mo = c2Mc1*c1Mo ;
366 
367  getPlaneInfo(oN, c1Mo, N1, d1) ;
368  getPlaneInfo(oN, c2Mo, N2, d2) ;
369 
370 
371  vpMatrix L(2,3), Lp ;
372  int k =0 ;
373  for (i=0 ; i < nbpoint ; i++)
374  {
375  p2[0] = c2P[i].get_x() ;
376  p2[1] = c2P[i].get_y() ;
377  p2[2] = 1.0 ;
378  p1[0] = c1P[i].get_x() ;
379  p1[1] = c1P[i].get_y() ;
380  p1[2] = 1.0 ;
381 
382  vpMatrix H(3,3) ;
383 
384  Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
385  Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
386 
387  Hp2 /= Hp2[2] ; // normalisation
388  Hp1 /= Hp1[2] ;
389 
390 
391  // set up the interaction matrix
392  double x = Hp2[0] ;
393  double y = Hp2[1] ;
394  double Z1 ;
395 
396  Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
397 
398 
399  H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
400  H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
401  H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
402  H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
403  H2 *=-1 ;
404 
405  vpMatrix c1CFc2(6,6) ;
406  {
407  vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
408  for (unsigned int k=0 ; k < 3 ; k++)
409  for (unsigned int l=0 ; l<3 ; l++)
410  {
411  c1CFc2[k][l] = c1Rc2[k][l] ;
412  c1CFc2[k+3][l+3] = c1Rc2[k][l] ;
413  c1CFc2[k][l+3] = sTR[k][l] ;
414  }
415  }
416  H2 = H2*c1CFc2 ;
417 
418 
419 
420  // Set up the error vector
421  e2[0] = Hp2[0] - c1P[i].get_x() ;
422  e2[1] = Hp2[1] - c1P[i].get_y() ;
423 
424  x = Hp1[0] ;
425  y = Hp1[1] ;
426 
427  Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
428 
429  H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
430  H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
431  H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
432  H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
433 
434  // Set up the error vector
435  e1[0] = Hp1[0] - c2P[i].get_x() ;
436  e1[1] = Hp1[1] - c2P[i].get_y() ;
437 
438 
439  if (only_2==1)
440  {
441  if (k == 0) { L = H2 ; e = e2 ; }
442  else
443  {
444  L = vpMatrix::stackMatrices(L,H2) ;
445  e = vpMatrix::stackMatrices(e,e2) ;
446  }
447  }
448  else
449  if (only_1==1)
450  {
451  if (k == 0) { L = H1 ; e= e1 ; }
452  else
453  {
454  L = vpMatrix::stackMatrices(L,H1) ;
455  e = vpMatrix::stackMatrices(e,e1) ;
456  }
457  }
458  else
459  {
460  if (k == 0) {L = H2 ; e = e2 ; }
461  else
462  {
463  L = vpMatrix::stackMatrices(L,H2) ;
464  e = vpMatrix::stackMatrices(e,e2) ;
465  }
466  L = vpMatrix::stackMatrices(L,H1) ;
467  e = vpMatrix::stackMatrices(e,e1) ;
468  }
469 
470 
471  k++ ;
472  }
473 
474  if (userobust)
475  {
476  robust.setIteration(0);
477  for (unsigned int k=0 ; k < n ; k++)
478  {
479  res[k] = vpMath::sqr(e[2*k]) + vpMath::sqr(e[2*k+1]) ;
480  }
481  robust.MEstimator(vpRobust::TUKEY, res, w);
482 
483 
484  // compute the pseudo inverse of the interaction matrix
485  for (unsigned int k=0 ; k < n ; k++)
486  {
487  W[2*k][2*k] = w[k] ;
488  W[2*k+1][2*k+1] = w[k] ;
489  }
490  }
491  else
492  {
493  for (unsigned int k=0 ; k < 2*n ; k++) W[k][k] = 1 ;
494  }
495  (W*L).pseudoInverse(Lp, 1e-16) ;
496  // Compute the camera velocity
497  vpColVector c2Tcc1 ;
498 
499  c2Tcc1 = -1*Lp*W*e ;
500 
501  // only for simulation
502 
503  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
504  // UpdatePose2(c2Tcc1, c2Mc1) ;
505  r =(W*e).sumSquare() ;
506 
507 
508 
509  if (r < 1e-15) {break ; }
510  if (iter>1000){break ; }
511  if (r>r_1) { break ; }
512  iter++ ;
513  }
514 
515  return (W*e).sumSquare() ;
516 
517 }
518 
519 
520 double
521 vpHomography::computeDisplacement(unsigned int nbpoint,
522  vpPoint *c1P,
523  vpPoint *c2P,
524  vpPlane *oN,
525  vpHomogeneousMatrix &c2Mc1,
526  vpHomogeneousMatrix &c1Mo,
527  int userobust
528  )
529 {
530 
531 
532  vpColVector e(2) ;
533  double r_1 = -1 ;
534 
535 
536 
537  vpColVector p2(3) ;
538  vpColVector p1(3) ;
539  vpColVector Hp2(3) ;
540  vpColVector Hp1(3) ;
541 
542  vpMatrix H2(2,6) ;
543  vpColVector e2(2) ;
544  vpMatrix H1(2,6) ;
545  vpColVector e1(2) ;
546 
547 
548  int only_1 = 1 ;
549  int only_2 = 0 ;
550  int iter = 0 ;
551  unsigned int i ;
552  unsigned int n=0 ;
553  n = nbpoint ;
554  if ((only_1==1) || (only_2==1)) ; else n *=2 ;
555 
556  vpRobust robust(n);
557  vpColVector res(n) ;
558  vpColVector w(n) ;
559  w =1 ;
560  robust.setThreshold(0.0000) ;
561  vpMatrix W(2*n,2*n) ;
562  W = 0 ;
563 
564  vpColVector N1(3), N2(3) ;
565  double d1, d2 ;
566 
567  double r =1e10 ;
568  iter =0 ;
569  while (vpMath::equal(r_1,r,threshold_displacement) == false )
570  {
571 
572  r_1 =r ;
573  // compute current position
574 
575 
576  //Change frame (current)
577  vpHomogeneousMatrix c1Mc2, c2Mo ;
578  vpRotationMatrix c1Rc2, c2Rc1 ;
579  vpTranslationVector c1Tc2, c2Tc1 ;
580  c1Mc2 = c2Mc1.inverse() ;
581  c2Mc1.extract(c2Rc1) ;
582  c2Mc1.extract(c2Tc1) ;
583  c2Mc1.extract(c1Rc2) ;
584  c1Mc2.extract(c1Tc2) ;
585 
586  c2Mo = c2Mc1*c1Mo ;
587 
588 
589 
590  vpMatrix L(2,3), Lp ;
591  int k =0 ;
592  for (i=0 ; i < nbpoint ; i++)
593  {
594  getPlaneInfo(oN[i], c1Mo, N1, d1) ;
595  getPlaneInfo(oN[i], c2Mo, N2, d2) ;
596  p2[0] = c2P[i].get_x() ;
597  p2[1] = c2P[i].get_y() ;
598  p2[2] = 1.0 ;
599  p1[0] = c1P[i].get_x() ;
600  p1[1] = c1P[i].get_y() ;
601  p1[2] = 1.0 ;
602 
603  vpMatrix H(3,3) ;
604 
605  Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
606  Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
607 
608  Hp2 /= Hp2[2] ; // normalisation
609  Hp1 /= Hp1[2] ;
610 
611 
612  // set up the interaction matrix
613  double x = Hp2[0] ;
614  double y = Hp2[1] ;
615  double Z1 ;
616 
617  Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
618 
619 
620  H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
621  H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
622  H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
623  H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
624  H2 *=-1 ;
625 
626  vpMatrix c1CFc2(6,6) ;
627  {
628  vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
629  for (unsigned int k=0 ; k < 3 ; k++)
630  for (unsigned int l=0 ; l<3 ; l++)
631  {
632  c1CFc2[k][l] = c1Rc2[k][l] ;
633  c1CFc2[k+3][l+3] = c1Rc2[k][l] ;
634  c1CFc2[k][l+3] = sTR[k][l] ;
635  }
636  }
637  H2 = H2*c1CFc2 ;
638 
639 
640 
641  // Set up the error vector
642  e2[0] = Hp2[0] - c1P[i].get_x() ;
643  e2[1] = Hp2[1] - c1P[i].get_y() ;
644 
645  x = Hp1[0] ;
646  y = Hp1[1] ;
647 
648  Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
649 
650  H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
651  H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
652  H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
653  H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
654 
655  // Set up the error vector
656  e1[0] = Hp1[0] - c2P[i].get_x() ;
657  e1[1] = Hp1[1] - c2P[i].get_y() ;
658 
659 
660  if (only_2==1)
661  {
662  if (k == 0) { L = H2 ; e = e2 ; }
663  else
664  {
665  L = vpMatrix::stackMatrices(L,H2) ;
666  e = vpMatrix::stackMatrices(e,e2) ;
667  }
668  }
669  else
670  if (only_1==1)
671  {
672  if (k == 0) { L = H1 ; e= e1 ; }
673  else
674  {
675  L = vpMatrix::stackMatrices(L,H1) ;
676  e = vpMatrix::stackMatrices(e,e1) ;
677  }
678  }
679  else
680  {
681  if (k == 0) {L = H2 ; e = e2 ; }
682  else
683  {
684  L = vpMatrix::stackMatrices(L,H2) ;
685  e = vpMatrix::stackMatrices(e,e2) ;
686  }
687  L = vpMatrix::stackMatrices(L,H1) ;
688  e = vpMatrix::stackMatrices(e,e1) ;
689  }
690 
691 
692  k++ ;
693  }
694 
695  if (userobust)
696  {
697  robust.setIteration(0);
698  for (unsigned int k=0 ; k < n ; k++)
699  {
700  res[k] = vpMath::sqr(e[2*k]) + vpMath::sqr(e[2*k+1]) ;
701  }
702  robust.MEstimator(vpRobust::TUKEY, res, w);
703 
704 
705  // compute the pseudo inverse of the interaction matrix
706  for (unsigned int k=0 ; k < n ; k++)
707  {
708  W[2*k][2*k] = w[k] ;
709  W[2*k+1][2*k+1] = w[k] ;
710  }
711  }
712  else
713  {
714  for (unsigned int k=0 ; k < 2*n ; k++) W[k][k] = 1 ;
715  }
716  (W*L).pseudoInverse(Lp, 1e-16) ;
717  // Compute the camera velocity
718  vpColVector c2Tcc1 ;
719 
720  c2Tcc1 = -1*Lp*W*e ;
721 
722  // only for simulation
723 
724  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
725  // UpdatePose2(c2Tcc1, c2Mc1) ;
726  r =(W*e).sumSquare() ;
727 
728 
729 
730  if (r < 1e-15) {break ; }
731  if (iter>1000){break ; }
732  if (r>r_1) { break ; }
733  iter++ ;
734  }
735 
736  return (W*e).sumSquare() ;
737 
738 }
739 
vpMatrix
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
vpRobust::MEstimator
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:138
vpHomogeneousMatrix
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Definition: vpHomogeneousMatrix.h:102
vpMath::equal
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:275
vpPoint::get_y
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
vpMatrix::sumSquare
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:760
vpMatrix::vpMatrix
vpMatrix()
Basic constructor.
Definition: vpMatrix.cpp:111
vpHomography::computeDisplacement
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Definition: vpHomographyExtract.cpp:671
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:65
vpMath::maximum
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
vpRotationMatrix
The vpRotationMatrix considers the particular case of a rotation matrix.
Definition: vpRotationMatrix.h:72
vpHomogeneousMatrix::insert
void insert(const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:344
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:106
vpColVector::t
vpRowVector t() const
transpose of Vector
Definition: vpColVector.cpp:307
vpHomography::computeRotation
static double computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpHomogeneousMatrix &c2Mc1, int userobust)
Definition: vpHomographyVVS.cpp:102
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:298
vpPoint::get_x
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
vpMatrix::stackMatrices
static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2263
vpMatrix::t
vpMatrix t() const
Definition: vpMatrix.cpp:1176
vpColVector
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:403
vpPlane::getB
double getB() const
Definition: vpPlane.h:114
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:65
vpRobust
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:63
vpPlane::getA
double getA() const
Definition: vpPlane.h:112
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1812
vpPlane::getC
double getC() const
Definition: vpPlane.h:116
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
vpRobust::setThreshold
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:128
vpRobust::setIteration
void setIteration(const unsigned int iter)
Set iteration.
Definition: vpRobust.h:122
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:95
vpPlane::getD
double getD() const
Definition: vpPlane.h:118
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94