Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-chessboard-pose.cpp
1
2#include <iostream>
3
4#include <visp3/core/vpConfig.h>
5
6#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
7
8#include <opencv2/calib3d/calib3d.hpp>
9#include <opencv2/core/core.hpp>
10#include <opencv2/highgui/highgui.hpp>
11#include <opencv2/imgproc/imgproc.hpp>
12
13#include <visp3/core/vpIoTools.h>
14#include <visp3/core/vpPixelMeterConversion.h>
15#include <visp3/core/vpPoint.h>
16#include <visp3/core/vpXmlParserCamera.h>
17#include <visp3/gui/vpDisplayD3D.h>
18#include <visp3/gui/vpDisplayGDI.h>
19#include <visp3/gui/vpDisplayOpenCV.h>
20#include <visp3/gui/vpDisplayX.h>
21#include <visp3/io/vpVideoReader.h>
22#include <visp3/vision/vpPose.h>
23
24namespace
25{
26void calcChessboardCorners(int width, int height, double squareSize, std::vector<vpPoint> &corners)
27{
28 corners.resize(0);
29
30 for (int i = 0; i < height; i++) {
31 for (int j = 0; j < width; j++) {
32 vpPoint pt;
33 pt.set_oX(j * squareSize);
34 pt.set_oY(i * squareSize);
35 pt.set_oZ(0.0);
36 corners.push_back(pt);
37 }
38 }
39}
40
41void usage(const char **argv, int error)
42{
43 std::cout << "Synopsis" << std::endl
44 << " " << argv[0] << " [-w <chessboard width>] [-h <chessboard height>]"
45 << " [--square_size <square size in meter>]"
46 << " [--input <input images path>]"
47 << " [--intrinsic <Camera intrinsic parameters xml file>]"
48 << " [--camera_name <Camera name in the xml intrinsic file>]"
49 << " [--output <camera pose files>]"
50 << " [--help] [-h]" << std::endl
51 << std::endl;
52 std::cout << "Description" << std::endl
53 << " -w <chessboard width> Chessboard width." << std::endl
54 << " Default: 9." << std::endl
55 << std::endl
56 << " -h <chessboard height> Chessboard height." << std::endl
57 << " Default: 6." << std::endl
58 << std::endl
59 << " --square_size <square size in meter> Chessboard square size in [m]." << std::endl
60 << " Default: 0.03." << std::endl
61 << std::endl
62 << " --input <input images path> Generic name of the images to process." << std::endl
63 << " Example: \"image-%02d.png\"." << std::endl
64 << std::endl
65 << " --intrinsic <Camera intrinsic parameters xml file> XML file that contains" << std::endl
66 << " camera parameters. " << std::endl
67 << " Default: \"camera.xml\"." << std::endl
68 << std::endl
69 << " --camera_name <Camera name in the xml intrinsic file> Camera name in the XML file." << std::endl
70 << " Default: \"Camera\"." << std::endl
71 << std::endl
72 << " --output <camera pose files> Generic name of the yaml files that contains the camera poses."
73 << std::endl
74 << " Example: \"pose_cPo_%d.yaml\"." << std::endl
75 << std::endl
76 << " --help, -h Print this helper message." << std::endl
77 << std::endl;
78 if (error) {
79 std::cout << "Error" << std::endl
80 << " "
81 << "Unsupported parameter " << argv[error] << std::endl;
82 }
83}
84} // namespace
85
86int main(int argc, const char **argv)
87{
88 int opt_chessboard_width = 9, opt_chessboard_height = 6;
89 double opt_chessboard_square_size = 0.03;
90 std::string opt_input_img_files = "";
91 std::string opt_intrinsic_file = "camera.xml";
92 std::string opt_camera_name = "Camera";
93 std::string opt_output_pose_files = "pose_cPo_%d.yaml";
94
95 for (int i = 1; i < argc; i++) {
96 if (std::string(argv[i]) == "-w" && i + 1 < argc) {
97 opt_chessboard_width = atoi(argv[i + 1]);
98 i++;
99 }
100 else if (std::string(argv[i]) == "-h" && i + 1 < argc) {
101 opt_chessboard_height = atoi(argv[i + 1]);
102 i++;
103 }
104 else if (std::string(argv[i]) == "--square_size" && i + 1 < argc) {
105 opt_chessboard_square_size = atof(argv[i + 1]);
106 i++;
107 }
108 else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
109 opt_input_img_files = std::string(argv[i + 1]);
110 i++;
111 }
112 else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
113 opt_intrinsic_file = std::string(argv[i + 1]);
114 i++;
115 }
116 else if (std::string(argv[i]) == "--output" && i + 1 < argc) {
117 opt_output_pose_files = std::string(argv[i + 1]);
118 i++;
119 }
120 else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
121 opt_camera_name = std::string(argv[i + 1]);
122 i++;
123 }
124 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
125 usage(argv, 0);
126 return EXIT_SUCCESS;
127 }
128 else {
129 usage(argv, i);
130 return EXIT_FAILURE;
131 }
132 }
133
134 if (!vpIoTools::checkFilename(opt_intrinsic_file)) {
135 std::cout << "Camera parameters file " << opt_intrinsic_file << " doesn't exist." << std::endl;
136 std::cout << "Use --help option to see how to set its location..." << std::endl;
137 return EXIT_SUCCESS;
138 }
139 std::cout << "Parameters:" << std::endl;
140 std::cout << " chessboard width : " << opt_chessboard_width << std::endl;
141 std::cout << " chessboard height : " << opt_chessboard_height << std::endl;
142 std::cout << " chessboard square size [m] : " << opt_chessboard_square_size << std::endl;
143 std::cout << " input images location : " << opt_input_img_files << std::endl;
144 std::cout << " camera param file name [.xml]: " << opt_intrinsic_file << std::endl;
145 std::cout << " camera name : " << opt_camera_name << std::endl;
146 std::cout << " output camera poses : " << opt_output_pose_files << std::endl << std::endl;
147
148 if (opt_input_img_files.empty()) {
149 std::cout << "Input images location empty." << std::endl;
150 std::cout << "Use --help option to see how to set input image location..." << std::endl;
151 return EXIT_FAILURE;
152 }
153
154 try {
155 vpVideoReader reader;
156 reader.setFileName(opt_input_img_files);
157
159 reader.open(I);
160
161#ifdef VISP_HAVE_X11
162 vpDisplayX d(I);
163#elif defined(VISP_HAVE_GDI)
164 vpDisplayGDI d(I);
165#elif defined(HAVE_OPENCV_HIGHGUI)
166 vpDisplayOpenCV d(I);
167#endif
168
169 std::vector<vpPoint> corners_pts;
170 calcChessboardCorners(opt_chessboard_width, opt_chessboard_height, opt_chessboard_square_size, corners_pts);
171
173 vpXmlParserCamera parser;
174 if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
175 if (parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithDistortion) !=
177 std::cout << "Unable to parse parameters with distortion for camera \"" << opt_camera_name << "\" from "
178 << opt_intrinsic_file << " file" << std::endl;
179 std::cout << "Attempt to find parameters without distortion" << std::endl;
180
181 if (parser.parse(cam, opt_intrinsic_file, opt_camera_name,
183 std::cout << "Unable to parse parameters without distortion for camera \"" << opt_camera_name << "\" from "
184 << opt_intrinsic_file << " file" << std::endl;
185 return EXIT_FAILURE;
186 }
187 }
188 }
189 std::cout << "Camera parameters used to compute the pose:\n" << cam << std::endl;
190
191 bool quit = false;
192 do {
193 reader.acquire(I);
195
196 cv::Mat matImg;
197 vpImageConvert::convert(I, matImg);
198
199 vpDisplay::displayText(I, 20, 20, "Right click to quit.", vpColor::red);
200
201 cv::Size chessboardSize(opt_chessboard_width, opt_chessboard_height);
202 std::vector<cv::Point2f> corners2D;
203 bool found = cv::findChessboardCorners(matImg, chessboardSize, corners2D,
204#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
205 cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK |
206 cv::CALIB_CB_NORMALIZE_IMAGE);
207#else
208 CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
209 CV_CALIB_CB_NORMALIZE_IMAGE);
210#endif
211
213 if (found) {
214 cv::Mat matImg_gray;
215 cv::cvtColor(matImg, matImg_gray, cv::COLOR_BGR2GRAY);
216 cv::cornerSubPix(matImg_gray, corners2D, cv::Size(11, 11), cv::Size(-1, -1),
217#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
218 cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
219#else
220 cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
221#endif
222
223 for (size_t i = 0; i < corners_pts.size(); i++) {
224 vpImagePoint imPt(corners2D[i].y, corners2D[i].x);
225 double x = 0.0, y = 0.0;
227 corners_pts[i].set_x(x);
228 corners_pts[i].set_y(y);
229 }
230
231 vpPose pose;
232 pose.addPoints(corners_pts);
234 std::cerr << "Problem when computing final pose using VVS" << std::endl;
235 return EXIT_FAILURE;
236 }
237
238 cv::drawChessboardCorners(matImg, chessboardSize, corners2D, found);
239 vpImageConvert::convert(matImg, I);
240 }
241
243
244 vpDisplay::displayText(I, 20, 20, "Left click for the next image, right click to quit.", vpColor::red);
245 if (found)
246 vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
247
249
250 if (found) {
251 vpPoseVector pose_vec(cMo);
252 char name[FILENAME_MAX];
253 snprintf(name, FILENAME_MAX, opt_output_pose_files.c_str(), reader.getFrameIndex());
254 std::string s = name;
255 std::cout << "Save " << s << std::endl;
256 pose_vec.saveYAML(s, pose_vec);
257 }
258
260 if (vpDisplay::getClick(I, button, true)) {
261 switch (button) {
263 quit = true;
264 break;
265
266 default:
267 break;
268 }
269 }
270 } while (!quit && !reader.end());
271 }
272 catch (const vpException &e) {
273 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
274 }
275
276 return EXIT_SUCCESS;
277}
278#else
279int main()
280{
281 std::cerr << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
282 return EXIT_SUCCESS;
283}
284#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
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...
Definition vpDisplayX.h:132
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void flush(const vpImage< unsigned char > &I)
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.
Definition vpException.h:59
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 ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
static bool checkFilename(const std::string &filename)
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:77
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition vpPoint.cpp:501
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition vpPoint.cpp:503
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition vpPoint.cpp:499
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...
Definition vpPose.h:81
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
void addPoints(const std::vector< vpPoint > &lP)
Definition vpPose.cpp:155
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
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, bool verbose=true)