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);
317  throw(vpCalibrationException(vpCalibrationException::convergencyError,
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");
351  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
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);
530  throw(vpCalibrationException(vpCalibrationException::convergencyError,
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);
776  throw(vpCalibrationException(vpCalibrationException::convergencyError,
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");
811  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
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);
1065  throw(vpCalibrationException(vpCalibrationException::convergencyError,
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
1102 vpCalibration::calibrationTsai(unsigned int nbPose,
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 
1248  vpTranslationVector b ;
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 
1454  vpTranslationVector b ;
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");
1522  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
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);
1700  throw(vpCalibrationException(vpCalibrationException::convergencyError,
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");
1741  throw(vpCalibrationException(vpCalibrationException::notInitializedError,
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);
1995  throw(vpCalibrationException(vpCalibrationException::convergencyError,
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 }
vpMatrix
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
vpImagePoint::get_v
double get_v() const
Definition: vpImagePoint.h:264
vpCalibrationException
Error that can be emited by the vpCalibration class.
Definition: vpCalibrationException.h:71
vpCameraParameters::get_u0
double get_u0() const
Definition: vpCameraParameters.h:296
vpHomogeneousMatrix
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Definition: vpHomogeneousMatrix.h:103
vpERROR_TRACE
#define vpERROR_TRACE
Definition: vpDebug.h:395
vpMath::equal
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:275
vpImagePoint::get_u
double get_u() const
Definition: vpImagePoint.h:253
vpRotationMatrix::t
vpRotationMatrix t() const
transpose
Definition: vpRotationMatrix.cpp:434
vpCameraParameters::get_py
double get_py() const
Definition: vpCameraParameters.h:295
vpRotationMatrix::setIdentity
void setIdentity()
Basic initialisation (identity)
Definition: vpRotationMatrix.cpp:100
vpCalibration
Tools for perspective camera calibration.
Definition: vpCalibration.h:78
vpMatrix::sumSquare
double sumSquare() const
Definition: vpMatrix.cpp:868
vpMath::sinc
static double sinc(double x)
Definition: vpMath.h:301
vpCalibration::cMo
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Definition: vpCalibration.h:92
vpRotationMatrix
The vpRotationMatrix considers the particular case of a rotation matrix.
Definition: vpRotationMatrix.h:74
vpColVector::size
unsigned int size() const
Definition: vpColVector.h:204
vpCameraParameters::initPersProjWithoutDistortion
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
Definition: vpCameraParameters.cpp:210
vpHomogeneousMatrix::insert
void insert(const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:532
vpMatrix::svd
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1822
vpCameraParameters::get_v0
double get_v0() const
Definition: vpCameraParameters.h:297
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:1479
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:106
vpCalibration::cMo_dist
vpHomogeneousMatrix cMo_dist
Definition: vpCalibration.h:94
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:210
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:486
vpMatrix::stackMatrices
void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.cpp:3272
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:292
vpCalibration::computeStdDeviation_dist
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Definition: vpCalibration.cpp:284
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:1308
vpCameraParameters::get_kud
double get_kud() const
Definition: vpCameraParameters.h:298
vpMatrix::t
vpMatrix t() const
Definition: vpMatrix.cpp:1296
vpCalibration::cam_dist
vpCameraParameters cam_dist
projection model with distortion
Definition: vpCalibration.h:99
vpColVector
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
vpCameraParameters::get_kdu
double get_kdu() const
Definition: vpCameraParameters.h:299
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:591
vpCalibrationException::convergencyError
iterative algorithm doesn't converge
Definition: vpCalibrationException.h:89
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:65
vpColVector::skew
static vpMatrix skew(const vpColVector &v)
Definition: vpColVector.cpp:718
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1932
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
vpCalibration::computeStdDeviation
void computeStdDeviation(double &deviation, double &deviation_dist)
Definition: vpCalibration.cpp:353
vpCalibration::cam
vpCameraParameters cam
projection model without distortion
Definition: vpCalibration.h:97
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:95
vpThetaUVector
Class that consider the case of the parameterization for the rotation.
Definition: vpThetaUVector.h:140
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:279