Main MRPT website > C++ reference for MRPT 1.9.9
checkerboard_cam_calib.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/filesystem.h>
14 
16 #include <mrpt/vision/pinhole.h>
18 
19 // Universal include for all versions of OpenCV
20 #include <mrpt/otherlibs/do_opencv_includes.h>
21 
22 using namespace mrpt;
23 using namespace mrpt::vision;
24 using namespace mrpt::img;
25 using namespace mrpt::config;
26 using namespace mrpt::math;
27 using namespace mrpt::poses;
28 using namespace std;
29 
30 /* -------------------------------------------------------
31  checkerBoardCameraCalibration
32  ------------------------------------------------------- */
34  TCalibrationImageList& images, unsigned int check_size_x,
35  unsigned int check_size_y, double check_squares_length_X_meters,
36  double check_squares_length_Y_meters, CMatrixDouble33& intrinsicParams,
37  std::vector<double>& distortionParams, bool normalize_image,
38  double* out_MSE, bool skipDrawDetectedImgs,
39  bool useScaramuzzaAlternativeDetector)
40 {
41  // Just a wrapper for the newer version of the function which uses TCamera:
42  TCamera cam;
44  images, check_size_x, check_size_y, check_squares_length_X_meters,
45  check_squares_length_Y_meters, cam, normalize_image, out_MSE,
46  skipDrawDetectedImgs, useScaramuzzaAlternativeDetector);
47 
48  intrinsicParams = cam.intrinsicParams;
49  distortionParams = cam.getDistortionParamsAsVector();
50  return ret;
51 }
52 
53 #if MRPT_HAS_OPENCV
54 // JL says: This was copied here since it seems OpenCV 2.3 had a broken
55 // <opencv2/core/eigen.hpp> header.
56 // It should be removed in the future when 2.3 becomes too old to support.
57 namespace cv
58 {
59 template <
60  typename _Tp, int _rows, int _cols, int _options, int _maxRows,
61  int _maxCols>
63  const Mat& src,
64  Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& dst)
65 {
66  CV_DbgAssert(src.rows == _rows && src.cols == _cols);
67  if (!(dst.Flags & Eigen::RowMajorBit))
68  {
69  Mat _dst(
70  src.cols, src.rows, DataType<_Tp>::type, dst.data(),
71  (size_t)(dst.stride() * sizeof(_Tp)));
72  if (src.type() == _dst.type())
73  transpose(src, _dst);
74  else if (src.cols == src.rows)
75  {
76  src.convertTo(_dst, _dst.type());
77  transpose(_dst, _dst);
78  }
79  else
80  Mat(src.t()).convertTo(_dst, _dst.type());
81  CV_DbgAssert(_dst.data == (uchar*)dst.data());
82  }
83  else
84  {
85  Mat _dst(
86  src.rows, src.cols, DataType<_Tp>::type, dst.data(),
87  (size_t)(dst.stride() * sizeof(_Tp)));
88  src.convertTo(_dst, _dst.type());
89  CV_DbgAssert(_dst.data == (uchar*)dst.data());
90  }
91 }
92 } // namespace cv
93 #endif
94 
95 /* -------------------------------------------------------
96  checkerBoardCameraCalibration
97  ------------------------------------------------------- */
99  TCalibrationImageList& images, unsigned int check_size_x,
100  unsigned int check_size_y, double check_squares_length_X_meters,
101  double check_squares_length_Y_meters, mrpt::img::TCamera& out_camera_params,
102  bool normalize_image, double* out_MSE, bool skipDrawDetectedImgs,
103  bool useScaramuzzaAlternativeDetector)
104 {
105  MRPT_UNUSED_PARAM(skipDrawDetectedImgs);
106 #if MRPT_HAS_OPENCV
107  try
108  {
109  ASSERT_(check_size_x > 2);
110  ASSERT_(check_size_y > 2);
111  ASSERT_(check_squares_length_X_meters > 0);
112  ASSERT_(check_squares_length_Y_meters > 0);
113 
114  if (images.size() < 1)
115  {
116  std::cout << "ERROR: No input images." << std::endl;
117  return false;
118  }
119 
120  const unsigned CORNERS_COUNT = check_size_x * check_size_y;
121  const CvSize check_size = cvSize(check_size_x, check_size_y);
122 
123  // Fill the pattern of expected pattern points only once out of the
124  // loop:
125  vector<cv::Point3f> pattern_obj_points(CORNERS_COUNT);
126  {
127  unsigned int y, k;
128  for (y = 0, k = 0; y < check_size_y; y++)
129  {
130  for (unsigned int x = 0; x < check_size_x; x++, k++)
131  {
132  pattern_obj_points[k].x = -check_squares_length_X_meters *
133  x; // The "-" is for convenience,
134  // so the camera poses appear
135  // with Z>0
136  pattern_obj_points[k].y = check_squares_length_Y_meters * y;
137  pattern_obj_points[k].z = 0;
138  }
139  }
140  }
141 
142  // First: Assure all images are loaded:
143  // -------------------------------------------
145  for (it = images.begin(); it != images.end(); ++it)
146  {
147  TImageCalibData& dat = it->second;
148 
149  dat.projectedPoints_distorted.clear(); // Clear reprojected points.
150  dat.projectedPoints_undistorted.clear();
151 
152  // Skip if images are marked as "externalStorage":
153  if (!dat.img_original.isExternallyStored() &&
154  !mrpt::system::extractFileExtension(it->first).empty())
155  {
156  if (!dat.img_original.loadFromFile(it->first))
158  "Error reading image: %s", it->first.c_str());
159 
160  dat.img_checkboard = dat.img_original;
161  dat.img_rectified = dat.img_original;
162  }
163  }
164 
165  // For each image, find checkerboard corners:
166  // -----------------------------------------------
167  vector<vector<cv::Point3f>>
168  objectPoints; // final container for detected stuff
169  vector<vector<cv::Point2f>>
170  imagePoints; // final container for detected stuff
171 
172  unsigned int valid_detected_imgs = 0;
173  vector<string> pointsIdx2imageFile;
174  cv::Size imgSize(0, 0);
175 
176  unsigned int i;
177  for (i = 0, it = images.begin(); it != images.end(); it++, i++)
178  {
179  TImageCalibData& dat = it->second;
180 
181  // Make grayscale version:
182  const CImage img_gray(
184 
185  if (!i)
186  {
187  imgSize = cv::Size(img_gray.getWidth(), img_gray.getHeight());
188  out_camera_params.ncols = imgSize.width;
189  out_camera_params.nrows = imgSize.height;
190  }
191  else
192  {
193  if (imgSize.height != (int)img_gray.getHeight() ||
194  imgSize.width != (int)img_gray.getWidth())
195  {
196  std::cout << "ERROR: All the images must have the same size"
197  << std::endl;
198  return false;
199  }
200  }
201 
202  // Try with expanded versions of the image if it fails to detect the
203  // checkerboard:
204  unsigned corners_count;
205  bool corners_found = false;
206 
207  corners_count = CORNERS_COUNT;
208 
209  vector<cv::Point2f> this_img_pts(
210  CORNERS_COUNT); // Temporary buffer for points, to be added if
211  // the points pass the checks.
212 
213  dat.detected_corners.clear();
214 
215  // Do detection (this includes the "refine corners" with
216  // cvFindCornerSubPix):
217  vector<TPixelCoordf> detectedCoords;
218  corners_found = mrpt::vision::findChessboardCorners(
219  img_gray, detectedCoords, check_size_x, check_size_y,
220  normalize_image, // normalize_image
221  useScaramuzzaAlternativeDetector);
222 
223  corners_count = detectedCoords.size();
224 
225  // Copy the data into the overall array of coords:
226  ASSERT_(detectedCoords.size() <= CORNERS_COUNT);
227  for (size_t p = 0; p < detectedCoords.size(); p++)
228  {
229  this_img_pts[p].x = detectedCoords[p].x;
230  this_img_pts[p].y = detectedCoords[p].y;
231  }
232 
233  if (corners_found && corners_count != CORNERS_COUNT)
234  corners_found = false;
235 
236  cout << format(
237  "Img %s: %s\n",
238  mrpt::system::extractFileName(it->first).c_str(),
239  corners_found ? "DETECTED" : "NOT DETECTED");
240 
241  if (corners_found)
242  {
243  // save the corners in the data structure:
244  int x, y;
245  unsigned int k;
246  for (y = 0, k = 0; y < check_size.height; y++)
247  for (x = 0; x < check_size.width; x++, k++)
249  this_img_pts[k].x, this_img_pts[k].y));
250 
251  // Draw the checkerboard in the corresponding image:
252  // ----------------------------------------------------
253  if (!dat.img_original.isExternallyStored())
254  {
255  const int r = 4;
256  CvPoint prev_pt = cvPoint(0, 0);
257  const int line_max = 8;
258  CvScalar line_colors[8];
259 
260  line_colors[0] = CV_RGB(255, 0, 0);
261  line_colors[1] = CV_RGB(255, 128, 0);
262  line_colors[2] = CV_RGB(255, 128, 0);
263  line_colors[3] = CV_RGB(200, 200, 0);
264  line_colors[4] = CV_RGB(0, 255, 0);
265  line_colors[5] = CV_RGB(0, 200, 200);
266  line_colors[6] = CV_RGB(0, 0, 255);
267  line_colors[7] = CV_RGB(255, 0, 255);
268 
269  // Checkboad as color image:
271 
272  void* rgb_img = dat.img_checkboard.getAs<IplImage>();
273 
274  for (y = 0, k = 0; y < check_size.height; y++)
275  {
276  CvScalar color = line_colors[y % line_max];
277  for (x = 0; x < check_size.width; x++, k++)
278  {
279  CvPoint pt;
280  pt.x = cvRound(this_img_pts[k].x);
281  pt.y = cvRound(this_img_pts[k].y);
282 
283  if (k != 0) cvLine(rgb_img, prev_pt, pt, color);
284 
285  cvLine(
286  rgb_img, cvPoint(pt.x - r, pt.y - r),
287  cvPoint(pt.x + r, pt.y + r), color);
288  cvLine(
289  rgb_img, cvPoint(pt.x - r, pt.y + r),
290  cvPoint(pt.x + r, pt.y - r), color);
291  cvCircle(rgb_img, pt, r + 1, color);
292  prev_pt = pt;
293  }
294  }
295  }
296 
297  // Accept this image as good:
298  pointsIdx2imageFile.push_back(it->first);
299  imagePoints.push_back(this_img_pts);
300  objectPoints.push_back(pattern_obj_points);
301 
302  valid_detected_imgs++;
303  }
304 
305  } // end find corners
306 
307  std::cout << valid_detected_imgs << " valid images." << std::endl;
308  if (!valid_detected_imgs)
309  {
310  std::cout << "ERROR: No valid images. Perhaps the checkerboard "
311  "size is incorrect?"
312  << std::endl;
313  return false;
314  }
315 
316  // ---------------------------------------------
317  // Calculate the camera parameters
318  // ---------------------------------------------
319  // Calibrate camera
320  cv::Mat cameraMatrix, distCoeffs(1, 5, CV_64F, cv::Scalar::all(0));
321  vector<cv::Mat> rvecs, tvecs;
322 
323  const double cv_calib_err = cv::calibrateCamera(
324  objectPoints, imagePoints, imgSize, cameraMatrix, distCoeffs, rvecs,
325  tvecs, 0 /*flags*/);
326 
327  // Load matrix:
328  out_camera_params.intrinsicParams =
329  CMatrixDouble33(cameraMatrix.ptr<double>());
330 
331  out_camera_params.dist.fill(0);
332  for (int k = 0; k < 5; k++)
333  out_camera_params.dist[k] = distCoeffs.ptr<double>()[k];
334 
335  // Load camera poses:
336  for (i = 0; i < valid_detected_imgs; i++)
337  {
338  CMatrixDouble44 HM;
339  HM.zeros();
340  HM(3, 3) = 1;
341 
342  {
343  // Convert rotation vectors -> rot matrices:
344  cv::Mat cv_rot;
345  cv::Rodrigues(rvecs[i], cv_rot);
346 
347  Eigen::Matrix3d rot;
348  cv::my_cv2eigen(cv_rot, rot);
349  HM.block<3, 3>(0, 0) = rot;
350  }
351 
352  {
353  Eigen::Matrix<double, 3, 1> trans;
354  cv::my_cv2eigen(tvecs[i], trans);
355  HM.block<3, 1>(0, 3) = trans;
356  }
357 
358  CPose3D p = CPose3D(0, 0, 0) - CPose3D(HM);
359 
360  images[pointsIdx2imageFile[i]].reconstructed_camera_pose = p;
361 
362  std::cout << "Img: "
363  << mrpt::system::extractFileName(pointsIdx2imageFile[i])
364  << ": " << p << std::endl;
365  }
366 
367  {
368  CConfigFileMemory cfg;
369  out_camera_params.saveToConfigFile("CAMERA_PARAMS", cfg);
370  std::cout << cfg.getContent() << std::endl;
371  }
372 
373  // ----------------------------------------
374  // Undistort images:
375  // ----------------------------------------
376  for (it = images.begin(); it != images.end(); ++it)
377  {
378  TImageCalibData& dat = it->second;
379  if (!dat.img_original.isExternallyStored())
381  dat.img_rectified, out_camera_params);
382  } // end undistort
383 
384  // -----------------------------------------------
385  // Reproject points to measure the fit sqr error
386  // -----------------------------------------------
387  double sqrErr = 0;
388 
389  for (i = 0; i < valid_detected_imgs; i++)
390  {
391  TImageCalibData& dat = images[pointsIdx2imageFile[i]];
392  if (dat.detected_corners.size() != CORNERS_COUNT) continue;
393 
394  // Reproject all the points into pixel coordinates:
395  // -----------------------------------------------------
396  vector<TPoint3D> lstPatternPoints(
397  CORNERS_COUNT); // Points as seen from the camera:
398  for (unsigned int p = 0; p < CORNERS_COUNT; p++)
399  lstPatternPoints[p] = TPoint3D(
400  pattern_obj_points[p].x, pattern_obj_points[p].y,
401  pattern_obj_points[p].z);
402 
403  vector<TPixelCoordf>& projectedPoints =
405  vector<TPixelCoordf>& projectedPoints_distorted =
407 
409  lstPatternPoints, // Input points
411  out_camera_params.intrinsicParams, // calib matrix
412  projectedPoints // Output points in pixels
413  );
414 
416  lstPatternPoints, // Input points
418  out_camera_params.intrinsicParams, // calib matrix
419  out_camera_params.getDistortionParamsAsVector(),
420  projectedPoints_distorted // Output points in pixels
421  );
422 
423  ASSERT_(projectedPoints.size() == CORNERS_COUNT);
424  ASSERT_(projectedPoints_distorted.size() == CORNERS_COUNT);
425 
426  for (unsigned int p = 0; p < CORNERS_COUNT; p++)
427  {
428  const double px = projectedPoints[p].x;
429  const double py = projectedPoints[p].y;
430 
431  const double px_d = projectedPoints_distorted[p].x;
432  const double py_d = projectedPoints_distorted[p].y;
433 
434  // Only draw if the img is NOT external:
435  if (!dat.img_original.isExternallyStored())
436  {
437  if (px >= 0 && px < imgSize.width && py >= 0 &&
438  py < imgSize.height)
439  cvCircle(
440  dat.img_rectified.getAs<IplImage>(),
441  cvPoint(px, py), 4, CV_RGB(0, 0, 255));
442  }
443 
444  // Accumulate error:
445  sqrErr +=
446  square(px_d - dat.detected_corners[p].x) +
447  square(py_d - dat.detected_corners[p].y); // Error relative
448  // to the
449  // original
450  // (distorted)
451  // image.
452  }
453  }
454 
455  if (valid_detected_imgs)
456  {
457  sqrErr /= CORNERS_COUNT * valid_detected_imgs;
458  std::cout << "Average err. of reprojection: " << sqrt(sqrErr)
459  << " pixels (OpenCV error=" << cv_calib_err << ")\n";
460  }
461  if (out_MSE) *out_MSE = sqrt(sqrErr);
462 
463  return true;
464  }
465  catch (std::exception& e)
466  {
467  std::cout << e.what() << std::endl;
468  return false;
469  }
470 #else
471  THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV");
472 #endif
473 }
This class implements a config file-like interface over a memory-stored string list.
Scalar * iterator
Definition: eigen_plugins.h:26
uint32_t nrows
Definition: TCamera.h:41
const T * getAs() const
Returns a pointer to a const T* containing the image - the idea is to call like "img.getAs<IplImage>()" so we can avoid here including OpenCV&#39;s headers.
Definition: img/CImage.h:599
GLdouble GLdouble z
Definition: glext.h:3872
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
std::map< std::string, TImageCalibData > TCalibrationImageList
A list of images, used in checkerBoardCameraCalibration.
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:20
STL namespace.
bool isExternallyStored() const noexcept
See setExternalStorage().
Definition: img/CImage.h:793
bool checkerBoardCameraCalibration(TCalibrationImageList &images, unsigned int check_size_x, unsigned int check_size_y, double check_squares_length_X_meters, double check_squares_length_Y_meters, mrpt::img::TCamera &out_camera_params, bool normalize_image=true, double *out_MSE=nullptr, bool skipDrawDetectedImgs=false, bool useScaramuzzaAlternativeDetector=false)
Performs a camera calibration (computation of projection and distortion parameters) from a sequence o...
GLuint src
Definition: glext.h:7278
T square(const T x)
Inline function for the square of a number.
GLuint color
Definition: glext.h:8300
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:44
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
bool loadFromFile(const std::string &fileName, int isColor=-1)
Load image from a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:271
mrpt::img::CImage img_checkboard
At output, this will contain the detected checkerboard overprinted to the image.
Data associated to each image in the calibration process mrpt::vision::checkerBoardCameraCalibration ...
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:864
GLuint dst
Definition: glext.h:7135
bool findChessboardCorners(const mrpt::img::CImage &img, std::vector< mrpt::img::TPixelCoordf > &cornerCoords, unsigned int check_size_x, unsigned int check_size_y, bool normalize_image=true, bool useScaramuzzaMethod=false)
Look for the corners of a chessboard in the image using one of two different methods.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
std::vector< mrpt::img::TPixelCoordf > detected_corners
At output, the detected corners (x,y) in pixel units.
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:29
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:97
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void colorImage(CImage &ret) const
Returns a RGB version of the grayscale image, or itself if it is already a RGB image.
Definition: CImage.cpp:2391
void projectPoints_with_distortion(const std::vector< mrpt::math::TPoint3D > &in_points_3D, const mrpt::poses::CPose3D &cameraPose, const mrpt::math::CMatrixDouble33 &intrinsicParams, const std::vector< double > &distortionParams, std::vector< mrpt::img::TPixelCoordf > &projectedPoints, bool accept_points_behind=false)
Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix and dis...
Definition: pinhole.cpp:51
std::array< double, 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:47
GLsizei GLboolean transpose
Definition: glext.h:4133
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::vector< mrpt::img::TPixelCoordf > projectedPoints_undistorted
At output, like projectedPoints_distorted but for the undistorted image.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
void rectifyImage(CImage &out_img, const mrpt::img::TCamera &cameraParams) const
Rectify (un-distort) the image according to some camera parameters, and returns an output un-distorte...
Definition: CImage.cpp:2082
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
void saveToConfigFile(const std::string &section, mrpt::config::CConfigFileBase &cfg) const
Save as a config block:
Definition: TCamera.cpp:114
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:57
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
void my_cv2eigen(const Mat &src, Eigen::Matrix< _Tp, _rows, _cols, _options, _maxRows, _maxCols > &dst)
GLenum GLint GLint y
Definition: glext.h:3538
void projectPoints_no_distortion(const std::vector< mrpt::math::TPoint3D > &in_points_3D, const mrpt::poses::CPose3D &cameraPose, const mrpt::math::CMatrixDouble33 &intrinsicParams, std::vector< mrpt::img::TPixelCoordf > &projectedPoints, bool accept_points_behind=false)
Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix (undist...
Definition: pinhole.cpp:31
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
std::string extractFileName(const std::string &filePath)
Extract just the name (without extension) of a filename from a complete path plus name plus extension...
Definition: filesystem.cpp:61
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
mrpt::poses::CPose3D reconstructed_camera_pose
At output, the reconstructed pose of the camera.
std::vector< mrpt::img::TPixelCoordf > projectedPoints_distorted
At output, only will have an empty vector if the checkerboard was not found in this image...
GLfloat GLfloat p
Definition: glext.h:6305
uint32_t ncols
Camera resolution.
Definition: TCamera.h:41
mrpt::img::CImage img_original
This image will be automatically loaded from the file name passed to checkerBoardCameraCalibration.
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3528
mrpt::img::CImage img_rectified
At output, this will be the rectified image.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
std::vector< double > getDistortionParamsAsVector() const
Get a vector with the distortion params of the camera.
Definition: TCamera.h:122
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019