40 #include <visp3/core/vpConfig.h> 42 #if VISP_HAVE_OPENCV_VERSION >= 0x020300 46 #include <opencv2/calib3d/calib3d.hpp> 47 #include <opencv2/core/core.hpp> 48 #include <opencv2/highgui/highgui.hpp> 49 #include <opencv2/imgproc/imgproc.hpp> 51 #include <visp3/vision/vpCalibration.h> 53 #include <visp3/core/vpIoTools.h> 54 #include <visp3/core/vpPoint.h> 55 #include <visp3/core/vpXmlParserCamera.h> 56 #include <visp3/core/vpIoTools.h> 57 #include <visp3/core/vpMeterPixelConversion.h> 58 #include <visp3/core/vpPixelMeterConversion.h> 59 #include <visp3/core/vpImageTools.h> 60 #include <visp3/gui/vpDisplayD3D.h> 61 #include <visp3/gui/vpDisplayGDI.h> 62 #include <visp3/gui/vpDisplayGTK.h> 63 #include <visp3/gui/vpDisplayOpenCV.h> 64 #include <visp3/gui/vpDisplayX.h> 65 #include <visp3/io/vpVideoReader.h> 67 #include "calibration-helper.hpp" 71 int main(
int argc,
const char **argv)
74 std::string opt_output_file_name =
"camera.xml";
76 const std::string opt_inputSettingsFile = argc > 1 ? argv[1] :
"default.cfg";
77 std::string opt_init_camera_xml_file;
78 std::string opt_camera_name =
"Camera";
80 for (
int i = 1; i < argc; i++) {
81 if (std::string(argv[i]) ==
"--init-from-xml")
82 opt_init_camera_xml_file = std::string(argv[i + 1]);
83 else if (std::string(argv[i]) ==
"--camera-name")
84 opt_camera_name = std::string(argv[i + 1]);
85 else if (std::string(argv[i]) ==
"--output")
86 opt_output_file_name = std::string(argv[i + 1]);
87 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
88 std::cout <<
"\nUsage: " << argv[0]
89 <<
" [<configuration file>.cfg] [--init-from-xml <camera-init.xml>]" 90 <<
" [--camera-name <name>] [--output <file.xml>] [--help] [-h] \n" << std::endl;
95 std::cout <<
"Settings from config file: " << argv[1] << std::endl;
96 if (!s.read(opt_inputSettingsFile)) {
97 std::cout <<
"Could not open the configuration file: \"" << opt_inputSettingsFile <<
"\"" << std::endl;
98 std::cout << std::endl <<
"Usage: " << argv[0] <<
" <configuration file>.cfg" << std::endl;
103 std::cout <<
"Invalid input detected. Application stopping. " << std::endl;
107 std::cout <<
"\nSettings from command line options: " << std::endl;
108 if (!opt_init_camera_xml_file.empty()) {
109 std::cout <<
"Init parameters: " << opt_init_camera_xml_file << std::endl;
111 std::cout <<
"Ouput xml file : " << opt_output_file_name << std::endl;
112 std::cout <<
"Camera name : " << opt_camera_name << std::endl;
116 std::cout <<
"\nOutput file name " << opt_output_file_name <<
" already exists." << std::endl;
117 std::cout <<
"Remove this file or change output file name using [--output <file.xml>] command line option." << std::endl;
130 std::cout <<
"Check if input images name \"" << s.input <<
"\" set in " << opt_inputSettingsFile <<
" config file is correct..." << std::endl;
136 #elif defined VISP_HAVE_GDI 138 #elif defined VISP_HAVE_GTK 140 #elif defined VISP_HAVE_OPENCV 145 bool init_from_xml =
false;
146 if (! opt_init_camera_xml_file.empty()) {
148 std::cout <<
"Input camera file \"" << opt_init_camera_xml_file <<
"\" doesn't exist!" << std::endl;
149 std::cout <<
"Modify [--init-from-xml <camera-init.xml>] option value" << std::endl;
152 init_from_xml =
true;
155 std::cout <<
"Initialize camera parameters from xml file: " << opt_init_camera_xml_file << std::endl;
158 std::cout <<
"Unable to find camera with name \"" << opt_camera_name <<
"\" in file: " << opt_init_camera_xml_file << std::endl;
159 std::cout <<
"Modify [--camera-name <name>] option value" << std::endl;
163 std::cout <<
"Initialize camera parameters with default values " << std::endl;
165 double px = cam_init.
get_px();
166 double py = cam_init.
get_py();
173 std::cout <<
"Camera parameters used for initialization:\n" << cam_init << std::endl;
175 std::vector<vpPoint> model;
176 std::vector<vpCalibration> calibrator;
178 for (
int i = 0; i < s.boardSize.height; i++) {
179 for (
int j = 0; j < s.boardSize.width; j++) {
180 model.push_back(
vpPoint(j * s.squareSize, i * s.squareSize, 0));
184 std::vector<CalibInfo> calib_info;
185 std::multimap< double, vpCameraParameters, std::less<double> > map_cam_sorted;
187 map_cam_sorted.insert(std::make_pair(1000, cam_init));
192 char filename[FILENAME_MAX];
193 sprintf(filename, s.input.c_str(), frame_index);
199 std::vector<cv::Point2f> pointBuf;
202 std::cout <<
"Process frame: " << frame_name << std::flush;
203 bool found = extractCalibrationPoints(s, cvI, pointBuf);
205 std::cout <<
", grid detection status: " << found;
207 std::cout <<
", image rejected" << std::endl;
209 std::cout <<
", image used as input data" << std::endl;
214 std::vector<vpImagePoint> data;
215 for (
unsigned int i = 0; i < pointBuf.size(); i++) {
222 std::vector<vpPoint> calib_points;
225 for (
unsigned int i = 0; i < model.size(); i++) {
226 calib.
addPoint(model[i].get_oX(), model[i].get_oY(), model[i].get_oZ(), data[i]);
227 calib_points.push_back(
vpPoint(model[i].get_oX(), model[i].get_oY(), model[i].get_oZ()));
228 calib_points.back().set_x(data[i].get_u());
229 calib_points.back().set_y(data[i].get_v());
233 bool calib_status =
false;
234 std::multimap<double, vpCameraParameters>::const_iterator it_cam;
235 for (it_cam = map_cam_sorted.begin(); it_cam != map_cam_sorted.end(); ++ it_cam) {
238 calibrator.push_back(calib);
240 calib_info.push_back(CalibInfo(I, calib_points, data, frame_name));
243 map_cam_sorted.insert(std::make_pair(residual, cam));
247 if (! calib_status) {
248 std::cout <<
"frame: " << frame_name <<
", unable to calibrate from single image, image rejected" << std::endl;
260 if (s.tempo > 10.f) {
269 }
while (!reader.
end());
273 if (calibrator.empty()) {
274 std::cerr <<
"Unable to calibrate. Image processing failed !" << std::endl;
279 drawCalibrationOccupancy(I, calib_info, s.boardSize.width);
287 std::stringstream ss_additional_info;
289 ss_additional_info <<
"<nb_calibration_images>" << calibrator.size() <<
"</nb_calibration_images>";
290 ss_additional_info <<
"<calibration_pattern_type>";
292 switch (s.calibrationPattern) {
293 case Settings::CHESSBOARD:
294 ss_additional_info <<
"Chessboard";
297 case Settings::CIRCLES_GRID:
298 ss_additional_info <<
"Circles grid";
301 case Settings::UNDEFINED:
303 ss_additional_info <<
"Undefined";
306 ss_additional_info <<
"</calibration_pattern_type>";
307 ss_additional_info <<
"<board_size>" << s.boardSize.width <<
"x" << s.boardSize.height <<
"</board_size>";
308 ss_additional_info <<
"<square_size>" << s.squareSize <<
"</square_size>";
313 std::cout <<
"\nCalibration without distortion in progress on " << calibrator.size() <<
" images..." << std::endl;
315 std::cout << cam << std::endl;
318 for (
size_t i = 0; i < calibrator.size(); i++) {
319 double reproj_error = sqrt(calibrator[i].getResidual()/calibrator[i].get_npt());
321 const CalibInfo& calib = calib_info[i];
322 std::cout <<
"Image " << calib.m_frame_name <<
" reprojection error: " << reproj_error << std::endl;
326 std::ostringstream ss;
327 ss <<
"Reprojection error: " << reproj_error;
333 for (
size_t idx = 0; idx < calib.m_points.size(); idx++) {
336 vpPoint pt = calib.m_points[idx];
348 std::cout <<
"\nGlobal reprojection error: " << error << std::endl;
349 ss_additional_info <<
"<global_reprojection_error><without_distortion>" << error <<
"</without_distortion>";
355 std::cout <<
"Camera parameters without distortion successfully saved in \"" << opt_output_file_name <<
"\"" 358 std::cout <<
"Failed to save the camera parameters without distortion in \"" << opt_output_file_name <<
"\"" 360 std::cout <<
"A file with the same name exists. Remove it to be able " 361 "to save the parameters..." 365 std::cout <<
"Calibration without distortion failed." << std::endl;
369 std::vector<vpCalibration> calibrator_without_dist = calibrator;
371 std::cout <<
"\n\nCalibration with distortion in progress on " << calibrator.size() <<
" images..." << std::endl;
374 std::cout << cam << std::endl;
377 for (
size_t i = 0; i < calibrator.size(); i++) {
378 double reproj_error = sqrt(calibrator[i].getResidual_dist()/calibrator[i].get_npt());
380 const CalibInfo& calib = calib_info[i];
381 std::cout <<
"Image " << calib.m_frame_name <<
" reprojection error: " << reproj_error << std::endl;
385 std::ostringstream ss;
386 ss <<
"Reprojection error: " << reproj_error;
392 for (
size_t idx = 0; idx < calib.m_points.size(); idx++) {
395 vpPoint pt = calib.m_points[idx];
396 pt.
project(calibrator[i].cMo_dist);
407 std::cout <<
"\nGlobal reprojection error: " << error << std::endl;
408 ss_additional_info <<
"<with_distortion>" << error <<
"</with_distortion></global_reprojection_error>";
413 #elif defined VISP_HAVE_GDI 415 #elif defined VISP_HAVE_GTK 417 #elif defined VISP_HAVE_OPENCV 423 for (
size_t idx = 0; idx < calib_info.size(); idx++) {
424 std::cout <<
"\nThis tool computes the line fitting error (mean distance error) on image points extracted from the raw distorted image." << std::endl;
426 I = calib_info[idx].m_img;
434 std::vector<vpImagePoint> grid_points = calib_info[idx].m_imPts;
435 for (
int i = 0; i < s.boardSize.height; i++) {
436 std::vector<vpImagePoint> current_line(grid_points.begin() + i*s.boardSize.width,
437 grid_points.begin() + (i+1)*s.boardSize.width);
439 std::vector<vpImagePoint> current_line_undist = undistort(cam, current_line);
440 double a = 0, b = 0, c = 0;
443 std::cout << calib_info[idx].m_frame_name <<
" line " << i + 1 <<
" fitting error on distorted points: " << line_fitting_error
444 <<
" ; on undistorted points: " << line_fitting_error_undist << std::endl;
451 std::cout <<
"\nThis tool computes the line fitting error (mean distance error) on image points extracted from the undistorted image" 452 <<
" (vpImageTools::undistort())." << std::endl;
454 std::vector<cv::Point2f> pointBuf;
457 bool found = extractCalibrationPoints(s, cvI, pointBuf);
459 std::vector<vpImagePoint> grid_points;
460 for (
unsigned int i = 0; i < pointBuf.size(); i++) {
462 grid_points.push_back(ip);
467 for (
int i = 0; i < s.boardSize.height; i++) {
468 std::vector<vpImagePoint> current_line(grid_points.begin() + i*s.boardSize.width,
469 grid_points.begin() + (i+1)*s.boardSize.width);
471 double a = 0, b = 0, c = 0;
473 std::cout << calib_info[idx].m_frame_name <<
" undistorted image, line " << i + 1 <<
" fitting error: " << line_fitting_error << std::endl;
481 std::string msg(
"Unable to detect grid on undistorted image");
482 std::cout << msg << std::endl;
483 std::cout <<
"Check that the grid is not too close to the image edges" << std::endl;
494 std::cout << std::endl;
498 ss_additional_info <<
"<camera_poses>";
499 for (
size_t i = 0; i < calibrator.size(); i++) {
501 ss_additional_info <<
"<cMo>" << pose.t() <<
"</cMo>";
503 for (
size_t i = 0; i < calibrator.size(); i++) {
505 ss_additional_info <<
"<cMo_dist>" << pose.t() <<
"</cMo_dist>";
507 ss_additional_info <<
"</camera_poses>";
509 if (xml.
save(cam, opt_output_file_name.c_str(), opt_camera_name, I.
getWidth(), I.
getHeight(), ss_additional_info.str()) ==
511 std::cout <<
"Camera parameters without distortion successfully saved in \"" << opt_output_file_name <<
"\"" 514 std::cout <<
"Failed to save the camera parameters without distortion in \"" << opt_output_file_name <<
"\"" 516 std::cout <<
"A file with the same name exists. Remove it to be able " 517 "to save the parameters..." 520 std::cout << std::endl;
521 std::cout <<
"Estimated pose using vpPoseVector format: [tx ty tz tux tuy tuz] with translation in meter and rotation in rad" << std::endl;
522 for (
unsigned int i = 0; i < calibrator.size(); i++)
523 std::cout <<
"Estimated pose on input data extracted from " << calib_info[i].m_frame_name <<
": " <<
vpPoseVector(calibrator[i].cMo_dist).
t()
527 std::cout <<
"Calibration with distortion failed." << std::endl;
533 std::cout <<
"Catch an exception: " << e << std::endl;
540 std::cout <<
"OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
541 std::cout <<
"Tip:" << std::endl;
542 std::cout <<
"- Install OpenCV, configure again ViSP using cmake and build again this example" << std::endl;
VISP_EXPORT int wait(double t0, double t)
long getFrameIndex() const
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
unsigned int getWidth() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
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.
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Display for windows using GDI (available on any windows 32 platform).
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
error that can be emited by ViSP classes.
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
double get_y() const
Get the point y coordinate in the image plane.
Tools for perspective camera calibration.
XML parser to load and save intrinsic camera parameters.
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
static void setLambda(const double &lambda)
set the gain for the virtual visual servoing algorithm
void open(vpImage< vpRGBa > &I)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void acquire(vpImage< vpRGBa > &I)
double get_x() const
Get the point x coordinate in the image plane.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
unsigned int getDownScalingFactor()
Implementation of a pose vector and operations on poses.
int save(const vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, unsigned int image_width=0, unsigned int image_height=0, const std::string &additionalInfo="")
void setFileName(const std::string &filename)
unsigned int getHeight() const
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam, double &globalReprojectionError, bool verbose=false)
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
double getResidual(void) const
get the residual in pixels