46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpPixelMeterConversion.h>
48 #include <visp3/vision/vpCalibration.h>
49 #include <visp3/vision/vpPose.h>
51 double vpCalibration::threshold = 1e-10f;
52 unsigned int vpCalibration::nbIterMax = 4000;
53 double vpCalibration::gain = 0.25;
61 residual = residual_dist = 1000.;
75 : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.),
84 : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.),
102 npt = twinCalibration.npt;
103 LoX = twinCalibration.LoX;
104 LoY = twinCalibration.LoY;
105 LoZ = twinCalibration.LoZ;
106 Lip = twinCalibration.Lip;
108 residual = twinCalibration.residual;
109 cMo = twinCalibration.
cMo;
110 residual_dist = twinCalibration.residual_dist;
113 cam = twinCalibration.
cam;
116 rMe = twinCalibration.
rMe;
118 eMc = twinCalibration.
eMc;
170 std::list<double>::const_iterator it_LoX = LoX.begin();
171 std::list<double>::const_iterator it_LoY = LoY.begin();
172 std::list<double>::const_iterator it_LoZ = LoZ.begin();
173 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
175 for (
unsigned int i = 0; i < npt; i++) {
176 vpPoint P(*it_LoX, *it_LoY, *it_LoZ);
204 if (residual_lagrange < residual_dementhon)
205 cMo_est = cMo_lagrange;
207 cMo_est = cMo_dementhon;
223 double residual_ = 0;
225 std::list<double>::const_iterator it_LoX = LoX.begin();
226 std::list<double>::const_iterator it_LoY = LoY.begin();
227 std::list<double>::const_iterator it_LoZ = LoZ.begin();
228 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
230 double u0 = camera.
get_u0();
231 double v0 = camera.
get_v0();
232 double px = camera.
get_px();
233 double py = camera.
get_py();
236 for (
unsigned int i = 0; i < npt; i++) {
241 double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
242 double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
243 double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
249 double u = ip.
get_u();
250 double v = ip.
get_v();
252 double xp = u0 + x * px;
253 double yp = v0 + y * py;
262 this->residual = residual_;
263 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 = camera.
get_u0();
282 double v0 = camera.
get_v0();
283 double px = camera.
get_px();
284 double py = camera.
get_py();
288 double inv_px = 1 / px;
289 double inv_py = 1 / px;
292 for (
unsigned int i = 0; i < npt; i++) {
297 double cX = oX * cMo_est[0][0] + oY * cMo_est[0][1] + oZ * cMo_est[0][2] + cMo_est[0][3];
298 double cY = oX * cMo_est[1][0] + oY * cMo_est[1][1] + oZ * cMo_est[1][2] + cMo_est[1][3];
299 double cZ = oX * cMo_est[2][0] + oY * cMo_est[2][1] + oZ * cMo_est[2][2] + cMo_est[2][3];
305 double u = ip.
get_u();
306 double v = ip.
get_v();
310 double xp = u0 + x * px * r2ud;
311 double yp = v0 + y * py * r2ud;
317 xp = u0 + x * px - kdu * (u - u0) * r2du;
318 yp = v0 + y * py - kdu * (v - v0) * r2du;
328 this->residual_dist = residual_;
329 return sqrt(residual_ / npt);
360 computePose(cam_est, cMo_est);
364 calibLagrange(cam_est, cMo_est);
379 std::cout <<
"start calibration without distortion" << std::endl;
381 calibVVS(cam_est, cMo_est, verbose);
403 std::cout <<
"start calibration with distortion" << std::endl;
405 calibVVSWithDistortion(cam_est, cMo_est, verbose);
428 std::cout <<
"Unable to calibrate the camera. Estimated parameters are negative." << std::endl;
456 unsigned int nbPose = (
unsigned int)table_cal.size();
457 for (
unsigned int i = 0; i < nbPose; i++) {
458 if (table_cal[i].
get_npt() > 3)
459 table_cal[i].computePose(cam_est, table_cal[i].
cMo);
464 std::cout <<
"this calibration method is not available in" << std::endl
465 <<
"vpCalibration::computeCalibrationMulti()" << std::endl;
468 table_cal[0].calibLagrange(cam_est, table_cal[0].
cMo);
469 table_cal[0].cam = cam_est;
470 table_cal[0].cam_dist = cam_est;
471 table_cal[0].cMo_dist = table_cal[0].cMo;
478 std::cout <<
"this calibration method is not available in" << std::endl
479 <<
"vpCalibration::computeCalibrationMulti()" << std::endl
480 <<
"with several images." << std::endl;
483 table_cal[0].calibLagrange(cam_est, table_cal[0].
cMo);
484 table_cal[0].cam = cam_est;
485 table_cal[0].cam_dist = cam_est;
486 table_cal[0].cMo_dist = table_cal[0].cMo;
488 calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
493 calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
513 std::cout <<
"Compute camera parameters with distortion" << std::endl;
515 calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose);
522 table_cal[0].cam.printParameters();
525 std::cout << std::endl;
529 std::cout <<
"Unable to calibrate the camera. Estimated parameters are negative." << std::endl;
548 std::ofstream f(filename);
551 std::list<double>::const_iterator it_LoX = LoX.begin();
552 std::list<double>::const_iterator it_LoY = LoY.begin();
553 std::list<double>::const_iterator it_LoZ = LoZ.begin();
554 std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
557 f.setf(std::ios::fixed, std::ios::floatfield);
558 f << LoX.size() << std::endl;
560 for (
unsigned int i = 0; i < LoX.size(); i++) {
567 double u = ip.
get_u();
568 double v = ip.
get_v();
570 f << oX <<
" " << oY <<
" " << oZ <<
" " << u <<
" " << v << std::endl;
596 std::cout <<
"There are " << n <<
" point on the calibration grid " << std::endl;
603 for (
unsigned int i = 0; i < n; i++) {
604 double x, y, z, u, v;
605 f >> x >> y >> z >> u >> v;
606 std::cout << x <<
" " << y <<
" " << z <<
" " << u <<
" " << v << std::endl;
634 int vpCalibration::readGrid(
const char *filename,
unsigned int &n, std::list<double> &oX, std::list<double> &oY,
635 std::list<double> &oZ,
bool verbose)
644 std::cout <<
"There are " << n <<
" points on the calibration grid " << std::endl;
656 for (
unsigned int i = 0; i < n; i++) {
657 f >> no_pt >> x >> y >> z;
659 std::cout << no_pt << std::endl;
660 std::cout << x <<
" " << y <<
" " << z << std::endl;
690 for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++it) {
692 if (subsampling_factor > 1.) {
725 std::list<double>::const_iterator it_LoX = LoX.begin();
726 std::list<double>::const_iterator it_LoY = LoY.begin();
727 std::list<double>::const_iterator it_LoZ = LoZ.begin();
729 for (
unsigned int i = 0; i < npt; i++) {
758 ip.
set_u(u0_dist + x * px_dist * r2);
759 ip.set_v(v0_dist + y * py_dist * r2);