MRPT  2.0.1
checkerboard_cam_calib.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <mrpt/system/filesystem.h>
17 #include <mrpt/vision/pinhole.h>
18 #include <Eigen/Dense>
19 
20 using namespace mrpt;
21 using namespace mrpt::vision;
22 using namespace mrpt::img;
23 using namespace mrpt::config;
24 using namespace mrpt::math;
25 using namespace mrpt::poses;
26 using namespace std;
27 
28 /* -------------------------------------------------------
29  checkerBoardCameraCalibration
30  ------------------------------------------------------- */
33  unsigned int check_size_y, double check_squares_length_X_meters,
34  double check_squares_length_Y_meters, CMatrixDouble33& intrinsicParams,
35  std::vector<double>& distortionParams, bool normalize_image,
36  double* out_MSE, bool skipDrawDetectedImgs,
37  bool useScaramuzzaAlternativeDetector)
38 {
39  // Just a wrapper for the newer version of the function which uses TCamera:
40  TCamera cam;
43  check_squares_length_Y_meters, cam, normalize_image, out_MSE,
44  skipDrawDetectedImgs, useScaramuzzaAlternativeDetector);
45 
46  intrinsicParams = cam.intrinsicParams;
47  distortionParams = cam.getDistortionParamsAsVector();
48  return ret;
49 }
50 
51 #if MRPT_HAS_OPENCV
52 #include <opencv2/core/eigen.hpp>
53 #endif
54 
55 /* -------------------------------------------------------
56  checkerBoardCameraCalibration
57  ------------------------------------------------------- */
60  unsigned int check_size_y, double check_squares_length_X_meters,
61  double check_squares_length_Y_meters, mrpt::img::TCamera& out_camera_params,
62  bool normalize_image, double* out_MSE,
63  [[maybe_unused]] bool skipDrawDetectedImgs,
64  bool useScaramuzzaAlternativeDetector)
65 {
66 #if MRPT_HAS_OPENCV
67  try
68  {
69  ASSERT_(check_size_x > 2);
70  ASSERT_(check_size_y > 2);
73 
74  if (images.size() < 1)
75  {
76  std::cout << "ERROR: No input images." << std::endl;
77  return false;
78  }
79 
80  const unsigned CORNERS_COUNT = check_size_x * check_size_y;
81  const CvSize check_size = cvSize(check_size_x, check_size_y);
82 
83  // Fill the pattern of expected pattern points only once out of the
84  // loop:
85  vector<cv::Point3f> pattern_obj_points(CORNERS_COUNT);
86  {
87  unsigned int y, k;
88  for (y = 0, k = 0; y < check_size_y; y++)
89  {
90  for (unsigned int x = 0; x < check_size_x; x++, k++)
91  {
92  pattern_obj_points[k].x = -check_squares_length_X_meters *
93  x; // The "-" is for convenience,
94  // so the camera poses appear
95  // with Z>0
96  pattern_obj_points[k].y = check_squares_length_Y_meters * y;
97  pattern_obj_points[k].z = 0;
98  }
99  }
100  }
101 
102  // First: Assure all images are loaded:
103  // -------------------------------------------
104  TCalibrationImageList::iterator it;
105  for (it = images.begin(); it != images.end(); ++it)
106  {
107  TImageCalibData& dat = it->second;
108 
109  dat.projectedPoints_distorted.clear(); // Clear reprojected points.
110  dat.projectedPoints_undistorted.clear();
111 
112  // Skip if images are marked as "externalStorage":
113  if (!dat.img_original.isExternallyStored() &&
114  !mrpt::system::extractFileExtension(it->first).empty())
115  {
116  if (!dat.img_original.loadFromFile(it->first))
118  "Error reading image: %s", it->first.c_str());
119 
120  dat.img_checkboard = dat.img_original;
121  dat.img_rectified = dat.img_original;
122  }
123  }
124 
125  // For each image, find checkerboard corners:
126  // -----------------------------------------------
127  vector<vector<cv::Point3f>>
128  objectPoints; // final container for detected stuff
129  vector<vector<cv::Point2f>>
130  imagePoints; // final container for detected stuff
131 
132  unsigned int valid_detected_imgs = 0;
133  vector<string> pointsIdx2imageFile;
134  cv::Size imgSize(0, 0);
135 
136  unsigned int i;
137  for (i = 0, it = images.begin(); it != images.end(); it++, i++)
138  {
139  TImageCalibData& dat = it->second;
140 
141  // Make grayscale version:
142  const CImage img_gray(
144 
145  if (!i)
146  {
147  imgSize = cv::Size(img_gray.getWidth(), img_gray.getHeight());
148  out_camera_params.ncols = imgSize.width;
149  out_camera_params.nrows = imgSize.height;
150  }
151  else
152  {
153  if (imgSize.height != (int)img_gray.getHeight() ||
154  imgSize.width != (int)img_gray.getWidth())
155  {
156  std::cout << "ERROR: All the images must have the same size"
157  << std::endl;
158  return false;
159  }
160  }
161 
162  // Try with expanded versions of the image if it fails to detect the
163  // checkerboard:
164  unsigned corners_count;
165  bool corners_found = false;
166 
167  corners_count = CORNERS_COUNT;
168 
169  vector<cv::Point2f> this_img_pts(
170  CORNERS_COUNT); // Temporary buffer for points, to be added if
171  // the points pass the checks.
172 
173  dat.detected_corners.clear();
174 
175  // Do detection (this includes the "refine corners" with
176  // cvFindCornerSubPix):
177  vector<TPixelCoordf> detectedCoords;
178  corners_found = mrpt::vision::findChessboardCorners(
179  img_gray, detectedCoords, check_size_x, check_size_y,
180  normalize_image, // normalize_image
181  useScaramuzzaAlternativeDetector);
182 
183  corners_count = detectedCoords.size();
184 
185  // Copy the data into the overall array of coords:
186  ASSERT_(detectedCoords.size() <= CORNERS_COUNT);
187  for (size_t p = 0; p < detectedCoords.size(); p++)
188  {
189  this_img_pts[p].x = detectedCoords[p].x;
190  this_img_pts[p].y = detectedCoords[p].y;
191  }
192 
193  if (corners_found && corners_count != CORNERS_COUNT)
194  corners_found = false;
195 
196  cout << format(
197  "Img %s: %s\n",
198  mrpt::system::extractFileName(it->first).c_str(),
199  corners_found ? "DETECTED" : "NOT DETECTED");
200 
201  if (corners_found)
202  {
203  // save the corners in the data structure:
204  int x, y;
205  unsigned int k;
206  for (y = 0, k = 0; y < check_size.height; y++)
207  for (x = 0; x < check_size.width; x++, k++)
208  dat.detected_corners.emplace_back(
209  this_img_pts[k].x, this_img_pts[k].y);
210 
211  // Draw the checkerboard in the corresponding image:
212  // ----------------------------------------------------
213  if (!dat.img_original.isExternallyStored())
214  {
215  const int r = 4;
216  cv::Point prev_pt = cvPoint(0, 0);
217  const int line_max = 8;
218  cv::Scalar line_colors[8];
219 
220  line_colors[0] = CV_RGB(255, 0, 0);
221  line_colors[1] = CV_RGB(255, 128, 0);
222  line_colors[2] = CV_RGB(255, 128, 0);
223  line_colors[3] = CV_RGB(200, 200, 0);
224  line_colors[4] = CV_RGB(0, 255, 0);
225  line_colors[5] = CV_RGB(0, 200, 200);
226  line_colors[6] = CV_RGB(0, 0, 255);
227  line_colors[7] = CV_RGB(255, 0, 255);
228 
229  // Checkboad as color image:
231 
232  cv::Mat rgb_img =
233  dat.img_checkboard.asCvMat<cv::Mat>(SHALLOW_COPY);
234 
235  for (y = 0, k = 0; y < check_size.height; y++)
236  {
237  cv::Scalar color = line_colors[y % line_max];
238  for (x = 0; x < check_size.width; x++, k++)
239  {
240  cv::Point pt{cvRound(this_img_pts[k].x),
241  cvRound(this_img_pts[k].y)};
242 
243  if (k != 0) cv::line(rgb_img, prev_pt, pt, color);
244 
245  cv::line(
246  rgb_img, cv::Point(pt.x - r, pt.y - r),
247  cv::Point(pt.x + r, pt.y + r), color);
248  cv::line(
249  rgb_img, cv::Point(pt.x - r, pt.y + r),
250  cv::Point(pt.x + r, pt.y - r), color);
251  cv::circle(rgb_img, pt, r + 1, color);
252  prev_pt = pt;
253  }
254  }
255  }
256 
257  // Accept this image as good:
258  pointsIdx2imageFile.push_back(it->first);
259  imagePoints.push_back(this_img_pts);
260  objectPoints.push_back(pattern_obj_points);
261 
262  valid_detected_imgs++;
263  }
264 
265  } // end find corners
266 
267  std::cout << valid_detected_imgs << " valid images." << std::endl;
268  if (!valid_detected_imgs)
269  {
270  std::cout << "ERROR: No valid images. Perhaps the checkerboard "
271  "size is incorrect?"
272  << std::endl;
273  return false;
274  }
275 
276  // ---------------------------------------------
277  // Calculate the camera parameters
278  // ---------------------------------------------
279  // Calibrate camera
280  cv::Mat cameraMatrix, distCoeffs(1, 5, CV_64F, cv::Scalar::all(0));
281  vector<cv::Mat> rvecs, tvecs;
282 
283  const double cv_calib_err = cv::calibrateCamera(
284  objectPoints, imagePoints, imgSize, cameraMatrix, distCoeffs, rvecs,
285  tvecs, 0 /*flags*/);
286 
287  // Load matrix:
288  {
289  Eigen::Matrix3d M;
290  cv::cv2eigen(cameraMatrix, M);
291  out_camera_params.intrinsicParams = M;
292  }
293 
294  out_camera_params.dist.fill(0);
295  for (int k = 0; k < 5; k++)
296  out_camera_params.dist[k] = distCoeffs.ptr<double>()[k];
297 
298  // Load camera poses:
299  for (i = 0; i < valid_detected_imgs; i++)
300  {
301  CMatrixDouble44 HM;
302  HM.setZero();
303  HM(3, 3) = 1;
304 
305  {
306  // Convert rotation vectors -> rot matrices:
307  cv::Mat cv_rot;
308  cv::Rodrigues(rvecs[i], cv_rot);
309 
310  Eigen::Matrix3d rot;
311  cv::cv2eigen(cv_rot, rot);
312  HM.block<3, 3>(0, 0) = rot;
313  }
314 
315  {
317  cv::cv2eigen(tvecs[i], trans);
318  HM.block<3, 1>(0, 3) = trans;
319  }
320 
321  CPose3D p = CPose3D(0, 0, 0) - CPose3D(HM);
322 
323  images[pointsIdx2imageFile[i]].reconstructed_camera_pose = p;
324 
325  std::cout << "Img: "
326  << mrpt::system::extractFileName(pointsIdx2imageFile[i])
327  << ": " << p << std::endl;
328  }
329 
330  {
331  CConfigFileMemory cfg;
332  out_camera_params.saveToConfigFile("CAMERA_PARAMS", cfg);
333  std::cout << cfg.getContent() << std::endl;
334  }
335 
336  // ----------------------------------------
337  // Undistort images:
338  // ----------------------------------------
339  for (it = images.begin(); it != images.end(); ++it)
340  {
341  TImageCalibData& dat = it->second;
342  if (!dat.img_original.isExternallyStored())
344  dat.img_rectified, out_camera_params);
345  } // end undistort
346 
347  // -----------------------------------------------
348  // Reproject points to measure the fit sqr error
349  // -----------------------------------------------
350  double sqrErr = 0;
351 
352  for (i = 0; i < valid_detected_imgs; i++)
353  {
354  TImageCalibData& dat = images[pointsIdx2imageFile[i]];
355  if (dat.detected_corners.size() != CORNERS_COUNT) continue;
356 
357  // Reproject all the points into pixel coordinates:
358  // -----------------------------------------------------
359  vector<TPoint3D> lstPatternPoints(
360  CORNERS_COUNT); // Points as seen from the camera:
361  for (unsigned int p = 0; p < CORNERS_COUNT; p++)
362  lstPatternPoints[p] = TPoint3D(
363  pattern_obj_points[p].x, pattern_obj_points[p].y,
364  pattern_obj_points[p].z);
365 
366  vector<TPixelCoordf>& projectedPoints =
368  vector<TPixelCoordf>& projectedPoints_distorted =
370 
372  lstPatternPoints, // Input points
374  out_camera_params.intrinsicParams, // calib matrix
375  projectedPoints // Output points in pixels
376  );
377 
379  lstPatternPoints, // Input points
381  out_camera_params.intrinsicParams, // calib matrix
382  out_camera_params.getDistortionParamsAsVector(),
383  projectedPoints_distorted // Output points in pixels
384  );
385 
386  ASSERT_(projectedPoints.size() == CORNERS_COUNT);
387  ASSERT_(projectedPoints_distorted.size() == CORNERS_COUNT);
388 
389  for (unsigned int p = 0; p < CORNERS_COUNT; p++)
390  {
391  const double px = projectedPoints[p].x;
392  const double py = projectedPoints[p].y;
393 
394  const double px_d = projectedPoints_distorted[p].x;
395  const double py_d = projectedPoints_distorted[p].y;
396 
397  // Only draw if the img is NOT external:
398  if (!dat.img_original.isExternallyStored())
399  {
400  if (px >= 0 && px < imgSize.width && py >= 0 &&
401  py < imgSize.height)
402  cv::circle(
403  dat.img_rectified.asCvMatRef(), cv::Point(px, py),
404  4, CV_RGB(0, 0, 255));
405  }
406 
407  // Accumulate error:
408  sqrErr +=
409  square(px_d - dat.detected_corners[p].x) +
410  square(py_d - dat.detected_corners[p].y); // Error relative
411  // to the
412  // original
413  // (distorted)
414  // image.
415  }
416  }
417 
418  if (valid_detected_imgs)
419  {
420  sqrErr /= CORNERS_COUNT * valid_detected_imgs;
421  std::cout << "Average err. of reprojection: " << sqrt(sqrErr)
422  << " pixels (OpenCV error=" << cv_calib_err << ")\n";
423  }
424  if (out_MSE) *out_MSE = sqrt(sqrErr);
425 
426  return true;
427  }
428  catch (const std::exception& e)
429  {
430  std::cout << e.what() << std::endl;
431  return false;
432  }
433 #else
434  THROW_EXCEPTION("Function not available: MRPT was compiled without OpenCV");
435 #endif
436 }
This class implements a config file-like interface over a memory-stored string list.
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:75
uint32_t nrows
Definition: TCamera.h:40
double Scalar
Definition: KmUtils.h:43
mrpt::vision::TCalibrationStereoImageList images
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
std::map< std::string, TImageCalibData > TCalibrationImageList
A list of images, used in checkerBoardCameraCalibration.
cv::Mat & asCvMatRef()
Get a reference to the internal cv::Mat, which can be resized, etc.
Definition: CImage.cpp:227
STL namespace.
bool isExternallyStored() const noexcept
See setExternalStorage().
Definition: img/CImage.h:782
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...
void asCvMat(cv::Mat &out_img, copy_type_t copy_type) const
Makes a shallow or deep copy of this image into the provided cv::Mat.
Definition: CImage.cpp:217
CImage colorImage() const
Returns a color (RGB) version of the grayscale image, or a shallow copy of itself if it is already a ...
Definition: CImage.cpp:1861
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:50
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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:305
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.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
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::vector< mrpt::img::TPixelCoordf > detected_corners
At output, the detected corners (x,y) in pixel units.
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
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
return_t square(const num_t x)
Inline function for the square of a number.
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
void saveToConfigFile(const std::string &section, mrpt::config::CConfigFileBase &cfg) const
Save as a config block:
Definition: TCamera.cpp:143
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
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
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:62
void undistort(CImage &out_img, const mrpt::img::TCamera &cameraParams) const
Undistort the image according to some camera parameters, and returns an output undistorted image...
Definition: CImage.cpp:1685
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
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...
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
mrpt::img::CImage img_original
This image will be automatically loaded from the file name passed to checkerBoardCameraCalibration.
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:148
std::vector< double > getDistortionParamsAsVector() const
Get a vector with the distortion params of the camera.
Definition: TCamera.h:131



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020