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;
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++) {
749 double cY = oX * cMo_dist[1][0] + oY * cMo_dist[1][1] + oZ * cMo_dist[1][2] + cMo_dist[1][3];
750 double cZ = oX * cMo_dist[2][0] + oY * cMo_dist[2][1] + oZ * cMo_dist[2][2] + cMo_dist[2][3];
758 ip.
set_u(u0_dist + x * px_dist * r2);
759 ip.set_v(v0_dist + y * py_dist * r2);
Used to indicate that a value is not in the allowed range.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose=false)
Implementation of an homogeneous matrix and operations on such kind of matrices.
int readData(const char *filename)
Class to define colors available for display functionnalities.
error that can be emited by ViSP classes.
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
void set_x(const double x)
Set the point x coordinate in the image plane.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Tools for perspective camera calibration.
vpHomogeneousMatrix cMo
(as a 3x4 matrix [R T])
Class that defines what is a point.
vpCalibration & operator=(const vpCalibration &twinCalibration)
int clearPoint()
Suppress all the point in the array of point.
void set_u(const double u)
static double sqr(double x)
int displayGrid(vpImage< unsigned char > &I, vpColor color=vpColor::yellow, unsigned int thickness=1, int subsampling_factor=1)
vpHomogeneousMatrix cMo_dist
void set_v(const double v)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix eMc_dist
void set_y(const double y)
Set the point y coordinate in the image plane.
unsigned int get_npt() const
get the number of points
vpHomogeneousMatrix rMe
reference coordinates (manipulator base coordinates)
double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
vpCameraParameters cam_dist
projection model with distortion
int writeData(const char *filename)
vpHomogeneousMatrix eMc
position of the camera in relation to the effector
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
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam, double &globalReprojectionError, bool verbose=false)
void addPoint(const vpPoint &P)
int displayData(vpImage< unsigned char > &I, vpColor color=vpColor::red, unsigned int thickness=1, int subsampling_factor=1)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...
static int readGrid(const char *filename, unsigned int &n, std::list< double > &oX, std::list< double > &oY, std::list< double > &oZ, bool verbose=false)