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