Visual Servoing Platform  version 3.0.0
vpHomographyVVS.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 //#include <computeHomography.h>
39 //#include <utilsHomography.h>
40 #include <visp3/core/vpRobust.h>
41 #include <visp3/core/vpHomogeneousMatrix.h>
42 #include <visp3/vision/vpHomography.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpMatrix.h>
45 #include <visp3/core/vpPoint.h>
46 #include <visp3/core/vpPlane.h>
47 #include <iostream>
48 #include <visp3/core/vpExponentialMap.h>
49 
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
57 updatePoseRotation(vpColVector& dx,vpHomogeneousMatrix& mati)
58 {
59  unsigned int i,j;
60 
61  double sinu,cosi,mcosi,u[3], s;
62  vpRotationMatrix rd ;
63 
64 
65  s = sqrt(dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2]);
66  if (s > 1.e-25)
67  {
68 
69  for (i=0;i<3;i++) u[i] = dx[i]/s;
70  sinu = sin(s);
71  cosi = cos(s);
72  mcosi = 1-cosi;
73  rd[0][0] = cosi + mcosi*u[0]*u[0];
74  rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
75  rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
76  rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
77  rd[1][1] = cosi + mcosi*u[1]*u[1];
78  rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
79  rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
80  rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
81  rd[2][2] = cosi + mcosi*u[2]*u[2];
82  }
83  else
84  {
85  for (i=0;i<3;i++)
86  {
87  for (j=0;j<3;j++) rd[i][j] = 0.0;
88  rd[i][i] = 1.0;
89  }
90 
91  }
92 
93  vpHomogeneousMatrix Delta ;
94  Delta.insert(rd) ;
95 
96  mati = Delta.inverse() * mati ;
97 }
98 
99 double
100 vpHomography::computeRotation(unsigned int nbpoint,
101  vpPoint *c1P,
102  vpPoint *c2P,
103  vpHomogeneousMatrix &c2Mc1,
104  int userobust
105  )
106 {
107  vpColVector e(2) ;
108  double r_1 = -1 ;
109 
110  vpColVector p2(3) ;
111  vpColVector p1(3) ;
112  vpColVector Hp2(3) ;
113  vpColVector Hp1(3) ;
114 
115  vpMatrix H2(2,3) ;
116  vpColVector e2(2) ;
117  vpMatrix H1(2,3) ;
118  vpColVector e1(2) ;
119 
120  int only_1 = 0 ;
121  int only_2 = 0 ;
122  int iter = 0 ;
123 
124  unsigned int n=0 ;
125  for (unsigned int i=0 ; i < nbpoint ; i++) {
126  // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
127  if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
128  &&
129  (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
130  {
131  n++ ;
132  }
133  }
134  //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
135  if ( (! only_1) && (! only_2) )
136  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::stack(L,H2) ;
210  e = vpColVector::stack(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::stack(L,H1) ;
220  e = vpColVector::stack(e,e1) ;
221  }
222  }
223  else
224  {
225  if (k == 0) {L = H2 ; e = e2 ; }
226  else
227  {
228  L = vpMatrix::stack(L,H2) ;
229  e = vpColVector::stack(e,e2) ;
230  }
231  L = vpMatrix::stack(L,H1) ;
232  e = vpColVector::stack(e,e1) ;
233  }
234 
235  k++ ;
236  }
237  }
238 
239  if (userobust)
240  {
241  robust.setIteration(0);
242 
243  for (unsigned int l=0 ; l < n ; l++)
244  {
245  res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
246  }
247  robust.MEstimator(vpRobust::TUKEY, res, w);
248 
249 
250  // compute the pseudo inverse of the interaction matrix
251  for (unsigned int l=0 ; l < n ; l++)
252  {
253  W[2*l][2*l] = w[l] ;
254  W[2*l+1][2*l+1] = w[l] ;
255  }
256  }
257  else
258  {
259  for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 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  vpColVector e(2) ;
311  double r_1 = -1 ;
312 
313  vpColVector p2(3) ;
314  vpColVector p1(3) ;
315  vpColVector Hp2(3) ;
316  vpColVector Hp1(3) ;
317 
318  vpMatrix H2(2,6) ;
319  vpColVector e2(2) ;
320  vpMatrix H1(2,6) ;
321  vpColVector e1(2) ;
322 
323  int only_1;
324  int only_2;
325  int iter = 0 ;
326  unsigned int n=0 ;
327  n = nbpoint ;
328  only_1 = 1 ;
329  only_2 = 0 ;
330 
331  //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
332  // Since only_1 is initialized to 1, the next 2 lines connt be reached. We remove them
333  //if ( (! only_1) && (! only_2) )
334  // n *=2 ;
335 
336  vpRobust robust(n);
337  vpColVector res(n) ;
338  vpColVector w(n) ;
339  w =1 ;
340  robust.setThreshold(0.0000) ;
341  vpMatrix W(2*n,2*n) ;
342  W = 0 ;
343 
344  vpColVector N1(3), N2(3) ;
345  double d1, d2 ;
346 
347  double r =1e10 ;
348  iter =0 ;
349  while (vpMath::equal(r_1,r,threshold_displacement) == false )
350  {
351  r_1 =r ;
352  // compute current position
353 
354  //Change frame (current)
355  vpHomogeneousMatrix c1Mc2, c2Mo ;
356  vpRotationMatrix c1Rc2, c2Rc1 ;
357  vpTranslationVector c1Tc2, c2Tc1 ;
358  c1Mc2 = c2Mc1.inverse() ;
359  c2Mc1.extract(c2Rc1) ;
360  c2Mc1.extract(c2Tc1) ;
361  c2Mc1.extract(c1Rc2) ;
362  c1Mc2.extract(c1Tc2) ;
363 
364  c2Mo = c2Mc1*c1Mo ;
365 
366  getPlaneInfo(oN, c1Mo, N1, d1) ;
367  getPlaneInfo(oN, c2Mo, N2, d2) ;
368 
369  vpMatrix L(2,3), Lp ;
370  int k =0 ;
371  for (unsigned int i=0 ; i < nbpoint ; i++)
372  {
373  p2[0] = c2P[i].get_x() ;
374  p2[1] = c2P[i].get_y() ;
375  p2[2] = 1.0 ;
376  p1[0] = c1P[i].get_x() ;
377  p1[1] = c1P[i].get_y() ;
378  p1[2] = 1.0 ;
379 
380  vpMatrix H(3,3) ;
381 
382  Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
383  Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
384 
385  Hp2 /= Hp2[2] ; // normalisation
386  Hp1 /= Hp1[2] ;
387 
388 
389  // set up the interaction matrix
390  double x = Hp2[0] ;
391  double y = Hp2[1] ;
392  double Z1 ;
393 
394  Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
395 
396 
397  H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
398  H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
399  H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
400  H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
401  H2 *=-1 ;
402 
403  vpMatrix c1CFc2(6,6) ;
404  {
405  vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
406  for (unsigned int k_=0 ; k_ < 3 ; k_++)
407  for (unsigned int l=0 ; l<3 ; l++)
408  {
409  c1CFc2[k_][l] = c1Rc2[k_][l] ;
410  c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
411  c1CFc2[k_][l+3] = sTR[k_][l] ;
412  }
413  }
414  H2 = H2*c1CFc2 ;
415 
416  // Set up the error vector
417  e2[0] = Hp2[0] - c1P[i].get_x() ;
418  e2[1] = Hp2[1] - c1P[i].get_y() ;
419 
420  x = Hp1[0] ;
421  y = Hp1[1] ;
422 
423  Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
424 
425  H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
426  H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
427  H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
428  H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
429 
430  // Set up the error vector
431  e1[0] = Hp1[0] - c2P[i].get_x() ;
432  e1[1] = Hp1[1] - c2P[i].get_y() ;
433 
434  if (only_2==1)
435  {
436  if (k == 0) { L = H2 ; e = e2 ; }
437  else
438  {
439  L = vpMatrix::stack(L,H2) ;
440  e = vpColVector::stack(e,e2) ;
441  }
442  }
443  else
444  if (only_1==1)
445  {
446  if (k == 0) { L = H1 ; e= e1 ; }
447  else
448  {
449  L = vpMatrix::stack(L,H1) ;
450  e = vpColVector::stack(e,e1) ;
451  }
452  }
453  else
454  {
455  if (k == 0) {L = H2 ; e = e2 ; }
456  else
457  {
458  L = vpMatrix::stack(L,H2) ;
459  e = vpColVector::stack(e,e2) ;
460  }
461  L = vpMatrix::stack(L,H1) ;
462  e = vpColVector::stack(e,e1) ;
463  }
464 
465 
466  k++ ;
467  }
468 
469  if (userobust)
470  {
471  robust.setIteration(0);
472  for (unsigned int l=0 ; l < n ; l++)
473  {
474  res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
475  }
476  robust.MEstimator(vpRobust::TUKEY, res, w);
477 
478  // compute the pseudo inverse of the interaction matrix
479  for (unsigned int l=0 ; l < n ; l++)
480  {
481  W[2*l][2*l] = w[l] ;
482  W[2*l+1][2*l+1] = w[l] ;
483  }
484  }
485  else
486  {
487  for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
488  }
489  (W*L).pseudoInverse(Lp, 1e-16) ;
490  // Compute the camera velocity
491  vpColVector c2Tcc1 ;
492 
493  c2Tcc1 = -1*Lp*W*e ;
494 
495  // only for simulation
496 
497  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
498  // UpdatePose2(c2Tcc1, c2Mc1) ;
499  r =(W*e).sumSquare() ;
500 
501  if (r < 1e-15) {break ; }
502  if (iter>1000){break ; }
503  if (r>r_1) { break ; }
504  iter++ ;
505  }
506 
507  return (W*e).sumSquare() ;
508 
509 }
510 
511 
512 double
513 vpHomography::computeDisplacement(unsigned int nbpoint,
514  vpPoint *c1P,
515  vpPoint *c2P,
516  vpPlane *oN,
517  vpHomogeneousMatrix &c2Mc1,
518  vpHomogeneousMatrix &c1Mo,
519  int userobust
520  )
521 {
522 
523 
524  vpColVector e(2) ;
525  double r_1 = -1 ;
526 
527  vpColVector p2(3) ;
528  vpColVector p1(3) ;
529  vpColVector Hp2(3) ;
530  vpColVector Hp1(3) ;
531 
532  vpMatrix H2(2,6) ;
533  vpColVector e2(2) ;
534  vpMatrix H1(2,6) ;
535  vpColVector e1(2) ;
536 
537 
538  int only_1;
539  int only_2;
540  int iter = 0 ;
541  unsigned int i ;
542  unsigned int n=0 ;
543  only_1 = 1 ;
544  only_2 = 0 ;
545  n = nbpoint ;
546 
547  // Remove next 2 lines, since when only_1 is initialized to 1 (as it is), we cannot reach the code
548  //if ( (! only_1) && (! only_2) )
549  // n *=2 ;
550 
551  vpRobust robust(n);
552  vpColVector res(n) ;
553  vpColVector w(n) ;
554  w =1 ;
555  robust.setThreshold(0.0000) ;
556  vpMatrix W(2*n,2*n) ;
557  W = 0 ;
558 
559  vpColVector N1(3), N2(3) ;
560  double d1, d2 ;
561 
562  double r =1e10 ;
563  iter =0 ;
564  while (vpMath::equal(r_1,r,threshold_displacement) == false )
565  {
566 
567  r_1 =r ;
568  // compute current position
569 
570 
571  //Change frame (current)
572  vpHomogeneousMatrix c1Mc2, c2Mo ;
573  vpRotationMatrix c1Rc2, c2Rc1 ;
574  vpTranslationVector c1Tc2, c2Tc1 ;
575  c1Mc2 = c2Mc1.inverse() ;
576  c2Mc1.extract(c2Rc1) ;
577  c2Mc1.extract(c2Tc1) ;
578  c2Mc1.extract(c1Rc2) ;
579  c1Mc2.extract(c1Tc2) ;
580 
581  c2Mo = c2Mc1*c1Mo ;
582 
583 
584 
585  vpMatrix L(2,3), Lp ;
586  int k =0 ;
587  for (i=0 ; i < nbpoint ; i++)
588  {
589  getPlaneInfo(oN[i], c1Mo, N1, d1) ;
590  getPlaneInfo(oN[i], c2Mo, N2, d2) ;
591  p2[0] = c2P[i].get_x() ;
592  p2[1] = c2P[i].get_y() ;
593  p2[2] = 1.0 ;
594  p1[0] = c1P[i].get_x() ;
595  p1[1] = c1P[i].get_y() ;
596  p1[2] = 1.0 ;
597 
598  vpMatrix H(3,3) ;
599 
600  Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
601  Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
602 
603  Hp2 /= Hp2[2] ; // normalisation
604  Hp1 /= Hp1[2] ;
605 
606 
607  // set up the interaction matrix
608  double x = Hp2[0] ;
609  double y = Hp2[1] ;
610  double Z1 ;
611 
612  Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
613 
614 
615  H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
616  H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
617  H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
618  H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
619  H2 *=-1 ;
620 
621  vpMatrix c1CFc2(6,6) ;
622  {
623  vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
624  for (unsigned int k_=0 ; k_ < 3 ; k_++)
625  for (unsigned int l=0 ; l<3 ; l++)
626  {
627  c1CFc2[k_][l] = c1Rc2[k_][l] ;
628  c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
629  c1CFc2[k_][l+3] = sTR[k_][l] ;
630  }
631  }
632  H2 = H2*c1CFc2 ;
633 
634  // Set up the error vector
635  e2[0] = Hp2[0] - c1P[i].get_x() ;
636  e2[1] = Hp2[1] - c1P[i].get_y() ;
637 
638  x = Hp1[0] ;
639  y = Hp1[1] ;
640 
641  Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
642 
643  H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
644  H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
645  H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
646  H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
647 
648  // Set up the error vector
649  e1[0] = Hp1[0] - c2P[i].get_x() ;
650  e1[1] = Hp1[1] - c2P[i].get_y() ;
651 
652 
653  if (only_2==1)
654  {
655  if (k == 0) { L = H2 ; e = e2 ; }
656  else
657  {
658  L = vpMatrix::stack(L,H2) ;
659  e = vpColVector::stack(e,e2) ;
660  }
661  }
662  else
663  if (only_1==1)
664  {
665  if (k == 0) { L = H1 ; e= e1 ; }
666  else
667  {
668  L = vpMatrix::stack(L,H1) ;
669  e = vpColVector::stack(e,e1) ;
670  }
671  }
672  else
673  {
674  if (k == 0) {L = H2 ; e = e2 ; }
675  else
676  {
677  L = vpMatrix::stack(L,H2) ;
678  e = vpColVector::stack(e,e2) ;
679  }
680  L = vpMatrix::stack(L,H1) ;
681  e = vpColVector::stack(e,e1) ;
682  }
683 
684 
685  k++ ;
686  }
687 
688  if (userobust)
689  {
690  robust.setIteration(0);
691  for (unsigned int k_=0 ; k_ < n ; k_++)
692  {
693  res[k_] = vpMath::sqr(e[2*k_]) + vpMath::sqr(e[2*k_+1]) ;
694  }
695  robust.MEstimator(vpRobust::TUKEY, res, w);
696 
697 
698  // compute the pseudo inverse of the interaction matrix
699  for (unsigned int k_=0 ; k_ < n ; k_++)
700  {
701  W[2*k_][2*k_] = w[k_] ;
702  W[2*k_+1][2*k_+1] = w[k_] ;
703  }
704  }
705  else
706  {
707  for (unsigned int k_=0 ; k_ < 2*n ; k_++) W[k_][k_] = 1 ;
708  }
709  (W*L).pseudoInverse(Lp, 1e-16) ;
710  // Compute the camera velocity
711  vpColVector c2Tcc1 ;
712 
713  c2Tcc1 = -1*Lp*W*e ;
714 
715  // only for simulation
716 
717  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
718  // UpdatePose2(c2Tcc1, c2Mc1) ;
719  r =(W*e).sumSquare() ;
720 
721 
722 
723  if (r < 1e-15) {break ; }
724  if (iter>1000){break ; }
725  if (r>r_1) { break ; }
726  iter++ ;
727  }
728 
729  return (W*e).sumSquare() ;
730 
731 }
732 
733 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
vpColVector::stack
void stack(const double &d)
Definition: vpColVector.cpp:822
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:93
vpMath::equal
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:297
vpMatrix::stack
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2922
vpPoint::get_y
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
vpHomography::computeDisplacement
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Definition: vpHomographyExtract.cpp:60
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:59
vpMath::maximum
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:141
vpRotationMatrix
Implementation of a rotation matrix and operations on such kind of matrices.
Definition: vpRotationMatrix.h:70
vpHomogeneousMatrix::insert
void insert(const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:574
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:110
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:531
vpPoint::get_x
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
vpTranslationVector::t
vpRowVector t() const
Definition: vpTranslationVector.cpp:502
vpHomography::robust
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)
Definition: vpHomography.cpp:632
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:631
vpPlane::getB
double getB() const
Definition: vpPlane.h:108
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:61
vpRobust
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:59
vpPlane::getA
double getA() const
Definition: vpPlane.h:106
vpPlane::getC
double getC() const
Definition: vpPlane.h:110
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:93
vpPlane::getD
double getD() const
Definition: vpPlane.h:112
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217