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);
453 std::vector<vpCalibration> &table_cal,
455 double &globalReprojectionError,
459 unsigned int nbPose = (
unsigned int) table_cal.size();
460 for(
unsigned int i=0;i<nbPose;i++){
462 table_cal[i].computePose(cam,table_cal[i].cMo);
467 std::cout <<
"this calibration method is not available in" << std::endl
468 <<
"vpCalibration::computeCalibrationMulti()" << std::endl;
472 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
473 table_cal[0].cam =
cam ;
474 table_cal[0].cam_dist =
cam ;
475 table_cal[0].cMo_dist = table_cal[0].cMo ;
481 std::cout <<
"this calibration method is not available in" << std::endl
482 <<
"vpCalibration::computeCalibrationMulti()" << std::endl
483 <<
"with several images." << std::endl;
487 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
488 table_cal[0].cam =
cam ;
489 table_cal[0].cam_dist =
cam ;
490 table_cal[0].cMo_dist = table_cal[0].cMo ;
495 calibVVSMulti(table_cal, cam, globalReprojectionError, verbose);
516 std::cout <<
"Compute camera parameters with distortion"<<std::endl;
518 calibVVSWithDistortionMulti(table_cal, cam, globalReprojectionError, verbose);
525 table_cal[0].cam.printParameters();
528 std::cout<<std::endl;
555 unsigned int nbPose = (
unsigned int)table_cal.size();
557 std::vector<vpHomogeneousMatrix> table_cMo(nbPose);
558 std::vector<vpHomogeneousMatrix> table_cMo_dist(nbPose);
559 std::vector<vpHomogeneousMatrix> table_rMe(nbPose);
561 for(
unsigned int i=0;i<nbPose;i++){
562 table_cMo[i] = table_cal[i].cMo;
563 table_cMo_dist[i] = table_cal[i].cMo_dist;
564 table_rMe[i] = table_cal[i].rMe;
572 vpERROR_TRACE(
"Three images are needed to compute Tsai calibration !\n");
591 std::ofstream f(filename) ;
594 std::list<double>::const_iterator it_LoX = LoX.begin();
595 std::list<double>::const_iterator it_LoY = LoY.begin();
596 std::list<double>::const_iterator it_LoZ = LoZ.begin();
597 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
600 f.setf(std::ios::fixed,std::ios::floatfield);
601 f << LoX.size() << std::endl ;
603 for (
unsigned int i =0 ; i < LoX.size() ; i++)
611 double u = ip.
get_u() ;
612 double v = ip.
get_v() ;
614 f << oX <<
" " << oY <<
" " << oZ <<
" " << u <<
" " << v <<std::endl ;
641 std::cout <<
"There are "<< n <<
" point on the calibration grid " << std::endl ;
644 for (
int i=0 ; i < n ; i++)
646 double x, y, z, u, v ;
647 f >> x >> y >> z >> u >> v ;
648 std::cout << x <<
" "<<y <<
" "<<z<<
" "<<u<<
" "<<v <<std::endl ;
678 std::list<double> &oX, std::list<double> &oY, std::list<double> &oZ,
bool verbose)
687 std::cout <<
"There are "<< n <<
" points on the calibration grid " << std::endl ;
696 for (
unsigned int i=0 ; i < n ; i++)
698 f >> no_pt >> x >> y >> z ;
700 std::cout << no_pt <<std::endl ;
701 std::cout << x <<
" "<< y <<
" "<< z <<std::endl ;
714 catch(...){
return -1;}
726 unsigned int thickness)
729 for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++ it) {
743 unsigned int thickness)
757 std::list<double>::const_iterator it_LoX = LoX.begin();
758 std::list<double>::const_iterator it_LoY = LoY.begin();
759 std::list<double>::const_iterator it_LoZ = LoZ.begin();
761 for (
unsigned int i =0 ; i < npt ; i++)
768 double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
769 double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
770 double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
782 cY = oX*cMo_dist[1][0]+oY*cMo_dist[1][1]+oZ*cMo_dist[1][2]+cMo_dist[1][3];
783 cZ = oX*cMo_dist[2][0]+oY*cMo_dist[2][1]+oZ*cMo_dist[2][2]+cMo_dist[2][3];
791 ip.
set_u( u0_dist + x*px_dist*r2 );
792 ip.set_v( v0_dist + y*py_dist*r2 );
806 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
830 for(
unsigned int i=0;i<nbPose;i++){
832 table_cal[i].computePose(cam,table_cal[i].cMo);
837 std::cout <<
"this calibration method is not available in" << std::endl
838 <<
"vpCalibration::computeCalibrationMulti()" << std::endl;
842 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
851 std::cout <<
"this calibration method is not available in" << std::endl
852 <<
"vpCalibration::computeCalibrationMulti()" << std::endl
853 <<
"with several images." << std::endl;
857 table_cal[0].calibLagrange(cam,table_cal[0].cMo);
865 calibVVSMulti(nbPose, table_cal, cam, verbose);
886 std::cout <<
"Compute camera parameters with distortion"<<std::endl;
888 calibVVSWithDistortionMulti(nbPose, table_cal, cam, verbose);
898 std::cout<<std::endl;
935 for(
unsigned int i=0;i<nbPose;i++){
936 table_cMo[i] = table_cal[i].
cMo;
937 table_cMo_dist[i] = table_cal[i].
cMo_dist;
938 table_rMe[i] = table_cal[i].
rMe;
944 delete [] table_cMo_dist;
950 vpERROR_TRACE(
"Three images are needed to compute Tsai calibration !\n");
988 std::cout <<
"There are "<< n <<
" points on the calibration grid " << std::endl ;
995 for (
unsigned int i=0 ; i < n ; i++)
997 f >> no_pt >> x >> y >> z ;
999 std::cout << no_pt <<std::endl ;
1000 std::cout << x <<
" "<< y <<
" "<< z <<std::endl ;
1013 catch(...){
return -1;}
1017 #endif // VISP_BUILD_DEPRECATED_FUNCTIONS