1 #include <visp/vpCalibration.h>
2 #include <visp/vpMath.h>
3 #include <visp/vpPose.h>
4 #include <visp/vpPixelMeterConversion.h>
13 vpCalibration::calibLagrange(
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();
27 for (
unsigned int i = 0 ; i < npt ; i++)
36 double xi = ip.
get_u() ;
37 double yi = ip.
get_v() ;
102 unsigned int imin = 1;
103 for (
unsigned int i=0;i<x1.getRows();i++)
114 for (
unsigned int i=0;i<x1.getRows();i++)
116 if (x1[i] < x1[imin]) imin = i;
119 for (
unsigned int i=0;i<x1.getRows();i++)
120 x1[i] = AtA[i][imin];
127 for (
unsigned int i=0;i<3;i++) sol[i] = x1[i];
128 for (
unsigned int i=0;i<9;i++)
133 if (sol[11] < 0.0)
for (
unsigned int i=0;i<12;i++) sol[i] = -sol[i];
135 resul[0] = sol[3]*sol[0]+sol[4]*sol[1]+sol[5]*sol[2];
137 resul[1] = sol[6]*sol[0]+sol[7]*sol[1]+sol[8]*sol[2];
139 resul[2] = sqrt(sol[3]*sol[3]+sol[4]*sol[4]+sol[5]*sol[5]
141 resul[3] = sqrt(sol[6]*sol[6]+sol[7]*sol[7]+sol[8]*sol[8]
146 resul[4] = (sol[9]-sol[11]*resul[0])/resul[2];
147 resul[5] = (sol[10]-sol[11]*resul[1])/resul[3];
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];
159 for (
unsigned int i=0 ; i < 3 ; i++)
161 for (
unsigned int j=0 ; j < 3 ; j++)
162 cMo[i][j] = rd[i][j];
164 for (
unsigned int i=0 ; i < 3 ; i++) cMo[i][3] = resul[i+4] ;
169 double deviation,deviation_dist;
176 vpCalibration::calibVVS(
181 std::cout.precision(10);
182 unsigned int n_points = npt ;
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();
200 for (
unsigned int i =0 ; i < n_points ; i++)
220 unsigned int iter = 0 ;
222 double residu_1 = 1e12 ;
224 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
237 for (
unsigned int i=0 ; i < n_points; i++)
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];
246 P[2*i] = cX[i]/cZ[i]*px + u0 ;
247 P[2*i+1] = cY[i]/cZ[i]*py + v0 ;
258 for (
unsigned int i=0 ; i < n_points; i++)
272 L[2*i][0] = px * (-inv_z) ;
274 L[2*i][2] = px*X*inv_z ;
276 L[2*i][4] = -px*(1+X*X) ;
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 ;
303 Lp = L.pseudoInverse(1e-10) ;
312 for (
unsigned int i=0 ; i <6 ; i++)
320 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
323 if (iter == nbIterMax)
325 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
327 "Maximum number of iterations reached")) ;
332 this->residual_dist = r;
334 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
339 vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal,
341 double &globalReprojectionError,
344 std::cout.precision(10);
345 unsigned int nbPoint[256];
346 unsigned int nbPointTotal = 0;
347 unsigned int nbPose = (
unsigned int)table_cal.size();
348 unsigned int nbPose6 = 6*nbPose;
350 for (
unsigned int i=0; i<nbPose ; i++)
352 nbPoint[i] = table_cal[i].npt;
353 nbPointTotal += nbPoint[i];
366 unsigned int curPoint = 0 ;
367 for (
unsigned int p=0; p<nbPose ; p++)
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();
374 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
376 oX[curPoint] = *it_LoX;
377 oY[curPoint] = *it_LoY;
378 oZ[curPoint] = *it_LoZ;
381 u[curPoint] = ip.
get_u() ;
382 v[curPoint] = ip.
get_v() ;
393 unsigned int iter = 0 ;
395 double residu_1 = 1e12 ;
397 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
410 for (
unsigned int p=0; p<nbPose ; p++)
413 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
415 unsigned int curPoint2 = 2*curPoint;
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];
424 Pd[curPoint2] = u[curPoint] ;
425 Pd[curPoint2+1] = v[curPoint] ;
427 P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
428 P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
440 vpMatrix L(nbPointTotal*2,nbPose6+4) ;
442 for (
unsigned int p=0; p<nbPose ; p++)
444 unsigned int q = 6*p;
445 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
447 unsigned int curPoint2 = 2*curPoint;
448 unsigned int curPoint21 = curPoint2 + 1;
450 double x = cX[curPoint] ;
451 double y = cY[curPoint] ;
452 double z = cZ[curPoint] ;
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 ;
470 L[curPoint2][nbPose6]= 1 ;
471 L[curPoint2][nbPose6+1]= 0 ;
472 L[curPoint2][nbPose6+2]= X ;
473 L[curPoint2][nbPose6+3]= 0;
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 ;
484 L[curPoint21][nbPose6]= 0 ;
485 L[curPoint21][nbPose6+1]= 1 ;
486 L[curPoint21][nbPose6+2]= 0;
487 L[curPoint21][nbPose6+3]= Y ;
495 Lp = L.pseudoInverse(1e-10) ;
504 for (
unsigned int i = 0 ; i < nbPose6 ; i++)
515 for (
unsigned int p = 0 ; p < nbPose ; p++)
517 for (
unsigned int i = 0 ; i < 6 ; i++)
518 Tc_v_Tmp[i] = Tc_v[6*p + i];
525 std::cout <<
" std dev " << sqrt(r/nbPointTotal) << std::endl;
528 if (iter == nbIterMax)
530 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
532 "Maximum number of iterations reached")) ;
534 for (
unsigned int p = 0 ; p < nbPose ; p++)
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);
542 globalReprojectionError = sqrt(r/nbPointTotal);
546 vpCalibration::calibVVSWithDistortion(
551 std::cout.precision(10);
552 unsigned int n_points =npt ;
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();
570 for (
unsigned int i =0 ; i < n_points ; i++)
587 unsigned int iter = 0 ;
589 double residu_1 = 1e12 ;
591 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
599 double u0 = cam.
get_u0() ;
600 double v0 = cam.
get_v0() ;
602 double px = cam.
get_px() ;
603 double py = cam.
get_py() ;
605 double inv_px = 1/px ;
606 double inv_py = 1/py ;
615 for (
unsigned int i=0 ; i < n_points; i++)
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;
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];
644 double up0 = up - u0;
645 double vp0 = vp - v0;
647 double xp0 = up0 * inv_px;
648 double xp02 = xp0 *xp0 ;
650 double yp0 = vp0 * inv_py;
651 double yp02 = yp0 * yp0;
653 double r2du = xp02 + yp02 ;
654 double kr2du = kdu * r2du;
656 P[i4] = u0 + px*X - kr2du *(up0) ;
657 P[i41] = v0 + py*Y - kr2du *(vp0) ;
659 double r2ud = X2 + Y2 ;
660 double kr2ud = 1 + kud * r2ud;
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;
670 P[i42] = u0 + px*X*kr2ud ;
671 P[i43] = v0 + py*Y*kr2ud ;
682 L[i4][0] = px * (-inv_z) ;
684 L[i4][2] = px*X*inv_z ;
686 L[i4][4] = -px*(1+X2) ;
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) ;
699 L[i41][1] = py*(-inv_z) ;
700 L[i41][2] = py*Y*inv_z ;
701 L[i41][3] = py* (1+Y2) ;
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 ;
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;
728 L[i42][11] = px*X*r2ud ;
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;
744 L[i43][11] = py*Y*r2ud ;
754 Lp = L.pseudoInverse(1e-10) ;
762 for (
unsigned int i=0 ; i <6 ; i++)
766 u0 + Tc[6], v0 + Tc[7],
772 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
775 if (iter == nbIterMax)
777 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
779 "Maximum number of iterations reached")) ;
781 this->residual_dist = r;
786 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
791 vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal,
795 std::cout.precision(10);
796 unsigned int nbPoint[1024];
797 unsigned int nbPointTotal = 0;
798 unsigned int nbPose = (
unsigned int)table_cal.size();
799 unsigned int nbPose6 = 6*nbPose;
800 for (
unsigned int i=0; i<nbPose ; i++)
802 nbPoint[i] = table_cal[i].npt;
803 nbPointTotal += nbPoint[i];
816 unsigned int curPoint = 0 ;
817 for (
unsigned int p=0; p<nbPose ; p++)
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();
824 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
826 oX[curPoint] = *it_LoX;
827 oY[curPoint] = *it_LoY;
828 oZ[curPoint] = *it_LoZ;
831 u[curPoint] = ip.
get_u() ;
832 v[curPoint] = ip.
get_v() ;
842 unsigned int iter = 0 ;
844 double residu_1 = 1e12 ;
846 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
853 for (
unsigned int p=0; p<nbPose ; p++)
856 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
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];
870 vpMatrix L(nbPointTotal*4,nbPose6+6) ;
872 double px = cam.
get_px() ;
873 double py = cam.
get_py() ;
874 double u0 = cam.
get_u0() ;
875 double v0 = cam.
get_v0() ;
877 double inv_px = 1/px ;
878 double inv_py = 1/py ;
886 for (
unsigned int p=0; p<nbPose ; p++)
888 unsigned int q = 6*p;
889 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
891 unsigned int curPoint4 = 4*curPoint;
892 double x = cX[curPoint] ;
893 double y = cY[curPoint] ;
894 double z = cZ[curPoint] ;
904 double up = u[curPoint] ;
905 double vp = v[curPoint] ;
908 Pd[curPoint4+1] = vp ;
910 double up0 = up - u0;
911 double vp0 = vp - v0;
913 double xp0 = up0 * inv_px;
914 double xp02 = xp0 *xp0 ;
916 double yp0 = vp0 * inv_py;
917 double yp02 = yp0 * yp0;
919 double r2du = xp02 + yp02 ;
920 double kr2du = kdu * r2du;
922 P[curPoint4] = u0 + px*X - kr2du *(up0) ;
923 P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
925 double r2ud = X2 + Y2 ;
926 double kr2ud = 1 + kud * r2ud;
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;
933 Pd[curPoint4+2] = up ;
934 Pd[curPoint4+3] = vp ;
936 P[curPoint4+2] = u0 + px*X*kr2ud ;
937 P[curPoint4+3] = v0 + py*Y*kr2ud ;
944 unsigned int curInd = curPoint4;
948 L[curInd][q] = px * (-inv_z) ;
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 ;
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 ;
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 ;
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 ;
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;
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 ;
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;
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 ;
1026 L.pseudoInverse(Lp,1e-10) ;
1031 for (
unsigned int i = 0 ; i < 6*nbPose ; i++)
1035 u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1036 kud + Tc[nbPose6+5],
1037 kdu + Tc[nbPose6+4]);
1040 for (
unsigned int p = 0 ; p < nbPose ; p++)
1042 for (
unsigned int i = 0 ; i < 6 ; i++)
1043 Tc_v_Tmp[i] = Tc_v[6*p + i];
1046 * table_cal[p].cMo_dist;
1049 std::cout <<
" std dev: " << sqrt(r/nbPointTotal) << std::endl;
1053 if (iter == nbIterMax)
1055 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
1057 "Maximum number of iterations reached")) ;
1060 double perViewError;
1061 double totalError = 0;
1062 int totalPoints = 0;
1063 for (
unsigned int p = 0 ; p < nbPose ; p++)
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;
1070 globalReprojectionError = sqrt(r/(nbPointTotal));
1073 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1100 unsigned int k = 0 ;
1102 for (
unsigned int i=0 ; i < nbPose ; i++)
1109 for (
unsigned int j=0 ; j < nbPose ; j++)
1124 double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1125 + rPeij[2]*rPeij[2]);
1127 for (
unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] *
vpMath::sinc(theta/2);
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);
1180 for (
unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1182 theta = 2*asin(theta) ;
1184 if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1186 for (
unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1204 for (
unsigned int i=0 ; i < nbPose ; i++)
1214 for (
unsigned int j=0 ; j < nbPose ; j++)
1231 rTeij = rRej.
t()*rTeij ;
1237 b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
1297 std::vector<vpHomogeneousMatrix>& rMe,
1301 unsigned int nbPose = (
unsigned int)cMo.
size();
1306 unsigned int k = 0 ;
1308 for (
unsigned int i=0 ; i < nbPose ; i++)
1311 rMe[i].extract(rRei) ;
1312 cMo[i].extract(ciRo) ;
1315 for (
unsigned int j=0 ; j < nbPose ; j++)
1320 rMe[j].extract(rRej) ;
1321 cMo[j].extract(cjRo) ;
1330 double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1331 + rPeij[2]*rPeij[2]);
1333 for (
unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] *
vpMath::sinc(theta/2);
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);
1386 for (
unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1388 theta = 2*asin(theta) ;
1390 if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1392 for (
unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1410 for (
unsigned int i=0 ; i < nbPose ; i++)
1414 rMe[i].extract(rRei) ;
1415 cMo[i].extract(ciRo) ;
1416 rMe[i].extract(rTei) ;
1417 cMo[i].extract(ciTo) ;
1420 for (
unsigned int j=0 ; j < nbPose ; j++)
1426 rMe[j].extract(rRej) ;
1427 cMo[j].extract(cjRo) ;
1430 rMe[j].extract(rTej) ;
1431 cMo[j].extract(cjTo) ;
1437 rTeij = rRej.
t()*rTeij ;
1443 b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
1489 vpCalibration::calibVVSMulti(
unsigned int nbPose,
1494 std::cout.precision(10);
1495 unsigned int nbPoint[256];
1496 unsigned int nbPointTotal = 0;
1498 unsigned int nbPose6 = 6*nbPose;
1500 for (
unsigned int i=0; i<nbPose ; i++)
1502 nbPoint[i] = table_cal[i].npt;
1503 nbPointTotal += nbPoint[i];
1516 unsigned int curPoint = 0 ;
1517 for (
unsigned int p=0; p<nbPose ; p++)
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();
1524 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
1526 oX[curPoint] = *it_LoX;
1527 oY[curPoint] = *it_LoY;
1528 oZ[curPoint] = *it_LoZ;
1531 u[curPoint] = ip.
get_u() ;
1532 v[curPoint] = ip.
get_v() ;
1543 unsigned int iter = 0 ;
1545 double residu_1 = 1e12 ;
1547 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
1553 double px = cam.
get_px();
1554 double py = cam.
get_py();
1555 double u0 = cam.
get_u0();
1556 double v0 = cam.
get_v0();
1560 for (
unsigned int p=0; p<nbPose ; p++)
1563 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
1565 unsigned int curPoint2 = 2*curPoint;
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];
1574 Pd[curPoint2] = u[curPoint] ;
1575 Pd[curPoint2+1] = v[curPoint] ;
1577 P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
1578 P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
1590 vpMatrix L(nbPointTotal*2,nbPose6+4) ;
1592 for (
unsigned int p=0; p<nbPose ; p++)
1594 unsigned int q = 6*p;
1595 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
1597 unsigned int curPoint2 = 2*curPoint;
1598 unsigned int curPoint21 = curPoint2 + 1;
1600 double x = cX[curPoint] ;
1601 double y = cY[curPoint] ;
1602 double z = cZ[curPoint] ;
1606 double X = x*inv_z ;
1607 double Y = y*inv_z ;
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 ;
1620 L[curPoint2][nbPose6]= 1 ;
1621 L[curPoint2][nbPose6+1]= 0 ;
1622 L[curPoint2][nbPose6+2]= X ;
1623 L[curPoint2][nbPose6+3]= 0;
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 ;
1634 L[curPoint21][nbPose6]= 0 ;
1635 L[curPoint21][nbPose6+1]= 1 ;
1636 L[curPoint21][nbPose6+2]= 0;
1637 L[curPoint21][nbPose6+3]= Y ;
1645 Lp = L.pseudoInverse(1e-10) ;
1654 for (
unsigned int i = 0 ; i < nbPose6 ; i++)
1665 for (
unsigned int p = 0 ; p < nbPose ; p++)
1667 for (
unsigned int i = 0 ; i < 6 ; i++)
1668 Tc_v_Tmp[i] = Tc_v[6*p + i];
1674 std::cout <<
" std dev " << sqrt(r/nbPointTotal) << std::endl;
1677 if (iter == nbIterMax)
1679 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
1681 "Maximum number of iterations reached")) ;
1683 for (
unsigned int p = 0 ; p < nbPose ; p++)
1688 double deviation,deviation_dist;
1692 std::cout <<
" Global std dev " << sqrt(r/nbPointTotal) << std::endl;
1697 vpCalibration::calibVVSWithDistortionMulti(
1698 unsigned int nbPose,
1703 std::cout.precision(10);
1704 unsigned int nbPoint[1024];
1705 unsigned int nbPointTotal = 0;
1707 unsigned int nbPose6 = 6*nbPose;
1708 for (
unsigned int i=0; i<nbPose ; i++)
1710 nbPoint[i] = table_cal[i].npt;
1711 nbPointTotal += nbPoint[i];
1724 unsigned int curPoint = 0 ;
1725 for (
unsigned int p=0; p<nbPose ; p++)
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();
1732 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
1734 oX[curPoint] = *it_LoX;
1735 oY[curPoint] = *it_LoY;
1736 oZ[curPoint] = *it_LoZ;
1739 u[curPoint] = ip.
get_u() ;
1740 v[curPoint] = ip.
get_v() ;
1750 unsigned int iter = 0 ;
1752 double residu_1 = 1e12 ;
1754 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
1761 for (
unsigned int p=0; p<nbPose ; p++)
1764 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
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];
1778 vpMatrix L(nbPointTotal*4,nbPose6+6) ;
1780 double px = cam.
get_px() ;
1781 double py = cam.
get_py() ;
1782 double u0 = cam.
get_u0() ;
1783 double v0 = cam.
get_v0() ;
1785 double inv_px = 1/px ;
1786 double inv_py = 1/py ;
1791 double k2ud = 2*kud;
1792 double k2du = 2*kdu;
1794 for (
unsigned int p=0; p<nbPose ; p++)
1796 unsigned int q = 6*p;
1797 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
1799 unsigned int curPoint4 = 4*curPoint;
1800 double x = cX[curPoint] ;
1801 double y = cY[curPoint] ;
1802 double z = cZ[curPoint] ;
1805 double X = x*inv_z ;
1806 double Y = y*inv_z ;
1812 double up = u[curPoint] ;
1813 double vp = v[curPoint] ;
1815 Pd[curPoint4] = up ;
1816 Pd[curPoint4+1] = vp ;
1818 double up0 = up - u0;
1819 double vp0 = vp - v0;
1821 double xp0 = up0 * inv_px;
1822 double xp02 = xp0 *xp0 ;
1824 double yp0 = vp0 * inv_py;
1825 double yp02 = yp0 * yp0;
1827 double r2du = xp02 + yp02 ;
1828 double kr2du = kdu * r2du;
1830 P[curPoint4] = u0 + px*X - kr2du *(up0) ;
1831 P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
1833 double r2ud = X2 + Y2 ;
1834 double kr2ud = 1 + kud * r2ud;
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;
1841 Pd[curPoint4+2] = up ;
1842 Pd[curPoint4+3] = vp ;
1844 P[curPoint4+2] = u0 + px*X*kr2ud ;
1845 P[curPoint4+3] = v0 + py*Y*kr2ud ;
1850 vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
1852 unsigned int curInd = curPoint4;
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 ;
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 ;
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 ;
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 ;
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;
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 ;
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;
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 ;
1934 L.pseudoInverse(Lp,1e-10) ;
1939 for (
unsigned int i = 0 ; i < 6*nbPose ; i++)
1943 u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1944 kud + Tc[nbPose6+5],
1945 kdu + Tc[nbPose6+4]);
1948 for (
unsigned int p = 0 ; p < nbPose ; p++)
1950 for (
unsigned int i = 0 ; i < 6 ; i++)
1951 Tc_v_Tmp[i] = Tc_v[6*p + i];
1957 std::cout <<
" std dev: " << sqrt(r/nbPointTotal) << std::endl;
1961 if (iter == nbIterMax)
1963 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
1965 "Maximum number of iterations reached")) ;
1968 for (
unsigned int p = 0 ; p < nbPose ; p++)
1975 std::cout <<
" Global std dev " << sqrt(r/(nbPointTotal)) << std::endl;