39 #include <visp3/vision/vpCalibration.h> 40 #include <visp3/core/vpMath.h> 41 #include <visp3/vision/vpPose.h> 42 #include <visp3/core/vpPixelMeterConversion.h> 57 std::list<double>::const_iterator it_LoX = LoX.begin();
58 std::list<double>::const_iterator it_LoY = LoY.begin();
59 std::list<double>::const_iterator it_LoZ = LoZ.begin();
60 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
64 for (
unsigned int i = 0 ; i < npt ; i++)
73 double xi = ip.
get_u() ;
74 double yi = ip.
get_v() ;
139 unsigned int imin = 1;
140 for (
unsigned int i=0;i<x1.getRows();i++)
151 for (
unsigned int i=0;i<x1.getRows();i++)
153 if (x1[i] < x1[imin]) imin = i;
156 for (
unsigned int i=0;i<x1.getRows();i++)
157 x1[i] = AtA[i][imin];
164 for (
unsigned int i=0;i<3;i++) sol[i] = x1[i];
165 for (
unsigned int i=0;i<9;i++)
170 if (sol[11] < 0.0)
for (
unsigned int i=0;i<12;i++) sol[i] = -sol[i];
172 resul[0] = sol[3]*sol[0]+sol[4]*sol[1]+sol[5]*sol[2];
174 resul[1] = sol[6]*sol[0]+sol[7]*sol[1]+sol[8]*sol[2];
176 resul[2] = sqrt(sol[3]*sol[3]+sol[4]*sol[4]+sol[5]*sol[5]
178 resul[3] = sqrt(sol[6]*sol[6]+sol[7]*sol[7]+sol[8]*sol[8]
183 resul[4] = (sol[9]-sol[11]*resul[0])/resul[2];
184 resul[5] = (sol[10]-sol[11]*resul[1])/resul[3];
189 for (
unsigned int i=0;i<3;i++) rd[0][i] = (sol[i+3]-sol[i]*resul[0])/resul[2];
190 for (
unsigned int i=0;i<3;i++) rd[1][i] = (sol[i+6]-sol[i]*resul[1])/resul[3];
191 for (
unsigned int i=0;i<3;i++) rd[2][i] = sol[i];
196 for (
unsigned int i=0 ; i < 3 ; i++)
198 for (
unsigned int j=0 ; j < 3 ; j++)
199 cMo_est[i][j] = rd[i][j];
201 for (
unsigned int i=0 ; i < 3 ; i++) cMo_est[i][3] = resul[i+4] ;
203 this->
cMo = cMo_est ;
206 double deviation,deviation_dist;
217 std::ios::fmtflags original_flags( std::cout.flags() );
218 std::cout.precision(10);
219 unsigned int n_points = npt ;
232 std::list<double>::const_iterator it_LoX = LoX.begin();
233 std::list<double>::const_iterator it_LoY = LoY.begin();
234 std::list<double>::const_iterator it_LoZ = LoZ.begin();
235 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
237 for (
unsigned int i =0 ; i < n_points ; i++)
255 unsigned int iter = 0 ;
257 double residu_1 = 1e12 ;
259 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
264 double px = cam_est.
get_px();
265 double py = cam_est.
get_py();
266 double u0 = cam_est.
get_u0();
267 double v0 = cam_est.
get_v0();
271 for (
unsigned int i=0 ; i < n_points; i++)
273 cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
274 cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
275 cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
280 P[2*i] = cX[i]/cZ[i]*px + u0 ;
281 P[2*i+1] = cY[i]/cZ[i]*py + v0 ;
291 for (
unsigned int i=0 ; i < n_points; i++)
303 L[2*i][0] = px * (-inv_z) ;
305 L[2*i][2] = px*X*inv_z ;
307 L[2*i][4] = -px*(1+X*X) ;
318 L[2*i+1][1] = py*(-inv_z) ;
319 L[2*i+1][2] = py*(Y*inv_z) ;
320 L[2*i+1][3] = py* (1+Y*Y) ;
321 L[2*i+1][4] = -py*X*Y ;
322 L[2*i+1][5] = -py*X ;
332 Lp = L.pseudoInverse(1e-10) ;
341 for (
unsigned int i=0 ; i <6 ; i++)
349 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
352 if (iter == nbIterMax)
354 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
356 "Maximum number of iterations reached")) ;
361 this->residual_dist = r;
363 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
365 std::cout.flags(original_flags);
369 vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal,
371 double &globalReprojectionError,
374 std::ios::fmtflags original_flags( std::cout.flags() );
375 std::cout.precision(10);
376 unsigned int nbPoint[256];
377 unsigned int nbPointTotal = 0;
378 unsigned int nbPose = (
unsigned int)table_cal.size();
379 unsigned int nbPose6 = 6*nbPose;
381 for (
unsigned int i=0; i<nbPose ; i++)
383 nbPoint[i] = table_cal[i].npt;
384 nbPointTotal += nbPoint[i];
387 if (nbPointTotal < 4) {
390 "Not enough point to calibrate")) ;
403 unsigned int curPoint = 0 ;
404 for (
unsigned int p=0; p<nbPose ; p++)
406 std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
407 std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
408 std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
409 std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
411 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
413 oX[curPoint] = *it_LoX;
414 oY[curPoint] = *it_LoY;
415 oZ[curPoint] = *it_LoZ;
418 u[curPoint] = ip.
get_u() ;
419 v[curPoint] = ip.
get_v() ;
430 unsigned int iter = 0 ;
432 double residu_1 = 1e12 ;
434 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
440 double px = cam_est.
get_px();
441 double py = cam_est.
get_py();
442 double u0 = cam_est.
get_u0();
443 double v0 = cam_est.
get_v0();
447 for (
unsigned int p=0; p<nbPose ; p++)
450 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
452 unsigned int curPoint2 = 2*curPoint;
454 cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
455 +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
456 cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
457 +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
458 cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
459 +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
461 Pd[curPoint2] = u[curPoint] ;
462 Pd[curPoint2+1] = v[curPoint] ;
464 P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
465 P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
477 vpMatrix L(nbPointTotal*2,nbPose6+4) ;
479 for (
unsigned int p=0; p<nbPose ; p++)
481 unsigned int q = 6*p;
482 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
484 unsigned int curPoint2 = 2*curPoint;
485 unsigned int curPoint21 = curPoint2 + 1;
487 double x = cX[curPoint] ;
488 double y = cY[curPoint] ;
489 double z = cZ[curPoint] ;
499 L[curPoint2][q] = px * (-inv_z) ;
500 L[curPoint2][q+1] = 0 ;
501 L[curPoint2][q+2] = px*(X*inv_z) ;
502 L[curPoint2][q+3] = px*X*Y ;
503 L[curPoint2][q+4] = -px*(1+X*X) ;
504 L[curPoint2][q+5] = px*Y ;
507 L[curPoint2][nbPose6]= 1 ;
508 L[curPoint2][nbPose6+1]= 0 ;
509 L[curPoint2][nbPose6+2]= X ;
510 L[curPoint2][nbPose6+3]= 0;
513 L[curPoint21][q] = 0 ;
514 L[curPoint21][q+1] = py*(-inv_z) ;
515 L[curPoint21][q+2] = py*(Y*inv_z) ;
516 L[curPoint21][q+3] = py* (1+Y*Y) ;
517 L[curPoint21][q+4] = -py*X*Y ;
518 L[curPoint21][q+5] = -py*X ;
521 L[curPoint21][nbPose6]= 0 ;
522 L[curPoint21][nbPose6+1]= 1 ;
523 L[curPoint21][nbPose6+2]= 0;
524 L[curPoint21][nbPose6+3]= Y ;
532 Lp = L.pseudoInverse(1e-10) ;
541 for (
unsigned int i = 0 ; i < nbPose6 ; i++)
552 for (
unsigned int p = 0 ; p < nbPose ; p++)
554 for (
unsigned int i = 0 ; i < 6 ; i++)
555 Tc_v_Tmp[i] = Tc_v[6*p + i];
562 std::cout <<
" std dev " << sqrt(r/nbPointTotal) << std::endl;
565 if (iter == nbIterMax)
567 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
569 "Maximum number of iterations reached")) ;
571 for (
unsigned int p = 0 ; p < nbPose ; p++)
573 table_cal[p].cMo_dist = table_cal[p].cMo ;
574 table_cal[p].cam = cam_est;
575 table_cal[p].cam_dist = cam_est;
576 double deviation,deviation_dist;
577 table_cal[p].computeStdDeviation(deviation,deviation_dist);
579 globalReprojectionError = sqrt(r/nbPointTotal);
581 std::cout.flags(original_flags);
589 std::ios::fmtflags original_flags( std::cout.flags() );
590 std::cout.precision(10);
591 unsigned int n_points =npt ;
602 std::list<double>::const_iterator it_LoX = LoX.begin();
603 std::list<double>::const_iterator it_LoY = LoY.begin();
604 std::list<double>::const_iterator it_LoZ = LoZ.begin();
605 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
609 for (
unsigned int i =0 ; i < n_points ; i++)
626 unsigned int iter = 0 ;
628 double residu_1 = 1e12 ;
630 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
636 double u0 = cam_est.
get_u0() ;
637 double v0 = cam_est.
get_v0() ;
639 double px = cam_est.
get_px() ;
640 double py = cam_est.
get_py() ;
642 double inv_px = 1/px ;
643 double inv_py = 1/py ;
645 double kud = cam_est.
get_kud() ;
646 double kdu = cam_est.
get_kdu() ;
652 for (
unsigned int i=0 ; i < n_points; i++)
654 unsigned int i4 = 4*i;
655 unsigned int i41 = 4*i+1;
656 unsigned int i42 = 4*i+2;
657 unsigned int i43 = 4*i+3;
659 cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
660 cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
661 cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
681 double up0 = up - u0;
682 double vp0 = vp - v0;
684 double xp0 = up0 * inv_px;
685 double xp02 = xp0 *xp0 ;
687 double yp0 = vp0 * inv_py;
688 double yp02 = yp0 * yp0;
690 double r2du = xp02 + yp02 ;
691 double kr2du = kdu * r2du;
693 P[i4] = u0 + px*X - kr2du *(up0) ;
694 P[i41] = v0 + py*Y - kr2du *(vp0) ;
696 double r2ud = X2 + Y2 ;
697 double kr2ud = 1 + kud * r2ud;
699 double Axx = px*(kr2ud+k2ud*X2);
700 double Axy = px*k2ud*XY;
701 double Ayy = py*(kr2ud+k2ud*Y2);
702 double Ayx = py*k2ud*XY;
707 P[i42] = u0 + px*X*kr2ud ;
708 P[i43] = v0 + py*Y*kr2ud ;
718 L[i4][0] = px * (-inv_z) ;
720 L[i4][2] = px*X*inv_z ;
722 L[i4][4] = -px*(1+X2) ;
726 L[i4][6]= 1 + kr2du + k2du*xp02 ;
727 L[i4][7]= k2du*up0*yp0*inv_py ;
728 L[i4][8]= X + k2du*xp02*xp0 ;
729 L[i4][9]= k2du*up0*yp02*inv_py ;
730 L[i4][10] = -(up0)*(r2du) ;
735 L[i41][1] = py*(-inv_z) ;
736 L[i41][2] = py*Y*inv_z ;
737 L[i41][3] = py* (1+Y2) ;
742 L[i41][6]= k2du*xp0*vp0*inv_px ;
743 L[i41][7]= 1 + kr2du + k2du*yp02;
744 L[i41][8]= k2du*vp0*xp02*inv_px;
745 L[i41][9]= Y + k2du*yp02*yp0;
746 L[i41][10] = -vp0*r2du ;
751 L[i42][0] = Axx*(-inv_z) ;
752 L[i42][1] = Axy*(-inv_z) ;
753 L[i42][2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
754 L[i42][3] = Axx*X*Y + Axy*(1+Y2);
755 L[i42][4] = -Axx*(1+X2) - Axy*XY;
756 L[i42][5] = Axx*Y -Axy*X;
764 L[i42][11] = px*X*r2ud ;
767 L[i43][0] = Ayx*(-inv_z) ;
768 L[i43][1] = Ayy*(-inv_z) ;
769 L[i43][2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
770 L[i43][3] = Ayx*XY + Ayy*(1+Y2) ;
771 L[i43][4] = -Ayx*(1+X2) -Ayy*XY ;
772 L[i43][5] = Ayx*Y -Ayy*X;
780 L[i43][11] = py*Y*r2ud ;
790 Lp = L.pseudoInverse(1e-10) ;
798 for (
unsigned int i=0 ; i <6 ; i++)
802 u0 + Tc[6], v0 + Tc[7],
808 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
811 if (iter == nbIterMax)
813 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
815 "Maximum number of iterations reached")) ;
817 this->residual_dist = r;
822 std::cout <<
" std dev " << sqrt(r/n_points) << std::endl;
825 std::cout.flags(original_flags);
830 vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal,
834 std::ios::fmtflags original_flags( std::cout.flags() );
835 std::cout.precision(10);
836 unsigned int nbPoint[1024];
837 unsigned int nbPointTotal = 0;
838 unsigned int nbPose = (
unsigned int)table_cal.size();
839 unsigned int nbPose6 = 6*nbPose;
840 for (
unsigned int i=0; i<nbPose ; i++)
842 nbPoint[i] = table_cal[i].npt;
843 nbPointTotal += nbPoint[i];
846 if (nbPointTotal < 4)
850 "Not enough point to calibrate")) ;
863 unsigned int curPoint = 0 ;
864 for (
unsigned int p=0; p<nbPose ; p++)
866 std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
867 std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
868 std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
869 std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
871 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
873 oX[curPoint] = *it_LoX;
874 oY[curPoint] = *it_LoY;
875 oZ[curPoint] = *it_LoZ;
878 u[curPoint] = ip.
get_u() ;
879 v[curPoint] = ip.
get_v() ;
889 unsigned int iter = 0 ;
891 double residu_1 = 1e12 ;
893 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
900 for (
unsigned int p=0; p<nbPose ; p++)
903 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
905 cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
906 +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
907 cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
908 +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
909 cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
910 +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
917 vpMatrix L(nbPointTotal*4,nbPose6+6) ;
919 double px = cam_est.
get_px() ;
920 double py = cam_est.
get_py() ;
921 double u0 = cam_est.
get_u0() ;
922 double v0 = cam_est.
get_v0() ;
924 double inv_px = 1/px ;
925 double inv_py = 1/py ;
927 double kud = cam_est.
get_kud() ;
928 double kdu = cam_est.
get_kdu() ;
933 for (
unsigned int p=0; p<nbPose ; p++)
935 unsigned int q = 6*p;
936 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
938 unsigned int curPoint4 = 4*curPoint;
939 double x = cX[curPoint] ;
940 double y = cY[curPoint] ;
941 double z = cZ[curPoint] ;
951 double up = u[curPoint] ;
952 double vp = v[curPoint] ;
955 Pd[curPoint4+1] = vp ;
957 double up0 = up - u0;
958 double vp0 = vp - v0;
960 double xp0 = up0 * inv_px;
961 double xp02 = xp0 *xp0 ;
963 double yp0 = vp0 * inv_py;
964 double yp02 = yp0 * yp0;
966 double r2du = xp02 + yp02 ;
967 double kr2du = kdu * r2du;
969 P[curPoint4] = u0 + px*X - kr2du *(up0) ;
970 P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
972 double r2ud = X2 + Y2 ;
973 double kr2ud = 1 + kud * r2ud;
975 double Axx = px*(kr2ud+k2ud*X2);
976 double Axy = px*k2ud*XY;
977 double Ayy = py*(kr2ud+k2ud*Y2);
978 double Ayx = py*k2ud*XY;
980 Pd[curPoint4+2] = up ;
981 Pd[curPoint4+3] = vp ;
983 P[curPoint4+2] = u0 + px*X*kr2ud ;
984 P[curPoint4+3] = v0 + py*Y*kr2ud ;
991 unsigned int curInd = curPoint4;
995 L[curInd][q] = px * (-inv_z) ;
997 L[curInd][q+2] = px*X*inv_z ;
998 L[curInd][q+3] = px*X*Y ;
999 L[curInd][q+4] = -px*(1+X2) ;
1000 L[curInd][q+5] = px*Y ;
1003 L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
1004 L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
1005 L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
1006 L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
1007 L[curInd][nbPose6+4] = -(up0)*(r2du) ;
1008 L[curInd][nbPose6+5] = 0 ;
1013 L[curInd][q+1] = py*(-inv_z) ;
1014 L[curInd][q+2] = py*Y*inv_z ;
1015 L[curInd][q+3] = py* (1+Y2) ;
1016 L[curInd][q+4] = -py*XY ;
1017 L[curInd][q+5] = -py*X ;
1020 L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
1021 L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
1022 L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
1023 L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
1024 L[curInd][nbPose6+4] = -vp0*r2du ;
1025 L[curInd][nbPose6+5] = 0 ;
1030 L[curInd][q] = Axx*(-inv_z) ;
1031 L[curInd][q+1] = Axy*(-inv_z) ;
1032 L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
1033 L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
1034 L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
1035 L[curInd][q+5] = Axx*Y -Axy*X;
1038 L[curInd][nbPose6]= 1 ;
1039 L[curInd][nbPose6+1]= 0 ;
1040 L[curInd][nbPose6+2]= X*kr2ud ;
1041 L[curInd][nbPose6+3]= 0;
1042 L[curInd][nbPose6+4] = 0 ;
1043 L[curInd][nbPose6+5] = px*X*r2ud ;
1047 L[curInd][q] = Ayx*(-inv_z) ;
1048 L[curInd][q+1] = Ayy*(-inv_z) ;
1049 L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
1050 L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
1051 L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
1052 L[curInd][q+5] = Ayx*Y -Ayy*X;
1055 L[curInd][nbPose6]= 0 ;
1056 L[curInd][nbPose6+1]= 1;
1057 L[curInd][nbPose6+2]= 0;
1058 L[curInd][nbPose6+3]= Y*kr2ud ;
1059 L[curInd][nbPose6+4] = 0 ;
1060 L[curInd][nbPose6+5] = py*Y*r2ud ;
1073 L.pseudoInverse(Lp,1e-10) ;
1078 for (
unsigned int i = 0 ; i < 6*nbPose ; i++)
1082 u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1083 kud + Tc[nbPose6+5],
1084 kdu + Tc[nbPose6+4]);
1087 for (
unsigned int p = 0 ; p < nbPose ; p++)
1089 for (
unsigned int i = 0 ; i < 6 ; i++)
1090 Tc_v_Tmp[i] = Tc_v[6*p + i];
1093 * table_cal[p].cMo_dist;
1096 std::cout <<
" std dev: " << sqrt(r/nbPointTotal) << std::endl;
1100 if (iter == nbIterMax)
1102 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
1104 "Maximum number of iterations reached")) ;
1110 for (
unsigned int p = 0 ; p < nbPose ; p++)
1112 table_cal[p].cam_dist = cam_est ;
1117 globalReprojectionError = sqrt(r/(nbPointTotal));
1120 std::cout.flags(original_flags);
1138 std::vector<vpHomogeneousMatrix>&
rMe,
1142 unsigned int nbPose = (
unsigned int)cMo.
size();
1147 unsigned int k = 0 ;
1149 for (
unsigned int i=0 ; i < nbPose ; i++)
1152 rMe[i].extract(rRei) ;
1153 cMo[i].extract(ciRo) ;
1156 for (
unsigned int j=0 ; j < nbPose ; j++)
1161 rMe[j].extract(rRej) ;
1162 cMo[j].extract(cjRo) ;
1171 double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1172 + rPeij[2]*rPeij[2]);
1174 for (
unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] *
vpMath::sinc(theta/2);
1177 theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
1178 + cijPo[2]*cijPo[2]);
1179 for (
unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] *
vpMath::sinc(theta/2);
1227 for (
unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1229 theta = 2*asin(theta) ;
1231 if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1233 for (
unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1251 for (
unsigned int i=0 ; i < nbPose ; i++)
1255 rMe[i].extract(rRei) ;
1256 cMo[i].extract(ciRo) ;
1257 rMe[i].extract(rTei) ;
1258 cMo[i].extract(ciTo) ;
1261 for (
unsigned int j=0 ; j < nbPose ; j++)
1267 rMe[j].extract(rRej) ;
1268 cMo[j].extract(cjRo) ;
1271 rMe[j].extract(rTej) ;
1272 cMo[j].extract(cjTo) ;
1278 rTeij = rRej.
t()*rTeij ;
1283 b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
1329 vpCalibration::calibVVSMulti(
unsigned int nbPose,
1334 std::ios::fmtflags original_flags( std::cout.flags() );
1335 std::cout.precision(10);
1336 unsigned int nbPoint[256];
1337 unsigned int nbPointTotal = 0;
1339 unsigned int nbPose6 = 6*nbPose;
1341 for (
unsigned int i=0; i<nbPose ; i++)
1343 nbPoint[i] = table_cal[i].npt;
1344 nbPointTotal += nbPoint[i];
1347 if (nbPointTotal < 4)
1351 "Not enough point to calibrate")) ;
1364 unsigned int curPoint = 0 ;
1365 for (
unsigned int p=0; p<nbPose ; p++)
1367 std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1368 std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1369 std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1370 std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1372 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
1374 oX[curPoint] = *it_LoX;
1375 oY[curPoint] = *it_LoY;
1376 oZ[curPoint] = *it_LoZ;
1379 u[curPoint] = ip.
get_u() ;
1380 v[curPoint] = ip.
get_v() ;
1391 unsigned int iter = 0 ;
1393 double residu_1 = 1e12 ;
1395 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
1401 double px = cam_est.
get_px();
1402 double py = cam_est.
get_py();
1403 double u0 = cam_est.
get_u0();
1404 double v0 = cam_est.
get_v0();
1408 for (
unsigned int p=0; p<nbPose ; p++)
1411 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
1413 unsigned int curPoint2 = 2*curPoint;
1415 cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
1416 +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
1417 cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
1418 +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
1419 cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
1420 +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
1422 Pd[curPoint2] = u[curPoint] ;
1423 Pd[curPoint2+1] = v[curPoint] ;
1425 P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
1426 P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
1438 vpMatrix L(nbPointTotal*2,nbPose6+4) ;
1440 for (
unsigned int p=0; p<nbPose ; p++)
1442 unsigned int q = 6*p;
1443 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
1445 unsigned int curPoint2 = 2*curPoint;
1446 unsigned int curPoint21 = curPoint2 + 1;
1448 double x = cX[curPoint] ;
1449 double y = cY[curPoint] ;
1450 double z = cZ[curPoint] ;
1454 double X = x*inv_z ;
1455 double Y = y*inv_z ;
1460 L[curPoint2][q] = px * (-inv_z) ;
1461 L[curPoint2][q+1] = 0 ;
1462 L[curPoint2][q+2] = px*(X*inv_z) ;
1463 L[curPoint2][q+3] = px*X*Y ;
1464 L[curPoint2][q+4] = -px*(1+X*X) ;
1465 L[curPoint2][q+5] = px*Y ;
1468 L[curPoint2][nbPose6]= 1 ;
1469 L[curPoint2][nbPose6+1]= 0 ;
1470 L[curPoint2][nbPose6+2]= X ;
1471 L[curPoint2][nbPose6+3]= 0;
1474 L[curPoint21][q] = 0 ;
1475 L[curPoint21][q+1] = py*(-inv_z) ;
1476 L[curPoint21][q+2] = py*(Y*inv_z) ;
1477 L[curPoint21][q+3] = py* (1+Y*Y) ;
1478 L[curPoint21][q+4] = -py*X*Y ;
1479 L[curPoint21][q+5] = -py*X ;
1482 L[curPoint21][nbPose6]= 0 ;
1483 L[curPoint21][nbPose6+1]= 1 ;
1484 L[curPoint21][nbPose6+2]= 0;
1485 L[curPoint21][nbPose6+3]= Y ;
1502 for (
unsigned int i = 0 ; i < nbPose6 ; i++)
1513 for (
unsigned int p = 0 ; p < nbPose ; p++)
1515 for (
unsigned int i = 0 ; i < 6 ; i++)
1516 Tc_v_Tmp[i] = Tc_v[6*p + i];
1522 std::cout <<
" std dev " << sqrt(r/nbPointTotal) << std::endl;
1525 if (iter == nbIterMax)
1527 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
1529 "Maximum number of iterations reached")) ;
1531 for (
unsigned int p = 0 ; p < nbPose ; p++)
1534 table_cal[p].
cam = cam_est;
1536 double deviation,deviation_dist;
1540 std::cout <<
" Global std dev " << sqrt(r/nbPointTotal) << std::endl;
1543 std::cout.flags(original_flags);
1548 vpCalibration::calibVVSWithDistortionMulti(
1549 unsigned int nbPose,
1554 std::ios::fmtflags original_flags( std::cout.flags() );
1555 std::cout.precision(10);
1556 unsigned int nbPoint[1024];
1557 unsigned int nbPointTotal = 0;
1559 unsigned int nbPose6 = 6*nbPose;
1560 for (
unsigned int i=0; i<nbPose ; i++)
1562 nbPoint[i] = table_cal[i].npt;
1563 nbPointTotal += nbPoint[i];
1566 if (nbPointTotal < 4)
1570 "Not enough point to calibrate")) ;
1583 unsigned int curPoint = 0 ;
1584 for (
unsigned int p=0; p<nbPose ; p++)
1586 std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
1587 std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
1588 std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
1589 std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
1591 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
1593 oX[curPoint] = *it_LoX;
1594 oY[curPoint] = *it_LoY;
1595 oZ[curPoint] = *it_LoZ;
1598 u[curPoint] = ip.
get_u() ;
1599 v[curPoint] = ip.
get_v() ;
1609 unsigned int iter = 0 ;
1611 double residu_1 = 1e12 ;
1613 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
1620 for (
unsigned int p=0; p<nbPose ; p++)
1623 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
1625 cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
1626 +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
1627 cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
1628 +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
1629 cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
1630 +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
1637 vpMatrix L(nbPointTotal*4,nbPose6+6) ;
1639 double px = cam_est.
get_px() ;
1640 double py = cam_est.
get_py() ;
1641 double u0 = cam_est.
get_u0() ;
1642 double v0 = cam_est.
get_v0() ;
1644 double inv_px = 1/px ;
1645 double inv_py = 1/py ;
1647 double kud = cam_est.
get_kud() ;
1648 double kdu = cam_est.
get_kdu() ;
1650 double k2ud = 2*kud;
1651 double k2du = 2*kdu;
1653 for (
unsigned int p=0; p<nbPose ; p++)
1655 unsigned int q = 6*p;
1656 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
1658 unsigned int curPoint4 = 4*curPoint;
1659 double x = cX[curPoint] ;
1660 double y = cY[curPoint] ;
1661 double z = cZ[curPoint] ;
1664 double X = x*inv_z ;
1665 double Y = y*inv_z ;
1671 double up = u[curPoint] ;
1672 double vp = v[curPoint] ;
1674 Pd[curPoint4] = up ;
1675 Pd[curPoint4+1] = vp ;
1677 double up0 = up - u0;
1678 double vp0 = vp - v0;
1680 double xp0 = up0 * inv_px;
1681 double xp02 = xp0 *xp0 ;
1683 double yp0 = vp0 * inv_py;
1684 double yp02 = yp0 * yp0;
1686 double r2du = xp02 + yp02 ;
1687 double kr2du = kdu * r2du;
1689 P[curPoint4] = u0 + px*X - kr2du *(up0) ;
1690 P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
1692 double r2ud = X2 + Y2 ;
1693 double kr2ud = 1 + kud * r2ud;
1695 double Axx = px*(kr2ud+k2ud*X2);
1696 double Axy = px*k2ud*XY;
1697 double Ayy = py*(kr2ud+k2ud*Y2);
1698 double Ayx = py*k2ud*XY;
1700 Pd[curPoint4+2] = up ;
1701 Pd[curPoint4+3] = vp ;
1703 P[curPoint4+2] = u0 + px*X*kr2ud ;
1704 P[curPoint4+3] = v0 + py*Y*kr2ud ;
1709 vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
1711 unsigned int curInd = curPoint4;
1715 L[curInd][q] = px * (-inv_z) ;
1716 L[curInd][q+1] = 0 ;
1717 L[curInd][q+2] = px*X*inv_z ;
1718 L[curInd][q+3] = px*X*Y ;
1719 L[curInd][q+4] = -px*(1+X2) ;
1720 L[curInd][q+5] = px*Y ;
1723 L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
1724 L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
1725 L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
1726 L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
1727 L[curInd][nbPose6+4] = -(up0)*(r2du) ;
1728 L[curInd][nbPose6+5] = 0 ;
1733 L[curInd][q+1] = py*(-inv_z) ;
1734 L[curInd][q+2] = py*Y*inv_z ;
1735 L[curInd][q+3] = py* (1+Y2) ;
1736 L[curInd][q+4] = -py*XY ;
1737 L[curInd][q+5] = -py*X ;
1740 L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
1741 L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
1742 L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
1743 L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
1744 L[curInd][nbPose6+4] = -vp0*r2du ;
1745 L[curInd][nbPose6+5] = 0 ;
1750 L[curInd][q] = Axx*(-inv_z) ;
1751 L[curInd][q+1] = Axy*(-inv_z) ;
1752 L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
1753 L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
1754 L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
1755 L[curInd][q+5] = Axx*Y -Axy*X;
1758 L[curInd][nbPose6]= 1 ;
1759 L[curInd][nbPose6+1]= 0 ;
1760 L[curInd][nbPose6+2]= X*kr2ud ;
1761 L[curInd][nbPose6+3]= 0;
1762 L[curInd][nbPose6+4] = 0 ;
1763 L[curInd][nbPose6+5] = px*X*r2ud ;
1767 L[curInd][q] = Ayx*(-inv_z) ;
1768 L[curInd][q+1] = Ayy*(-inv_z) ;
1769 L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
1770 L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
1771 L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
1772 L[curInd][q+5] = Ayx*Y -Ayy*X;
1775 L[curInd][nbPose6]= 0 ;
1776 L[curInd][nbPose6+1]= 1;
1777 L[curInd][nbPose6+2]= 0;
1778 L[curInd][nbPose6+3]= Y*kr2ud ;
1779 L[curInd][nbPose6+4] = 0 ;
1780 L[curInd][nbPose6+5] = py*Y*r2ud ;
1798 for (
unsigned int i = 0 ; i < 6*nbPose ; i++)
1802 u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1803 kud + Tc[nbPose6+5],
1804 kdu + Tc[nbPose6+4]);
1807 for (
unsigned int p = 0 ; p < nbPose ; p++)
1809 for (
unsigned int i = 0 ; i < 6 ; i++)
1810 Tc_v_Tmp[i] = Tc_v[6*p + i];
1816 std::cout <<
" std dev: " << sqrt(r/nbPointTotal) << std::endl;
1820 if (iter == nbIterMax)
1822 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
1824 "Maximum number of iterations reached")) ;
1827 for (
unsigned int p = 0 ; p < nbPose ; p++)
1833 std::cout <<
" Global std dev " << sqrt(r/(nbPointTotal)) << std::endl;
1836 std::cout.flags(original_flags);
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Error that can be emited by the vpCalibration class.
void stack(const double &d)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static bool equal(double x, double y, double s=0.001)
void stack(const vpMatrix &A)
vpHomogeneousMatrix inverse() const
unsigned int size() const
Return the number of elements of the 2D array.
Tools for perspective camera calibration.
vpRotationMatrix t() const
static double sinc(double x)
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void insert(const vpRotationMatrix &R)
void svd(vpColVector &w, vpMatrix &v)
something is not initialized
static double sqr(double x)
vpHomogeneousMatrix cMo_dist
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix rMe
reference coordinates (manipulator base coordinates)
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static void calibrationTsai(std::vector< vpHomogeneousMatrix > &cMo, std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
calibration method of effector-camera from R. Tsai and R. Lorenz .
vpCameraParameters cam_dist
projection model with distortion
Implementation of column vector and the associated operations.
iterative algorithm doesn't converge
static vpHomogeneousMatrix direct(const vpColVector &v)
vpHomogeneousMatrix eMc
position of the camera in relation to the effector
static vpMatrix skew(const vpColVector &v)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void computeStdDeviation(double &deviation, double &deviation_dist)
vpCameraParameters cam
projection model without distortion
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)