Merge pull request #950 from sovrasov:aruco_pose_estimation_initial_guess

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> &board,
                                    InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec,
-                                   OutputArray tvec);
+                                   OutputArray tvec, bool useExtrinsicGuess = false);
 
 
 

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<CharucoBoard> &board, InputArray cameraMatrix,
-                                           InputArray distCoeffs, OutputArray rvec, OutputArray tvec);
+                                           InputArray distCoeffs, OutputArray rvec, OutputArray tvec,
+                                           bool useExtrinsicGuess = false);
 
 
 

modules/aruco/src/aruco.cpp

@@ -1237,7 +1237,7 @@ void refineDetectedMarkers(InputArray _image, const Ptr<Board> &_board,
   */
 int estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<Board> &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<Bo
     if(objPoints.total() == 0) // 0 of the detected markers in board
         return 0;
 
-    bool useExtrinsicGuess = true;
     if (_rvec.empty() || _tvec.empty())
     {
         _rvec.create(3, 1, CV_64FC1);
         _tvec.create(3, 1, CV_64FC1);
         useExtrinsicGuess = false;
     }
+
     solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
 
     // divide by four since all the four corners are concatenated in the array for each marker

modules/aruco/src/charuco.cpp

@@ -656,7 +656,7 @@ static bool _arePointsEnoughForPoseEstimation(const vector< Point3f > &points) {
   */
 bool estimatePoseCharucoBoard(InputArray _charucoCorners, InputArray _charucoIds,
                               const Ptr<CharucoBoard> &_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;
 }

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);