37#include <visp3/core/vpConfig.h>
39#if (VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(HAVE_OPENCV_CALIB3D) && defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_IMGPROC)
43#include <opencv2/calib3d/calib3d.hpp>
44#include <opencv2/core/core.hpp>
45#include <opencv2/highgui/highgui.hpp>
46#include <opencv2/imgproc/imgproc.hpp>
48#include <visp3/vision/vpCalibration.h>
50#include <visp3/core/vpImageTools.h>
51#include <visp3/core/vpIoTools.h>
52#include <visp3/core/vpMeterPixelConversion.h>
53#include <visp3/core/vpPixelMeterConversion.h>
54#include <visp3/core/vpPoint.h>
55#include <visp3/core/vpXmlParserCamera.h>
56#include <visp3/gui/vpDisplayD3D.h>
57#include <visp3/gui/vpDisplayGDI.h>
58#include <visp3/gui/vpDisplayGTK.h>
59#include <visp3/gui/vpDisplayOpenCV.h>
60#include <visp3/gui/vpDisplayX.h>
61#include <visp3/io/vpVideoReader.h>
63#include "calibration-helper.hpp"
65using namespace calib_helper;
67void usage(
const char *argv[],
int error)
69 std::cout <<
"Synopsis" << std::endl
70 <<
" " << argv[0] <<
" <configuration file>.cfg [--init-from-xml <camera-init.xml>]"
71 <<
" [--camera-name <name>] [--aspect-ratio <ratio>] [--output <file.xml>] [--help] [-h]" << std::endl
73 std::cout <<
"Description" << std::endl
74 <<
" <configuration file>.cfg Configuration file. See example in" << std::endl
75 <<
" \"default-chessboard.cfg\" or in \"default-circles.cfg\"." << std::endl
76 <<
" Default: \"default.cfg\"." << std::endl
78 <<
" --init-from-xml <camera-init.xml> XML file that contains camera parameters" << std::endl
79 <<
" used to initialize the calibration process." << std::endl
81 <<
" --camera-name <name> Camera name in the XML file set using \"--init-from-xml\" option." << std::endl
82 <<
" Default: \"Camera\"." << std::endl
84 <<
" --aspect-ratio <ratio> Pixel aspect ratio. " << std::endl
85 <<
" To estimate px = py, use \"--aspect-ratio 1\" option. Set to -1" << std::endl
86 <<
" to unset any constraint for px and py parameters. " << std::endl
87 <<
" Default: -1." << std::endl
89 <<
" --output <file.xml> XML file containing estimated camera parameters." << std::endl
90 <<
" Default: \"camera.xml\"." << std::endl
92 <<
" --help, -h Print this helper message." << std::endl
95 std::cout <<
"Error" << std::endl
97 <<
"Unsupported parameter " << argv[error] << std::endl;
101int main(
int argc,
const char *argv[])
108 std::string opt_output_file_name =
"camera.xml";
110 const std::string opt_inputSettingsFile = argc > 1 ? argv[1] :
"default.cfg";
111 std::string opt_init_camera_xml_file;
112 std::string opt_camera_name =
"Camera";
113 double opt_aspect_ratio = -1;
115 for (
int i = 2; i < argc; i++) {
116 if (std::string(argv[i]) ==
"--init-from-xml" && i + 1 < argc) {
117 opt_init_camera_xml_file = std::string(argv[i + 1]);
119 }
else if (std::string(argv[i]) ==
"--camera-name" && i + 1 < argc) {
120 opt_camera_name = std::string(argv[i + 1]);
122 }
else if (std::string(argv[i]) ==
"--output" && i + 1 < argc) {
123 opt_output_file_name = std::string(argv[i + 1]);
125 }
else if (std::string(argv[i]) ==
"--aspect-ratio" && i + 1 < argc) {
126 opt_aspect_ratio = std::atof(argv[i + 1]);
128 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
137 std::cout <<
"Settings from config file: " << argv[1] << std::endl;
138 if (!s.read(opt_inputSettingsFile)) {
139 std::cout <<
"Could not open the configuration file: \"" << opt_inputSettingsFile <<
"\"" << std::endl;
145 std::cout <<
"Invalid input detected. Application stopping. " << std::endl;
149 std::cout <<
"\nSettings from command line options: " << std::endl;
150 if (!opt_init_camera_xml_file.empty()) {
151 std::cout <<
"Init parameters: " << opt_init_camera_xml_file << std::endl;
153 std::cout <<
"Ouput xml file : " << opt_output_file_name << std::endl;
154 std::cout <<
"Camera name : " << opt_camera_name << std::endl;
158 std::cout <<
"\nOutput file name " << opt_output_file_name <<
" already exists." << std::endl;
159 std::cout <<
"Remove this file or change output file name using [--output <file.xml>] command line option."
172 std::cout <<
"Check if input images name \"" << s.input <<
"\" set in " << opt_inputSettingsFile
173 <<
" config file is correct..." << std::endl;
177#if defined(VISP_HAVE_X11)
179#elif defined(VISP_HAVE_GDI)
181#elif defined(VISP_HAVE_GTK)
183#elif defined(HAVE_OPENCV_HIGHGUI)
188 bool init_from_xml =
false;
189 if (!opt_init_camera_xml_file.empty()) {
191 std::cout <<
"Input camera file \"" << opt_init_camera_xml_file <<
"\" doesn't exist!" << std::endl;
192 std::cout <<
"Modify [--init-from-xml <camera-init.xml>] option value" << std::endl;
195 init_from_xml =
true;
198 std::cout <<
"Initialize camera parameters from xml file: " << opt_init_camera_xml_file << std::endl;
200 if (parser.
parse(cam_init, opt_init_camera_xml_file, opt_camera_name,
202 std::cout <<
"Unable to find camera with name \"" << opt_camera_name
203 <<
"\" in file: " << opt_init_camera_xml_file << std::endl;
204 std::cout <<
"Modify [--camera-name <name>] option value" << std::endl;
208 std::cout <<
"Initialize camera parameters with default values " << std::endl;
210 double px = cam_init.
get_px();
211 double py = cam_init.
get_py();
218 std::cout <<
"Camera parameters used for initialization:\n" << cam_init << std::endl;
220 std::vector<vpPoint> model;
221 std::vector<vpCalibration> calibrator;
223 for (
int i = 0; i < s.boardSize.height; i++) {
224 for (
int j = 0; j < s.boardSize.width; j++) {
225 model.push_back(
vpPoint(j * s.squareSize, i * s.squareSize, 0));
229 std::vector<CalibInfo> calib_info;
230 std::multimap<double, vpCameraParameters, std::less<double> > map_cam_sorted;
232 map_cam_sorted.insert(std::make_pair(1000, cam_init));
237 char filename[FILENAME_MAX];
238 snprintf(filename, FILENAME_MAX, s.input.c_str(), frame_index);
244 std::vector<cv::Point2f> pointBuf;
247 std::cout <<
"Process frame: " << frame_name << std::flush;
248 bool found = extractCalibrationPoints(s, cvI, pointBuf);
250 std::cout <<
", grid detection status: " << found;
252 std::cout <<
", image rejected" << std::endl;
254 std::cout <<
", image used as input data" << std::endl;
259 std::vector<vpImagePoint> data;
260 for (
unsigned int i = 0; i < pointBuf.size(); i++) {
267 std::vector<vpPoint> calib_points;
271 for (
unsigned int i = 0; i < model.size(); i++) {
272 calib.
addPoint(model[i].get_oX(), model[i].get_oY(), model[i].get_oZ(), data[i]);
273 calib_points.push_back(
vpPoint(model[i].get_oX(), model[i].get_oY(), model[i].get_oZ()));
274 calib_points.back().set_x(data[i].get_u());
275 calib_points.back().set_y(data[i].get_v());
279 bool calib_status =
false;
280 std::multimap<double, vpCameraParameters>::const_iterator it_cam;
281 for (it_cam = map_cam_sorted.begin(); it_cam != map_cam_sorted.end(); ++it_cam) {
284 calibrator.push_back(calib);
286 calib_info.push_back(CalibInfo(I, calib_points, data, frame_name));
289 map_cam_sorted.insert(std::make_pair(residual, cam));
294 std::cout <<
"frame: " << frame_name <<
", unable to calibrate from single image, image rejected"
307 if (s.tempo > 10.f) {
316 }
while (!reader.
end());
320 if (calibrator.empty()) {
321 std::cerr <<
"Unable to calibrate. Image processing failed !" << std::endl;
326 drawCalibrationOccupancy(I, calib_info, s.boardSize.width);
331 cv::applyColorMap(img, imgColor, cv::COLORMAP_JET);
334 for (
size_t idx1 = 0; idx1 < calib_info.size(); idx1++) {
335 const CalibInfo &calib = calib_info[idx1];
337 for (
size_t idx2 = 0; idx2 < calib.m_imPts.size(); idx2++) {
339 cv::rectangle(imgColor,
340 cv::Rect(
static_cast<int>(imPt.
get_u() - 2),
static_cast<int>(imPt.
get_v() - 2),
342 cv::Scalar(0, 0, 0), -1);
349 d.
init(I_color, 0, 0,
"Calibration pattern occupancy");
356 if (s.tempo > 10.f) {
370 std::stringstream ss_additional_info;
372 ss_additional_info <<
"<nb_calibration_images>" << calibrator.size() <<
"</nb_calibration_images>";
373 ss_additional_info <<
"<calibration_pattern_type>";
375 switch (s.calibrationPattern) {
376 case Settings::CHESSBOARD:
377 ss_additional_info <<
"Chessboard";
380 case Settings::CIRCLES_GRID:
381 ss_additional_info <<
"Circles grid";
384 case Settings::UNDEFINED:
386 ss_additional_info <<
"Undefined";
389 ss_additional_info <<
"</calibration_pattern_type>";
390 ss_additional_info <<
"<board_size>" << s.boardSize.width <<
"x" << s.boardSize.height <<
"</board_size>";
391 ss_additional_info <<
"<square_size>" << s.squareSize <<
"</square_size>";
396 std::cout <<
"\nCalibration without distortion in progress on " << calibrator.size() <<
" images..." << std::endl;
399 std::cout << cam << std::endl;
402 for (
size_t i = 0; i < calibrator.size(); i++) {
403 double reproj_error = sqrt(calibrator[i].getResidual() / calibrator[i].get_npt());
405 const CalibInfo &calib = calib_info[i];
406 std::cout <<
"Image " << calib.m_frame_name <<
" reprojection error: " << reproj_error << std::endl;
410 std::ostringstream ss;
411 ss <<
"Reprojection error: " << reproj_error;
421 for (
size_t idx = 0; idx < calib.m_points.size(); idx++) {
424 vpPoint pt = calib.m_points[idx];
431 if (s.tempo > 10.f) {
443 std::cout <<
"\nGlobal reprojection error: " << error << std::endl;
444 ss_additional_info <<
"<global_reprojection_error><without_distortion>" << error <<
"</without_distortion>";
450 std::cout <<
"Camera parameters without distortion successfully saved in \"" << opt_output_file_name <<
"\""
453 std::cout <<
"Failed to save the camera parameters without distortion in \"" << opt_output_file_name <<
"\""
455 std::cout <<
"A file with the same name exists. Remove it to be able "
456 "to save the parameters..."
460 std::cout <<
"Calibration without distortion failed." << std::endl;
464 std::vector<vpCalibration> calibrator_without_dist = calibrator;
466 std::cout <<
"\n\nCalibration with distortion in progress on " << calibrator.size() <<
" images..." << std::endl;
469 std::cout << cam << std::endl;
472 for (
size_t i = 0; i < calibrator.size(); i++) {
473 double reproj_error = sqrt(calibrator[i].getResidual_dist() / calibrator[i].get_npt());
475 const CalibInfo &calib = calib_info[i];
476 std::cout <<
"Image " << calib.m_frame_name <<
" reprojection error: " << reproj_error << std::endl;
480 std::ostringstream ss;
481 ss <<
"Reprojection error: " << reproj_error;
491 for (
size_t idx = 0; idx < calib.m_points.size(); idx++) {
494 vpPoint pt = calib.m_points[idx];
495 pt.
project(calibrator[i].cMo_dist);
501 if (s.tempo > 10.f) {
513 std::cout <<
"\nGlobal reprojection error: " << error << std::endl;
514 ss_additional_info <<
"<with_distortion>" << error <<
"</with_distortion></global_reprojection_error>";
519 d.
init(I_dist_undist, 0, 0,
"Straight lines have to be straight - distorted image / undistorted image");
521 for (
size_t idx = 0; idx < calib_info.size(); idx++) {
522 std::cout <<
"\nThis tool computes the line fitting error (mean distance error) on image points extracted from "
523 "the raw distorted image."
526 I = calib_info[idx].m_img;
535 calib_info[idx].m_frame_name + std::string(
" distorted"),
vpColor::red);
539 std::vector<vpImagePoint> grid_points = calib_info[idx].m_imPts;
540 for (
int i = 0; i < s.boardSize.height; i++) {
541 std::vector<vpImagePoint> current_line(grid_points.begin() + i * s.boardSize.width,
542 grid_points.begin() + (i + 1) * s.boardSize.width);
544 std::vector<vpImagePoint> current_line_undist = undistort(cam, current_line);
545 double a = 0, b = 0, c = 0;
548 std::cout << calib_info[idx].m_frame_name <<
" line " << i + 1
549 <<
" fitting error on distorted points: " << line_fitting_error
550 <<
" ; on undistorted points: " << line_fitting_error_undist << std::endl;
557 std::cout <<
"\nThis tool computes the line fitting error (mean distance error) on image points extracted from "
558 "the undistorted image"
559 <<
" (vpImageTools::undistort())." << std::endl;
561 std::vector<cv::Point2f> pointBuf;
564 bool found = extractCalibrationPoints(s, cvI, pointBuf);
566 std::vector<vpImagePoint> grid_points;
567 for (
unsigned int i = 0; i < pointBuf.size(); i++) {
569 grid_points.push_back(ip);
574 calib_info[idx].m_frame_name + std::string(
" undistorted"),
vpColor::red);
575 for (
int i = 0; i < s.boardSize.height; i++) {
576 std::vector<vpImagePoint> current_line(grid_points.begin() + i * s.boardSize.width,
577 grid_points.begin() + (i + 1) * s.boardSize.width);
579 double a = 0, b = 0, c = 0;
581 std::cout << calib_info[idx].m_frame_name <<
" undistorted image, line " << i + 1
582 <<
" fitting error: " << line_fitting_error << std::endl;
589 std::string msg(
"Unable to detect grid on undistorted image");
590 std::cout << msg << std::endl;
591 std::cout <<
"Check that the grid is not too close to the image edges" << std::endl;
594 calib_info[idx].m_frame_name + std::string(
" undistorted"),
vpColor::red);
599 if (s.tempo > 10.f) {
612 std::cout << std::endl;
616 ss_additional_info <<
"<camera_poses>";
617 for (
size_t i = 0; i < calibrator.size(); i++) {
619 ss_additional_info <<
"<cMo>" << pose.t() <<
"</cMo>";
621 for (
size_t i = 0; i < calibrator.size(); i++) {
623 ss_additional_info <<
"<cMo_dist>" << pose.t() <<
"</cMo_dist>";
625 ss_additional_info <<
"</camera_poses>";
629 std::cout <<
"Camera parameters without distortion successfully saved in \"" << opt_output_file_name <<
"\""
632 std::cout <<
"Failed to save the camera parameters without distortion in \"" << opt_output_file_name <<
"\""
634 std::cout <<
"A file with the same name exists. Remove it to be able "
635 "to save the parameters..."
638 std::cout << std::endl;
639 std::cout <<
"Estimated pose using vpPoseVector format: [tx ty tz tux tuy tuz] with translation in meter and "
642 for (
unsigned int i = 0; i < calibrator.size(); i++)
643 std::cout <<
"Estimated pose on input data extracted from " << calib_info[i].m_frame_name <<
": "
646 std::cout <<
"Calibration with distortion failed." << std::endl;
650 std::cout <<
"\nCamera calibration succeeded. Results are savec in " <<
"\"" << opt_output_file_name <<
"\"" << std::endl;
653 std::cout <<
"Catch an exception: " << e << std::endl;
660 std::cout <<
"OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
661 std::cout <<
"Tip:" << std::endl;
662 std::cout <<
"- Install OpenCV, configure again ViSP using cmake and build again this example" << std::endl;
Tools for perspective camera calibration.
static void setLambda(const double &lambda)
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose=false)
double getResidual(void) const
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose=false)
void setAspectRatio(double aspect_ratio)
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor green
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static void close(vpImage< unsigned char > &I)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void flush(const vpImage< unsigned char > &I)
unsigned int getDownScalingFactor()
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
const std::string & getStringMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
static double lineFitting(const std::vector< vpImagePoint > &imPts, double &a, double &b, double &c)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_y() const
Get the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
Implementation of a pose vector and operations on poses.
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
long getFrameIndex() const
XML parser to load and save intrinsic camera parameters.
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="", bool verbose=true)
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, bool verbose=true)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")