diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index aedbaf930a..9fc6773450 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -3835,10 +3835,25 @@ namespace fisheye @param distorted Output array of image points, 1xN/Nx1 2-channel, or vector\ . Note that the function assumes the camera intrinsic matrix of the undistorted points to be identity. - This means if you want to distort image points you have to multiply them with \f$K^{-1}\f$. + This means if you want to distort image points you have to multiply them with \f$K^{-1}\f$ or + use another function overload. */ CV_EXPORTS_W void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0); + /** @overload + Overload of distortPoints function to handle cases when undistorted points are got with non-identity + camera matrix, e.g. output of #estimateNewCameraMatrixForUndistortRectify. + @param undistorted Array of object points, 1xN/Nx1 2-channel (or vector\ ), where N is + the number of points in the view. + @param Kundistorted Camera intrinsic matrix used as new camera matrix for undistortion. + @param K Camera intrinsic matrix \f$cameramatrix{K}\f$. + @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. + @param alpha The skew coefficient. + @param distorted Output array of image points, 1xN/Nx1 2-channel, or vector\ . + @sa estimateNewCameraMatrixForUndistortRectify + */ + CV_EXPORTS_W void distortPoints(InputArray undistorted, OutputArray distorted, InputArray Kundistorted, InputArray K, InputArray D, double alpha = 0); + /** @brief Undistorts 2D points using fisheye model @param distorted Array of object points, 1xN/Nx1 2-channel (or vector\ ), where N is the diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 751a1aa6da..4aec4324e0 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -315,6 +315,48 @@ void cv::fisheye::distortPoints(InputArray undistorted, OutputArray distorted, I } } +void cv::fisheye::distortPoints(InputArray _undistorted, OutputArray distorted, InputArray Kundistorted, InputArray K, InputArray D, double alpha) +{ + CV_INSTRUMENT_REGION(); + + CV_Assert(_undistorted.type() == CV_32FC2 || _undistorted.type() == CV_64FC2); + CV_Assert(Kundistorted.size() == Size(3,3) && (Kundistorted.type() == CV_32F || Kundistorted.type() == CV_64F)); + + cv::Mat undistorted = _undistorted.getMat(); + cv::Mat normalized(undistorted.size(), CV_64FC2); + + Mat Knew = Kundistorted.getMat(); + + double cx, cy, fx, fy; + if (Knew.depth() == CV_32F) + { + fx = (double)Knew.at(0, 0); + fy = (double)Knew.at(1, 1); + cx = (double)Knew.at(0, 2); + cy = (double)Knew.at(1, 2); + } + else + { + fx = Knew.at(0, 0); + fy = Knew.at(1, 1); + cx = Knew.at(0, 2); + cy = Knew.at(1, 2); + } + + size_t n = undistorted.total(); + const Vec2f* Xf = undistorted.ptr(); + const Vec2d* Xd = undistorted.ptr(); + Vec2d* normXd = normalized.ptr(); + for (size_t i = 0; i < n; i++) + { + Vec2d p = undistorted.depth() == CV_32F ? (Vec2d)Xf[i] : Xd[i]; + normXd[i][0] = (p[0] - cx) / fx; + normXd[i][1] = (p[1] - cy) / fy; + } + + cv::fisheye::distortPoints(normalized, distorted, K, D, alpha); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// cv::fisheye::undistortPoints diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index 36b7d0d653..d7368c3190 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -107,7 +107,6 @@ TEST_F(fisheyeTest, distortUndistortPoints) int height = imageSize.height; /* Create test points */ - std::vector points0Vector; cv::Mat principalPoints = (cv::Mat_(5, 2) << K(0, 2), K(1, 2), // (cx, cy) /* Image corners */ 0, 0, @@ -150,6 +149,95 @@ TEST_F(fisheyeTest, distortUndistortPoints) } } +TEST_F(fisheyeTest, distortUndistortPointsNewCameraFixed) +{ + int width = imageSize.width; + int height = imageSize.height; + + /* Random points inside image */ + cv::Mat xy[2] = {}; + xy[0].create(100, 1, CV_64F); + theRNG().fill(xy[0], cv::RNG::UNIFORM, 0, width); // x + xy[1].create(100, 1, CV_64F); + theRNG().fill(xy[1], cv::RNG::UNIFORM, 0, height); // y + + cv::Mat randomPoints; + merge(xy, 2, randomPoints); + + cv::Mat points0 = randomPoints; + cv::Mat Reye = cv::Mat::eye(3, 3, CV_64FC1); + + cv::Mat Knew; + cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, imageSize, Reye, Knew); + + /* Distort -> Undistort */ + cv::Mat distortedPoints; + cv::fisheye::distortPoints(points0, distortedPoints, Knew, K, D); + cv::Mat undistortedPoints; + cv::fisheye::undistortPoints(distortedPoints, undistortedPoints, K, D, Reye, Knew); + + EXPECT_MAT_NEAR(points0, undistortedPoints, 1e-8); + + /* Undistort -> Distort */ + cv::fisheye::undistortPoints(points0, undistortedPoints, K, D, Reye, Knew); + cv::fisheye::distortPoints(undistortedPoints, distortedPoints, Knew, K, D); + + EXPECT_MAT_NEAR(points0, distortedPoints, 1e-8); +} + +TEST_F(fisheyeTest, distortUndistortPointsNewCameraRandom) +{ + int width = imageSize.width; + int height = imageSize.height; + + /* Create test points */ + std::vector points0Vector; + cv::Mat principalPoints = (cv::Mat_(5, 2) << K(0, 2), K(1, 2), // (cx, cy) + /* Image corners */ + 0, 0, + 0, height, + width, 0, + width, height + ); + + /* Random points inside image */ + cv::Mat xy[2] = {}; + xy[0].create(100, 1, CV_64F); + theRNG().fill(xy[0], cv::RNG::UNIFORM, 0, width); // x + xy[1].create(100, 1, CV_64F); + theRNG().fill(xy[1], cv::RNG::UNIFORM, 0, height); // y + + cv::Mat randomPoints; + merge(xy, 2, randomPoints); + + cv::Mat points0; + cv::Mat Reye = cv::Mat::eye(3, 3, CV_64FC1); + cv::vconcat(principalPoints.reshape(2), randomPoints, points0); + + /* Test with random D set */ + for (size_t i = 0; i < 10; ++i) { + cv::Mat distortion(1, 4, CV_64F); + theRNG().fill(distortion, cv::RNG::UNIFORM, -0.001, 0.001); + + cv::Mat Knew; + cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, distortion, imageSize, Reye, Knew); + + /* Distort -> Undistort */ + cv::Mat distortedPoints; + cv::fisheye::distortPoints(points0, distortedPoints, Knew, K, distortion); + cv::Mat undistortedPoints; + cv::fisheye::undistortPoints(distortedPoints, undistortedPoints, K, distortion, Reye, Knew); + + EXPECT_MAT_NEAR(points0, undistortedPoints, 1e-8); + + /* Undistort -> Distort */ + cv::fisheye::undistortPoints(points0, undistortedPoints, K, distortion, Reye, Knew); + cv::fisheye::distortPoints(undistortedPoints, distortedPoints, Knew, K, distortion); + + EXPECT_MAT_NEAR(points0, distortedPoints, 1e-8); + } +} + TEST_F(fisheyeTest, solvePnP) { const int n = 16;