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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019