ViSP  2.9.0
vpHomographyVVS.cpp
1 /****************************************************************************
2  *
3  * $Id: vpHomographyVVS.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 #ifndef DOXYGEN_SHOULD_SKIP_THIS
59 
60 static void
61 updatePoseRotation(vpColVector& dx,vpHomogeneousMatrix& mati)
62 {
63  unsigned int i,j;
64 
65  double sinu,cosi,mcosi,u[3], s;
66  vpRotationMatrix rd ;
67 
68 
69  s = sqrt(dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2]);
70  if (s > 1.e-25)
71  {
72 
73  for (i=0;i<3;i++) u[i] = dx[i]/s;
74  sinu = sin(s);
75  cosi = cos(s);
76  mcosi = 1-cosi;
77  rd[0][0] = cosi + mcosi*u[0]*u[0];
78  rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
79  rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
80  rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
81  rd[1][1] = cosi + mcosi*u[1]*u[1];
82  rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
83  rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
84  rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
85  rd[2][2] = cosi + mcosi*u[2]*u[2];
86  }
87  else
88  {
89  for (i=0;i<3;i++)
90  {
91  for (j=0;j<3;j++) rd[i][j] = 0.0;
92  rd[i][i] = 1.0;
93  }
94 
95  }
96 
97  vpHomogeneousMatrix Delta ;
98  Delta.insert(rd) ;
99 
100  mati = Delta.inverse() * mati ;
101 }
102 
103 double
104 vpHomography::computeRotation(unsigned int nbpoint,
105  vpPoint *c1P,
106  vpPoint *c2P,
107  vpHomogeneousMatrix &c2Mc1,
108  int userobust
109  )
110 {
111  vpColVector e(2) ;
112  double r_1 = -1 ;
113 
114  vpColVector p2(3) ;
115  vpColVector p1(3) ;
116  vpColVector Hp2(3) ;
117  vpColVector Hp1(3) ;
118 
119  vpMatrix H2(2,3) ;
120  vpColVector e2(2) ;
121  vpMatrix H1(2,3) ;
122  vpColVector e1(2) ;
123 
124  int only_1 = 0 ;
125  int only_2 = 0 ;
126  int iter = 0 ;
127 
128  unsigned int n=0 ;
129  for (unsigned int i=0 ; i < nbpoint ; i++) {
130  // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
131  if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
132  &&
133  (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
134  {
135  n++ ;
136  }
137  }
138  //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
139  if ( (! only_1) && (! only_2) )
140  n *=2 ;
141 
142  vpRobust robust(n);
143  vpColVector res(n) ;
144  vpColVector w(n) ;
145  w =1 ;
146  robust.setThreshold(0.0000) ;
147  vpMatrix W(2*n,2*n) ;
148  W = 0 ;
149  vpMatrix c2Rc1(3,3) ;
150  double r =0 ;
151  while (vpMath::equal(r_1,r,threshold_rotation) == false )
152  {
153 
154  r_1 =r ;
155  // compute current position
156 
157  //Change frame (current)
158  for (unsigned int i=0 ; i < 3 ; i++)
159  for (unsigned int j=0 ; j < 3 ; j++)
160  c2Rc1[i][j] = c2Mc1[i][j] ;
161 
162  vpMatrix L(2,3), Lp ;
163  int k =0 ;
164  for (unsigned int i=0 ; i < nbpoint ; i++) {
165  //if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
166  if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
167  &&
168  (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
169  {
170  p2[0] = c2P[i].get_x() ;
171  p2[1] = c2P[i].get_y() ;
172  p2[2] = 1.0 ;
173  p1[0] = c1P[i].get_x() ;
174  p1[1] = c1P[i].get_y() ;
175  p1[2] = 1.0 ;
176 
177  Hp2 = c2Rc1.t()*p2 ; // p2 = Hp1
178  Hp1 = c2Rc1*p1 ; // p1 = Hp2
179 
180  Hp2 /= Hp2[2] ; // normalisation
181  Hp1 /= Hp1[2] ;
182 
183 
184  // set up the interaction matrix
185  double x = Hp2[0] ;
186  double y = Hp2[1] ;
187 
188  H2[0][0] = x*y ; H2[0][1] = -(1+x*x) ; H2[0][2] = y ;
189  H2[1][0] = 1+y*y ; H2[1][1] = -x*y ; H2[1][2] = -x ;
190  H2 *=-1 ;
191  H2 = H2*c2Rc1.t() ;
192 
193  // Set up the error vector
194  e2[0] = Hp2[0] - c1P[i].get_x() ;
195  e2[1] = Hp2[1] - c1P[i].get_y() ;
196 
197  // set up the interaction matrix
198  x = Hp1[0] ;
199  y = Hp1[1] ;
200 
201  H1[0][0] = x*y ; H1[0][1] = -(1+x*x) ; H1[0][2] = y ;
202  H1[1][0] = 1+y*y ; H1[1][1] = -x*y ; H1[1][2] = -x ;
203 
204  // Set up the error vector
205  e1[0] = Hp1[0] - c2P[i].get_x() ;
206  e1[1] = Hp1[1] - c2P[i].get_y() ;
207 
208  if (only_2==1)
209  {
210  if (k == 0) { L = H2 ; e = e2 ; }
211  else
212  {
213  L = vpMatrix::stackMatrices(L,H2) ;
214  e = vpMatrix::stackMatrices(e,e2) ;
215  }
216  }
217  else
218  if (only_1==1)
219  {
220  if (k == 0) { L = H1 ; e= e1 ; }
221  else
222  {
223  L = vpMatrix::stackMatrices(L,H1) ;
224  e = vpMatrix::stackMatrices(e,e1) ;
225  }
226  }
227  else
228  {
229  if (k == 0) {L = H2 ; e = e2 ; }
230  else
231  {
232  L = vpMatrix::stackMatrices(L,H2) ;
233  e = vpMatrix::stackMatrices(e,e2) ;
234  }
235  L = vpMatrix::stackMatrices(L,H1) ;
236  e = vpMatrix::stackMatrices(e,e1) ;
237  }
238 
239  k++ ;
240  }
241  }
242 
243  if (userobust)
244  {
245  robust.setIteration(0);
246 
247  for (unsigned int l=0 ; l < n ; l++)
248  {
249  res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
250  }
251  robust.MEstimator(vpRobust::TUKEY, res, w);
252 
253 
254  // compute the pseudo inverse of the interaction matrix
255  for (unsigned int l=0 ; l < n ; l++)
256  {
257  W[2*l][2*l] = w[l] ;
258  W[2*l+1][2*l+1] = w[l] ;
259  }
260  }
261  else
262  {
263  for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
264  }
265  // CreateDiagonalMatrix(w, W) ;
266  (L).pseudoInverse(Lp, 1e-6) ;
267  // Compute the camera velocity
268  vpColVector c2rc1, v(6) ;
269 
270  c2rc1 = -2*Lp*W*e ;
271  for (unsigned int i=0 ; i < 3 ; i++) v[i+3] = c2rc1[i] ;
272 
273  // only for simulation
274 
275  updatePoseRotation(c2rc1, c2Mc1) ;
276  r =e.sumSquare() ;
277 
278  if ((W*e).sumSquare() < 1e-10) break ;
279  if (iter>25) break ;
280  iter++ ; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e=" <<(W*e).sumSquare() <<std::endl ;
281 
282  }
283 
284  // std::cout << c2Mc1 <<std::endl ;
285  return (W*e).sumSquare() ;
286 }
287 
288 
289 static void
290 getPlaneInfo(vpPlane &oN, vpHomogeneousMatrix &cMo, vpColVector &cN, double &cd)
291 {
292  double A1 = cMo[0][0]*oN.getA()+ cMo[0][1]*oN.getB() + cMo[0][2]*oN.getC();
293  double B1 = cMo[1][0]*oN.getA()+ cMo[1][1]*oN.getB() + cMo[1][2]*oN.getC();
294  double C1 = cMo[2][0]*oN.getA()+ cMo[2][1]*oN.getB() + cMo[2][2]*oN.getC();
295  double D1 = oN.getD() - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
296 
297  cN.resize(3) ;
298  cN[0] = A1 ;
299  cN[1] = B1 ;
300  cN[2] = C1 ;
301  cd = -D1 ;
302 }
303 
304 double
305 vpHomography::computeDisplacement(unsigned int nbpoint,
306  vpPoint *c1P,
307  vpPoint *c2P,
308  vpPlane &oN,
309  vpHomogeneousMatrix &c2Mc1,
310  vpHomogeneousMatrix &c1Mo,
311  int userobust
312  )
313 {
314  vpColVector e(2) ;
315  double r_1 = -1 ;
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;
328  int only_2;
329  int iter = 0 ;
330  unsigned int n=0 ;
331  n = nbpoint ;
332  only_1 = 1 ;
333  only_2 = 0 ;
334 
335  //if ((only_1==1) || (only_2==1)) ; else n *=2 ;
336  if ( (! only_1) && (! only_2) )
337  n *=2 ;
338 
339  vpRobust robust(n);
340  vpColVector res(n) ;
341  vpColVector w(n) ;
342  w =1 ;
343  robust.setThreshold(0.0000) ;
344  vpMatrix W(2*n,2*n) ;
345  W = 0 ;
346 
347  vpColVector N1(3), N2(3) ;
348  double d1, d2 ;
349 
350  double r =1e10 ;
351  iter =0 ;
352  while (vpMath::equal(r_1,r,threshold_displacement) == false )
353  {
354  r_1 =r ;
355  // compute current position
356 
357  //Change frame (current)
358  vpHomogeneousMatrix c1Mc2, c2Mo ;
359  vpRotationMatrix c1Rc2, c2Rc1 ;
360  vpTranslationVector c1Tc2, c2Tc1 ;
361  c1Mc2 = c2Mc1.inverse() ;
362  c2Mc1.extract(c2Rc1) ;
363  c2Mc1.extract(c2Tc1) ;
364  c2Mc1.extract(c1Rc2) ;
365  c1Mc2.extract(c1Tc2) ;
366 
367  c2Mo = c2Mc1*c1Mo ;
368 
369  getPlaneInfo(oN, c1Mo, N1, d1) ;
370  getPlaneInfo(oN, c2Mo, N2, d2) ;
371 
372  vpMatrix L(2,3), Lp ;
373  int k =0 ;
374  for (unsigned int i=0 ; i < nbpoint ; i++)
375  {
376  p2[0] = c2P[i].get_x() ;
377  p2[1] = c2P[i].get_y() ;
378  p2[2] = 1.0 ;
379  p1[0] = c1P[i].get_x() ;
380  p1[1] = c1P[i].get_y() ;
381  p1[2] = 1.0 ;
382 
383  vpMatrix H(3,3) ;
384 
385  Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
386  Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
387 
388  Hp2 /= Hp2[2] ; // normalisation
389  Hp1 /= Hp1[2] ;
390 
391 
392  // set up the interaction matrix
393  double x = Hp2[0] ;
394  double y = Hp2[1] ;
395  double Z1 ;
396 
397  Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
398 
399 
400  H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
401  H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
402  H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
403  H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
404  H2 *=-1 ;
405 
406  vpMatrix c1CFc2(6,6) ;
407  {
408  vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
409  for (unsigned int k_=0 ; k_ < 3 ; k_++)
410  for (unsigned int l=0 ; l<3 ; l++)
411  {
412  c1CFc2[k_][l] = c1Rc2[k_][l] ;
413  c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
414  c1CFc2[k_][l+3] = sTR[k_][l] ;
415  }
416  }
417  H2 = H2*c1CFc2 ;
418 
419  // Set up the error vector
420  e2[0] = Hp2[0] - c1P[i].get_x() ;
421  e2[1] = Hp2[1] - c1P[i].get_y() ;
422 
423  x = Hp1[0] ;
424  y = Hp1[1] ;
425 
426  Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
427 
428  H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
429  H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
430  H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
431  H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
432 
433  // Set up the error vector
434  e1[0] = Hp1[0] - c2P[i].get_x() ;
435  e1[1] = Hp1[1] - c2P[i].get_y() ;
436 
437  if (only_2==1)
438  {
439  if (k == 0) { L = H2 ; e = e2 ; }
440  else
441  {
442  L = vpMatrix::stackMatrices(L,H2) ;
443  e = vpMatrix::stackMatrices(e,e2) ;
444  }
445  }
446  else
447  if (only_1==1)
448  {
449  if (k == 0) { L = H1 ; e= e1 ; }
450  else
451  {
452  L = vpMatrix::stackMatrices(L,H1) ;
453  e = vpMatrix::stackMatrices(e,e1) ;
454  }
455  }
456  else
457  {
458  if (k == 0) {L = H2 ; e = e2 ; }
459  else
460  {
461  L = vpMatrix::stackMatrices(L,H2) ;
462  e = vpMatrix::stackMatrices(e,e2) ;
463  }
464  L = vpMatrix::stackMatrices(L,H1) ;
465  e = vpMatrix::stackMatrices(e,e1) ;
466  }
467 
468 
469  k++ ;
470  }
471 
472  if (userobust)
473  {
474  robust.setIteration(0);
475  for (unsigned int l=0 ; l < n ; l++)
476  {
477  res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
478  }
479  robust.MEstimator(vpRobust::TUKEY, res, w);
480 
481  // compute the pseudo inverse of the interaction matrix
482  for (unsigned int l=0 ; l < n ; l++)
483  {
484  W[2*l][2*l] = w[l] ;
485  W[2*l+1][2*l+1] = w[l] ;
486  }
487  }
488  else
489  {
490  for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
491  }
492  (W*L).pseudoInverse(Lp, 1e-16) ;
493  // Compute the camera velocity
494  vpColVector c2Tcc1 ;
495 
496  c2Tcc1 = -1*Lp*W*e ;
497 
498  // only for simulation
499 
500  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
501  // UpdatePose2(c2Tcc1, c2Mc1) ;
502  r =(W*e).sumSquare() ;
503 
504  if (r < 1e-15) {break ; }
505  if (iter>1000){break ; }
506  if (r>r_1) { break ; }
507  iter++ ;
508  }
509 
510  return (W*e).sumSquare() ;
511 
512 }
513 
514 
515 double
516 vpHomography::computeDisplacement(unsigned int nbpoint,
517  vpPoint *c1P,
518  vpPoint *c2P,
519  vpPlane *oN,
520  vpHomogeneousMatrix &c2Mc1,
521  vpHomogeneousMatrix &c1Mo,
522  int userobust
523  )
524 {
525 
526 
527  vpColVector e(2) ;
528  double r_1 = -1 ;
529 
530  vpColVector p2(3) ;
531  vpColVector p1(3) ;
532  vpColVector Hp2(3) ;
533  vpColVector Hp1(3) ;
534 
535  vpMatrix H2(2,6) ;
536  vpColVector e2(2) ;
537  vpMatrix H1(2,6) ;
538  vpColVector e1(2) ;
539 
540 
541  int only_1;
542  int only_2;
543  int iter = 0 ;
544  unsigned int i ;
545  unsigned int n=0 ;
546  only_1 = 1 ;
547  only_2 = 0 ;
548  n = nbpoint ;
549  // if ((only_1==1) || (only_2==1)) ; else n *=2 ;
550  if ( (! only_1) && (! only_2) )
551  n *=2 ;
552 
553  vpRobust robust(n);
554  vpColVector res(n) ;
555  vpColVector w(n) ;
556  w =1 ;
557  robust.setThreshold(0.0000) ;
558  vpMatrix W(2*n,2*n) ;
559  W = 0 ;
560 
561  vpColVector N1(3), N2(3) ;
562  double d1, d2 ;
563 
564  double r =1e10 ;
565  iter =0 ;
566  while (vpMath::equal(r_1,r,threshold_displacement) == false )
567  {
568 
569  r_1 =r ;
570  // compute current position
571 
572 
573  //Change frame (current)
574  vpHomogeneousMatrix c1Mc2, c2Mo ;
575  vpRotationMatrix c1Rc2, c2Rc1 ;
576  vpTranslationVector c1Tc2, c2Tc1 ;
577  c1Mc2 = c2Mc1.inverse() ;
578  c2Mc1.extract(c2Rc1) ;
579  c2Mc1.extract(c2Tc1) ;
580  c2Mc1.extract(c1Rc2) ;
581  c1Mc2.extract(c1Tc2) ;
582 
583  c2Mo = c2Mc1*c1Mo ;
584 
585 
586 
587  vpMatrix L(2,3), Lp ;
588  int k =0 ;
589  for (i=0 ; i < nbpoint ; i++)
590  {
591  getPlaneInfo(oN[i], c1Mo, N1, d1) ;
592  getPlaneInfo(oN[i], c2Mo, N2, d2) ;
593  p2[0] = c2P[i].get_x() ;
594  p2[1] = c2P[i].get_y() ;
595  p2[2] = 1.0 ;
596  p1[0] = c1P[i].get_x() ;
597  p1[1] = c1P[i].get_y() ;
598  p1[2] = 1.0 ;
599 
600  vpMatrix H(3,3) ;
601 
602  Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
603  Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
604 
605  Hp2 /= Hp2[2] ; // normalisation
606  Hp1 /= Hp1[2] ;
607 
608 
609  // set up the interaction matrix
610  double x = Hp2[0] ;
611  double y = Hp2[1] ;
612  double Z1 ;
613 
614  Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
615 
616 
617  H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
618  H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
619  H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
620  H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
621  H2 *=-1 ;
622 
623  vpMatrix c1CFc2(6,6) ;
624  {
625  vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
626  for (unsigned int k_=0 ; k_ < 3 ; k_++)
627  for (unsigned int l=0 ; l<3 ; l++)
628  {
629  c1CFc2[k_][l] = c1Rc2[k_][l] ;
630  c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
631  c1CFc2[k_][l+3] = sTR[k_][l] ;
632  }
633  }
634  H2 = H2*c1CFc2 ;
635 
636  // Set up the error vector
637  e2[0] = Hp2[0] - c1P[i].get_x() ;
638  e2[1] = Hp2[1] - c1P[i].get_y() ;
639 
640  x = Hp1[0] ;
641  y = Hp1[1] ;
642 
643  Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
644 
645  H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
646  H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
647  H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
648  H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
649 
650  // Set up the error vector
651  e1[0] = Hp1[0] - c2P[i].get_x() ;
652  e1[1] = Hp1[1] - c2P[i].get_y() ;
653 
654 
655  if (only_2==1)
656  {
657  if (k == 0) { L = H2 ; e = e2 ; }
658  else
659  {
660  L = vpMatrix::stackMatrices(L,H2) ;
661  e = vpMatrix::stackMatrices(e,e2) ;
662  }
663  }
664  else
665  if (only_1==1)
666  {
667  if (k == 0) { L = H1 ; e= e1 ; }
668  else
669  {
670  L = vpMatrix::stackMatrices(L,H1) ;
671  e = vpMatrix::stackMatrices(e,e1) ;
672  }
673  }
674  else
675  {
676  if (k == 0) {L = H2 ; e = e2 ; }
677  else
678  {
679  L = vpMatrix::stackMatrices(L,H2) ;
680  e = vpMatrix::stackMatrices(e,e2) ;
681  }
682  L = vpMatrix::stackMatrices(L,H1) ;
683  e = vpMatrix::stackMatrices(e,e1) ;
684  }
685 
686 
687  k++ ;
688  }
689 
690  if (userobust)
691  {
692  robust.setIteration(0);
693  for (unsigned int k_=0 ; k_ < n ; k_++)
694  {
695  res[k_] = vpMath::sqr(e[2*k_]) + vpMath::sqr(e[2*k_+1]) ;
696  }
697  robust.MEstimator(vpRobust::TUKEY, res, w);
698 
699 
700  // compute the pseudo inverse of the interaction matrix
701  for (unsigned int k_=0 ; k_ < n ; k_++)
702  {
703  W[2*k_][2*k_] = w[k_] ;
704  W[2*k_+1][2*k_+1] = w[k_] ;
705  }
706  }
707  else
708  {
709  for (unsigned int k_=0 ; k_ < 2*n ; k_++) W[k_][k_] = 1 ;
710  }
711  (W*L).pseudoInverse(Lp, 1e-16) ;
712  // Compute the camera velocity
713  vpColVector c2Tcc1 ;
714 
715  c2Tcc1 = -1*Lp*W*e ;
716 
717  // only for simulation
718 
719  c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
720  // UpdatePose2(c2Tcc1, c2Mc1) ;
721  r =(W*e).sumSquare() ;
722 
723 
724 
725  if (r < 1e-15) {break ; }
726  if (iter>1000){break ; }
727  if (r>r_1) { break ; }
728  iter++ ;
729  }
730 
731  return (W*e).sumSquare() ;
732 
733 }
734 
735 #endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
vpMatrix
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
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:809
vpHomography::computeDisplacement
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
Definition: vpHomographyExtract.cpp:64
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:305
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
void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.cpp:3003
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:603
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:113
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:111
vpPlane::getC
double getC() const
Definition: vpPlane.h:115
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:95
vpPlane::getD
double getD() const
Definition: vpPlane.h:117
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94