Merge pull request #23138 from AleksandrPanov:aruco_fix_matchImagePoints

asmorkalov · web-flow · commit af1c63c0a027 · 2023-04-27T13:55:46.000+03:00
fix charuco matchImagePoints
diff --git a/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp b/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp
@@ -58,14 +58,22 @@ class CV_EXPORTS_W_SIMPLE Board {
     CV_WRAP const Point3f& getRightBottomCorner() const;
 
     /** @brief Given a board configuration and a set of detected markers, returns the corresponding
-     * image points and object points to call solvePnP()
+     * image points and object points, can be used in solvePnP()
      *
      * @param detectedCorners List of detected marker corners of the board.
-     * For CharucoBoard class you can set list of charuco corners.
-     * @param detectedIds List of identifiers for each marker or list of charuco identifiers for each corner.
-     * For CharucoBoard class you can set list of charuco identifiers for each corner.
-     * @param objPoints Vector of vectors of board marker points in the board coordinate space.
-     * @param imgPoints Vector of vectors of the projections of board marker corner points.
+     * For cv::Board and cv::GridBoard the method expects std::vector<std::vector<Point2f>> or std::vector<Mat> with Aruco marker corners.
+     * For cv::CharucoBoard the method expects std::vector<Point2f> or Mat with ChAruco corners (chess board corners matched with Aruco markers).
+     *
+     * @param detectedIds List of identifiers for each marker or charuco corner.
+     * For any Board class the method expects std::vector<int> or Mat.
+     *
+     * @param objPoints Vector of marker points in the board coordinate space.
+     * For any Board class the method expects std::vector<cv::Point3f> objectPoints or cv::Mat
+     *
+     * @param imgPoints Vector of marker points in the image coordinate space.
+     * For any Board class the method expects std::vector<cv::Point2f> objectPoints or cv::Mat
+     *
+     * @sa solvePnP
      */
     CV_WRAP void matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds,
                                   OutputArray objPoints, OutputArray imgPoints) const;
diff --git a/modules/objdetect/misc/python/test/test_objdetect_aruco.py b/modules/objdetect/misc/python/test/test_objdetect_aruco.py
@@ -313,5 +313,32 @@ def test_charuco_detector_accuracy(self):
                         reprErr = cv.norm(charucoCorners[i] - projectedCharucoCorners[currentId])
                         self.assertLessEqual(reprErr, 5)
 
+    def test_aruco_match_image_points(self):
+        aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_50)
+        board_size = (3, 4)
+        board = cv.aruco.GridBoard(board_size, 5.0, 1.0, aruco_dict)
+        aruco_corners = np.array(board.getObjPoints())[:, :, :2]
+        aruco_ids = board.getIds()
+        obj_points, img_points = board.matchImagePoints(aruco_corners, aruco_ids)
+        aruco_corners = aruco_corners.reshape(-1, 2)
+
+        self.assertEqual(aruco_corners.shape[0], obj_points.shape[0])
+        self.assertEqual(img_points.shape[0], obj_points.shape[0])
+        self.assertEqual(2, img_points.shape[2])
+        np.testing.assert_array_equal(aruco_corners, obj_points[:, :, :2].reshape(-1, 2))
+
+    def test_charuco_match_image_points(self):
+        aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_50)
+        board_size = (3, 4)
+        board = cv.aruco.CharucoBoard(board_size, 5.0, 1.0, aruco_dict)
+        chessboard_corners = np.array(board.getChessboardCorners())[:, :2]
+        chessboard_ids = board.getIds()
+        obj_points, img_points = board.matchImagePoints(chessboard_corners, chessboard_ids)
+
+        self.assertEqual(chessboard_corners.shape[0], obj_points.shape[0])
+        self.assertEqual(img_points.shape[0], obj_points.shape[0])
+        self.assertEqual(2, img_points.shape[2])
+        np.testing.assert_array_equal(chessboard_corners, obj_points[:, :, :2].reshape(-1, 2))
+
 if __name__ == '__main__':
     NewOpenCVTests.bootstrap()
diff --git a/modules/objdetect/src/aruco/aruco_board.cpp b/modules/objdetect/src/aruco/aruco_board.cpp
@@ -27,17 +27,17 @@ struct Board::Impl {
     Impl(const Impl&) = delete;
     Impl& operator=(const Impl&) = delete;
 
-    virtual void matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints,
+    virtual void matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds, OutputArray _objPoints,
                                   OutputArray imgPoints) const;
 
     virtual void generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const;
 };
 
