Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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/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 
24 namespace {
25 void calcChessboardCorners(const int width, const int height, const 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 } //namespace
39 
40 int main(int argc, const char ** argv) {
41  int chessboard_width = 9, chessboard_height = 6;
42  double chessboard_square_size = 0.03;
43  std::string input_filename = "";
44  std::string intrinsic_file = "camera.xml";
45  std::string camera_name = "Camera";
46 
47  for (int i = 1; i < argc; i++) {
48  if (std::string(argv[i]) == "-w" && i+1 < argc) {
49  chessboard_width = atoi(argv[i+1]);
50  } else if (std::string(argv[i]) == "-h" && i+1 < argc) {
51  chessboard_height = atoi(argv[i+1]);
52  } else if (std::string(argv[i]) == "--square_size" && i+1 < argc) {
53  chessboard_square_size = atof(argv[i+1]);
54  } else if (std::string(argv[i]) == "--input" && i+1 < argc) {
55  input_filename = std::string(argv[i+1]);
56  } else if (std::string(argv[i]) == "--intrinsic" && i+1 < argc) {
57  intrinsic_file = std::string(argv[i+1]);
58  } else if (std::string(argv[i]) == "--camera_name" && i+1 < argc) {
59  camera_name = std::string(argv[i+1]);
60  }
61  else if (std::string(argv[i]) == "--help") {
62  std::cout << argv[0] << " [-w <chessboard width>] [-w <chessboard height>] [--square_size <square size in meter>] [--input <input images path>] [--intrinsic <Camera intrinsic parameters xml file>] [--camera_name <Camera name in the xml intrinsic file>]" << std::endl;
63  return EXIT_SUCCESS;
64  }
65  }
66 
67  std::cout << "Parameters:" << std::endl;
68  std::cout << "chessboard_width=" << chessboard_width << std::endl;
69  std::cout << "chessboard_height=" << chessboard_height << std::endl;
70  std::cout << "chessboard_square_size=" << chessboard_square_size << std::endl;
71  std::cout << "input_filename=" << input_filename << std::endl;
72  std::cout << "intrinsic_file=" << intrinsic_file << std::endl;
73  std::cout << "camera_name=" << camera_name << std::endl;
74 
75  vpVideoReader reader;
76  if (input_filename.empty()) {
77  std::cerr << "input_filename.empty()" << std::endl;
78  return EXIT_FAILURE;
79  }
80  reader.setFileName(input_filename);
81 
83  reader.open(I);
84 
85 #ifdef VISP_HAVE_X11
86  vpDisplayX d(I);
87 #elif defined VISP_HAVE_GDI
88  vpDisplayGDI d(I);
89 #elif defined VISP_HAVE_OPENCV
90  vpDisplayOpenCV d(I);
91 #endif
92 
93  std::vector<vpPoint> corners_pts;
94  calcChessboardCorners(chessboard_width, chessboard_height, chessboard_square_size, corners_pts);
95 
97 #ifdef VISP_HAVE_XML2
98  vpXmlParserCamera parser;
99  if (!intrinsic_file.empty() && !camera_name.empty()) {
100  parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithDistortion);
101  }
102 #endif
103  std::cout << "cam:\n" << cam << std::endl;
104 
105  bool quit = false;
106  while (!quit && !reader.end()) {
107  reader.acquire(I);
108 
109  cv::Mat matImg;
110  vpImageConvert::convert(I, matImg);
111 
112  vpDisplay::displayText(I, 20, 20, "Right click to quit.", vpColor::red);
113 
114  cv::Size chessboardSize(chessboard_width, chessboard_height);
115  std::vector<cv::Point2f> corners2D;
116  bool found = cv::findChessboardCorners(matImg, chessboardSize, corners2D,
117 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
118  cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
119 #else
120  CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
121 #endif
122 
124  if (found) {
125  cv::Mat matImg_gray;
126  cv::cvtColor(matImg, matImg_gray, cv::COLOR_BGR2GRAY);
127  cv::cornerSubPix(matImg_gray, corners2D, cv::Size(11,11),
128  cv::Size(-1,-1),
129 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
130  cv::TermCriteria( cv::TermCriteria::EPS+cv::TermCriteria::COUNT, 30, 0.1 ));
131 #else
132  cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
133 #endif
134 
135  for (size_t i = 0; i < corners_pts.size(); i++) {
136  vpImagePoint imPt(corners2D[i].y, corners2D[i].x);
137  double x = 0.0, y = 0.0;
138  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
139  corners_pts[i].set_x(x);
140  corners_pts[i].set_y(y);
141  }
142 
143  vpPose pose;
144  pose.addPoints(corners_pts);
145  vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
146  double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
147  bool pose_dementhon = pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
148  if (pose_dementhon)
149  r_dementhon = pose.computeResidual(cMo_dementhon);
150 
151  bool pose_lagrange = pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
152  if (pose_lagrange)
153  r_lagrange = pose.computeResidual(cMo_lagrange);
154 
155  cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
156  if (!pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
157  std::cerr << "Problem when computing final pose using VVS" << std::endl;
158  return EXIT_FAILURE;
159  }
160 
161  cv::drawChessboardCorners(matImg, chessboardSize, corners2D, found);
162  vpImageConvert::convert(matImg, I);
163  }
164 
166 
167  vpDisplay::displayText(I, 20, 20, "Left click for the next image, right click to quit.", vpColor::red);
168  if (found)
169  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
170 
171  vpDisplay::flush(I);
172 
173  if (found) {
174  vpPoseVector pose_vec(cMo);
175  std::stringstream ss;
176  ss << "pose_cPo_" << reader.getFrameIndex() << ".yaml";
177  std::cout << "Save " << ss.str() << std::endl;
178  pose_vec.saveYAML(ss.str(), pose_vec);
179  }
180 
182  if (vpDisplay::getClick(I, button, true)) {
183  switch (button) {
185  quit = true;
186  break;
187 
188  default:
189  break;
190  }
191  }
192  }
193 
194  return EXIT_SUCCESS;
195 }
196 #else
197 int main() {
198  std::cerr << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
199  return EXIT_SUCCESS;
200 }
201 #endif
202 
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:362
long getFrameIndex() const
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:465
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:152
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
static const vpColor none
Definition: vpColor.h:192
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
XML parser to load and save intrinsic camera parameters.
static void flush(const vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
void open(vpImage< vpRGBa > &I)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
Generic class defining intrinsic camera parameters.
void set_oX(const double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:461
void acquire(vpImage< vpRGBa > &I)
void setFileName(const char *filename)
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))
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
Definition: vpPoint.cpp:463
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width=0, const unsigned int image_height=0)
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:324