diff --git a/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp b/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp index 90ad2b8a7e..bdf2fac8dd 100644 --- a/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp +++ b/modules/objdetect/include/opencv2/objdetect/aruco_board.hpp @@ -58,14 +58,22 @@ public: 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> or std::vector with Aruco marker corners. + * For cv::CharucoBoard the method expects std::vector 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 or Mat. + * + * @param objPoints Vector of marker points in the board coordinate space. + * For any Board class the method expects std::vector objectPoints or cv::Mat + * + * @param imgPoints Vector of marker points in the image coordinate space. + * For any Board class the method expects std::vector 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 index 23234603d8..eb88f2c8f6 100644 --- a/modules/objdetect/misc/python/test/test_objdetect_aruco.py +++ b/modules/objdetect/misc/python/test/test_objdetect_aruco.py @@ -313,5 +313,32 @@ class aruco_objdetect_test(NewOpenCVTests): 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 index 1f582e06a9..c16cd1e931 100644 --- 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 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(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(0)[p]); + imgPnts.push_back(detectedCornersVecMat[i].ptr(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 objPnts(nDetected); - vector imgPnts(nDetected); - for(size_t i = 0ull; i < nDetected; i++) { - int pointId = detectedIds.getMat().at((int)i); - CV_Assert(pointId >= 0 && pointId < (int)chessboardCorners.size()); - objPnts[i] = chessboardCorners[pointId]; - imgPnts[i] = detectedCorners.getMat().at((int)i); - } - Mat(objPnts).copyTo(_objPoints); - Mat(imgPnts).copyTo(imgPoints); +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 or Mat. + // Python bindings could add extra dimension to detectedCharuco and therefore vector case is additionally processed. + CV_Assert((detectedCharuco.isMat() || detectedCharuco.isVector() || detectedCharuco.isMatVector() || detectedCharuco.isUMatVector()) + && detectedCharuco.depth() == CV_32F); + size_t nDetected = detectedCharuco.total(); + vector objPnts(nDetected); + vector imgPnts(nDetected); + Mat detectedCharucoMat, detectedIdsMat = detectedIds.getMat(); + if (!detectedCharuco.isMatVector()) { + detectedCharucoMat = detectedCharuco.getMat(); + CV_Assert(detectedCharucoMat.checkVector(2)); } + + std::vector detectedCharucoVecMat; + if (detectedCharuco.isMatVector()) { + detectedCharuco.getMatVector(detectedCharucoVecMat); + } + + for(size_t i = 0ull; i < nDetected; i++) { + int pointId = detectedIdsMat.at((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(0)[0]; + } + else + imgPnts[i] = detectedCharucoMat.ptr(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(impl); - CV_Assert(board->chessboardCorners.size() >= charucoIds.getMat().total()); + CV_Assert(board->chessboardCorners.size() >= charucoIdsMat.total()); - Vec point0(board->chessboardCorners[charucoIds.getMat().at(0)].x, - board->chessboardCorners[charucoIds.getMat().at(0)].y, 1); + Vec point0(board->chessboardCorners[charucoIdsMat.at(0)].x, + board->chessboardCorners[charucoIdsMat.at(0)].y, 1); - Vec point1(board->chessboardCorners[charucoIds.getMat().at(1)].x, - board->chessboardCorners[charucoIds.getMat().at(1)].y, 1); + Vec point1(board->chessboardCorners[charucoIdsMat.at(1)].x, + board->chessboardCorners[charucoIdsMat.at(1)].y, 1); // create a line from the first two points. Vec 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(i)].x; - testPoint(1) = board->chessboardCorners[charucoIds.getMat().at(i)].y; + testPoint(0) = board->chessboardCorners[charucoIdsMat.at(i)].x; + testPoint(1) = board->chessboardCorners[charucoIdsMat.at(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 index b5ca70a274..c39c8b5cc7 100644 --- 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 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 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> tmpMarkerCorners; vector 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> tmpMarkerCorners; vector tmpMarkerIds; diff --git a/modules/objdetect/test/test_charucodetection.cpp b/modules/objdetect/test/test_charucodetection.cpp index 63b28d8e5f..97a6eb704d 100644 --- 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 detectedIds; + vector detectedCharucoCorners; + for (const Point3f& point : chessboardPoints) { + detectedIds.push_back((int)detectedCharucoCorners.size()); + detectedCharucoCorners.push_back({2.f*point.x, 2.f*point.y}); + } + + vector objPoints; + vector 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