Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
tutorial-chessboard-pose.cpp
1 #include <iostream>
3 
4 #include <visp3/core/vpConfig.h>
5 
6 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
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 
24 namespace
25 {
26 void 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 
41 void 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_cMo-%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 
86 int 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  } else if (std::string(argv[i]) == "-h" && i + 1 < argc) {
100  opt_chessboard_height = atoi(argv[i + 1]);
101  i++;
102  } else if (std::string(argv[i]) == "--square_size" && i + 1 < argc) {
103  opt_chessboard_square_size = atof(argv[i + 1]);
104  i++;
105  } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
106  opt_input_img_files = std::string(argv[i + 1]);
107  i++;
108  } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
109  opt_intrinsic_file = std::string(argv[i + 1]);
110  i++;
111  } else if (std::string(argv[i]) == "--output" && i + 1 < argc) {
112  opt_output_pose_files = std::string(argv[i + 1]);
113  i++;
114  } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
115  opt_camera_name = std::string(argv[i + 1]);
116  i++;
117  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
118  usage(argv, 0);
119  return EXIT_SUCCESS;
120  } else {
121  usage(argv, i);
122  return EXIT_FAILURE;
123  }
124  }
125 
126  if (!vpIoTools::checkFilename(opt_intrinsic_file)) {
127  std::cout << "Camera parameters file " << opt_intrinsic_file << " doesn't exist." << std::endl;
128  std::cout << "Use --help option to see how to set its location..." << std::endl;
129  return EXIT_SUCCESS;
130  }
131  std::cout << "Parameters:" << std::endl;
132  std::cout << " chessboard width : " << opt_chessboard_width << std::endl;
133  std::cout << " chessboard height : " << opt_chessboard_height << std::endl;
134  std::cout << " chessboard square size [m] : " << opt_chessboard_square_size << std::endl;
135  std::cout << " input images location : " << opt_input_img_files << std::endl;
136  std::cout << " camera param file name [.xml]: " << opt_intrinsic_file << std::endl;
137  std::cout << " camera name : " << opt_camera_name << std::endl;
138  std::cout << " output camera poses : " << opt_output_pose_files << std::endl << std::endl;
139 
140  if (opt_input_img_files.empty()) {
141  std::cout << "Input images location empty." << std::endl;
142  std::cout << "Use --help option to see how to set input image location..." << std::endl;
143  return EXIT_FAILURE;
144  }
145 
146  try {
147  vpVideoReader reader;
148  reader.setFileName(opt_input_img_files);
149 
150  vpImage<vpRGBa> I;
151  reader.open(I);
152 
153 #ifdef VISP_HAVE_X11
154  vpDisplayX d(I);
155 #elif defined VISP_HAVE_GDI
156  vpDisplayGDI d(I);
157 #elif defined VISP_HAVE_OPENCV
158  vpDisplayOpenCV d(I);
159 #endif
160 
161  std::vector<vpPoint> corners_pts;
162  calcChessboardCorners(opt_chessboard_width, opt_chessboard_height, opt_chessboard_square_size, corners_pts);
163 
164  vpCameraParameters cam;
165  vpXmlParserCamera parser;
166  if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
167  if (parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithDistortion) !=
169  std::cout << "Unable to parse parameters with distorsion for camera \"" << opt_camera_name << "\" from "
170  << opt_intrinsic_file << " file" << std::endl;
171  std::cout << "Attempt to find parameters without distorsion" << std::endl;
172 
173  if (parser.parse(cam, opt_intrinsic_file, opt_camera_name,
175  std::cout << "Unable to parse parameters without distorsion for camera \"" << opt_camera_name << "\" from "
176  << opt_intrinsic_file << " file" << std::endl;
177  return EXIT_FAILURE;
178  }
179  }
180  }
181  std::cout << "Camera parameters used to compute the pose:\n" << cam << std::endl;
182 
183  bool quit = false;
184  do {
185  reader.acquire(I);
186  vpDisplay::setTitle(I, reader.getFrameName());
187 
188  cv::Mat matImg;
189  vpImageConvert::convert(I, matImg);
190 
191  vpDisplay::displayText(I, 20, 20, "Right click to quit.", vpColor::red);
192 
193  cv::Size chessboardSize(opt_chessboard_width, opt_chessboard_height);
194  std::vector<cv::Point2f> corners2D;
195  bool found = cv::findChessboardCorners(matImg, chessboardSize, corners2D,
196 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
197  cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK |
198  cv::CALIB_CB_NORMALIZE_IMAGE);
199 #else
200  CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
201  CV_CALIB_CB_NORMALIZE_IMAGE);
202 #endif
203 
205  if (found) {
206  cv::Mat matImg_gray;
207  cv::cvtColor(matImg, matImg_gray, cv::COLOR_BGR2GRAY);
208  cv::cornerSubPix(matImg_gray, corners2D, cv::Size(11, 11), cv::Size(-1, -1),
209 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
210  cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
211 #else
212  cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
213 #endif
214 
215  for (size_t i = 0; i < corners_pts.size(); i++) {
216  vpImagePoint imPt(corners2D[i].y, corners2D[i].x);
217  double x = 0.0, y = 0.0;
218  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
219  corners_pts[i].set_x(x);
220  corners_pts[i].set_y(y);
221  }
222 
223  vpPose pose;
224  pose.addPoints(corners_pts);
225  vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
226  double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
227  bool pose_dementhon = pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
228  if (pose_dementhon)
229  r_dementhon = pose.computeResidual(cMo_dementhon);
230 
231  bool pose_lagrange = pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
232  if (pose_lagrange)
233  r_lagrange = pose.computeResidual(cMo_lagrange);
234 
235  cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
236  if (!pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
237  std::cerr << "Problem when computing final pose using VVS" << std::endl;
238  return EXIT_FAILURE;
239  }
240 
241  cv::drawChessboardCorners(matImg, chessboardSize, corners2D, found);
242  vpImageConvert::convert(matImg, I);
243  }
244 
246 
247  vpDisplay::displayText(I, 20, 20, "Left click for the next image, right click to quit.", vpColor::red);
248  if (found)
249  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
250 
251  vpDisplay::flush(I);
252 
253  if (found) {
254  vpPoseVector pose_vec(cMo);
255  char name[FILENAME_MAX];
256  sprintf(name, opt_output_pose_files.c_str(), reader.getFrameIndex());
257  std::string s = name;
258  std::cout << "Save " << s << std::endl;
259  pose_vec.saveYAML(s, pose_vec);
260  }
261 
263  if (vpDisplay::getClick(I, button, true)) {
264  switch (button) {
266  quit = true;
267  break;
268 
269  default:
270  break;
271  }
272  }
273  } while (!quit && !reader.end());
274  } catch (const vpException &e) {
275  std::cout << "Catch an exception: " << e.getMessage() << std::endl;
276  }
277 
278  return EXIT_SUCCESS;
279 }
280 #else
281 int main()
282 {
283  std::cerr << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
284  return EXIT_SUCCESS;
285 }
286 #endif
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:87
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:89
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:800
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:90
@ DEMENTHON
Definition: vpPose.h:95
@ VIRTUAL_VS
Definition: vpPose.h:104
@ LAGRANGE
Definition: vpPose.h:94
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:163
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:335
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:373
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)