-void Board::Impl::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray _objPoints,
-                                        OutputArray imgPoints) const {
-
-    CV_Assert(ids.size() == objPoints.size());
+void Board::Impl::matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds, OutputArray _objPoints,
+                                   OutputArray imgPoints) const {
     CV_Assert(detectedIds.total() == detectedCorners.total());
+    CV_Assert(detectedIds.total() > 0ull);
+    CV_Assert(detectedCorners.depth() == CV_32F);
 
     size_t nDetectedMarkers = detectedIds.total();
 
@@ -48,13 +48,19 @@ void Board::Impl::matchImagePoints(InputArray detectedCorners, InputArray detect
     imgPnts.reserve(nDetectedMarkers);
 
     // look for detected markers that belong to the board and get their information
+    Mat detectedIdsMat = detectedIds.getMat();
+    vector<Mat> detectedCornersVecMat;
+
+    detectedCorners.getMatVector(detectedCornersVecMat);
+    CV_Assert((int)detectedCornersVecMat.front().total()*detectedCornersVecMat.front().channels() == 8);
+
     for(unsigned int i = 0; i < nDetectedMarkers; i++) {
-        int currentId = detectedIds.getMat().ptr< int >(0)[i];
+        int currentId = detectedIdsMat.at<int>(i);
         for(unsigned int j = 0; j < ids.size(); j++) {
             if(currentId == ids[j]) {
                 for(int p = 0; p < 4; p++) {
                     objPnts.push_back(objPoints[j][p]);
-                    imgPnts.push_back(detectedCorners.getMat(i).ptr<Point2f>(0)[p]);
+                    imgPnts.push_back(detectedCornersVecMat[i].ptr<Point2f>(0)[p]);
                 }
             }
         }
@@ -212,7 +218,7 @@ void Board::generateImage(Size outSize, OutputArray img, int marginSize, int bor
     impl->generateImage(outSize, img, marginSize, borderBits);
 }
 
-void Board::matchImagePoints(InputArray detectedCorners, InputArray detectedIds, OutputArray objPoints,
+void Board::matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds, OutputArray objPoints,
                              OutputArray imgPoints) const {
     CV_Assert(this->impl);
     impl->matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints);
@@ -312,7 +318,7 @@ struct CharucoBoardImpl : Board::Impl {
 
     void calcNearestMarkerCorners();
 
-    void matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds,
+    void matchImagePoints(InputArrayOfArrays detectedCharuco, InputArray detectedIds,
                           OutputArray objPoints, OutputArray imgPoints) const override;
 
     void generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const override;
@@ -367,25 +373,42 @@ void CharucoBoardImpl::calcNearestMarkerCorners() {
     }
 }
 
-void CharucoBoardImpl::matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds,
-                                   OutputArray _objPoints, OutputArray imgPoints) const {
-    if (detectedCorners.kind() == _InputArray::STD_VECTOR_VECTOR ||
-        detectedCorners.isMatVector() || detectedCorners.isUMatVector())
-        Board::Impl::matchImagePoints(detectedCorners, detectedIds, _objPoints, imgPoints);
-    else {
-        CV_Assert(detectedCorners.isMat() || detectedCorners.isVector());
-        size_t nDetected = detectedCorners.total();
-        vector<Point3f> objPnts(nDetected);
-        vector<Point2f> imgPnts(nDetected);
-        for(size_t i = 0ull; i < nDetected; i++) {
-            int pointId = detectedIds.getMat().at<int>((int)i);
-            CV_Assert(pointId >= 0 && pointId < (int)chessboardCorners.size());
-            objPnts[i] = chessboardCorners[pointId];
-            imgPnts[i] = detectedCorners.getMat().at<Point2f>((int)i);
+void CharucoBoardImpl::matchImagePoints(InputArrayOfArrays detectedCharuco, InputArray detectedIds,
+                                        OutputArray outObjPoints, OutputArray outImgPoints) const {
+    CV_CheckEQ(detectedIds.total(), detectedCharuco.total(), "Number of corners and ids must be equal");
+    CV_Assert(detectedIds.total() > 0ull);
+    CV_Assert(detectedCharuco.depth() == CV_32F);
+    // detectedCharuco includes charuco corners as vector<Point2f> or Mat.
+    // Python bindings could add extra dimension to detectedCharuco and therefore vector<Mat> case is additionally processed.
+    CV_Assert((detectedCharuco.isMat() || detectedCharuco.isVector() || detectedCharuco.isMatVector() || detectedCharuco.isUMatVector())
+               && detectedCharuco.depth() == CV_32F);
+    size_t nDetected = detectedCharuco.total();
+    vector<Point3f> objPnts(nDetected);
+    vector<Point2f> imgPnts(nDetected);
+    Mat detectedCharucoMat, detectedIdsMat = detectedIds.getMat();
+    if (!detectedCharuco.isMatVector()) {
+        detectedCharucoMat = detectedCharuco.getMat();
+        CV_Assert(detectedCharucoMat.checkVector(2));
+    }
+
+    std::vector<Mat> detectedCharucoVecMat;
+    if (detectedCharuco.isMatVector()) {
+        detectedCharuco.getMatVector(detectedCharucoVecMat);
+    }
+
+    for(size_t i = 0ull; i < nDetected; i++) {
+        int pointId = detectedIdsMat.at<int>((int)i);
+        CV_Assert(pointId >= 0 && pointId < (int)chessboardCorners.size());
+        objPnts[i] = chessboardCorners[pointId];
+        if (detectedCharuco.isMatVector()) {
+            CV_Assert((int)detectedCharucoVecMat[i].total() * detectedCharucoVecMat[i].channels() == 2);
+            imgPnts[i] = detectedCharucoVecMat[i].ptr<Point2f>(0)[0];
         }
-        Mat(objPnts).copyTo(_objPoints);
-        Mat(imgPnts).copyTo(imgPoints);
+        else
+            imgPnts[i] = detectedCharucoMat.ptr<Point2f>(0)[i];
     }
+    Mat(objPnts).copyTo(outObjPoints);
+    Mat(imgPnts).copyTo(outImgPoints);
 }
 
 void CharucoBoardImpl::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const {
@@ -518,20 +541,21 @@ float CharucoBoard::getMarkerLength() const {
 
 bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
     CV_Assert(impl);
+    Mat charucoIdsMat = charucoIds.getMat();
 
-    unsigned int nCharucoCorners = (unsigned int)charucoIds.getMat().total();
+    unsigned int nCharucoCorners = (unsigned int)charucoIdsMat.total();
     if (nCharucoCorners <= 2)
         return true;
 
     // only test if there are 3 or more corners
     auto board = static_pointer_cast<CharucoBoardImpl>(impl);
-    CV_Assert(board->chessboardCorners.size() >= charucoIds.getMat().total());
+    CV_Assert(board->chessboardCorners.size() >= charucoIdsMat.total());
 
-    Vec<double, 3> point0(board->chessboardCorners[charucoIds.getMat().at<int>(0)].x,
-                          board->chessboardCorners[charucoIds.getMat().at<int>(0)].y, 1);
+    Vec<double, 3> point0(board->chessboardCorners[charucoIdsMat.at<int>(0)].x,
+                          board->chessboardCorners[charucoIdsMat.at<int>(0)].y, 1);
 
-    Vec<double, 3> point1(board->chessboardCorners[charucoIds.getMat().at<int>(1)].x,
-                          board->chessboardCorners[charucoIds.getMat().at<int>(1)].y, 1);
+    Vec<double, 3> point1(board->chessboardCorners[charucoIdsMat.at<int>(1)].x,
+                          board->chessboardCorners[charucoIdsMat.at<int>(1)].y, 1);
 
     // create a line from the first two points.
     Vec<double, 3> testLine = point0.cross(point1);
@@ -545,8 +569,8 @@ bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
 
     double dotProduct;
     for (unsigned int i = 2; i < nCharucoCorners; i++){
-        testPoint(0) = board->chessboardCorners[charucoIds.getMat().at<int>(i)].x;
-        testPoint(1) = board->chessboardCorners[charucoIds.getMat().at<int>(i)].y;
+        testPoint(0) = board->chessboardCorners[charucoIdsMat.at<int>(i)].x;
+        testPoint(1) = board->chessboardCorners[charucoIdsMat.at<int>(i)].y;
 
         // if testPoint is on testLine, dotProduct will be zero (or very, very close)
         dotProduct = testPoint.dot(testLine);
diff --git a/modules/objdetect/src/aruco/charuco_detector.cpp b/modules/objdetect/src/aruco/charuco_detector.cpp
@@ -129,25 +129,21 @@ struct CharucoDetector::CharucoDetectorImpl {
         // approximated pose estimation using marker corners
         Mat approximatedRvec, approximatedTvec;
         Mat objPoints, imgPoints; // object and image points for the solvePnP function
-        // printf("before board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);\n");
-        board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);
-        // printf("after board.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);\n");
+        Board simpleBoard(board.getObjPoints(), board.getDictionary(), board.getIds());
+        simpleBoard.matchImagePoints(markerCorners, markerIds, objPoints, imgPoints);
         if (objPoints.total() < 4ull)  // need, at least, 4 corners
             return;
 
         solvePnP(objPoints, imgPoints, charucoParameters.cameraMatrix, charucoParameters.distCoeffs, approximatedRvec, approximatedTvec);
-        // printf("after solvePnP\n");
 
         // project chessboard corners
         vector<Point2f> allChessboardImgPoints;
         projectPoints(board.getChessboardCorners(), approximatedRvec, approximatedTvec, charucoParameters.cameraMatrix,
                       charucoParameters.distCoeffs, allChessboardImgPoints);
-        // printf("after projectPoints\n");
         // calculate maximum window sizes for subpixel refinement. The size is limited by the distance
         // to the closes marker corner to avoid erroneous displacements to marker corners
         vector<Size> subPixWinSizes = getMaximumSubPixWindowSizes(markerCorners, markerIds, allChessboardImgPoints);
         // filter corners outside the image and subpixel-refine charuco corners
-        // printf("before selectAndRefineChessboardCorners\n");
         selectAndRefineChessboardCorners(allChessboardImgPoints, image, charucoCorners, charucoIds, subPixWinSizes);
     }
 
@@ -292,7 +288,7 @@ void CharucoDetector::setRefineParameters(const RefineParameters& refineParamete
 
 void CharucoDetector::detectBoard(InputArray image, OutputArray charucoCorners, OutputArray charucoIds,
                                   InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) const {
-    CV_Assert((markerCorners.empty() && markerIds.empty() && !image.empty()) || (markerCorners.size() == markerIds.size()));
+    CV_Assert((markerCorners.empty() && markerIds.empty() && !image.empty()) || (markerCorners.total() == markerIds.total()));
     vector<vector<Point2f>> tmpMarkerCorners;
     vector<int> tmpMarkerIds;
     InputOutputArrayOfArrays _markerCorners = markerCorners.needed() ? markerCorners : tmpMarkerCorners;
@@ -321,7 +317,7 @@ void CharucoDetector::detectBoard(InputArray image, OutputArray charucoCorners,
 void CharucoDetector::detectDiamonds(InputArray image, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds,
                                      InputOutputArrayOfArrays inMarkerCorners, InputOutputArrayOfArrays inMarkerIds) const {
     CV_Assert(getBoard().getChessboardSize() == Size(3, 3));
-    CV_Assert((inMarkerCorners.empty() && inMarkerIds.empty() && !image.empty()) || (inMarkerCorners.size() == inMarkerIds.size()));
+    CV_Assert((inMarkerCorners.empty() && inMarkerIds.empty() && !image.empty()) || (inMarkerCorners.total() == inMarkerIds.total()));
 
     vector<vector<Point2f>> tmpMarkerCorners;
     vector<int> tmpMarkerIds;
diff --git a/modules/objdetect/test/test_charucodetection.cpp b/modules/objdetect/test/test_charucodetection.cpp
@@ -656,4 +656,31 @@ TEST(Charuco, issue_14014)
     EXPECT_EQ(Size(4, 1), rejectedPoints[0].size()); // check dimension of rejected corners after successfully refine
 }
 
+
+TEST(Charuco, testmatchImagePoints)
+{
+    aruco::CharucoBoard board(Size(2, 3), 1.f, 0.5f, aruco::getPredefinedDictionary(aruco::DICT_4X4_50));
+    auto chessboardPoints = board.getChessboardCorners();
+
+    vector<int> detectedIds;
+    vector<Point2f> detectedCharucoCorners;
+    for (const Point3f& point : chessboardPoints) {
+        detectedIds.push_back((int)detectedCharucoCorners.size());
+        detectedCharucoCorners.push_back({2.f*point.x, 2.f*point.y});
+    }
+
+    vector<Point3f> objPoints;
+    vector<Point2f> imagePoints;
+    board.matchImagePoints(detectedCharucoCorners, detectedIds, objPoints, imagePoints);
+
+    ASSERT_EQ(detectedCharucoCorners.size(), objPoints.size());
+    ASSERT_EQ(detectedCharucoCorners.size(), imagePoints.size());
+
+    for (size_t i = 0ull; i < detectedCharucoCorners.size(); i++) {
+        EXPECT_EQ(detectedCharucoCorners[i], imagePoints[i]);
+        EXPECT_EQ(chessboardPoints[i].x, objPoints[i].x);
+        EXPECT_EQ(chessboardPoints[i].y, objPoints[i].y);
+    }
+}
+
 }} // namespace
