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