12 #include <mrpt/3rdparty/do_opencv_includes.h>
18 #include <Eigen/Dense>
35 std::vector<double>& distortionParams,
bool normalize_image,
36 double* out_MSE,
bool skipDrawDetectedImgs,
37 bool useScaramuzzaAlternativeDetector)
44 skipDrawDetectedImgs, useScaramuzzaAlternativeDetector);
52 #include <opencv2/core/eigen.hpp>
62 bool normalize_image,
double* out_MSE,
63 [[maybe_unused]]
bool skipDrawDetectedImgs,
64 bool useScaramuzzaAlternativeDetector)
76 std::cout <<
"ERROR: No input images." << std::endl;
85 vector<cv::Point3f> pattern_obj_points(CORNERS_COUNT);
97 pattern_obj_points[k].z = 0;
104 TCalibrationImageList::iterator it;
118 "Error reading image: %s", it->first.c_str());
127 vector<vector<cv::Point3f>>
129 vector<vector<cv::Point2f>>
132 unsigned int valid_detected_imgs = 0;
133 vector<string> pointsIdx2imageFile;
134 cv::Size imgSize(0, 0);
137 for (i = 0, it =
images.begin(); it !=
images.end(); it++, i++)
147 imgSize = cv::Size(img_gray.getWidth(), img_gray.getHeight());
148 out_camera_params.
ncols = imgSize.width;
149 out_camera_params.
nrows = imgSize.height;
153 if (imgSize.height != (
int)img_gray.getHeight() ||
154 imgSize.width != (
int)img_gray.getWidth())
156 std::cout <<
"ERROR: All the images must have the same size"
164 unsigned corners_count;
165 bool corners_found =
false;
167 corners_count = CORNERS_COUNT;
169 vector<cv::Point2f> this_img_pts(
177 vector<TPixelCoordf> detectedCoords;
181 useScaramuzzaAlternativeDetector);
183 corners_count = detectedCoords.size();
186 ASSERT_(detectedCoords.size() <= CORNERS_COUNT);
187 for (
size_t p = 0; p < detectedCoords.size(); p++)
189 this_img_pts[p].x = detectedCoords[p].x;
190 this_img_pts[p].y = detectedCoords[p].y;
193 if (corners_found && corners_count != CORNERS_COUNT)
194 corners_found =
false;
199 corners_found ?
"DETECTED" :
"NOT DETECTED");
206 for (y = 0, k = 0; y < check_size.height; y++)
207 for (x = 0; x < check_size.width; x++, k++)
209 this_img_pts[k].x, this_img_pts[k].y);
216 cv::Point prev_pt = cvPoint(0, 0);
217 const int line_max = 8;
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);
235 for (y = 0, k = 0; y < check_size.height; y++)
238 for (x = 0; x < check_size.width; x++, k++)
240 cv::Point pt{cvRound(this_img_pts[k].x),
241 cvRound(this_img_pts[k].y)};
243 if (k != 0) cv::line(rgb_img, prev_pt, pt, color);
246 rgb_img, cv::Point(pt.x - r, pt.y - r),
247 cv::Point(pt.x + r, pt.y + r), color);
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);
258 pointsIdx2imageFile.push_back(it->first);
259 imagePoints.push_back(this_img_pts);
260 objectPoints.push_back(pattern_obj_points);
262 valid_detected_imgs++;
267 std::cout << valid_detected_imgs <<
" valid images." << std::endl;
268 if (!valid_detected_imgs)
270 std::cout <<
"ERROR: No valid images. Perhaps the checkerboard "
280 cv::Mat cameraMatrix, distCoeffs(1, 5, CV_64F, cv::Scalar::all(0));
281 vector<cv::Mat> rvecs, tvecs;
283 const double cv_calib_err = cv::calibrateCamera(
284 objectPoints, imagePoints, imgSize, cameraMatrix, distCoeffs, rvecs,
290 cv::cv2eigen(cameraMatrix, M);
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];
299 for (i = 0; i < valid_detected_imgs; i++)
308 cv::Rodrigues(rvecs[i], cv_rot);
311 cv::cv2eigen(cv_rot, rot);
312 HM.
block<3, 3>(0, 0) = rot;
317 cv::cv2eigen(tvecs[i], trans);
318 HM.
block<3, 1>(0, 3) = trans;
323 images[pointsIdx2imageFile[i]].reconstructed_camera_pose = p;
327 <<
": " << p << std::endl;
352 for (i = 0; i < valid_detected_imgs; i++)
359 vector<TPoint3D> lstPatternPoints(
361 for (
unsigned int p = 0; p < CORNERS_COUNT; p++)
363 pattern_obj_points[p].x, pattern_obj_points[p].y,
364 pattern_obj_points[p].z);
366 vector<TPixelCoordf>& projectedPoints =
368 vector<TPixelCoordf>& projectedPoints_distorted =
383 projectedPoints_distorted
386 ASSERT_(projectedPoints.size() == CORNERS_COUNT);
387 ASSERT_(projectedPoints_distorted.size() == CORNERS_COUNT);
389 for (
unsigned int p = 0; p < CORNERS_COUNT; p++)
391 const double px = projectedPoints[p].x;
392 const double py = projectedPoints[p].y;
394 const double px_d = projectedPoints_distorted[p].x;
395 const double py_d = projectedPoints_distorted[p].y;
400 if (px >= 0 && px < imgSize.width && py >= 0 &&
404 4, CV_RGB(0, 0, 255));
418 if (valid_detected_imgs)
420 sqrErr /= CORNERS_COUNT * valid_detected_imgs;
421 std::cout <<
"Average err. of reprojection: " << sqrt(sqrErr)
422 <<
" pixels (OpenCV error=" << cv_calib_err <<
")\n";
424 if (out_MSE) *out_MSE = sqrt(sqrErr);
428 catch (
const std::exception& e)
430 std::cout << e.what() << std::endl;
434 THROW_EXCEPTION(
"Function not available: MRPT was compiled without OpenCV");