Visual Servoing Platform version 3.5.0
tutorial-chessboard-pose.cpp
1
2#include <iostream>
3
4#include <visp3/core/vpConfig.h>
5
6#if VISP_HAVE_OPENCV_VERSION >= 0x020300
7
8#include <opencv2/core/core.hpp>
9#include <opencv2/imgproc/imgproc.hpp>
10#include <opencv2/calib3d/calib3d.hpp>
11#include <opencv2/highgui/highgui.hpp>
12
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>
23
24namespace {
25 void calcChessboardCorners(int width, int height, double squareSize, std::vector<vpPoint> &corners) {
26 corners.resize(0);
27
28 for (int i = 0; i < height; i++) {
29 for (int j = 0; j < width; j++) {
30 vpPoint pt;
31 pt.set_oX(j*squareSize);
32 pt.set_oY(i*squareSize);
33 pt.set_oZ(0.0);
34 corners.push_back(pt);
35 }
36 }
37 }
38
39 void usage(const char **argv, int error)
40 {
41 std::cout << "Synopsis" << std::endl
42 << " " << argv[0]
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;
63 if (error) {
64 std::cout << "Error" << std::endl
65 << " " << "Unsupported parameter " << argv[error] << std::endl;
66 }
67 }
68} //namespace
69
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";
76
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]);
80 i ++;
81 } else if (std::string(argv[i]) == "-h" && i+1 < argc) {
82 chessboard_height = atoi(argv[i+1]);
83 i ++;
84 } else if (std::string(argv[i]) == "--square_size" && i+1 < argc) {
85 chessboard_square_size = atof(argv[i+1]);
86 i ++;
87 } else if (std::string(argv[i]) == "--input" && i+1 < argc) {
88 input_filename = std::string(argv[i+1]);
89 i ++;
90 } else if (std::string(argv[i]) == "--intrinsic" && i+1 < argc) {
91 intrinsic_file = std::string(argv[i+1]);
92 i ++;
93 } else if (std::string(argv[i]) == "--camera_name" && i+1 < argc) {
94 camera_name = std::string(argv[i+1]);
95 i ++;
96 }
97 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
98 usage(argv, 0);
99 return EXIT_SUCCESS;
100 }
101 else {
102 usage(argv, i);
103 return EXIT_FAILURE;
104 }
105 }
106
107 if (! vpIoTools::checkFilename(intrinsic_file)) {
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;
110 return EXIT_SUCCESS;
111 }
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;
119
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;
123 return EXIT_FAILURE;
124 }
125
126 try {
127 vpVideoReader reader;
128 reader.setFileName(input_filename);
129
131 reader.open(I);
132
133#ifdef VISP_HAVE_X11
134 vpDisplayX d(I);
135#elif defined VISP_HAVE_GDI
136 vpDisplayGDI d(I);
137#elif defined VISP_HAVE_OPENCV
138 vpDisplayOpenCV d(I);
139#endif
140
141 std::vector<vpPoint> corners_pts;
142 calcChessboardCorners(chessboard_width, chessboard_height, chessboard_square_size, corners_pts);
143
145 vpXmlParserCamera parser;
146 if (!intrinsic_file.empty() && !camera_name.empty()) {
147 if (parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithDistortion) != vpXmlParserCamera::SEQUENCE_OK) {
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;
150
151 if (parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion) != vpXmlParserCamera::SEQUENCE_OK) {
152 std::cout << "Unable to parse parameters without distorsion for camera \"" << camera_name << "\" from " << intrinsic_file << " file" << std::endl;
153 return EXIT_FAILURE;
154 }
155 }
156 }
157 std::cout << "Camera parameters used to compute the pose:\n" << cam << std::endl;
158
159 bool quit = false;
160 do {
161 reader.acquire(I);
163
164 cv::Mat matImg;
165 vpImageConvert::convert(I, matImg);
166
167 vpDisplay::displayText(I, 20, 20, "Right click to quit.", vpColor::red);
168
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);
174#else
175 CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
176#endif
177
179 if (found) {
180 cv::Mat matImg_gray;
181 cv::cvtColor(matImg, matImg_gray, cv::COLOR_BGR2GRAY);
182 cv::cornerSubPix(matImg_gray, corners2D, cv::Size(11,11),
183 cv::Size(-1,-1),
184 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
185 cv::TermCriteria( cv::TermCriteria::EPS+cv::TermCriteria::COUNT, 30, 0.1 ));
186#else
187 cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
188#endif
189
190 for (size_t i = 0; i < corners_pts.size(); i++) {
191 vpImagePoint imPt(corners2D[i].y, corners2D[i].x);
192 double x = 0.0, y = 0.0;
194 corners_pts[i].set_x(x);
195 corners_pts[i].set_y(y);
196 }
197
198 vpPose pose;
199 pose.addPoints(corners_pts);
200 vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
201 double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
202 bool pose_dementhon = pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
203 if (pose_dementhon)
204 r_dementhon = pose.computeResidual(cMo_dementhon);
205
206 bool pose_lagrange = pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
207 if (pose_lagrange)
208 r_lagrange = pose.computeResidual(cMo_lagrange);
209
210 cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
211 if (!pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
212 std::cerr << "Problem when computing final pose using VVS" << std::endl;
213 return EXIT_FAILURE;
214 }
215
216 cv::drawChessboardCorners(matImg, chessboardSize, corners2D, found);
217 vpImageConvert::convert(matImg, I);
218 }
219
221
222 vpDisplay::displayText(I, 20, 20, "Left click for the next image, right click to quit.", vpColor::red);
223 if (found)
224 vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
225
227
228 if (found) {
229 vpPoseVector pose_vec(cMo);
230 std::stringstream ss;
231 ss << "pose_cPo_" << reader.getFrameIndex() << ".yaml";
232 std::cout << "Save " << ss.str() << std::endl;
233 pose_vec.saveYAML(ss.str(), pose_vec);
234 }
235
237 if (vpDisplay::getClick(I, button, true)) {
238 switch (button) {
240 quit = true;
241 break;
242
243 default:
244 break;
245 }
246 }
247 } while (!quit && !reader.end());
248 }
249 catch (const vpException &e) {
250 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
251 }
252
253 return EXIT_SUCCESS;
254}
255#else
256int main() {
257 std::cerr << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
258 return EXIT_SUCCESS;
259}
260#endif
261
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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...
Definition: vpDisplayX.h:135
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.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:90
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: vpImagePoint.h:88
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:802
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 ...
Definition: vpPoint.h:82
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:504
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:506
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:502
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:81
@ DEMENTHON
Definition: vpPose.h:86
@ VIRTUAL_VS
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:85
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:164
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:336
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
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)