From bd73b7bcf51319e552a28c2b4345c0043970106d Mon Sep 17 00:00:00 2001 From: Alexander Smorkalov <2536374+asmorkalov@users.noreply.github.com> Date: Fri, 16 Feb 2024 17:54:40 +0300 Subject: [PATCH] Merge pull request #25028 from asmorkalov:as/fisheye_solvepnp solvePnP implementation for Fisheye camera model #25028 Credits to Linfei Pan Extracted from https://github.com/opencv/opencv/pull/24052 **Warning:** The patch changes Obj-C generator behaviour and adds "fisheye_" prefix for all ObjC functions from namespace. ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [ ] The feature is well documented and sample code can be built with the project CMake Co-authored-by: lpanaf Co-authored-by: Vadim Levin --- modules/calib3d/include/opencv2/calib3d.hpp | 39 +++++++++++++++++++++ modules/calib3d/misc/objc/gen_dict.json | 3 ++ modules/calib3d/src/fisheye.cpp | 14 ++++++++ modules/calib3d/test/test_fisheye.cpp | 22 ++++++++++++ 4 files changed, 78 insertions(+) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 7b155636fe..65beca3283 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -4055,6 +4055,45 @@ optimization. It is the \f$max(width,height)/\pi\f$ or the provided \f$f_x\f$, \ OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC, TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); + /** + @brief Finds an object pose from 3D-2D point correspondences for fisheye camera moodel. + + @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or + 1xN/Nx1 3-channel, where N is the number of points. vector\ can be also passed here. + @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, + where N is the number of points. vector\ can be also passed here. + @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . + @param distCoeffs Input vector of distortion coefficients (4x1/1x4). + @param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from + the model coordinate system to the camera coordinate system. + @param tvec Output translation vector. + @param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses + the provided rvec and tvec values as initial approximations of the rotation and translation + vectors, respectively, and further optimizes them. + @param flags Method for solving a PnP problem: see @ref calib3d_solvePnP_flags + This function returns the rotation and the translation vectors that transform a 3D point expressed in the object + coordinate frame to the camera coordinate frame, using different methods: + - P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): need 4 input points to return a unique solution. + - @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar. + - @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation. + Number of input points must be 4. Object points must be defined in the following order: + - point 0: [-squareLength / 2, squareLength / 2, 0] + - point 1: [ squareLength / 2, squareLength / 2, 0] + - point 2: [ squareLength / 2, -squareLength / 2, 0] + - point 3: [-squareLength / 2, -squareLength / 2, 0] + - for all the other flags, number of input points must be >= 4 and object points can be in any configuration. + @param criteria Termination criteria for internal undistortPoints call. + The function interally undistorts points with @ref undistortPoints and call @ref cv::solvePnP, + thus the input are very similar. Check there and Perspective-n-Points is described in @ref calib3d_solvePnP + for more information. + */ + CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, + bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE, + TermCriteria criteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 10, 1e-8) + ); + //! @} calib3d_fisheye } // end namespace fisheye diff --git a/modules/calib3d/misc/objc/gen_dict.json b/modules/calib3d/misc/objc/gen_dict.json index bace946846..655ca2426d 100644 --- a/modules/calib3d/misc/objc/gen_dict.json +++ b/modules/calib3d/misc/objc/gen_dict.json @@ -1,4 +1,7 @@ { + "namespaces_dict": { + "cv.fisheye": "fisheye" + }, "func_arg_fix" : { "Calib3d" : { "findCirclesGrid" : { "blobDetector" : {"defval" : "cv::SimpleBlobDetector::create()"} } diff --git a/modules/calib3d/src/fisheye.cpp b/modules/calib3d/src/fisheye.cpp index 911e16da9e..751a1aa6da 100644 --- a/modules/calib3d/src/fisheye.cpp +++ b/modules/calib3d/src/fisheye.cpp @@ -1148,6 +1148,20 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO return rms; } +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// cv::fisheye::solvePnP + +bool cv::fisheye::solvePnP( InputArray opoints, InputArray ipoints, + InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, + int flags, TermCriteria criteria) +{ + + Mat imagePointsNormalized; + cv::fisheye::undistortPoints(ipoints, imagePointsNormalized, cameraMatrix, distCoeffs, noArray(), cameraMatrix, criteria); + return cv::solvePnP(opoints, imagePointsNormalized, cameraMatrix, noArray(), rvec, tvec, useExtrinsicGuess, flags); +} + namespace cv{ namespace { void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows) { diff --git a/modules/calib3d/test/test_fisheye.cpp b/modules/calib3d/test/test_fisheye.cpp index f48635e664..2479dac8a8 100644 --- a/modules/calib3d/test/test_fisheye.cpp +++ b/modules/calib3d/test/test_fisheye.cpp @@ -150,6 +150,28 @@ TEST_F(fisheyeTest, distortUndistortPoints) } } +TEST_F(fisheyeTest, solvePnP) +{ + const int n = 16; + + cv::Mat obj_points(1, n, CV_64FC3); + theRNG().fill(obj_points, cv::RNG::NORMAL, 2, 1); + obj_points = cv::abs(obj_points) * 10; + + cv::Mat rvec; + cv::Rodrigues(this->R, rvec); + cv::Mat img_points; + cv::fisheye::projectPoints(obj_points, img_points, rvec, this->T, this->K, this->D); + + cv::Mat rvec_pred; + cv::Mat tvec_pred; + bool converged = cv::fisheye::solvePnP(obj_points, img_points, this->K, this->D, rvec_pred, tvec_pred); + EXPECT_MAT_NEAR(rvec, rvec_pred, 1e-6); + EXPECT_MAT_NEAR(this->T, tvec_pred, 1e-6); + + ASSERT_TRUE(converged); +} + TEST_F(fisheyeTest, undistortImage) { // we use it to reduce patch size for images in testdata