50 #include <visp/vpCalibration.h>
51 #include <visp/vpDebug.h>
52 #include <visp/vpPose.h>
53 #include <visp/vpPixelMeterConversion.h>
55 double vpCalibration::threshold = 1e-10f;
56 unsigned int vpCalibration::nbIterMax = 4000;
57 double vpCalibration::gain = 0.25;
94 npt = twinCalibration.npt ;
95 LoX = twinCalibration.LoX ;
96 LoY = twinCalibration.LoY ;
97 LoZ = twinCalibration.LoZ ;
98 Lip = twinCalibration.Lip ;
100 residual = twinCalibration.residual;
101 cMo = twinCalibration.
cMo;
102 residual_dist = twinCalibration.residual_dist;
105 cam = twinCalibration.
cam ;
108 rMe = twinCalibration.
rMe;
110 eMc = twinCalibration.
eMc;
161 std::list<double>::const_iterator it_LoX = LoX.begin();
162 std::list<double>::const_iterator it_LoY = LoY.begin();
163 std::list<double>::const_iterator it_LoZ = LoZ.begin();
164 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
166 for (
unsigned int i =0 ; i < npt ; i++)
197 if (residual_lagrange < residual_dementhon)
219 double residual = 0 ;
221 std::list<double>::const_iterator it_LoX = LoX.begin();
222 std::list<double>::const_iterator it_LoY = LoY.begin();
223 std::list<double>::const_iterator it_LoZ = LoZ.begin();
224 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
226 double u0 = cam.
get_u0() ;
227 double v0 = cam.
get_v0() ;
228 double px = cam.
get_px() ;
229 double py = cam.
get_py() ;
232 for (
unsigned int i =0 ; i < npt ; i++)
239 double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
240 double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
241 double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
247 double u = ip.
get_u() ;
248 double v = ip.
get_v() ;
250 double xp = u0 + x*px;
251 double yp = v0 + y*py;
260 this->residual = residual ;
261 return sqrt(residual/npt) ;
274 double residual = 0 ;
276 std::list<double>::const_iterator it_LoX = LoX.begin();
277 std::list<double>::const_iterator it_LoY = LoY.begin();
278 std::list<double>::const_iterator it_LoZ = LoZ.begin();
279 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
281 double u0 = cam.
get_u0() ;
282 double v0 = cam.
get_v0() ;
283 double px = cam.
get_px() ;
284 double py = cam.
get_py() ;
288 double inv_px = 1/px;
289 double inv_py = 1/px;
292 for (
unsigned int i =0 ; i < npt ; i++)
299 double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
300 double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
301 double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
307 double u = ip.
get_u() ;
308 double v = ip.
get_v() ;
312 double xp = u0 + x*px*r2ud;
313 double yp = v0 + y*py*r2ud;
319 xp = u0 + x*px - kdu*(u-u0)*r2du;
320 yp = v0 + y*py - kdu*(v-v0)*r2du;
330 this->residual_dist = residual;
331 return sqrt(residual/npt) ;
365 computePose(cam,cMo);
371 calibLagrange(cam, cMo);
388 if (verbose){std::cout <<
"start calibration without distortion"<< std::endl;}
389 calibVVS(cam, cMo, verbose);
412 if (verbose){std::cout <<
"start calibration with distortion"<< std::endl;}
413 calibVVSWithDistortion(cam, cMo, verbose);
459 for(
unsigned int i=0;i<nbPose;i++){
461 table_cal[i].computePose(cam,table_cal[i].cMo);
466 std::cout <<
"this calibration method is not available in" << std::endl
467 <<
"vpCalibration::computeCalibrationMulti()" << std::endl;
471 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
480 std::cout <<
"this calibration method is not available in" << std::endl
481 <<
"vpCalibration::computeCalibrationMulti()" << std::endl
482 <<
"with several images." << std::endl;
486 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
494 calibVVSMulti(nbPose, table_cal, cam, verbose);
515 std::cout <<
"Compute camera parameters with distortion"<<std::endl;
517 calibVVSWithDistortionMulti(nbPose, table_cal, cam, verbose);
527 std::cout<<std::endl;
564 for(
unsigned int i=0;i<nbPose;i++){
565 table_cMo[i] = table_cal[i].
cMo;
566 table_cMo_dist[i] = table_cal[i].
cMo_dist;
567 table_rMe[i] = table_cal[i].
rMe;
573 delete [] table_cMo_dist;
579 vpERROR_TRACE(
"Three images are needed to compute Tsai calibration !\n");
598 std::ofstream f(filename) ;
601 std::list<double>::const_iterator it_LoX = LoX.begin();
602 std::list<double>::const_iterator it_LoY = LoY.begin();
603 std::list<double>::const_iterator it_LoZ = LoZ.begin();
604 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
607 f.setf(std::ios::fixed,std::ios::floatfield);
608 f << LoX.size() << std::endl ;
610 for (
unsigned int i =0 ; i < LoX.size() ; i++)
618 double u = ip.
get_u() ;
619 double v = ip.
get_v() ;
621 f << oX <<
" " << oY <<
" " << oZ <<
" " << u <<
" " << v <<std::endl ;
648 std::cout <<
"There are "<< n <<
" point on the calibration grid " << std::endl ;
651 for (
int i=0 ; i < n ; i++)
653 double x, y, z, u, v ;
654 f >> x >> y >> z >> u >> v ;
655 std::cout << x <<
" "<<y <<
" "<<z<<
" "<<u<<
" "<<v <<std::endl ;
685 std::list<double> &oX, std::list<double> &oY, std::list<double> &oZ,
bool verbose)
694 std::cout <<
"There are "<< n <<
" points on the calibration grid " << std::endl ;
703 for (
unsigned int i=0 ; i < n ; i++)
705 f >> no_pt >> x >> y >> z ;
707 std::cout << no_pt <<std::endl ;
708 std::cout << x <<
" "<< y <<
" "<< z <<std::endl ;
721 catch(...){
return -1;}
733 unsigned int thickness)
736 for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++ it) {
750 unsigned int thickness)
764 std::list<double>::const_iterator it_LoX = LoX.begin();
765 std::list<double>::const_iterator it_LoY = LoY.begin();
766 std::list<double>::const_iterator it_LoZ = LoZ.begin();
768 for (
unsigned int i =0 ; i < npt ; i++)
775 double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
776 double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
777 double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
789 cY = oX*cMo_dist[1][0]+oY*cMo_dist[1][1]+oZ*cMo_dist[1][2]+cMo_dist[1][3];
790 cZ = oX*cMo_dist[2][0]+oY*cMo_dist[2][1]+oZ*cMo_dist[2][2]+cMo_dist[2][3];
798 ip.
set_u( u0_dist + x*px_dist*r2 );
799 ip.set_v( v0_dist + y*py_dist*r2 );
813 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
843 std::cout <<
"There are "<< n <<
" points on the calibration grid " << std::endl ;
850 for (
unsigned int i=0 ; i < n ; i++)
852 f >> no_pt >> x >> y >> z ;
854 std::cout << no_pt <<std::endl ;
855 std::cout << x <<
" "<< y <<
" "<< z <<std::endl ;
868 catch(...){
return -1;}
872 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS