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(
unsigned int nbPose,
344 std::cout.precision(10);
345 unsigned int nbPoint[256];
346 unsigned int nbPointTotal = 0;
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];
524 std::cout <<
" std dev " << sqrt(r/nbPointTotal) << std::endl;
527 if (iter == nbIterMax)
529 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
531 "Maximum number of iterations reached")) ;
533 for (
unsigned int p = 0 ; p < nbPose ; p++)
538 double deviation,deviation_dist;
542 std::cout <<
" std dev " << sqrt(r/nbPointTotal) << std::endl;
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(
797 std::cout.precision(10);
798 unsigned int nbPoint[256];
799 unsigned int nbPointTotal = 0;
801 unsigned int nbPose6 = 6*nbPose;
802 for (
unsigned int i=0; i<nbPose ; i++)
804 nbPoint[i] = table_cal[i].npt;
805 nbPointTotal += nbPoint[i];
818 unsigned int curPoint = 0 ;
819 for (
unsigned int p=0; p<nbPose ; p++)
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();
826 for (
unsigned int i =0 ; i < nbPoint[p] ; i++)
828 oX[curPoint] = *it_LoX;
829 oY[curPoint] = *it_LoY;
830 oZ[curPoint] = *it_LoZ;
833 u[curPoint] = ip.
get_u() ;
834 v[curPoint] = ip.
get_v() ;
844 unsigned int iter = 0 ;
846 double residu_1 = 1e12 ;
848 while (
vpMath::equal(residu_1,r,threshold) ==
false && iter < nbIterMax)
855 for (
unsigned int p=0; p<nbPose ; p++)
858 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
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];
872 vpMatrix L(nbPointTotal*4,nbPose6+6) ;
874 double px = cam.
get_px() ;
875 double py = cam.
get_py() ;
876 double u0 = cam.
get_u0() ;
877 double v0 = cam.
get_v0() ;
879 double inv_px = 1/px ;
880 double inv_py = 1/py ;
888 for (
unsigned int p=0; p<nbPose ; p++)
890 unsigned int q = 6*p;
891 for (
unsigned int i=0 ; i < nbPoint[p]; i++)
893 unsigned int curPoint4 = 4*curPoint;
894 double x = cX[curPoint] ;
895 double y = cY[curPoint] ;
896 double z = cZ[curPoint] ;
906 double up = u[curPoint] ;
907 double vp = v[curPoint] ;
910 Pd[curPoint4+1] = vp ;
912 double up0 = up - u0;
913 double vp0 = vp - v0;
915 double xp0 = up0 * inv_px;
916 double xp02 = xp0 *xp0 ;
918 double yp0 = vp0 * inv_py;
919 double yp02 = yp0 * yp0;
921 double r2du = xp02 + yp02 ;
922 double kr2du = kdu * r2du;
924 P[curPoint4] = u0 + px*X - kr2du *(up0) ;
925 P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
927 double r2ud = X2 + Y2 ;
928 double kr2ud = 1 + kud * r2ud;
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;
935 Pd[curPoint4+2] = up ;
936 Pd[curPoint4+3] = vp ;
938 P[curPoint4+2] = u0 + px*X*kr2ud ;
939 P[curPoint4+3] = v0 + py*Y*kr2ud ;
946 unsigned int curInd = curPoint4;
950 L[curInd][q] = px * (-inv_z) ;
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 ;
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 ;
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 ;
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 ;
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;
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 ;
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;
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 ;
1028 L.pseudoInverse(Lp,1e-10) ;
1033 for (
unsigned int i = 0 ; i < 6*nbPose ; i++)
1037 u0+Tc[nbPose6], v0+Tc[nbPose6+1],
1038 kud + Tc[nbPose6+5],
1039 kdu + Tc[nbPose6+4]);
1042 for (
unsigned int p = 0 ; p < nbPose ; p++)
1044 for (
unsigned int i = 0 ; i < 6 ; i++)
1045 Tc_v_Tmp[i] = Tc_v[6*p + i];
1051 std::cout <<
" std dev: " << sqrt(r/nbPointTotal) << std::endl;
1055 if (iter == nbIterMax)
1057 vpERROR_TRACE(
"Iterations number exceed the maximum allowed (%d)",nbIterMax);
1059 "Maximum number of iterations reached")) ;
1061 for (
unsigned int p = 0 ; p < nbPose ; p++)
1068 std::cout <<
" std dev " << sqrt(r/(nbPointTotal)) << std::endl;
1101 unsigned int k = 0 ;
1103 for (
unsigned int i=0 ; i < nbPose ; i++)
1110 for (
unsigned int j=0 ; j < nbPose ; j++)
1125 double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1126 + rPeij[2]*rPeij[2]);
1128 for (
unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] *
vpMath::sinc(theta/2);
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);
1181 for (
unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1183 theta = 2*asin(theta) ;
1185 if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1187 for (
unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1205 for (
unsigned int i=0 ; i < nbPose ; i++)
1215 for (
unsigned int j=0 ; j < nbPose ; j++)
1232 rTeij = rRej.
t()*rTeij ;
1238 b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
1301 std::vector<vpHomogeneousMatrix>& rMe,
1305 unsigned int nbPose = cMo.
size();
1310 unsigned int k = 0 ;
1312 for (
unsigned int i=0 ; i < nbPose ; i++)
1315 rMe[i].extract(rRei) ;
1316 cMo[i].extract(ciRo) ;
1319 for (
unsigned int j=0 ; j < nbPose ; j++)
1324 rMe[j].extract(rRej) ;
1325 cMo[j].extract(cjRo) ;
1334 double theta = sqrt(rPeij[0]*rPeij[0] + rPeij[1]*rPeij[1]
1335 + rPeij[2]*rPeij[2]);
1337 for (
unsigned int m=0;m<3;m++) rPeij[m] = rPeij[m] *
vpMath::sinc(theta/2);
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);
1390 for (
unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
1392 theta = 2*asin(theta) ;
1394 if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
1396 for (
unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
1414 for (
unsigned int i=0 ; i < nbPose ; i++)
1418 rMe[i].extract(rRei) ;
1419 cMo[i].extract(ciRo) ;
1420 rMe[i].extract(rTei) ;
1421 cMo[i].extract(ciTo) ;
1424 for (
unsigned int j=0 ; j < nbPose ; j++)
1430 rMe[j].extract(rRej) ;
1431 cMo[j].extract(cjRo) ;
1434 rMe[j].extract(rTej) ;
1435 cMo[j].extract(cjTo) ;
1441 rTeij = rRej.
t()*rTeij ;
1447 b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;