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