diff --git a/modules/calib/include/opencv2/calib.hpp b/modules/calib/include/opencv2/calib.hpp index 245829bdc1..9b2d009498 100644 --- a/modules/calib/include/opencv2/calib.hpp +++ b/modules/calib/include/opencv2/calib.hpp @@ -453,7 +453,8 @@ enum { CALIB_USE_INTRINSIC_GUESS = 0x00001, //!< Use user provided intrinsics as // fisheye only flags CALIB_RECOMPUTE_EXTRINSIC = (1 << 23), //!< For fisheye model only. Recompute board position on each calibration iteration CALIB_CHECK_COND = (1 << 24), //!< For fisheye model only. Check SVD decomposition quality for each frame during extrinsics estimation - CALIB_FIX_SKEW = (1 << 25) //!< For fisheye model only. Skew coefficient (alpha) is set to zero and stay zero. + CALIB_FIX_SKEW = (1 << 25), //!< For fisheye model only. Skew coefficient (alpha) is set to zero and stay zero. + CALIB_STEREO_REGISTRATION = (1 << 26) //!< For multiview calibration only. Use stereo correspondence approach for initial extrinsics guess. Limitation: all cameras should have the same type. }; enum HandEyeCalibrationMethod diff --git a/modules/calib/src/multiview_calibration.cpp b/modules/calib/src/multiview_calibration.cpp index 680fb5ec87..ba01051a57 100644 --- a/modules/calib/src/multiview_calibration.cpp +++ b/modules/calib/src/multiview_calibration.cpp @@ -260,6 +260,69 @@ static void thresholdPatternCameraAngles (int NUM_PATTERN_PTS, double THR_PATTER } } +static void pairwiseRegistration (const std::vector> &pairs, + const cv::Mat &models, const std::vector &objPoints_norm, + const std::vector> &imagePoints, const std::vector> &overlaps, + const std::vector> &detection_mask_mat, const std::vector &Ks, + const std::vector &distortions, std::vector &Rs_vec, std::vector &Ts_vec, + Mat &intrinsic_flags, int extrinsic_flags = 0) { + CV_UNUSED(intrinsic_flags); + const int NUM_FRAMES = (int)objPoints_norm.size(); + const int NUM_CAMERAS = (int)detection_mask_mat.size(); + std::vector Rs_prior; + std::vector Ts_prior; + if (extrinsic_flags & cv::CALIB_USE_EXTRINSIC_GUESS) { + Rs_prior.resize(NUM_CAMERAS); + Ts_prior.resize(NUM_CAMERAS); + for (int i = 0; i < NUM_CAMERAS; i++) { + Rs_vec[i].copyTo(Rs_prior[i]); + Ts_vec[i].copyTo(Ts_prior[i]); + } + } + + for (const auto &pair : pairs) { + const int c1 = pair.first, c2 = pair.second, overlap = overlaps[c1][c2]; + // prepare image points of two cameras and grid points + std::vector image_points1, image_points2, grid_points1, grid_points2; + grid_points1.reserve(overlap); + grid_points2.reserve(overlap); + image_points1.reserve(overlap); + image_points2.reserve(overlap); + for (int f = 0; f < NUM_FRAMES; f++) { + if (detection_mask_mat[c1][f] && detection_mask_mat[c2][f]) { + grid_points1.emplace_back(objPoints_norm[f]); + grid_points2.emplace_back(objPoints_norm[f]); + image_points1.emplace_back(imagePoints[c1][f]); + image_points2.emplace_back(imagePoints[c2][f]); + } + } + Matx33d R; + Vec3d T; + + if (extrinsic_flags & cv::CALIB_USE_EXTRINSIC_GUESS) { + R = Rs_prior[c2] * Rs_prior[c1].t(); + T = -R * Ts_prior[c1] + Ts_prior[c2]; + } + + extrinsic_flags |= CALIB_FIX_INTRINSIC; + registerCameras(grid_points1, grid_points2, image_points1, image_points2, + Ks[c1], distortions[c1], cv::CameraModel(models.at(c1)), + Ks[c2], distortions[c2], cv::CameraModel(models.at(c2)), + R, T, noArray(), noArray(), noArray(), noArray(), noArray(), extrinsic_flags); + + // R_0 = I + // R_ij = R_i R_j^T => R_i = R_ij R_j + // t_ij = ti - R_ij tj => t_i = t_ij + R_ij t_j + if (c1 == 0) { + Rs_vec[c2] = R; + Ts_vec[c2] = T; + } else { + Rs_vec[c2] = Matx33d(Mat(R * Rs_vec[c1])); + Ts_vec[c2] = Vec3d(Mat(T + R * Ts_vec[c1])); + } + } +} + static void pairwiseStereoCalibration (const std::vector> &pairs, const cv::Mat &models, const std::vector &objPoints_norm, const std::vector> &imagePoints, const std::vector> &overlaps, @@ -528,19 +591,22 @@ double calibrateMultiview( CV_CheckEQ(detection_mask_.type(), CV_8U, "detectionMask must be of type CV_8U"); CV_CheckEQ(models_mat.type(), CV_8U, "models must be of type CV_8U"); - bool is_fisheye = false; - bool is_pinhole = false; + if(flags & cv::CALIB_STEREO_REGISTRATION) + { + bool is_fisheye = false; + bool is_pinhole = false; - for (int i = 0; i < (int)models_mat.total(); i++) { - if (models_mat.at(i) == cv::CALIB_MODEL_FISHEYE) { - is_fisheye = true; - } else if (models_mat.at(i) == cv::CALIB_MODEL_PINHOLE) { - is_pinhole = true; - } else { - CV_Error(Error::StsBadArg, "Unsupported camera model"); + for (int i = 0; i < (int)models_mat.total(); i++) { + if (models_mat.at(i) == cv::CALIB_MODEL_FISHEYE) { + is_fisheye = true; + } else if (models_mat.at(i) == cv::CALIB_MODEL_PINHOLE) { + is_pinhole = true; + } else { + CV_Error(Error::StsBadArg, "Unsupported camera model"); + } } + CV_CheckEQ(is_fisheye && is_pinhole, false, "Mix of pinhole and fisheye cameras is not supported with CALIB_STEREO_REGISTRATION flag"); } - CV_CheckEQ(is_fisheye && is_pinhole, false, "Mix of pinhole and fisheye cameras is not supported for now"); // equal number of cameras CV_Assert(imageSize.size() == imagePoints.size()); @@ -732,8 +798,14 @@ double calibrateMultiview( } pairs_mat.copyTo(initializationPairs); } - multiview::pairwiseStereoCalibration(pairs, models_mat, objPoints_norm, imagePoints, - overlaps, detection_mask_mat, Ks_vec, distortions_vec, Rs_vec, Ts_vec, flagsForIntrinsics_mat); + + if(flags & cv::CALIB_STEREO_REGISTRATION) { + multiview::pairwiseStereoCalibration(pairs, models_mat, objPoints_norm, imagePoints, + overlaps, detection_mask_mat, Ks_vec, distortions_vec, Rs_vec, Ts_vec, flagsForIntrinsics_mat); + } else { + multiview::pairwiseRegistration(pairs, models_mat, objPoints_norm, imagePoints, + overlaps, detection_mask_mat, Ks_vec, distortions_vec, Rs_vec, Ts_vec, flagsForIntrinsics_mat); + } const int NUM_VALID_FRAMES = countNonZero(valid_frames); const int nparams = (NUM_VALID_FRAMES + NUM_CAMERAS - 1) * 6; // rvecs + tvecs (6) diff --git a/modules/calib/test/test_multiview_calib.cpp b/modules/calib/test/test_multiview_calib.cpp index 361d6c4a58..160bc5f8ca 100644 --- a/modules/calib/test/test_multiview_calib.cpp +++ b/modules/calib/test/test_multiview_calib.cpp @@ -259,6 +259,8 @@ struct MultiViewTest : public ::testing::Test for (size_t c = 1; c < num_cameras; c++) { validateCameraPose(Rs[c], Ts[c], Rs_gt[c], Ts_gt[c], angle_tol, pos_tol); + double distance0 = cv::norm(Rs[c].t()*Ts[c]); + CV_LOG_INFO(NULL, "distance to camera #0: " << distance0); } } }; @@ -416,7 +418,7 @@ TEST_F(MultiViewTest, OneLineInitialGuess) cv::Rodrigues(Rs[c], Rs_rvec[c]); } - int flags = cv::CALIB_USE_EXTRINSIC_GUESS | cv::CALIB_USE_INTRINSIC_GUESS; + int flags = cv::CALIB_USE_EXTRINSIC_GUESS | cv::CALIB_USE_INTRINSIC_GUESS | cv::CALIB_STEREO_REGISTRATION; double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models, Rs_rvec, Ts, Ks, distortions, flags, flagsForIntrinsics); CV_LOG_INFO(NULL, "RMS: " << rms); @@ -500,6 +502,75 @@ TEST_F(MultiViewTest, CamsToFloor) validateAllPoses(Rs_gt, Ts_gt, Rs, Ts); } +TEST_F(MultiViewTest, Hetero) +{ + applyTestTag(CV_TEST_TAG_VERYLONG); + const string root = cvtest::TS::ptr()->get_data_path() + "cv/cameracalibration/multiview/3cams-hetero/"; + const std::vector cam_names = {"cam_7", "cam_4", "cam_8"}; + std::vector image_sizes = {{1920, 1080}, {1920, 1080}, {2048, 2048}}; + std::vector models = { cv::CALIB_MODEL_PINHOLE, cv::CALIB_MODEL_PINHOLE, cv::CALIB_MODEL_FISHEYE}; + + double rs_1_gt_data[9] = { + 0.9927140815671712, 0.1070962138895326, 0.05521913824730116, + -0.05355858010980671, -0.01832224712027507, 0.9983966014350634, + 0.1079362346706077, -0.994079823872807, -0.0124528315711911 + }; + double rs_2_gt_data[9] = { + 0.9974414183162762, 0.06892036265048015, 0.0189894876008139, + -0.06886936047115397, 0.9976201373221448, -0.003327581349727079, + -0.0191736333413733, 0.002011273594291581, 0.9998141460106571 + }; + + double ts_1_gt_data[3] = {0.5106665738153067, -0.3450096979616873, 0.7854530821015541}; + double ts_2_gt_data[3] = {1.01304902944076, 0.01197702701032772, -0.01801263208619407}; + + std::vector Rs_gt = { + cv::Mat::eye(3, 3, CV_64FC1), + cv::Mat(3, 3, CV_64FC1, rs_1_gt_data), + cv::Mat(3, 3, CV_64FC1, rs_2_gt_data) + }; + + std::vector Ts_gt = { + cv::Mat::zeros(3, 1, CV_64FC1), + cv::Mat(3, 1, CV_64FC1, ts_1_gt_data), + cv::Mat(3, 1, CV_64FC1, ts_2_gt_data) + }; + + const int num_frames = 127; + std::vector> image_points_all; + cv::Mat visibility; + loadImagePoints(root, cam_names, num_frames, image_points_all, visibility); + EXPECT_EQ(cam_names.size(), image_points_all.size()); + for(size_t i = 0; i < cam_names.size(); i++) + { + EXPECT_TRUE(!image_points_all[i].empty()); + } + + std::vector board_pattern = genAsymmetricObjectPoints(); + std::vector> objPoints(num_frames, board_pattern); + + std::vector flagsForIntrinsics= { + cv::CALIB_RATIONAL_MODEL, cv::CALIB_RATIONAL_MODEL, + cv::CALIB_RECOMPUTE_EXTRINSIC | cv::CALIB_FIX_SKEW}; + + std::vector Ks, distortions, Rs, Rs_rvec, Ts; + double rms = calibrateMultiview(objPoints, image_points_all, image_sizes, visibility, models, + Rs_rvec, Ts, Ks, distortions, 0, flagsForIntrinsics); + CV_LOG_INFO(NULL, "RMS: " << rms); + + EXPECT_LE(rms, 2.5); + + Rs.resize(Rs_rvec.size()); + for(int c = 0; c < 3; c++) + { + cv::Rodrigues(Rs_rvec[c], Rs[c]); + CV_LOG_INFO(NULL, "R" << c << ":" << Rs[c]); + CV_LOG_INFO(NULL, "T" << c << ":" << Ts[c]); + } + + validateAllPoses(Rs_gt, Ts_gt, Rs, Ts); +} + struct RegisterCamerasTest: public MultiViewTest { void filterPoints(const std::vector>& image_points_all, @@ -523,7 +594,7 @@ TEST_F(RegisterCamerasTest, hetero1) const std::vector cam_names = {"cam_7", "cam_4"}; std::vector image_sizes = {{1920, 1080}, {2048, 2048}}; std::vector models = {cv::CALIB_MODEL_PINHOLE, cv::CALIB_MODEL_FISHEYE}; - std::vector flagsForIntrinsics = {cv::CALIB_RATIONAL_MODEL, cv::CALIB_RECOMPUTE_EXTRINSIC+cv::CALIB_FIX_SKEW}; + std::vector flagsForIntrinsics = {cv::CALIB_RATIONAL_MODEL, cv::CALIB_RECOMPUTE_EXTRINSIC | cv::CALIB_FIX_SKEW}; const int num_frames = 127; std::vector board_pattern = genAsymmetricObjectPoints(); @@ -580,7 +651,7 @@ TEST_F(RegisterCamerasTest, hetero2) const std::vector cam_names = {"cam_4", "cam_8"}; std::vector image_sizes = {{2048, 2048}, {1920, 1080}}; std::vector models = {cv::CALIB_MODEL_FISHEYE, cv::CALIB_MODEL_PINHOLE}; - std::vector flagsForIntrinsics = { cv::CALIB_RECOMPUTE_EXTRINSIC+cv::CALIB_FIX_SKEW, cv::CALIB_RATIONAL_MODEL}; + std::vector flagsForIntrinsics = { cv::CALIB_RECOMPUTE_EXTRINSIC | cv::CALIB_FIX_SKEW, cv::CALIB_RATIONAL_MODEL}; const int num_frames = 127; std::vector board_pattern = genAsymmetricObjectPoints();