Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpCalibrationTools.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  * Camera calibration.
32  *
33  * Authors:
34  * Eric Marchand
35  * Francois Chaumette
36  *
37  *****************************************************************************/
38 
39 #include <visp3/vision/vpCalibration.h>
40 #include <visp3/core/vpMath.h>
41 #include <visp3/vision/vpPose.h>
42 #include <visp3/core/vpPixelMeterConversion.h>
43 
44 #include <cmath> // std::fabs
45 #include <limits> // numeric_limits
46 
47 #undef MAX
48 #undef MIN
49 
50 void
51 vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est)
52 {
53 
54  vpMatrix A(2*npt,3) ;
55  vpMatrix B(2*npt,9) ;
56 
57  std::list<double>::const_iterator it_LoX = LoX.begin();
58  std::list<double>::const_iterator it_LoY = LoY.begin();
59  std::list<double>::const_iterator it_LoZ = LoZ.begin();
60  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
61 
62  vpImagePoint ip;
63 
64  for (unsigned int i = 0 ; i < npt ; i++)
65  {
66 
67  double x0 = *it_LoX;
68  double y0 = *it_LoY;
69  double z0 = *it_LoZ;
70 
71  ip = *it_Lip;
72 
73  double xi = ip.get_u() ;
74  double yi = ip.get_v() ;
75 
76  A[2*i][0] = x0*xi;
77  A[2*i][1] = y0*xi;
78  A[2*i][2] = z0*xi;
79  B[2*i][0] = -x0;
80  B[2*i][1] = -y0;
81  B[2*i][2] = -z0;
82  B[2*i][3] = 0.0;
83  B[2*i][4] = 0.0;
84  B[2*i][5] = 0.0;
85  B[2*i][6] = -1.0;
86  B[2*i][7] = 0.0;
87  B[2*i][8] = xi;
88  A[2*i+1][0] = x0*yi;
89  A[2*i+1][1] = y0*yi;
90  A[2*i+1][2] = z0*yi;
91  B[2*i+1][0] = 0.0;
92  B[2*i+1][1] = 0.0;
93  B[2*i+1][2] = 0.0;
94  B[2*i+1][3] = -x0;
95  B[2*i+1][4] = -y0;
96  B[2*i+1][5] = -z0;
97  B[2*i+1][6] = 0.0;
98  B[2*i+1][7] = -1.0;
99  B[2*i+1][8] = yi;
100 
101  ++it_LoX;
102  ++it_LoY;
103  ++it_LoZ;
104  ++it_Lip;
105  }
106 
107  vpMatrix BtB ; /* compute B^T B */
108  BtB = B.t() * B ;
109 
110  /* compute (B^T B)^(-1) */
111  /* input : btb (dimension 9 x 9) = (B^T B) */
112  /* output : btbinv (dimension 9 x 9) = (B^T B)^(-1) */
113 
114  vpMatrix BtBinv ;
115  BtBinv = BtB.pseudoInverse(1e-16) ;
116 
117  vpMatrix BtA ;
118  BtA = B.t()*A ; /* compute B^T A */
119 
120 
121  vpMatrix r ;
122  r = BtBinv*BtA ; /* compute (B^T B)^(-1) B^T A */
123 
124  vpMatrix e ; /* compute - A^T B (B^T B)^(-1) B^T A*/
125  e = -(A.t()*B)*r ;
126 
127  vpMatrix AtA ; /* compute A^T A */
128  AtA = A.AtA() ;
129 
130  e += AtA ; /* compute E = A^T A - A^T B (B^T B)^(-1) B^T A */
131 
132  vpColVector x1(3) ;
133  vpColVector x2 ;
134 
135  e.svd(x1,AtA) ;// destructive on e
136  // eigenvector computation of E corresponding to the min eigenvalue.
137  /* SVmax computation*/
138  double svm = 0.0;
139  unsigned int imin = 1;
140  for (unsigned int i=0;i<x1.getRows();i++)
141  {
142  if (x1[i] > svm)
143  {
144  svm = x1[i];
145  imin = i;
146  }
147  }
148 
149  //svm *= 0.1; /* for the rank */
150 
151  for (unsigned int i=0;i<x1.getRows();i++)
152  {
153  if (x1[i] < x1[imin]) imin = i;
154  }
155 
156  for (unsigned int i=0;i<x1.getRows();i++)
157  x1[i] = AtA[i][imin];
158 
159  x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
160 
161 
162  vpColVector sol(12) ;
163  vpColVector resul(7) ;
164  for (unsigned int i=0;i<3;i++) sol[i] = x1[i]; /* X_1 */
165  for (unsigned int i=0;i<9;i++) /* X_2 = - (B^T B)^(-1) B^T A X_1 */
166  {
167  sol[i+3] = x2[i];
168  }
169 
170  if (sol[11] < 0.0) for (unsigned int i=0;i<12;i++) sol[i] = -sol[i]; /* since Z0 > 0 */
171 
172  resul[0] = sol[3]*sol[0]+sol[4]*sol[1]+sol[5]*sol[2]; /* u0 */
173 
174  resul[1] = sol[6]*sol[0]+sol[7]*sol[1]+sol[8]*sol[2]; /* v0 */
175 
176  resul[2] = sqrt(sol[3]*sol[3]+sol[4]*sol[4]+sol[5]*sol[5] /* px */
177  -resul[0]*resul[0]);
178  resul[3] = sqrt(sol[6]*sol[6]+sol[7]*sol[7]+sol[8]*sol[8] /* py */
179  -resul[1]*resul[1]);
180 
181  cam_est.initPersProjWithoutDistortion(resul[2],resul[3],resul[0],resul[1]);
182 
183  resul[4] = (sol[9]-sol[11]*resul[0])/resul[2]; /* X0 */
184  resul[5] = (sol[10]-sol[11]*resul[1])/resul[3]; /* Y0 */
185  resul[6] = sol[11]; /* Z0 */
186 
187  vpMatrix rd(3,3) ;
188  /* fill rotation matrix */
189  for (unsigned int i=0;i<3;i++) rd[0][i] = (sol[i+3]-sol[i]*resul[0])/resul[2];
190  for (unsigned int i=0;i<3;i++) rd[1][i] = (sol[i+6]-sol[i]*resul[1])/resul[3];
191  for (unsigned int i=0;i<3;i++) rd[2][i] = sol[i];
192 
193  // std::cout << "norme X1 " << x1.sumSquare() <<std::endl;
194  // std::cout << rd*rd.t() ;
195 
196  for (unsigned int i=0 ; i < 3 ; i++)
197  {
198  for (unsigned int j=0 ; j < 3 ; j++)
199  cMo_est[i][j] = rd[i][j];
200  }
201  for (unsigned int i=0 ; i < 3 ; i++) cMo_est[i][3] = resul[i+4] ;
202 
203  this->cMo = cMo_est ;
204  this->cMo_dist = cMo_est;
205 
206  double deviation,deviation_dist;
207  this->computeStdDeviation(deviation,deviation_dist);
208 
209 }
210 
211 
212 void
213 vpCalibration::calibVVS(vpCameraParameters &cam_est,
214  vpHomogeneousMatrix &cMo_est,
215  bool verbose)
216 {
217  std::ios::fmtflags original_flags( std::cout.flags() );
218  std::cout.precision(10);
219  unsigned int n_points = npt ;
220 
221  vpColVector oX(n_points), cX(n_points) ;
222  vpColVector oY(n_points), cY(n_points) ;
223  vpColVector oZ(n_points), cZ(n_points) ;
224  vpColVector u(n_points) ;
225  vpColVector v(n_points) ;
226 
227  vpColVector P(2*n_points) ;
228  vpColVector Pd(2*n_points) ;
229 
230  vpImagePoint ip;
231 
232  std::list<double>::const_iterator it_LoX = LoX.begin();
233  std::list<double>::const_iterator it_LoY = LoY.begin();
234  std::list<double>::const_iterator it_LoZ = LoZ.begin();
235  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
236 
237  for (unsigned int i =0 ; i < n_points ; i++)
238  {
239  oX[i] = *it_LoX;
240  oY[i] = *it_LoY;
241  oZ[i] = *it_LoZ;
242 
243  ip = *it_Lip;
244 
245  u[i] = ip.get_u() ;
246  v[i] = ip.get_v() ;
247 
248  ++it_LoX;
249  ++it_LoY;
250  ++it_LoZ;
251  ++it_Lip;
252  }
253 
254  // double lambda = 0.1 ;
255  unsigned int iter = 0 ;
256 
257  double residu_1 = 1e12 ;
258  double r =1e12-1;
259  while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
260  {
261  iter++ ;
262  residu_1 = r ;
263 
264  double px = cam_est.get_px();
265  double py = cam_est.get_py();
266  double u0 = cam_est.get_u0();
267  double v0 = cam_est.get_v0();
268 
269  r = 0 ;
270 
271  for (unsigned int i=0 ; i < n_points; i++)
272  {
273  cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
274  cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
275  cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
276 
277  Pd[2*i] = u[i] ;
278  Pd[2*i+1] = v[i] ;
279 
280  P[2*i] = cX[i]/cZ[i]*px + u0 ;
281  P[2*i+1] = cY[i]/cZ[i]*py + v0 ;
282 
283  r += ((vpMath::sqr(P[2*i]-Pd[2*i]) + vpMath::sqr(P[2*i+1]-Pd[2*i+1]))) ;
284  }
285 
286  vpColVector error ;
287  error = P-Pd ;
288  //r = r/n_points ;
289 
290  vpMatrix L(n_points*2,10) ;
291  for (unsigned int i=0 ; i < n_points; i++)
292  {
293  double x = cX[i] ;
294  double y = cY[i] ;
295  double z = cZ[i] ;
296  double inv_z = 1/z;
297 
298  double X = x*inv_z ;
299  double Y = y*inv_z ;
300 
301  //---------------
302  {
303  L[2*i][0] = px * (-inv_z) ;
304  L[2*i][1] = 0 ;
305  L[2*i][2] = px*X*inv_z ;
306  L[2*i][3] = px*X*Y ;
307  L[2*i][4] = -px*(1+X*X) ;
308  L[2*i][5] = px*Y ;
309  }
310  {
311  L[2*i][6]= 1 ;
312  L[2*i][7]= 0 ;
313  L[2*i][8]= X ;
314  L[2*i][9]= 0;
315  }
316  {
317  L[2*i+1][0] = 0 ;
318  L[2*i+1][1] = py*(-inv_z) ;
319  L[2*i+1][2] = py*(Y*inv_z) ;
320  L[2*i+1][3] = py* (1+Y*Y) ;
321  L[2*i+1][4] = -py*X*Y ;
322  L[2*i+1][5] = -py*X ;
323  }
324  {
325  L[2*i+1][6]= 0 ;
326  L[2*i+1][7]= 1 ;
327  L[2*i+1][8]= 0;
328  L[2*i+1][9]= Y ;
329  }
330  } // end interaction
331  vpMatrix Lp ;
332  Lp = L.pseudoInverse(1e-10) ;
333 
334  vpColVector e ;
335  e = Lp*error ;
336 
337  vpColVector Tc, Tc_v(6) ;
338  Tc = -e*gain ;
339 
340  // Tc_v =0 ;
341  for (unsigned int i=0 ; i <6 ; i++)
342  Tc_v[i] = Tc[i] ;
343 
344  cam_est.initPersProjWithoutDistortion(px+Tc[8],py+Tc[9],
345  u0+Tc[6],v0+Tc[7]) ;
346 
347  cMo_est = vpExponentialMap::direct(Tc_v).inverse()*cMo_est ;
348  if (verbose)
349  std::cout << " std dev " << sqrt(r/n_points) << std::endl;
350 
351  }
352  if (iter == nbIterMax)
353  {
354  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
355  throw(vpCalibrationException(vpCalibrationException::convergencyError,
356  "Maximum number of iterations reached")) ;
357  }
358  this->cMo = cMo_est;
359  this->cMo_dist = cMo_est;
360  this->residual = r;
361  this->residual_dist = r;
362  if (verbose)
363  std::cout << " std dev " << sqrt(r/n_points) << std::endl;
364  // Restore ostream format
365  std::cout.flags(original_flags);
366 }
367 
368 void
369 vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal,
370  vpCameraParameters &cam_est,
371  double &globalReprojectionError,
372  bool verbose)
373 {
374  std::ios::fmtflags original_flags( std::cout.flags() );
375  std::cout.precision(10);
376  unsigned int nbPoint[256]; //number of points by image
377  unsigned int nbPointTotal = 0; //total number of points
378  unsigned int nbPose = (unsigned int)table_cal.size();
379  unsigned int nbPose6 = 6*nbPose;
380 
381  for (unsigned int i=0; i<nbPose ; i++)
382  {
383  nbPoint[i] = table_cal[i].npt;
384  nbPointTotal += nbPoint[i];
385  }
386 
387  if (nbPointTotal < 4) {
388  //vpERROR_TRACE("Not enough point to calibrate");
389  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
390  "Not enough point to calibrate")) ;
391  }
392 
393  vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
394  vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
395  vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
396  vpColVector u(nbPointTotal) ;
397  vpColVector v(nbPointTotal) ;
398 
399  vpColVector P(2*nbPointTotal) ;
400  vpColVector Pd(2*nbPointTotal) ;
401  vpImagePoint ip;
402 
403  unsigned int curPoint = 0 ; //current point indice
404  for (unsigned int p=0; p<nbPose ; p++)
405  {
406  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
407  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
408  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
409  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
410 
411  for (unsigned int i =0 ; i < nbPoint[p] ; i++)
412  {
413  oX[curPoint] = *it_LoX;
414  oY[curPoint] = *it_LoY;
415  oZ[curPoint] = *it_LoZ;
416 
417  ip = *it_Lip;
418  u[curPoint] = ip.get_u() ;
419  v[curPoint] = ip.get_v() ;
420 
421  ++ it_LoX;
422  ++ it_LoY;
423  ++ it_LoZ;
424  ++ it_Lip;
425 
426  curPoint++;
427  }
428  }
429  // double lambda = 0.1 ;
430  unsigned int iter = 0 ;
431 
432  double residu_1 = 1e12 ;
433  double r =1e12-1;
434  while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
435  {
436 
437  iter++ ;
438  residu_1 = r ;
439 
440  double px = cam_est.get_px();
441  double py = cam_est.get_py();
442  double u0 = cam_est.get_u0();
443  double v0 = cam_est.get_v0();
444 
445  r = 0 ;
446  curPoint = 0 ; //current point indice
447  for (unsigned int p=0; p<nbPose ; p++)
448  {
449  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
450  for (unsigned int i=0 ; i < nbPoint[p]; i++)
451  {
452  unsigned int curPoint2 = 2*curPoint;
453 
454  cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
455  +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
456  cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
457  +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
458  cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
459  +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
460 
461  Pd[curPoint2] = u[curPoint] ;
462  Pd[curPoint2+1] = v[curPoint] ;
463 
464  P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
465  P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
466 
467  r += (vpMath::sqr(P[curPoint2]-Pd[curPoint2])
468  + vpMath::sqr(P[curPoint2+1]-Pd[curPoint2+1])) ;
469  curPoint++;
470  }
471  }
472 
473  vpColVector error ;
474  error = P-Pd ;
475  //r = r/nbPointTotal ;
476 
477  vpMatrix L(nbPointTotal*2,nbPose6+4) ;
478  curPoint = 0 ; //current point indice
479  for (unsigned int p=0; p<nbPose ; p++)
480  {
481  unsigned int q = 6*p;
482  for (unsigned int i=0 ; i < nbPoint[p]; i++)
483  {
484  unsigned int curPoint2 = 2*curPoint;
485  unsigned int curPoint21 = curPoint2 + 1;
486 
487  double x = cX[curPoint] ;
488  double y = cY[curPoint] ;
489  double z = cZ[curPoint] ;
490 
491  double inv_z = 1/z;
492 
493  double X = x*inv_z ;
494  double Y = y*inv_z ;
495 
496  //---------------
497  {
498  {
499  L[curPoint2][q] = px * (-inv_z) ;
500  L[curPoint2][q+1] = 0 ;
501  L[curPoint2][q+2] = px*(X*inv_z) ;
502  L[curPoint2][q+3] = px*X*Y ;
503  L[curPoint2][q+4] = -px*(1+X*X) ;
504  L[curPoint2][q+5] = px*Y ;
505  }
506  {
507  L[curPoint2][nbPose6]= 1 ;
508  L[curPoint2][nbPose6+1]= 0 ;
509  L[curPoint2][nbPose6+2]= X ;
510  L[curPoint2][nbPose6+3]= 0;
511  }
512  {
513  L[curPoint21][q] = 0 ;
514  L[curPoint21][q+1] = py*(-inv_z) ;
515  L[curPoint21][q+2] = py*(Y*inv_z) ;
516  L[curPoint21][q+3] = py* (1+Y*Y) ;
517  L[curPoint21][q+4] = -py*X*Y ;
518  L[curPoint21][q+5] = -py*X ;
519  }
520  {
521  L[curPoint21][nbPose6]= 0 ;
522  L[curPoint21][nbPose6+1]= 1 ;
523  L[curPoint21][nbPose6+2]= 0;
524  L[curPoint21][nbPose6+3]= Y ;
525  }
526 
527  }
528  curPoint++;
529  } // end interaction
530  }
531  vpMatrix Lp ;
532  Lp = L.pseudoInverse(1e-10) ;
533 
534  vpColVector e ;
535  e = Lp*error ;
536 
537  vpColVector Tc, Tc_v(nbPose6) ;
538  Tc = -e*gain ;
539 
540  // Tc_v =0 ;
541  for (unsigned int i = 0 ; i < nbPose6 ; i++)
542  Tc_v[i] = Tc[i] ;
543 
544  cam_est.initPersProjWithoutDistortion(px+Tc[nbPose6+2],
545  py+Tc[nbPose6+3],
546  u0+Tc[nbPose6],
547  v0+Tc[nbPose6+1]) ;
548 
549  // cam.setKd(get_kd() + Tc[10]) ;
550  vpColVector Tc_v_Tmp(6) ;
551 
552  for (unsigned int p = 0 ; p < nbPose ; p++)
553  {
554  for (unsigned int i = 0 ; i < 6 ; i++)
555  Tc_v_Tmp[i] = Tc_v[6*p + i];
556 
557  table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
558  * table_cal[p].cMo;
559  }
560 
561  if (verbose)
562  std::cout << " std dev " << sqrt(r/nbPointTotal) << std::endl;
563 
564  }
565  if (iter == nbIterMax)
566  {
567  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
568  throw(vpCalibrationException(vpCalibrationException::convergencyError,
569  "Maximum number of iterations reached")) ;
570  }
571  for (unsigned int p = 0 ; p < nbPose ; p++)
572  {
573  table_cal[p].cMo_dist = table_cal[p].cMo ;
574  table_cal[p].cam = cam_est;
575  table_cal[p].cam_dist = cam_est;
576  double deviation,deviation_dist;
577  table_cal[p].computeStdDeviation(deviation,deviation_dist);
578  }
579  globalReprojectionError = sqrt(r/nbPointTotal);
580  // Restore ostream format
581  std::cout.flags(original_flags);
582 }
583 
584 void
585 vpCalibration::calibVVSWithDistortion(vpCameraParameters& cam_est,
586  vpHomogeneousMatrix& cMo_est,
587  bool verbose)
588 {
589  std::ios::fmtflags original_flags( std::cout.flags() );
590  std::cout.precision(10);
591  unsigned int n_points =npt ;
592 
593  vpColVector oX(n_points), cX(n_points) ;
594  vpColVector oY(n_points), cY(n_points) ;
595  vpColVector oZ(n_points), cZ(n_points) ;
596  vpColVector u(n_points) ;
597  vpColVector v(n_points) ;
598 
599  vpColVector P(4*n_points) ;
600  vpColVector Pd(4*n_points) ;
601 
602  std::list<double>::const_iterator it_LoX = LoX.begin();
603  std::list<double>::const_iterator it_LoY = LoY.begin();
604  std::list<double>::const_iterator it_LoZ = LoZ.begin();
605  std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
606 
607  vpImagePoint ip;
608 
609  for (unsigned int i =0 ; i < n_points ; i++)
610  {
611  oX[i] = *it_LoX;
612  oY[i] = *it_LoY;
613  oZ[i] = *it_LoZ;
614 
615  ip = *it_Lip;
616  u[i] = ip.get_u();
617  v[i] = ip.get_v();
618 
619  ++ it_LoX;
620  ++ it_LoY;
621  ++ it_LoZ;
622  ++ it_Lip;
623  }
624 
625  // double lambda = 0.1 ;
626  unsigned int iter = 0 ;
627 
628  double residu_1 = 1e12 ;
629  double r =1e12-1;
630  while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
631  {
632  iter++ ;
633  residu_1 = r ;
634 
635  r = 0 ;
636  double u0 = cam_est.get_u0() ;
637  double v0 = cam_est.get_v0() ;
638 
639  double px = cam_est.get_px() ;
640  double py = cam_est.get_py() ;
641 
642  double inv_px = 1/px ;
643  double inv_py = 1/py ;
644 
645  double kud = cam_est.get_kud() ;
646  double kdu = cam_est.get_kdu() ;
647 
648  double k2ud = 2*kud;
649  double k2du = 2*kdu;
650  vpMatrix L(n_points*4,12) ;
651 
652  for (unsigned int i=0 ; i < n_points; i++)
653  {
654  unsigned int i4 = 4*i;
655  unsigned int i41 = 4*i+1;
656  unsigned int i42 = 4*i+2;
657  unsigned int i43 = 4*i+3;
658 
659  cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
660  cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
661  cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
662 
663  double x = cX[i] ;
664  double y = cY[i] ;
665  double z = cZ[i] ;
666  double inv_z = 1/z;
667 
668  double X = x*inv_z ;
669  double Y = y*inv_z ;
670 
671  double X2 = X*X;
672  double Y2 = Y*Y;
673  double XY = X*Y;
674 
675  double up = u[i] ;
676  double vp = v[i] ;
677 
678  Pd[i4] = up ;
679  Pd[i41] = vp ;
680 
681  double up0 = up - u0;
682  double vp0 = vp - v0;
683 
684  double xp0 = up0 * inv_px;
685  double xp02 = xp0 *xp0 ;
686 
687  double yp0 = vp0 * inv_py;
688  double yp02 = yp0 * yp0;
689 
690  double r2du = xp02 + yp02 ;
691  double kr2du = kdu * r2du;
692 
693  P[i4] = u0 + px*X - kr2du *(up0) ;
694  P[i41] = v0 + py*Y - kr2du *(vp0) ;
695 
696  double r2ud = X2 + Y2 ;
697  double kr2ud = 1 + kud * r2ud;
698 
699  double Axx = px*(kr2ud+k2ud*X2);
700  double Axy = px*k2ud*XY;
701  double Ayy = py*(kr2ud+k2ud*Y2);
702  double Ayx = py*k2ud*XY;
703 
704  Pd[i42] = up ;
705  Pd[i43] = vp ;
706 
707  P[i42] = u0 + px*X*kr2ud ;
708  P[i43] = v0 + py*Y*kr2ud ;
709 
710  r += (vpMath::sqr(P[i4]-Pd[i4]) +
711  vpMath::sqr(P[i41]-Pd[i41]) +
712  vpMath::sqr(P[i42]-Pd[i42]) +
713  vpMath::sqr(P[i43]-Pd[i43]))*0.5;
714 
715  //--distorted to undistorted
716  {
717  {
718  L[i4][0] = px * (-inv_z) ;
719  L[i4][1] = 0 ;
720  L[i4][2] = px*X*inv_z ;
721  L[i4][3] = px*X*Y ;
722  L[i4][4] = -px*(1+X2) ;
723  L[i4][5] = px*Y ;
724  }
725  {
726  L[i4][6]= 1 + kr2du + k2du*xp02 ;
727  L[i4][7]= k2du*up0*yp0*inv_py ;
728  L[i4][8]= X + k2du*xp02*xp0 ;
729  L[i4][9]= k2du*up0*yp02*inv_py ;
730  L[i4][10] = -(up0)*(r2du) ;
731  L[i4][11] = 0 ;
732  }
733  {
734  L[i41][0] = 0 ;
735  L[i41][1] = py*(-inv_z) ;
736  L[i41][2] = py*Y*inv_z ;
737  L[i41][3] = py* (1+Y2) ;
738  L[i41][4] = -py*XY ;
739  L[i41][5] = -py*X ;
740  }
741  {
742  L[i41][6]= k2du*xp0*vp0*inv_px ;
743  L[i41][7]= 1 + kr2du + k2du*yp02;
744  L[i41][8]= k2du*vp0*xp02*inv_px;
745  L[i41][9]= Y + k2du*yp02*yp0;
746  L[i41][10] = -vp0*r2du ;
747  L[i41][11] = 0 ;
748  }
749  //---undistorted to distorted
750  {
751  L[i42][0] = Axx*(-inv_z) ;
752  L[i42][1] = Axy*(-inv_z) ;
753  L[i42][2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
754  L[i42][3] = Axx*X*Y + Axy*(1+Y2);
755  L[i42][4] = -Axx*(1+X2) - Axy*XY;
756  L[i42][5] = Axx*Y -Axy*X;
757  }
758  {
759  L[i42][6]= 1 ;
760  L[i42][7]= 0 ;
761  L[i42][8]= X*kr2ud ;
762  L[i42][9]= 0;
763  L[i42][10] = 0 ;
764  L[i42][11] = px*X*r2ud ;
765  }
766  {
767  L[i43][0] = Ayx*(-inv_z) ;
768  L[i43][1] = Ayy*(-inv_z) ;
769  L[i43][2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
770  L[i43][3] = Ayx*XY + Ayy*(1+Y2) ;
771  L[i43][4] = -Ayx*(1+X2) -Ayy*XY ;
772  L[i43][5] = Ayx*Y -Ayy*X;
773  }
774  {
775  L[i43][6]= 0 ;
776  L[i43][7]= 1;
777  L[i43][8]= 0;
778  L[i43][9]= Y*kr2ud ;
779  L[i43][10] = 0 ;
780  L[i43][11] = py*Y*r2ud ;
781  }
782  } // end interaction
783  } // end interaction
784 
785  vpColVector error ;
786  error = P-Pd ;
787  //r = r/n_points ;
788 
789  vpMatrix Lp ;
790  Lp = L.pseudoInverse(1e-10) ;
791 
792  vpColVector e ;
793  e = Lp*error ;
794 
795  vpColVector Tc, Tc_v(6) ;
796  Tc = -e*gain ;
797 
798  for (unsigned int i=0 ; i <6 ; i++)
799  Tc_v[i] = Tc[i] ;
800 
801  cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9],
802  u0 + Tc[6], v0 + Tc[7],
803  kud + Tc[11],
804  kdu + Tc[10]);
805 
806  cMo_est = vpExponentialMap::direct(Tc_v).inverse()*cMo_est ;
807  if (verbose)
808  std::cout << " std dev " << sqrt(r/n_points) << std::endl;
809 
810  }
811  if (iter == nbIterMax)
812  {
813  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
814  throw(vpCalibrationException(vpCalibrationException::convergencyError,
815  "Maximum number of iterations reached")) ;
816  }
817  this->residual_dist = r;
818  this->cMo_dist = cMo_est ;
819  this->cam_dist = cam_est ;
820 
821  if (verbose)
822  std::cout << " std dev " << sqrt(r/n_points) << std::endl;
823 
824  // Restore ostream format
825  std::cout.flags(original_flags);
826 }
827 
828 
829 void
830 vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal,
831  vpCameraParameters &cam_est, double &globalReprojectionError,
832  bool verbose)
833 {
834  std::ios::fmtflags original_flags( std::cout.flags() );
835  std::cout.precision(10);
836  unsigned int nbPoint[1024]; //number of points by image
837  unsigned int nbPointTotal = 0; //total number of points
838  unsigned int nbPose = (unsigned int)table_cal.size();
839  unsigned int nbPose6 = 6*nbPose;
840  for (unsigned int i=0; i<nbPose ; i++)
841  {
842  nbPoint[i] = table_cal[i].npt;
843  nbPointTotal += nbPoint[i];
844  }
845 
846  if (nbPointTotal < 4)
847  {
848  //vpERROR_TRACE("Not enough point to calibrate");
849  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
850  "Not enough point to calibrate")) ;
851  }
852 
853  vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
854  vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
855  vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
856  vpColVector u(nbPointTotal) ;
857  vpColVector v(nbPointTotal) ;
858 
859  vpColVector P(4*nbPointTotal) ;
860  vpColVector Pd(4*nbPointTotal) ;
861  vpImagePoint ip;
862 
863  unsigned int curPoint = 0 ; //current point indice
864  for (unsigned int p=0; p<nbPose ; p++)
865  {
866  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
867  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
868  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
869  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
870 
871  for (unsigned int i =0 ; i < nbPoint[p] ; i++)
872  {
873  oX[curPoint] = *it_LoX;
874  oY[curPoint] = *it_LoY;
875  oZ[curPoint] = *it_LoZ;
876 
877  ip = *it_Lip;
878  u[curPoint] = ip.get_u() ;
879  v[curPoint] = ip.get_v() ;
880 
881  ++ it_LoX;
882  ++ it_LoY;
883  ++ it_LoZ;
884  ++ it_Lip;
885  curPoint++;
886  }
887  }
888  // double lambda = 0.1 ;
889  unsigned int iter = 0 ;
890 
891  double residu_1 = 1e12 ;
892  double r =1e12-1;
893  while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
894  {
895  iter++ ;
896  residu_1 = r ;
897 
898  r = 0 ;
899  curPoint = 0 ; //current point indice
900  for (unsigned int p=0; p<nbPose ; p++)
901  {
902  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
903  for (unsigned int i=0 ; i < nbPoint[p]; i++)
904  {
905  cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
906  +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
907  cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
908  +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
909  cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
910  +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
911 
912  curPoint++;
913  }
914  }
915 
916 
917  vpMatrix L(nbPointTotal*4,nbPose6+6) ;
918  curPoint = 0 ; //current point indice
919  double px = cam_est.get_px() ;
920  double py = cam_est.get_py() ;
921  double u0 = cam_est.get_u0() ;
922  double v0 = cam_est.get_v0() ;
923 
924  double inv_px = 1/px ;
925  double inv_py = 1/py ;
926 
927  double kud = cam_est.get_kud() ;
928  double kdu = cam_est.get_kdu() ;
929 
930  double k2ud = 2*kud;
931  double k2du = 2*kdu;
932 
933  for (unsigned int p=0; p<nbPose ; p++)
934  {
935  unsigned int q = 6*p;
936  for (unsigned int i=0 ; i < nbPoint[p]; i++)
937  {
938  unsigned int curPoint4 = 4*curPoint;
939  double x = cX[curPoint] ;
940  double y = cY[curPoint] ;
941  double z = cZ[curPoint] ;
942 
943  double inv_z = 1/z;
944  double X = x*inv_z ;
945  double Y = y*inv_z ;
946 
947  double X2 = X*X;
948  double Y2 = Y*Y;
949  double XY = X*Y;
950 
951  double up = u[curPoint] ;
952  double vp = v[curPoint] ;
953 
954  Pd[curPoint4] = up ;
955  Pd[curPoint4+1] = vp ;
956 
957  double up0 = up - u0;
958  double vp0 = vp - v0;
959 
960  double xp0 = up0 * inv_px;
961  double xp02 = xp0 *xp0 ;
962 
963  double yp0 = vp0 * inv_py;
964  double yp02 = yp0 * yp0;
965 
966  double r2du = xp02 + yp02 ;
967  double kr2du = kdu * r2du;
968 
969  P[curPoint4] = u0 + px*X - kr2du *(up0) ;
970  P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
971 
972  double r2ud = X2 + Y2 ;
973  double kr2ud = 1 + kud * r2ud;
974 
975  double Axx = px*(kr2ud+k2ud*X2);
976  double Axy = px*k2ud*XY;
977  double Ayy = py*(kr2ud+k2ud*Y2);
978  double Ayx = py*k2ud*XY;
979 
980  Pd[curPoint4+2] = up ;
981  Pd[curPoint4+3] = vp ;
982 
983  P[curPoint4+2] = u0 + px*X*kr2ud ;
984  P[curPoint4+3] = v0 + py*Y*kr2ud ;
985 
986  r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
987  vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
988  vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
989  vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
990 
991  unsigned int curInd = curPoint4;
992  //---------------
993  {
994  {
995  L[curInd][q] = px * (-inv_z) ;
996  L[curInd][q+1] = 0 ;
997  L[curInd][q+2] = px*X*inv_z ;
998  L[curInd][q+3] = px*X*Y ;
999  L[curInd][q+4] = -px*(1+X2) ;
1000  L[curInd][q+5] = px*Y ;
1001  }
1002  {
1003  L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
1004  L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
1005  L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
1006  L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
1007  L[curInd][nbPose6+4] = -(up0)*(r2du) ;
1008  L[curInd][nbPose6+5] = 0 ;
1009  }
1010  curInd++;
1011  {
1012  L[curInd][q] = 0 ;
1013  L[curInd][q+1] = py*(-inv_z) ;
1014  L[curInd][q+2] = py*Y*inv_z ;
1015  L[curInd][q+3] = py* (1+Y2) ;
1016  L[curInd][q+4] = -py*XY ;
1017  L[curInd][q+5] = -py*X ;
1018  }
1019  {
1020  L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
1021  L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
1022  L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
1023  L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
1024  L[curInd][nbPose6+4] = -vp0*r2du ;
1025  L[curInd][nbPose6+5] = 0 ;
1026  }
1027  curInd++;
1028  //---undistorted to distorted
1029  {
1030  L[curInd][q] = Axx*(-inv_z) ;
1031  L[curInd][q+1] = Axy*(-inv_z) ;
1032  L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
1033  L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
1034  L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
1035  L[curInd][q+5] = Axx*Y -Axy*X;
1036  }
1037  {
1038  L[curInd][nbPose6]= 1 ;
1039  L[curInd][nbPose6+1]= 0 ;
1040  L[curInd][nbPose6+2]= X*kr2ud ;
1041  L[curInd][nbPose6+3]= 0;
1042  L[curInd][nbPose6+4] = 0 ;
1043  L[curInd][nbPose6+5] = px*X*r2ud ;
1044  }
1045  curInd++;
1046  {
1047  L[curInd][q] = Ayx*(-inv_z) ;
1048  L[curInd][q+1] = Ayy*(-inv_z) ;
1049  L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
1050  L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
1051  L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
1052  L[curInd][q+5] = Ayx*Y -Ayy*X;
1053  }
1054  {
1055  L[curInd][nbPose6]= 0 ;
1056  L[curInd][nbPose6+1]= 1;
1057  L[curInd][nbPose6+2]= 0;
1058  L[curInd][nbPose6+3]= Y*kr2ud ;
1059  L[curInd][nbPose6+4] = 0 ;
1060  L[curInd][nbPose6+5] = py*Y*r2ud ;
1061  }
1062  } // end interaction
1063  curPoint++;
1064  } // end interaction
1065  }
1066 
1067  vpColVector error ;
1068  error = P-Pd ;
1069  //r = r/nbPointTotal ;
1070 
1071  vpMatrix Lp ;
1072  /*double rank =*/
1073  L.pseudoInverse(Lp,1e-10) ;
1074  vpColVector e ;
1075  e = Lp*error ;
1076  vpColVector Tc, Tc_v(6*nbPose) ;
1077  Tc = -e*gain ;
1078  for (unsigned int i = 0 ; i < 6*nbPose ; i++)
1079  Tc_v[i] = Tc[i] ;
1080 
1081  cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
1082  u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1083  kud + Tc[nbPose6+5],
1084  kdu + Tc[nbPose6+4]);
1085 
1086  vpColVector Tc_v_Tmp(6) ;
1087  for (unsigned int p = 0 ; p < nbPose ; p++)
1088  {
1089  for (unsigned int i = 0 ; i < 6 ; i++)
1090  Tc_v_Tmp[i] = Tc_v[6*p + i];
1091 
1092  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
1093  * table_cal[p].cMo_dist;
1094  }
1095  if (verbose)
1096  std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
1097  //std::cout << " residual: " << r << std::endl;
1098 
1099  }
1100  if (iter == nbIterMax)
1101  {
1102  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
1103  throw(vpCalibrationException(vpCalibrationException::convergencyError,
1104  "Maximum number of iterations reached")) ;
1105  }
1106 
1107  //double perViewError;
1108  //double totalError = 0;
1109  //int totalPoints = 0;
1110  for (unsigned int p = 0 ; p < nbPose ; p++)
1111  {
1112  table_cal[p].cam_dist = cam_est ;
1113  //perViewError = table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
1114  //totalError += perViewError*perViewError * table_cal[p].npt;
1115  //totalPoints += (int)table_cal[p].npt;
1116  }
1117  globalReprojectionError = sqrt(r/(nbPointTotal));
1118 
1119  // Restore ostream format
1120  std::cout.flags(original_flags);
1121 }
1122 
1137 void vpCalibration::calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
1138  std::vector<vpHomogeneousMatrix>& rMe,
1139  vpHomogeneousMatrix &eMc){
1140 
1141  vpColVector x ;
1142  unsigned int nbPose = (unsigned int)cMo.size();
1143  if(cMo.size()!=rMe.size()) throw vpCalibrationException(vpCalibrationException::dimensionError,"cMo and rMe have different sizes");
1144  {
1145  vpMatrix A ;
1146  vpColVector B ;
1147  unsigned int k = 0 ;
1148  // for all couples ij
1149  for (unsigned int i=0 ; i < nbPose ; i++)
1150  {
1151  vpRotationMatrix rRei, ciRo ;
1152  rMe[i].extract(rRei) ;
1153  cMo[i].extract(ciRo) ;
1154  //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
1155 
1156  for (unsigned int j=0 ; j < nbPose ; j++)
1157  {
1158  if (j>i) // we don't use two times same couples...
1159  {
1160  vpRotationMatrix rRej, cjRo ;
1161  rMe[j].extract(rRej) ;
1162  cMo[j].extract(cjRo) ;
1163  //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
1164 
1165  vpRotationMatrix rReij = rRej.t() * rRei;
1166 
1167  vpRotationMatrix cijRo = cjRo * ciRo.t();
1168 
1169  vpThetaUVector rPeij(rReij);
1170 
1171  double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1172  + rPeij[2]*rPeij[2]);
1173 
1174  for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);
1175 
1176  vpThetaUVector cijPo(cijRo) ;
1177  theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
1178  + cijPo[2]*cijPo[2]);
1179  for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);
1180 
1181  vpMatrix As;
1182  vpColVector b(3) ;
1183 
1184  As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;
1185 
1186  b = (vpColVector)cijPo - (vpColVector)rPeij ; // A.40
1187 
1188  if (k==0)
1189  {
1190  A = As ;
1191  B = b ;
1192  }
1193  else
1194  {
1195  A = vpMatrix::stack(A,As) ;
1196  B = vpColVector::stack(B,b) ;
1197  }
1198  k++ ;
1199  }
1200  }
1201  }
1202 
1203  // the linear system is defined
1204  // x = AtA^-1AtB is solved
1205  vpMatrix AtA = A.AtA() ;
1206 
1207  vpMatrix Ap ;
1208  AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
1209  x = Ap*A.t()*B ;
1210 
1211 // {
1212 // // Residual
1213 // vpColVector residual;
1214 // residual = A*x-B;
1215 // std::cout << "Residual: " << std::endl << residual << std::endl;
1216 
1217 // double res = 0;
1218 // for (int i=0; i < residual.getRows(); i++)
1219 // res += residual[i]*residual[i];
1220 // res = sqrt(res/residual.getRows());
1221 // printf("Mean residual = %lf\n",res);
1222 // }
1223 
1224  // extraction of theta and U
1225  double theta ;
1226  double d=x.sumSquare() ;
1227  for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1228  theta = sqrt(x.sumSquare())/2 ;
1229  theta = 2*asin(theta) ;
1230  //if (theta !=0)
1231  if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1232  {
1233  for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1234  }
1235  else
1236  x = 0 ;
1237  }
1238 
1239  // Building of the rotation matrix eRc
1240  vpThetaUVector xP(x[0],x[1],x[2]);
1241  vpRotationMatrix eRc(xP);
1242 
1243  {
1244  vpMatrix A ;
1245  vpColVector B ;
1246  // Building of the system for the translation estimation
1247  // for all couples ij
1248  vpRotationMatrix I3 ;
1249  I3.eye() ;
1250  int k = 0 ;
1251  for (unsigned int i=0 ; i < nbPose ; i++)
1252  {
1253  vpRotationMatrix rRei, ciRo ;
1254  vpTranslationVector rTei, ciTo ;
1255  rMe[i].extract(rRei) ;
1256  cMo[i].extract(ciRo) ;
1257  rMe[i].extract(rTei) ;
1258  cMo[i].extract(ciTo) ;
1259 
1260 
1261  for (unsigned int j=0 ; j < nbPose ; j++)
1262  {
1263  if (j>i) // we don't use two times same couples...
1264  {
1265 
1266  vpRotationMatrix rRej, cjRo ;
1267  rMe[j].extract(rRej) ;
1268  cMo[j].extract(cjRo) ;
1269 
1270  vpTranslationVector rTej, cjTo ;
1271  rMe[j].extract(rTej) ;
1272  cMo[j].extract(cjTo) ;
1273 
1274  vpRotationMatrix rReij = rRej.t() * rRei ;
1275 
1276  vpTranslationVector rTeij = rTej+ (-rTei);
1277 
1278  rTeij = rRej.t()*rTeij ;
1279 
1280  vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);
1281 
1282  vpTranslationVector b ;
1283  b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
1284 
1285  if (k==0)
1286  {
1287  A = a ;
1288  B = b ;
1289  }
1290  else
1291  {
1292  A = vpMatrix::stack(A,a) ;
1293  B = vpColVector::stack(B,b) ;
1294  }
1295  k++ ;
1296  }
1297  }
1298  }
1299 
1300  // the linear system is solved
1301  // x = AtA^-1AtB is solved
1302  vpMatrix AtA = A.AtA() ;
1303  vpMatrix Ap ;
1304  vpColVector AeTc ;
1305  AtA.pseudoInverse(Ap, 1e-6) ;
1306  AeTc = Ap*A.t()*B ;
1307 
1308 // {
1309 // // residual
1310 // vpColVector residual;
1311 // residual = A*AeTc-B;
1312 // std::cout << "Residual: " << std::endl << residual << std::endl;
1313 // double res = 0;
1314 // for (int i=0; i < residual.getRows(); i++)
1315 // res += residual[i]*residual[i];
1316 // res = sqrt(res/residual.getRows());
1317 // printf("mean residual = %lf\n",res);
1318 // }
1319 
1320  vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);
1321 
1322  eMc.insert(eTc) ;
1323  eMc.insert(eRc) ;
1324  }
1325 }
1326 
1327 
1328 void
1329 vpCalibration::calibVVSMulti(unsigned int nbPose,
1330  vpCalibration table_cal[],
1331  vpCameraParameters &cam_est,
1332  bool verbose)
1333 {
1334  std::ios::fmtflags original_flags( std::cout.flags() );
1335  std::cout.precision(10);
1336  unsigned int nbPoint[256]; //number of points by image
1337  unsigned int nbPointTotal = 0; //total number of points
1338 
1339  unsigned int nbPose6 = 6*nbPose;
1340 
1341  for (unsigned int i=0; i<nbPose ; i++)
1342  {
1343  nbPoint[i] = table_cal[i].npt;
1344  nbPointTotal += nbPoint[i];
1345  }
1346 
1347  if (nbPointTotal < 4)
1348  {
1349  //vpERROR_TRACE("Not enough point to calibrate");
1350  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
1351  "Not enough point to calibrate")) ;
1352  }
1353 
1354  vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
1355  vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
1356  vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
1357  vpColVector u(nbPointTotal) ;
1358  vpColVector v(nbPointTotal) ;
1359 
1360  vpColVector P(2*nbPointTotal) ;
1361  vpColVector Pd(2*nbPointTotal) ;
1362  vpImagePoint ip;
1363 
1364  unsigned int curPoint = 0 ; //current point indice
1365  for (unsigned int p=0; p<nbPose ; p++)
1366  {
1367  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1368  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1369  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1370  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1371 
1372  for (unsigned int i =0 ; i < nbPoint[p] ; i++)
1373  {
1374  oX[curPoint] = *it_LoX;
1375  oY[curPoint] = *it_LoY;
1376  oZ[curPoint] = *it_LoZ;
1377 
1378  ip = *it_Lip;
1379  u[curPoint] = ip.get_u() ;
1380  v[curPoint] = ip.get_v() ;
1381 
1382  ++ it_LoX;
1383  ++ it_LoY;
1384  ++ it_LoZ;
1385  ++ it_Lip;
1386 
1387  curPoint++;
1388  }
1389  }
1390  // double lambda = 0.1 ;
1391  unsigned int iter = 0 ;
1392 
1393  double residu_1 = 1e12 ;
1394  double r =1e12-1;
1395  while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
1396  {
1397 
1398  iter++ ;
1399  residu_1 = r ;
1400 
1401  double px = cam_est.get_px();
1402  double py = cam_est.get_py();
1403  double u0 = cam_est.get_u0();
1404  double v0 = cam_est.get_v0();
1405 
1406  r = 0 ;
1407  curPoint = 0 ; //current point indice
1408  for (unsigned int p=0; p<nbPose ; p++)
1409  {
1410  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
1411  for (unsigned int i=0 ; i < nbPoint[p]; i++)
1412  {
1413  unsigned int curPoint2 = 2*curPoint;
1414 
1415  cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
1416  +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
1417  cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
1418  +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
1419  cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
1420  +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
1421 
1422  Pd[curPoint2] = u[curPoint] ;
1423  Pd[curPoint2+1] = v[curPoint] ;
1424 
1425  P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
1426  P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
1427 
1428  r += (vpMath::sqr(P[curPoint2]-Pd[curPoint2])
1429  + vpMath::sqr(P[curPoint2+1]-Pd[curPoint2+1])) ;
1430  curPoint++;
1431  }
1432  }
1433 
1434  vpColVector error ;
1435  error = P-Pd ;
1436  //r = r/nbPointTotal ;
1437 
1438  vpMatrix L(nbPointTotal*2,nbPose6+4) ;
1439  curPoint = 0 ; //current point indice
1440  for (unsigned int p=0; p<nbPose ; p++)
1441  {
1442  unsigned int q = 6*p;
1443  for (unsigned int i=0 ; i < nbPoint[p]; i++)
1444  {
1445  unsigned int curPoint2 = 2*curPoint;
1446  unsigned int curPoint21 = curPoint2 + 1;
1447 
1448  double x = cX[curPoint] ;
1449  double y = cY[curPoint] ;
1450  double z = cZ[curPoint] ;
1451 
1452  double inv_z = 1/z;
1453 
1454  double X = x*inv_z ;
1455  double Y = y*inv_z ;
1456 
1457  //---------------
1458  {
1459  {
1460  L[curPoint2][q] = px * (-inv_z) ;
1461  L[curPoint2][q+1] = 0 ;
1462  L[curPoint2][q+2] = px*(X*inv_z) ;
1463  L[curPoint2][q+3] = px*X*Y ;
1464  L[curPoint2][q+4] = -px*(1+X*X) ;
1465  L[curPoint2][q+5] = px*Y ;
1466  }
1467  {
1468  L[curPoint2][nbPose6]= 1 ;
1469  L[curPoint2][nbPose6+1]= 0 ;
1470  L[curPoint2][nbPose6+2]= X ;
1471  L[curPoint2][nbPose6+3]= 0;
1472  }
1473  {
1474  L[curPoint21][q] = 0 ;
1475  L[curPoint21][q+1] = py*(-inv_z) ;
1476  L[curPoint21][q+2] = py*(Y*inv_z) ;
1477  L[curPoint21][q+3] = py* (1+Y*Y) ;
1478  L[curPoint21][q+4] = -py*X*Y ;
1479  L[curPoint21][q+5] = -py*X ;
1480  }
1481  {
1482  L[curPoint21][nbPose6]= 0 ;
1483  L[curPoint21][nbPose6+1]= 1 ;
1484  L[curPoint21][nbPose6+2]= 0;
1485  L[curPoint21][nbPose6+3]= Y ;
1486  }
1487 
1488  }
1489  curPoint++;
1490  } // end interaction
1491  }
1492  vpMatrix Lp ;
1493  Lp = L.pseudoInverse(1e-10) ;
1494 
1495  vpColVector e ;
1496  e = Lp*error ;
1497 
1498  vpColVector Tc, Tc_v(nbPose6) ;
1499  Tc = -e*gain ;
1500 
1501  // Tc_v =0 ;
1502  for (unsigned int i = 0 ; i < nbPose6 ; i++)
1503  Tc_v[i] = Tc[i] ;
1504 
1505  cam_est.initPersProjWithoutDistortion(px+Tc[nbPose6+2],
1506  py+Tc[nbPose6+3],
1507  u0+Tc[nbPose6],
1508  v0+Tc[nbPose6+1]) ;
1509 
1510  // cam.setKd(get_kd() + Tc[10]) ;
1511  vpColVector Tc_v_Tmp(6) ;
1512 
1513  for (unsigned int p = 0 ; p < nbPose ; p++)
1514  {
1515  for (unsigned int i = 0 ; i < 6 ; i++)
1516  Tc_v_Tmp[i] = Tc_v[6*p + i];
1517 
1518  table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
1519  * table_cal[p].cMo;
1520  }
1521  if (verbose)
1522  std::cout << " std dev " << sqrt(r/nbPointTotal) << std::endl;
1523 
1524  }
1525  if (iter == nbIterMax)
1526  {
1527  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
1528  throw(vpCalibrationException(vpCalibrationException::convergencyError,
1529  "Maximum number of iterations reached")) ;
1530  }
1531  for (unsigned int p = 0 ; p < nbPose ; p++)
1532  {
1533  table_cal[p].cMo_dist = table_cal[p].cMo ;
1534  table_cal[p].cam = cam_est;
1535  table_cal[p].cam_dist = cam_est;
1536  double deviation,deviation_dist;
1537  table_cal[p].computeStdDeviation(deviation,deviation_dist);
1538  }
1539  if (verbose)
1540  std::cout << " Global std dev " << sqrt(r/nbPointTotal) << std::endl;
1541 
1542  // Restore ostream format
1543  std::cout.flags(original_flags);
1544 }
1545 
1546 
1547 void
1548 vpCalibration::calibVVSWithDistortionMulti(
1549  unsigned int nbPose,
1550  vpCalibration table_cal[],
1551  vpCameraParameters &cam_est,
1552  bool verbose)
1553 {
1554  std::ios::fmtflags original_flags( std::cout.flags() );
1555  std::cout.precision(10);
1556  unsigned int nbPoint[1024]; //number of points by image
1557  unsigned int nbPointTotal = 0; //total number of points
1558 
1559  unsigned int nbPose6 = 6*nbPose;
1560  for (unsigned int i=0; i<nbPose ; i++)
1561  {
1562  nbPoint[i] = table_cal[i].npt;
1563  nbPointTotal += nbPoint[i];
1564  }
1565 
1566  if (nbPointTotal < 4)
1567  {
1568  //vpERROR_TRACE("Not enough point to calibrate");
1569  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
1570  "Not enough point to calibrate")) ;
1571  }
1572 
1573  vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
1574  vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
1575  vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
1576  vpColVector u(nbPointTotal) ;
1577  vpColVector v(nbPointTotal) ;
1578 
1579  vpColVector P(4*nbPointTotal) ;
1580  vpColVector Pd(4*nbPointTotal) ;
1581  vpImagePoint ip;
1582 
1583  unsigned int curPoint = 0 ; //current point indice
1584  for (unsigned int p=0; p<nbPose ; p++)
1585  {
1586  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1587  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1588  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1589  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1590 
1591  for (unsigned int i =0 ; i < nbPoint[p] ; i++)
1592  {
1593  oX[curPoint] = *it_LoX;
1594  oY[curPoint] = *it_LoY;
1595  oZ[curPoint] = *it_LoZ;
1596 
1597  ip = *it_Lip;
1598  u[curPoint] = ip.get_u() ;
1599  v[curPoint] = ip.get_v() ;
1600 
1601  ++ it_LoX;
1602  ++ it_LoY;
1603  ++ it_LoZ;
1604  ++ it_Lip;
1605  curPoint++;
1606  }
1607  }
1608  // double lambda = 0.1 ;
1609  unsigned int iter = 0 ;
1610 
1611  double residu_1 = 1e12 ;
1612  double r =1e12-1;
1613  while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
1614  {
1615  iter++ ;
1616  residu_1 = r ;
1617 
1618  r = 0 ;
1619  curPoint = 0 ; //current point indice
1620  for (unsigned int p=0; p<nbPose ; p++)
1621  {
1622  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
1623  for (unsigned int i=0 ; i < nbPoint[p]; i++)
1624  {
1625  cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
1626  +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
1627  cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
1628  +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
1629  cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
1630  +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
1631 
1632  curPoint++;
1633  }
1634  }
1635 
1636 
1637  vpMatrix L(nbPointTotal*4,nbPose6+6) ;
1638  curPoint = 0 ; //current point indice
1639  double px = cam_est.get_px() ;
1640  double py = cam_est.get_py() ;
1641  double u0 = cam_est.get_u0() ;
1642  double v0 = cam_est.get_v0() ;
1643 
1644  double inv_px = 1/px ;
1645  double inv_py = 1/py ;
1646 
1647  double kud = cam_est.get_kud() ;
1648  double kdu = cam_est.get_kdu() ;
1649 
1650  double k2ud = 2*kud;
1651  double k2du = 2*kdu;
1652 
1653  for (unsigned int p=0; p<nbPose ; p++)
1654  {
1655  unsigned int q = 6*p;
1656  for (unsigned int i=0 ; i < nbPoint[p]; i++)
1657  {
1658  unsigned int curPoint4 = 4*curPoint;
1659  double x = cX[curPoint] ;
1660  double y = cY[curPoint] ;
1661  double z = cZ[curPoint] ;
1662 
1663  double inv_z = 1/z;
1664  double X = x*inv_z ;
1665  double Y = y*inv_z ;
1666 
1667  double X2 = X*X;
1668  double Y2 = Y*Y;
1669  double XY = X*Y;
1670 
1671  double up = u[curPoint] ;
1672  double vp = v[curPoint] ;
1673 
1674  Pd[curPoint4] = up ;
1675  Pd[curPoint4+1] = vp ;
1676 
1677  double up0 = up - u0;
1678  double vp0 = vp - v0;
1679 
1680  double xp0 = up0 * inv_px;
1681  double xp02 = xp0 *xp0 ;
1682 
1683  double yp0 = vp0 * inv_py;
1684  double yp02 = yp0 * yp0;
1685 
1686  double r2du = xp02 + yp02 ;
1687  double kr2du = kdu * r2du;
1688 
1689  P[curPoint4] = u0 + px*X - kr2du *(up0) ;
1690  P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
1691 
1692  double r2ud = X2 + Y2 ;
1693  double kr2ud = 1 + kud * r2ud;
1694 
1695  double Axx = px*(kr2ud+k2ud*X2);
1696  double Axy = px*k2ud*XY;
1697  double Ayy = py*(kr2ud+k2ud*Y2);
1698  double Ayx = py*k2ud*XY;
1699 
1700  Pd[curPoint4+2] = up ;
1701  Pd[curPoint4+3] = vp ;
1702 
1703  P[curPoint4+2] = u0 + px*X*kr2ud ;
1704  P[curPoint4+3] = v0 + py*Y*kr2ud ;
1705 
1706  r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
1707  vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
1708  vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
1709  vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
1710 
1711  unsigned int curInd = curPoint4;
1712  //---------------
1713  {
1714  {
1715  L[curInd][q] = px * (-inv_z) ;
1716  L[curInd][q+1] = 0 ;
1717  L[curInd][q+2] = px*X*inv_z ;
1718  L[curInd][q+3] = px*X*Y ;
1719  L[curInd][q+4] = -px*(1+X2) ;
1720  L[curInd][q+5] = px*Y ;
1721  }
1722  {
1723  L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
1724  L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
1725  L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
1726  L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
1727  L[curInd][nbPose6+4] = -(up0)*(r2du) ;
1728  L[curInd][nbPose6+5] = 0 ;
1729  }
1730  curInd++;
1731  {
1732  L[curInd][q] = 0 ;
1733  L[curInd][q+1] = py*(-inv_z) ;
1734  L[curInd][q+2] = py*Y*inv_z ;
1735  L[curInd][q+3] = py* (1+Y2) ;
1736  L[curInd][q+4] = -py*XY ;
1737  L[curInd][q+5] = -py*X ;
1738  }
1739  {
1740  L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
1741  L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
1742  L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
1743  L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
1744  L[curInd][nbPose6+4] = -vp0*r2du ;
1745  L[curInd][nbPose6+5] = 0 ;
1746  }
1747  curInd++;
1748  //---undistorted to distorted
1749  {
1750  L[curInd][q] = Axx*(-inv_z) ;
1751  L[curInd][q+1] = Axy*(-inv_z) ;
1752  L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
1753  L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
1754  L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
1755  L[curInd][q+5] = Axx*Y -Axy*X;
1756  }
1757  {
1758  L[curInd][nbPose6]= 1 ;
1759  L[curInd][nbPose6+1]= 0 ;
1760  L[curInd][nbPose6+2]= X*kr2ud ;
1761  L[curInd][nbPose6+3]= 0;
1762  L[curInd][nbPose6+4] = 0 ;
1763  L[curInd][nbPose6+5] = px*X*r2ud ;
1764  }
1765  curInd++;
1766  {
1767  L[curInd][q] = Ayx*(-inv_z) ;
1768  L[curInd][q+1] = Ayy*(-inv_z) ;
1769  L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
1770  L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
1771  L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
1772  L[curInd][q+5] = Ayx*Y -Ayy*X;
1773  }
1774  {
1775  L[curInd][nbPose6]= 0 ;
1776  L[curInd][nbPose6+1]= 1;
1777  L[curInd][nbPose6+2]= 0;
1778  L[curInd][nbPose6+3]= Y*kr2ud ;
1779  L[curInd][nbPose6+4] = 0 ;
1780  L[curInd][nbPose6+5] = py*Y*r2ud ;
1781  }
1782  } // end interaction
1783  curPoint++;
1784  } // end interaction
1785  }
1786 
1787  vpColVector error ;
1788  error = P-Pd ;
1789  //r = r/nbPointTotal ;
1790 
1791  vpMatrix Lp ;
1792  /*double rank =*/
1793  L.pseudoInverse(Lp,1e-10) ;
1794  vpColVector e ;
1795  e = Lp*error ;
1796  vpColVector Tc, Tc_v(6*nbPose) ;
1797  Tc = -e*gain ;
1798  for (unsigned int i = 0 ; i < 6*nbPose ; i++)
1799  Tc_v[i] = Tc[i] ;
1800 
1801  cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
1802  u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1803  kud + Tc[nbPose6+5],
1804  kdu + Tc[nbPose6+4]);
1805 
1806  vpColVector Tc_v_Tmp(6) ;
1807  for (unsigned int p = 0 ; p < nbPose ; p++)
1808  {
1809  for (unsigned int i = 0 ; i < 6 ; i++)
1810  Tc_v_Tmp[i] = Tc_v[6*p + i];
1811 
1812  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
1813  * table_cal[p].cMo_dist;
1814  }
1815  if (verbose)
1816  std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
1817  //std::cout << " residual: " << r << std::endl;
1818 
1819  }
1820  if (iter == nbIterMax)
1821  {
1822  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
1823  throw(vpCalibrationException(vpCalibrationException::convergencyError,
1824  "Maximum number of iterations reached")) ;
1825  }
1826 
1827  for (unsigned int p = 0 ; p < nbPose ; p++)
1828  {
1829  table_cal[p].cam_dist = cam_est ;
1830  table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
1831  }
1832  if (verbose)
1833  std::cout <<" Global std dev " << sqrt(r/(nbPointTotal)) << std::endl;
1834 
1835  // Restore ostream format
1836  std::cout.flags(original_flags);
1837 }
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
vpImagePoint::get_v
double get_v() const
Definition: vpImagePoint.h:268
vpCalibrationException
Error that can be emited by the vpCalibration class.
Definition: vpCalibrationException.h:64
vpCameraParameters::get_u0
double get_u0() const
Definition: vpCameraParameters.h:307
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
vpERROR_TRACE
#define vpERROR_TRACE
Definition: vpDebug.h:391
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
vpImagePoint::get_u
double get_u() const
Definition: vpImagePoint.h:257
vpRotationMatrix::t
vpRotationMatrix t() const
Definition: vpRotationMatrix.cpp:417
vpCameraParameters::get_py
double get_py() const
Definition: vpCameraParameters.h:306
vpArray2D::size
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:156
vpCalibration
Tools for perspective camera calibration.
Definition: vpCalibration.h:71
vpMath::sinc
static double sinc(double x)
Definition: vpMath.cpp:169
vpCalibration::cMo
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Definition: vpCalibration.h:85
vpRotationMatrix
Implementation of a rotation matrix and operations on such kind of matrices.
Definition: vpRotationMatrix.h:70
vpCameraParameters::initPersProjWithoutDistortion
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
Definition: vpCameraParameters.cpp:206
vpHomogeneousMatrix::insert
void insert(const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:616
vpMatrix::svd
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1631
vpCameraParameters::get_v0
double get_v0() const
Definition: vpCameraParameters.h:308
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:376
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:110
vpCalibration::cMo_dist
vpHomogeneousMatrix cMo_dist
Definition: vpCalibration.h:87
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:221
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:303
vpCalibration::computeStdDeviation_dist
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Definition: vpCalibration.cpp:279
vpCalibration::calibrationTsai
static void calibrationTsai(std::vector< vpHomogeneousMatrix > &cMo, std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
calibration method of effector-camera from R. Tsai and R. Lorenz .
Definition: vpCalibrationTools.cpp:1137
vpCameraParameters::get_kud
double get_kud() const
Definition: vpCameraParameters.h:309
vpColVector::sumSquare
double sumSquare() const
Definition: vpColVector.cpp:1393
vpMatrix::t
vpMatrix t() const
Definition: vpMatrix.cpp:207
vpCalibration::cam_dist
vpCameraParameters cam_dist
projection model with distortion
Definition: vpCalibration.h:92
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpCameraParameters::get_kdu
double get_kdu() const
Definition: vpCameraParameters.h:310
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:673
vpCalibrationException::convergencyError
iterative algorithm doesn't converge
Definition: vpCalibrationException.h:82
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:59
vpColVector::skew
static vpMatrix skew(const vpColVector &v)
Definition: vpColVector.cpp:1078
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1741
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
vpCalibration::computeStdDeviation
void computeStdDeviation(double &deviation, double &deviation_dist)
Definition: vpCalibration.cpp:348
vpCalibration::cam
vpCameraParameters cam
projection model without distortion
Definition: vpCalibration.h:90
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:93
vpThetaUVector
Implementation of a rotation vector as axis-angle minimal representation.
Definition: vpThetaUVector.h:147
vpCameraParameters::initPersProjWithDistortion
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
Definition: vpCameraParameters.cpp:275