aruco::estimatePoseBoard(): rvec and tvec are now used as intrinsic guess if not empty

modules/aruco/include/opencv2/aruco.hpp

@@ -322,8 +322,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).
+ * (@sa Rodrigues). Used as initial guess if not empty.
  * @param tvec Output vector (e.g. cv::Mat) corresponding to the translation vector of the board.
+ * Used as initial guess if not empty.
  *
  * This function receives the detected markers and returns the pose of a marker board composed
  * by those markers.

modules/aruco/src/aruco.cpp

@@ -1311,9 +1311,14 @@ int estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, Ptr<Board> &
     if(objPoints.total() == 0) // 0 of the detected markers in board
         return 0;
 
-    _rvec.create(3, 1, CV_64FC1);
-    _tvec.create(3, 1, CV_64FC1);
-    solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec);
+    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
     return (int)objPoints.total() / 4;

modules/aruco/test/test_boarddetection.cpp

@@ -338,3 +338,72 @@ TEST(CV_ArucoRefine, accuracy) {
     CV_ArucoRefine test;
     test.safe_run();
 }
+
+TEST(CV_ArucoBoardPose, CheckNegativeZ)
+{
+    double matrixData[9] = { -3.9062571886921410e+02, 0., 4.2350000000000000e+02,
+                              0., 3.9062571886921410e+02, 2.3950000000000000e+02,
+                              0., 0., 1 };
+    cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, matrixData);
+
+    cv::Ptr<cv::aruco::Board> boardPtr(new cv::aruco::Board);
+    cv::aruco::Board& board = *boardPtr;
+    board.ids.push_back(0);
+    board.ids.push_back(1);
+
+    vector<cv::Point3f> pts3d;
+    pts3d.push_back(cv::Point3f(0.326198f, -0.030621f, 0.303620f));
+    pts3d.push_back(cv::Point3f(0.325340f, -0.100594f, 0.301862f));
+    pts3d.push_back(cv::Point3f(0.255859f, -0.099530f, 0.293416f));
+    pts3d.push_back(cv::Point3f(0.256717f, -0.029557f, 0.295174f));
+    board.objPoints.push_back(pts3d);
+    pts3d.clear();
+    pts3d.push_back(cv::Point3f(-0.033144f, -0.034819f, 0.245216f));
+    pts3d.push_back(cv::Point3f(-0.035507f, -0.104705f, 0.241987f));
+    pts3d.push_back(cv::Point3f(-0.105289f, -0.102120f, 0.237120f));
+    pts3d.push_back(cv::Point3f(-0.102926f, -0.032235f, 0.240349f));
+    board.objPoints.push_back(pts3d);
+
+    vector<vector<Point2f> > corners;
+    vector<Point2f> pts2d;
+    pts2d.push_back(cv::Point2f(37.7f, 203.3f));
+    pts2d.push_back(cv::Point2f(38.5f, 120.5f));
+    pts2d.push_back(cv::Point2f(105.5f, 115.8f));
+    pts2d.push_back(cv::Point2f(104.2f, 202.7f));
+    corners.push_back(pts2d);
+    pts2d.clear();
+    pts2d.push_back(cv::Point2f(476.0f, 184.2f));
+    pts2d.push_back(cv::Point2f(479.6f, 73.8f));
+    pts2d.push_back(cv::Point2f(590.9f, 77.0f));
+    pts2d.push_back(cv::Point2f(587.5f, 188.1f));
+    corners.push_back(pts2d);
+
+    Vec3d rvec, tvec;
+    int nUsed = cv::aruco::estimatePoseBoard(corners, board.ids, boardPtr, cameraMatrix, Mat(), rvec, tvec);
+    ASSERT_EQ(nUsed, 2);
+
+    cv::Matx33d rotm; cv::Point3d out;
+    cv::Rodrigues(rvec, rotm);
+    out = cv::Point3d(tvec) + rotm*Point3d(board.objPoints[0][0]);
+    ASSERT_GT(out.z, 0);
+
+    corners.clear(); pts2d.clear();
+    pts2d.push_back(cv::Point2f(38.4f, 204.5f));
+    pts2d.push_back(cv::Point2f(40.0f, 124.7f));
+    pts2d.push_back(cv::Point2f(102.0f, 119.1f));
+    pts2d.push_back(cv::Point2f(99.9f, 203.6f));
+    corners.push_back(pts2d);
+    pts2d.clear();
+    pts2d.push_back(cv::Point2f(476.0f, 184.3f));
+    pts2d.push_back(cv::Point2f(479.2f, 75.1f));
+    pts2d.push_back(cv::Point2f(588.7f, 79.2f));
+    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);
+    ASSERT_EQ(nUsed, 2);
+
+    cv::Rodrigues(rvec, rotm);
+    out = cv::Point3d(tvec) + rotm*Point3d(board.objPoints[0][0]);
+    ASSERT_GT(out.z, 0);
+}

modules/aruco/tutorials/aruco_board_detection/aruco_board_detection.markdown

@@ -74,7 +74,7 @@ The parameters of estimatePoseBoard are:
 - ```markerCorners``` and ```markerIds```: structures of detected markers from ```detectMarkers()``` function.
 - ```board```: the ```Board``` object that defines the board layout and its ids
 - ```cameraMatrix``` and ```distCoeffs```: camera calibration parameters necessary for pose estimation.
-- ```rvec``` and ```tvec```: estimated pose of the Board.
+- ```rvec``` and ```tvec```: estimated pose of the Board. If not empty then treated as initial guess.
 - The function returns the total number of markers employed for estimating the board pose. Note that not all the
  markers provided in ```markerCorners``` and ```markerIds``` should be used, since only the markers whose ids are
 listed in the ```Board::ids``` structure are considered.