4#include <visp3/core/vpConfig.h>
6#if VISP_HAVE_OPENCV_VERSION >= 0x020300
8#include <opencv2/core/core.hpp>
9#include <opencv2/imgproc/imgproc.hpp>
10#include <opencv2/calib3d/calib3d.hpp>
11#include <opencv2/highgui/highgui.hpp>
13#include <visp3/gui/vpDisplayX.h>
14#include <visp3/gui/vpDisplayGDI.h>
15#include <visp3/gui/vpDisplayOpenCV.h>
16#include <visp3/gui/vpDisplayD3D.h>
17#include <visp3/core/vpIoTools.h>
18#include <visp3/core/vpPoint.h>
19#include <visp3/core/vpPixelMeterConversion.h>
20#include <visp3/core/vpXmlParserCamera.h>
21#include <visp3/io/vpVideoReader.h>
22#include <visp3/vision/vpPose.h>
25 void calcChessboardCorners(
int width,
int height,
double squareSize, std::vector<vpPoint> &corners) {
28 for (
int i = 0; i < height; i++) {
29 for (
int j = 0; j < width; j++) {
34 corners.push_back(pt);
39 void usage(
const char **argv,
int error)
41 std::cout <<
"Synopsis" << std::endl
43 <<
" [-w <chessboard width>] [-h <chessboard height>]"
44 <<
" [--square_size <square size in meter>]"
45 <<
" [--input <input images path>]"
46 <<
" [--intrinsic <Camera intrinsic parameters xml file>]"
47 <<
" [--camera_name <Camera name in the xml intrinsic file>]" << std::endl << std::endl;
48 std::cout <<
"Description" << std::endl
49 <<
" -w <chessboard width> Chessboard width." << std::endl
50 <<
" Default: 9." << std::endl << std::endl
51 <<
" -h <chessboard height> Chessboard height." << std::endl
52 <<
" Default: 6." << std::endl << std::endl
53 <<
" --square_size <square size in meter> Chessboard square size in [m]." << std::endl
54 <<
" Default: 0.03." << std::endl << std::endl
55 <<
" --input <input images path> Generic name of the images to process." << std::endl
56 <<
" Example: \"image-%02d.png\"." << std::endl << std::endl
57 <<
" --intrinsic <Camera intrinsic parameters xml file> XML file that contains" << std::endl
58 <<
" camera parameters. " << std::endl
59 <<
" Default: \"camera.xml\"." << std::endl << std::endl
60 <<
" --camera_name <Camera name in the xml intrinsic file> Camera name in the XML file." << std::endl
61 <<
" Default: \"Camera\"." << std::endl << std::endl
62 <<
" --help, -h Print this helper message." << std::endl << std::endl;
64 std::cout <<
"Error" << std::endl
65 <<
" " <<
"Unsupported parameter " << argv[error] << std::endl;
70int main(
int argc,
const char **argv) {
71 int chessboard_width = 9, chessboard_height = 6;
72 double chessboard_square_size = 0.03;
73 std::string input_filename =
"";
74 std::string intrinsic_file =
"camera.xml";
75 std::string camera_name =
"Camera";
77 for (
int i = 1; i < argc; i++) {
78 if (std::string(argv[i]) ==
"-w" && i+1 < argc) {
79 chessboard_width = atoi(argv[i+1]);
81 }
else if (std::string(argv[i]) ==
"-h" && i+1 < argc) {
82 chessboard_height = atoi(argv[i+1]);
84 }
else if (std::string(argv[i]) ==
"--square_size" && i+1 < argc) {
85 chessboard_square_size = atof(argv[i+1]);
87 }
else if (std::string(argv[i]) ==
"--input" && i+1 < argc) {
88 input_filename = std::string(argv[i+1]);
90 }
else if (std::string(argv[i]) ==
"--intrinsic" && i+1 < argc) {
91 intrinsic_file = std::string(argv[i+1]);
93 }
else if (std::string(argv[i]) ==
"--camera_name" && i+1 < argc) {
94 camera_name = std::string(argv[i+1]);
97 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
108 std::cout <<
"Camera parameters file " << intrinsic_file <<
" doesn't exist." << std::endl;
109 std::cout <<
"Use --help option to see how to set its location..." << std::endl;
112 std::cout <<
"Parameters:" << std::endl;
113 std::cout <<
" chessboard width : " << chessboard_width << std::endl;
114 std::cout <<
" chessboard height : " << chessboard_height << std::endl;
115 std::cout <<
" chessboard square size [m] : " << chessboard_square_size << std::endl;
116 std::cout <<
" input images location : " << input_filename << std::endl;
117 std::cout <<
" camera param file name [.xml]: " << intrinsic_file << std::endl;
118 std::cout <<
" camera name : " << camera_name << std::endl << std::endl;
120 if (input_filename.empty()) {
121 std::cout <<
"Input images location empty." << std::endl;
122 std::cout <<
"Use --help option to see how to set input image location..." << std::endl;
135#elif defined VISP_HAVE_GDI
137#elif defined VISP_HAVE_OPENCV
141 std::vector<vpPoint> corners_pts;
142 calcChessboardCorners(chessboard_width, chessboard_height, chessboard_square_size, corners_pts);
146 if (!intrinsic_file.empty() && !camera_name.empty()) {
148 std::cout <<
"Unable to parse parameters with distorsion for camera \"" << camera_name <<
"\" from " << intrinsic_file <<
" file" << std::endl;
149 std::cout <<
"Attempt to find parameters without distorsion" << std::endl;
152 std::cout <<
"Unable to parse parameters without distorsion for camera \"" << camera_name <<
"\" from " << intrinsic_file <<
" file" << std::endl;
157 std::cout <<
"Camera parameters used to compute the pose:\n" << cam << std::endl;
169 cv::Size chessboardSize(chessboard_width, chessboard_height);
170 std::vector<cv::Point2f> corners2D;
171 bool found = cv::findChessboardCorners(matImg, chessboardSize, corners2D,
172 #
if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
173 cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
175 CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
181 cv::cvtColor(matImg, matImg_gray, cv::COLOR_BGR2GRAY);
182 cv::cornerSubPix(matImg_gray, corners2D, cv::Size(11,11),
184 #
if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
185 cv::TermCriteria( cv::TermCriteria::EPS+cv::TermCriteria::COUNT, 30, 0.1 ));
187 cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
190 for (
size_t i = 0; i < corners_pts.size(); i++) {
192 double x = 0.0, y = 0.0;
194 corners_pts[i].set_x(x);
195 corners_pts[i].set_y(y);
201 double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
210 cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
212 std::cerr <<
"Problem when computing final pose using VVS" << std::endl;
216 cv::drawChessboardCorners(matImg, chessboardSize, corners2D, found);
230 std::stringstream ss;
232 std::cout <<
"Save " << ss.str() << std::endl;
233 pose_vec.saveYAML(ss.str(), pose_vec);
247 }
while (!quit && !reader.
end());
250 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
257 std::cerr <<
"OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
@ perspectiveProjWithoutDistortion
static const vpColor none
Display for windows using GDI (available on any windows 32 platform).
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...
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void flush(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
const char * getMessage() 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 ...
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_oY(double oY)
Set the point oY coordinate in the object frame.
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Implementation of a pose vector and operations on poses.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void addPoints(const std::vector< vpPoint > &lP)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
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)
std::string getFrameName() const
long getFrameIndex() const
XML parser to load and save intrinsic camera parameters.
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)