ViSP  2.7.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(unsigned int nbPose,
340  vpCalibration table_cal[],
341  vpCameraParameters &cam,
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 
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  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;
537  table_cal[p].cam_dist = cam;
538  double deviation,deviation_dist;
539  table_cal[p].computeStdDeviation(deviation,deviation_dist);
540  }
541  if (verbose)
542  std::cout << " std dev " << sqrt(r/nbPointTotal) << std::endl;
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(
792  unsigned int nbPose,
793  vpCalibration table_cal[],
794  vpCameraParameters &cam,
795  bool verbose)
796 {
797  std::cout.precision(10);
798  unsigned int nbPoint[256]; //number of points by image
799  unsigned int nbPointTotal = 0; //total number of points
800 
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  vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
809  vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
810  vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
811  vpColVector u(nbPointTotal) ;
812  vpColVector v(nbPointTotal) ;
813 
814  vpColVector P(4*nbPointTotal) ;
815  vpColVector Pd(4*nbPointTotal) ;
816  vpImagePoint ip;
817 
818  unsigned int curPoint = 0 ; //current point indice
819  for (unsigned int p=0; p<nbPose ; p++)
820  {
821  std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
822  std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
823  std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
824  std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
825 
826  for (unsigned int i =0 ; i < nbPoint[p] ; i++)
827  {
828  oX[curPoint] = *it_LoX;
829  oY[curPoint] = *it_LoY;
830  oZ[curPoint] = *it_LoZ;
831 
832  ip = *it_Lip;
833  u[curPoint] = ip.get_u() ;
834  v[curPoint] = ip.get_v() ;
835 
836  ++ it_LoX;
837  ++ it_LoY;
838  ++ it_LoZ;
839  ++ it_Lip;
840  curPoint++;
841  }
842  }
843  // double lambda = 0.1 ;
844  unsigned int iter = 0 ;
845 
846  double residu_1 = 1e12 ;
847  double r =1e12-1;
848  while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
849  {
850  iter++ ;
851  residu_1 = r ;
852 
853  r = 0 ;
854  curPoint = 0 ; //current point indice
855  for (unsigned int p=0; p<nbPose ; p++)
856  {
857  vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
858  for (unsigned int i=0 ; i < nbPoint[p]; i++)
859  {
860  cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
861  +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
862  cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
863  +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
864  cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
865  +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
866 
867  curPoint++;
868  }
869  }
870 
871 
872  vpMatrix L(nbPointTotal*4,nbPose6+6) ;
873  curPoint = 0 ; //current point indice
874  double px = cam.get_px() ;
875  double py = cam.get_py() ;
876  double u0 = cam.get_u0() ;
877  double v0 = cam.get_v0() ;
878 
879  double inv_px = 1/px ;
880  double inv_py = 1/py ;
881 
882  double kud = cam.get_kud() ;
883  double kdu = cam.get_kdu() ;
884 
885  double k2ud = 2*kud;
886  double k2du = 2*kdu;
887 
888  for (unsigned int p=0; p<nbPose ; p++)
889  {
890  unsigned int q = 6*p;
891  for (unsigned int i=0 ; i < nbPoint[p]; i++)
892  {
893  unsigned int curPoint4 = 4*curPoint;
894  double x = cX[curPoint] ;
895  double y = cY[curPoint] ;
896  double z = cZ[curPoint] ;
897 
898  double inv_z = 1/z;
899  double X = x*inv_z ;
900  double Y = y*inv_z ;
901 
902  double X2 = X*X;
903  double Y2 = Y*Y;
904  double XY = X*Y;
905 
906  double up = u[curPoint] ;
907  double vp = v[curPoint] ;
908 
909  Pd[curPoint4] = up ;
910  Pd[curPoint4+1] = vp ;
911 
912  double up0 = up - u0;
913  double vp0 = vp - v0;
914 
915  double xp0 = up0 * inv_px;
916  double xp02 = xp0 *xp0 ;
917 
918  double yp0 = vp0 * inv_py;
919  double yp02 = yp0 * yp0;
920 
921  double r2du = xp02 + yp02 ;
922  double kr2du = kdu * r2du;
923 
924  P[curPoint4] = u0 + px*X - kr2du *(up0) ;
925  P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
926 
927  double r2ud = X2 + Y2 ;
928  double kr2ud = 1 + kud * r2ud;
929 
930  double Axx = px*(kr2ud+k2ud*X2);
931  double Axy = px*k2ud*XY;
932  double Ayy = py*(kr2ud+k2ud*Y2);
933  double Ayx = py*k2ud*XY;
934 
935  Pd[curPoint4+2] = up ;
936  Pd[curPoint4+3] = vp ;
937 
938  P[curPoint4+2] = u0 + px*X*kr2ud ;
939  P[curPoint4+3] = v0 + py*Y*kr2ud ;
940 
941  r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
942  vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
943  vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
944  vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
945 
946  unsigned int curInd = curPoint4;
947  //---------------
948  {
949  {
950  L[curInd][q] = px * (-inv_z) ;
951  L[curInd][q+1] = 0 ;
952  L[curInd][q+2] = px*X*inv_z ;
953  L[curInd][q+3] = px*X*Y ;
954  L[curInd][q+4] = -px*(1+X2) ;
955  L[curInd][q+5] = px*Y ;
956  }
957  {
958  L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
959  L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
960  L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
961  L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
962  L[curInd][nbPose6+4] = -(up0)*(r2du) ;
963  L[curInd][nbPose6+5] = 0 ;
964  }
965  curInd++;
966  {
967  L[curInd][q] = 0 ;
968  L[curInd][q+1] = py*(-inv_z) ;
969  L[curInd][q+2] = py*Y*inv_z ;
970  L[curInd][q+3] = py* (1+Y2) ;
971  L[curInd][q+4] = -py*XY ;
972  L[curInd][q+5] = -py*X ;
973  }
974  {
975  L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
976  L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
977  L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
978  L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
979  L[curInd][nbPose6+4] = -vp0*r2du ;
980  L[curInd][nbPose6+5] = 0 ;
981  }
982  curInd++;
983  //---undistorted to distorted
984  {
985  L[curInd][q] = Axx*(-inv_z) ;
986  L[curInd][q+1] = Axy*(-inv_z) ;
987  L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
988  L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
989  L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
990  L[curInd][q+5] = Axx*Y -Axy*X;
991  }
992  {
993  L[curInd][nbPose6]= 1 ;
994  L[curInd][nbPose6+1]= 0 ;
995  L[curInd][nbPose6+2]= X*kr2ud ;
996  L[curInd][nbPose6+3]= 0;
997  L[curInd][nbPose6+4] = 0 ;
998  L[curInd][nbPose6+5] = px*X*r2ud ;
999  }
1000  curInd++;
1001  {
1002  L[curInd][q] = Ayx*(-inv_z) ;
1003  L[curInd][q+1] = Ayy*(-inv_z) ;
1004  L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
1005  L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
1006  L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
1007  L[curInd][q+5] = Ayx*Y -Ayy*X;
1008  }
1009  {
1010  L[curInd][nbPose6]= 0 ;
1011  L[curInd][nbPose6+1]= 1;
1012  L[curInd][nbPose6+2]= 0;
1013  L[curInd][nbPose6+3]= Y*kr2ud ;
1014  L[curInd][nbPose6+4] = 0 ;
1015  L[curInd][nbPose6+5] = py*Y*r2ud ;
1016  }
1017  } // end interaction
1018  curPoint++;
1019  } // end interaction
1020  }
1021 
1022  vpColVector error ;
1023  error = P-Pd ;
1024  //r = r/nbPointTotal ;
1025 
1026  vpMatrix Lp ;
1027  /*double rank =*/
1028  L.pseudoInverse(Lp,1e-10) ;
1029  vpColVector e ;
1030  e = Lp*error ;
1031  vpColVector Tc, Tc_v(6*nbPose) ;
1032  Tc = -e*gain ;
1033  for (unsigned int i = 0 ; i < 6*nbPose ; i++)
1034  Tc_v[i] = Tc[i] ;
1035 
1036  cam.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
1037  u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1038  kud + Tc[nbPose6+5],
1039  kdu + Tc[nbPose6+4]);
1040 
1041  vpColVector Tc_v_Tmp(6) ;
1042  for (unsigned int p = 0 ; p < nbPose ; p++)
1043  {
1044  for (unsigned int i = 0 ; i < 6 ; i++)
1045  Tc_v_Tmp[i] = Tc_v[6*p + i];
1046 
1047  table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
1048  * table_cal[p].cMo_dist;
1049  }
1050  if (verbose)
1051  std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
1052  //std::cout << " residual: " << r << std::endl;
1053 
1054  }
1055  if (iter == nbIterMax)
1056  {
1057  vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
1058  throw(vpCalibrationException(vpCalibrationException::convergencyError,
1059  "Maximum number of iterations reached")) ;
1060  }
1061  for (unsigned int p = 0 ; p < nbPose ; p++)
1062  {
1063  table_cal[p].cam_dist = cam ;
1064  table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist,
1065  cam);
1066  }
1067  if (verbose)
1068  std::cout <<" std dev " << sqrt(r/(nbPointTotal)) << std::endl;
1069 
1070 }
1090 void
1091 vpCalibration::calibrationTsai(unsigned int nbPose,
1092  vpHomogeneousMatrix cMo[],
1093  vpHomogeneousMatrix rMe[],
1094  vpHomogeneousMatrix &eMc)
1095 {
1096 
1097  vpColVector x ;
1098  {
1099  vpMatrix A ;
1100  vpColVector B ;
1101  unsigned int k = 0 ;
1102  // for all couples ij
1103  for (unsigned int i=0 ; i < nbPose ; i++)
1104  {
1105  vpRotationMatrix rRei, ciRo ;
1106  rMe[i].extract(rRei) ;
1107  cMo[i].extract(ciRo) ;
1108  //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
1109 
1110  for (unsigned int j=0 ; j < nbPose ; j++)
1111  {
1112  if (j>i) // we don't use two times same couples...
1113  {
1114  vpRotationMatrix rRej, cjRo ;
1115  rMe[j].extract(rRej) ;
1116  cMo[j].extract(cjRo) ;
1117  //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
1118 
1119  vpRotationMatrix rReij = rRej.t() * rRei;
1120 
1121  vpRotationMatrix cijRo = cjRo * ciRo.t();
1122 
1123  vpThetaUVector rPeij(rReij);
1124 
1125  double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1126  + rPeij[2]*rPeij[2]);
1127 
1128  for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);
1129 
1130  vpThetaUVector cijPo(cijRo) ;
1131  theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
1132  + cijPo[2]*cijPo[2]);
1133  for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);
1134 
1135  vpMatrix As;
1136  vpColVector b(3) ;
1137 
1138  As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;
1139 
1140  b = (vpColVector)cijPo - (vpColVector)rPeij ; // A.40
1141 
1142  if (k==0)
1143  {
1144  A = As ;
1145  B = b ;
1146  }
1147  else
1148  {
1149  A = vpMatrix::stackMatrices(A,As) ;
1150  B = vpMatrix::stackMatrices(B,b) ;
1151  }
1152  k++ ;
1153  }
1154  }
1155  }
1156 
1157  // the linear system is defined
1158  // x = AtA^-1AtB is solved
1159  vpMatrix AtA = A.AtA() ;
1160 
1161  vpMatrix Ap ;
1162  AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
1163  x = Ap*A.t()*B ;
1164 
1165 // {
1166 // // Residual
1167 // vpColVector residual;
1168 // residual = A*x-B;
1169 // std::cout << "Residual: " << std::endl << residual << std::endl;
1170 
1171 // double res = 0;
1172 // for (int i=0; i < residual.getRows(); i++)
1173 // res += residual[i]*residual[i];
1174 // res = sqrt(res/residual.getRows());
1175 // printf("Mean residual = %lf\n",res);
1176 // }
1177 
1178  // extraction of theta and U
1179  double theta ;
1180  double d=x.sumSquare() ;
1181  for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1182  theta = sqrt(x.sumSquare())/2 ;
1183  theta = 2*asin(theta) ;
1184  //if (theta !=0)
1185  if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1186  {
1187  for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1188  }
1189  else
1190  x = 0 ;
1191  }
1192 
1193  // Building of the rotation matrix eRc
1194  vpThetaUVector xP(x[0],x[1],x[2]);
1195  vpRotationMatrix eRc(xP);
1196 
1197  {
1198  vpMatrix A ;
1199  vpColVector B ;
1200  // Building of the system for the translation estimation
1201  // for all couples ij
1202  vpRotationMatrix I3 ;
1203  I3.setIdentity() ;
1204  int k = 0 ;
1205  for (unsigned int i=0 ; i < nbPose ; i++)
1206  {
1207  vpRotationMatrix rRei, ciRo ;
1208  vpTranslationVector rTei, ciTo ;
1209  rMe[i].extract(rRei) ;
1210  cMo[i].extract(ciRo) ;
1211  rMe[i].extract(rTei) ;
1212  cMo[i].extract(ciTo) ;
1213 
1214 
1215  for (unsigned int j=0 ; j < nbPose ; j++)
1216  {
1217  if (j>i) // we don't use two times same couples...
1218  {
1219 
1220  vpRotationMatrix rRej, cjRo ;
1221  rMe[j].extract(rRej) ;
1222  cMo[j].extract(cjRo) ;
1223 
1224  vpTranslationVector rTej, cjTo ;
1225  rMe[j].extract(rTej) ;
1226  cMo[j].extract(cjTo) ;
1227 
1228  vpRotationMatrix rReij = rRej.t() * rRei ;
1229 
1230  vpTranslationVector rTeij = rTej+ (-rTei);
1231 
1232  rTeij = rRej.t()*rTeij ;
1233 
1234  vpMatrix a ;
1235  a = (vpMatrix)rReij - (vpMatrix)I3 ;
1236 
1237  vpTranslationVector b ;
1238  b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
1239 
1240  if (k==0)
1241  {
1242  A = a ;
1243  B = b ;
1244  }
1245  else
1246  {
1247  A = vpMatrix::stackMatrices(A,a) ;
1248  B = vpMatrix::stackMatrices(B,b) ;
1249  }
1250  k++ ;
1251  }
1252  }
1253  }
1254 
1255  // the linear system is solved
1256  // x = AtA^-1AtB is solved
1257  vpMatrix AtA = A.AtA() ;
1258  vpMatrix Ap ;
1259  vpColVector AeTc ;
1260  AtA.pseudoInverse(Ap, 1e-6) ;
1261  AeTc = Ap*A.t()*B ;
1262 
1263 // {
1264 // // residual
1265 // vpColVector residual;
1266 // residual = A*AeTc-B;
1267 // std::cout << "Residual: " << std::endl << residual << std::endl;
1268 // double res = 0;
1269 // for (int i=0; i < residual.getRows(); i++)
1270 // res += residual[i]*residual[i];
1271 // res = sqrt(res/residual.getRows());
1272 // printf("mean residual = %lf\n",res);
1273 // }
1274 
1275  vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);
1276 
1277  eMc.insert(eTc) ;
1278  eMc.insert(eRc) ;
1279  }
1280 }
1281 
1300 void vpCalibration::calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
1301  std::vector<vpHomogeneousMatrix>& rMe,
1302  vpHomogeneousMatrix &eMc){
1303 
1304  vpColVector x ;
1305  unsigned int nbPose = cMo.size();
1306  if(cMo.size()!=rMe.size()) throw vpCalibrationException(vpCalibrationException::dimensionError,"cMo and rMe have different sizes");
1307  {
1308  vpMatrix A ;
1309  vpColVector B ;
1310  unsigned int k = 0 ;
1311  // for all couples ij
1312  for (unsigned int i=0 ; i < nbPose ; i++)
1313  {
1314  vpRotationMatrix rRei, ciRo ;
1315  rMe[i].extract(rRei) ;
1316  cMo[i].extract(ciRo) ;
1317  //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
1318 
1319  for (unsigned int j=0 ; j < nbPose ; j++)
1320  {
1321  if (j>i) // we don't use two times same couples...
1322  {
1323  vpRotationMatrix rRej, cjRo ;
1324  rMe[j].extract(rRej) ;
1325  cMo[j].extract(cjRo) ;
1326  //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
1327 
1328  vpRotationMatrix rReij = rRej.t() * rRei;
1329 
1330  vpRotationMatrix cijRo = cjRo * ciRo.t();
1331 
1332  vpThetaUVector rPeij(rReij);
1333 
1334  double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1335  + rPeij[2]*rPeij[2]);
1336 
1337  for (unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] * vpMath::sinc(theta/2);
1338 
1339  vpThetaUVector cijPo(cijRo) ;
1340  theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
1341  + cijPo[2]*cijPo[2]);
1342  for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);
1343 
1344  vpMatrix As;
1345  vpColVector b(3) ;
1346 
1347  As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;
1348 
1349  b = (vpColVector)cijPo - (vpColVector)rPeij ; // A.40
1350 
1351  if (k==0)
1352  {
1353  A = As ;
1354  B = b ;
1355  }
1356  else
1357  {
1358  A = vpMatrix::stackMatrices(A,As) ;
1359  B = vpMatrix::stackMatrices(B,b) ;
1360  }
1361  k++ ;
1362  }
1363  }
1364  }
1365 
1366  // the linear system is defined
1367  // x = AtA^-1AtB is solved
1368  vpMatrix AtA = A.AtA() ;
1369 
1370  vpMatrix Ap ;
1371  AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
1372  x = Ap*A.t()*B ;
1373 
1374 // {
1375 // // Residual
1376 // vpColVector residual;
1377 // residual = A*x-B;
1378 // std::cout << "Residual: " << std::endl << residual << std::endl;
1379 
1380 // double res = 0;
1381 // for (int i=0; i < residual.getRows(); i++)
1382 // res += residual[i]*residual[i];
1383 // res = sqrt(res/residual.getRows());
1384 // printf("Mean residual = %lf\n",res);
1385 // }
1386 
1387  // extraction of theta and U
1388  double theta ;
1389  double d=x.sumSquare() ;
1390  for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1391  theta = sqrt(x.sumSquare())/2 ;
1392  theta = 2*asin(theta) ;
1393  //if (theta !=0)
1394  if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1395  {
1396  for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1397  }
1398  else
1399  x = 0 ;
1400  }
1401 
1402  // Building of the rotation matrix eRc
1403  vpThetaUVector xP(x[0],x[1],x[2]);
1404  vpRotationMatrix eRc(xP);
1405 
1406  {
1407  vpMatrix A ;
1408  vpColVector B ;
1409  // Building of the system for the translation estimation
1410  // for all couples ij
1411  vpRotationMatrix I3 ;
1412  I3.setIdentity() ;
1413  int k = 0 ;
1414  for (unsigned int i=0 ; i < nbPose ; i++)
1415  {
1416  vpRotationMatrix rRei, ciRo ;
1417  vpTranslationVector rTei, ciTo ;
1418  rMe[i].extract(rRei) ;
1419  cMo[i].extract(ciRo) ;
1420  rMe[i].extract(rTei) ;
1421  cMo[i].extract(ciTo) ;
1422 
1423 
1424  for (unsigned int j=0 ; j < nbPose ; j++)
1425  {
1426  if (j>i) // we don't use two times same couples...
1427  {
1428 
1429  vpRotationMatrix rRej, cjRo ;
1430  rMe[j].extract(rRej) ;
1431  cMo[j].extract(cjRo) ;
1432 
1433  vpTranslationVector rTej, cjTo ;
1434  rMe[j].extract(rTej) ;
1435  cMo[j].extract(cjTo) ;
1436 
1437  vpRotationMatrix rReij = rRej.t() * rRei ;
1438 
1439  vpTranslationVector rTeij = rTej+ (-rTei);
1440 
1441  rTeij = rRej.t()*rTeij ;
1442 
1443  vpMatrix a ;
1444  a = (vpMatrix)rReij - (vpMatrix)I3 ;
1445 
1446  vpTranslationVector b ;
1447  b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
1448 
1449  if (k==0)
1450  {
1451  A = a ;
1452  B = b ;
1453  }
1454  else
1455  {
1456  A = vpMatrix::stackMatrices(A,a) ;
1457  B = vpMatrix::stackMatrices(B,b) ;
1458  }
1459  k++ ;
1460  }
1461  }
1462  }
1463 
1464  // the linear system is solved
1465  // x = AtA^-1AtB is solved
1466  vpMatrix AtA = A.AtA() ;
1467  vpMatrix Ap ;
1468  vpColVector AeTc ;
1469  AtA.pseudoInverse(Ap, 1e-6) ;
1470  AeTc = Ap*A.t()*B ;
1471 
1472 // {
1473 // // residual
1474 // vpColVector residual;
1475 // residual = A*AeTc-B;
1476 // std::cout << "Residual: " << std::endl << residual << std::endl;
1477 // double res = 0;
1478 // for (int i=0; i < residual.getRows(); i++)
1479 // res += residual[i]*residual[i];
1480 // res = sqrt(res/residual.getRows());
1481 // printf("mean residual = %lf\n",res);
1482 // }
1483 
1484  vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);
1485 
1486  eMc.insert(eTc) ;
1487  eMc.insert(eRc) ;
1488  }
1489 }
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:233
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:232
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:758
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:153
vpHomogeneousMatrix::insert
void insert(const vpRotationMatrix &R)
Definition: vpHomogeneousMatrix.cpp:344
vpCalibration::calibrationTsai
static void calibrationTsai(unsigned int nbPose, vpHomogeneousMatrix cMo[], vpHomogeneousMatrix rMe[], vpHomogeneousMatrix &eMc)
calibration method of effector-camera from R. Tsai and R. Lorenz
Definition: vpCalibrationTools.cpp:1091
vpMatrix::svd
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1700
vpCameraParameters::get_v0
double get_v0() const
Definition: vpCameraParameters.h:234
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:1357
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:197
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:298
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:229
vpMatrix::stackMatrices
static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2261
vpCameraParameters::get_kud
double get_kud() const
Definition: vpCameraParameters.h:235
vpMatrix::t
vpMatrix t() const
Definition: vpMatrix.cpp:1174
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:236
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:1810
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:190