diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index 23cfa98889..fe4100d831 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -861,6 +861,111 @@ TEST_F(fisheyeTest, CalibrationWithDifferentPointsNumber) cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6)); } +TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations) +{ + const int n_images = 34; + + const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY"); + + std::vector > leftPoints(n_images); + std::vector > rightPoints(n_images); + std::vector > objectPoints(n_images); + + cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ); + CV_Assert(fs_left.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_left[cv::format("image_%d", i )] >> leftPoints[i]; + fs_left.release(); + + cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ); + CV_Assert(fs_right.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_right[cv::format("image_%d", i )] >> rightPoints[i]; + fs_right.release(); + + cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ); + CV_Assert(fs_object.isOpened()); + for(int i = 0; i < n_images; ++i) + fs_object[cv::format("image_%d", i )] >> objectPoints[i]; + fs_object.release(); + + cv::Matx33d K1, K2, theR; + cv::Vec3d theT; + cv::Vec4d D1, D2; + + std::vector rvecs, tvecs; + + int flag = 0; + flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC; + flag |= cv::fisheye::CALIB_CHECK_COND; + flag |= cv::fisheye::CALIB_FIX_SKEW; + + double rmsErrorStereoCalib = cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints, + K1, D1, K2, D2, imageSize, theR, theT, rvecs, tvecs, flag, + cv::TermCriteria(3, 12, 0)); + + std::vector reprojectedImgPts[2] = {std::vector(n_images), std::vector(n_images)}; + size_t totalPoints = 0; + float totalMSError[2] = { 0, 0 }, viewMSError[2]; + for( size_t i = 0; i < n_images; i++ ) + { + cv::Matx33d viewRotMat1, viewRotMat2; + cv::Vec3d viewT1, viewT2; + cv::Mat rVec; + cv::Rodrigues( rvecs[i], rVec ); + rVec.convertTo(viewRotMat1, CV_64F); + tvecs[i].convertTo(viewT1, CV_64F); + + viewRotMat2 = theR * viewRotMat1; + cv::Vec3d T2t = theR * viewT1; + viewT2 = T2t + theT; + + cv::Vec3d viewRotVec1, viewRotVec2; + cv::Rodrigues(viewRotMat1, viewRotVec1); + cv::Rodrigues(viewRotMat2, viewRotVec2); + + double alpha1 = K1(0, 1) / K1(0, 0); + double alpha2 = K2(0, 1) / K2(0, 0); + cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[0], viewRotVec1, viewT1, K1, D1, alpha1); + cv::fisheye::projectPoints(objectPoints[i], reprojectedImgPts[1], viewRotVec2, viewT2, K2, D2, alpha2); + + viewMSError[0] = cv::norm(leftPoints[i], reprojectedImgPts[0], cv::NORM_L2SQR); + viewMSError[1] = cv::norm(rightPoints[i], reprojectedImgPts[1], cv::NORM_L2SQR); + + size_t n = objectPoints[i].size(); + totalMSError[0] += viewMSError[0]; + totalMSError[1] += viewMSError[1]; + totalPoints += n; + } + double rmsErrorFromReprojectedImgPts = std::sqrt((totalMSError[0] + totalMSError[1]) / (2 * totalPoints)); + + cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523, + -0.06956823121068059, 0.9975601387249519, 0.005833595226966235, + -0.006071257768382089, -0.006271040135405457, 0.9999619062167968); + cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699); + cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412, + 0, 562.849402029712, 380.555455380889, + 0, 0, 1); + + cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359, + 0, 561.90171021422, 380.401340535339, + 0, 0, 1); + + cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771); + cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222); + + EXPECT_MAT_NEAR(theR, R_correct, 1e-10); + EXPECT_MAT_NEAR(theT, T_correct, 1e-10); + + EXPECT_MAT_NEAR(K1, K1_correct, 1e-10); + EXPECT_MAT_NEAR(K2, K2_correct, 1e-10); + + EXPECT_MAT_NEAR(D1, D1_correct, 1e-10); + EXPECT_MAT_NEAR(D2, D2_correct, 1e-10); + + EXPECT_NEAR(rmsErrorStereoCalib, rmsErrorFromReprojectedImgPts, 1e-4); +} + TEST_F(fisheyeTest, estimateNewCameraMatrixForUndistortRectify) { cv::Size size(1920, 1080);