From b89181dfad6fe4adfb4218426908eed2c0046c3a Mon Sep 17 00:00:00 2001 From: Vladislav Sovrasov Date: Mon, 16 Jan 2017 14:44:05 +0300 Subject: [PATCH] Add useExtrinsicGuess flag in estimatePoseBoard() for aruco and charuco --- modules/aruco/include/opencv2/aruco.hpp | 5 +++-- modules/aruco/include/opencv2/aruco/charuco.hpp | 6 ++++-- modules/aruco/src/aruco.cpp | 4 ++-- modules/aruco/src/charuco.cpp | 11 +++++++++-- modules/aruco/test/test_boarddetection.cpp | 2 +- 5 files changed, 19 insertions(+), 9 deletions(-) diff --git a/modules/aruco/include/opencv2/aruco.hpp b/modules/aruco/include/opencv2/aruco.hpp index e9e88c513..e36ef61dd 100644 --- a/modules/aruco/include/opencv2/aruco.hpp +++ b/modules/aruco/include/opencv2/aruco.hpp @@ -331,8 +331,9 @@ class CV_EXPORTS_W GridBoard : public Board { * @param distCoeffs vector of distortion coefficients * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements * @param rvec Output vector (e.g. cv::Mat) corresponding to the rotation vector of the board - * (@sa Rodrigues). Used as initial guess if not empty. + * (see cv::Rodrigues). Used as initial guess if not empty. * @param tvec Output vector (e.g. cv::Mat) corresponding to the translation vector of the board. + * @param useExtrinsicGuess defines whether initial guess for \b rvec and \b tvec will be used or not. * Used as initial guess if not empty. * * This function receives the detected markers and returns the pose of a marker board composed @@ -346,7 +347,7 @@ class CV_EXPORTS_W GridBoard : public Board { */ CV_EXPORTS_W int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr &board, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, - OutputArray tvec); + OutputArray tvec, bool useExtrinsicGuess = false); diff --git a/modules/aruco/include/opencv2/aruco/charuco.hpp b/modules/aruco/include/opencv2/aruco/charuco.hpp index 7be82ed57..be535c760 100644 --- a/modules/aruco/include/opencv2/aruco/charuco.hpp +++ b/modules/aruco/include/opencv2/aruco/charuco.hpp @@ -174,8 +174,9 @@ CV_EXPORTS_W int interpolateCornersCharuco(InputArrayOfArrays markerCorners, Inp * @param distCoeffs vector of distortion coefficients * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements * @param rvec Output vector (e.g. cv::Mat) corresponding to the rotation vector of the board - * (@sa Rodrigues). + * (see cv::Rodrigues). * @param tvec Output vector (e.g. cv::Mat) corresponding to the translation vector of the board. + * @param useExtrinsicGuess defines whether initial guess for \b rvec and \b tvec will be used or not. * * This function estimates a Charuco board pose from some detected corners. * The function checks if the input corners are enough and valid to perform pose estimation. @@ -183,7 +184,8 @@ CV_EXPORTS_W int interpolateCornersCharuco(InputArrayOfArrays markerCorners, Inp */ CV_EXPORTS_W bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds, const Ptr &board, InputArray cameraMatrix, - InputArray distCoeffs, OutputArray rvec, OutputArray tvec); + InputArray distCoeffs, OutputArray rvec, OutputArray tvec, + bool useExtrinsicGuess = false); diff --git a/modules/aruco/src/aruco.cpp b/modules/aruco/src/aruco.cpp index e6135fab7..0c3b2ee0c 100644 --- a/modules/aruco/src/aruco.cpp +++ b/modules/aruco/src/aruco.cpp @@ -1237,7 +1237,7 @@ void refineDetectedMarkers(InputArray _image, const Ptr &_board, */ int estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr &board, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, - OutputArray _tvec) { + OutputArray _tvec, bool useExtrinsicGuess) { CV_Assert(_corners.total() == _ids.total()); @@ -1250,13 +1250,13 @@ int estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr &points) { */ bool estimatePoseCharucoBoard(InputArray _charucoCorners, InputArray _charucoIds, const Ptr &_board, InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec) { + OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess) { CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total())); @@ -674,7 +674,14 @@ bool estimatePoseCharucoBoard(InputArray _charucoCorners, InputArray _charucoIds // points need to be in different lines, check if detected points are enough if(!_arePointsEnoughForPoseEstimation(objPoints)) return false; - solvePnP(objPoints, _charucoCorners, _cameraMatrix, _distCoeffs, _rvec, _tvec); + if (_rvec.empty() || _tvec.empty()) + { + _rvec.create(3, 1, CV_64FC1); + _tvec.create(3, 1, CV_64FC1); + useExtrinsicGuess = false; + } + + solvePnP(objPoints, _charucoCorners, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess); return true; } diff --git a/modules/aruco/test/test_boarddetection.cpp b/modules/aruco/test/test_boarddetection.cpp index e5a2bd49c..6e9bb96ea 100644 --- a/modules/aruco/test/test_boarddetection.cpp +++ b/modules/aruco/test/test_boarddetection.cpp @@ -400,7 +400,7 @@ TEST(CV_ArucoBoardPose, CheckNegativeZ) pts2d.push_back(cv::Point2f(586.3f, 188.5f)); corners.push_back(pts2d); - nUsed = cv::aruco::estimatePoseBoard(corners, board.ids, boardPtr, cameraMatrix, Mat(), rvec, tvec); + nUsed = cv::aruco::estimatePoseBoard(corners, board.ids, boardPtr, cameraMatrix, Mat(), rvec, tvec, true); ASSERT_EQ(nUsed, 2); cv::Rodrigues(rvec, rotm);