mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-11-28 05:06:29 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

fix charuco matchImagePoints

Browse Source
This commit is contained in:
Alex 2023-04-21 00:43:03 +03:00 committed by Alexander Smorkalov
parent 4c06a721ef
commit 4ba06c3ed0
5 changed files with 131 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
modules/objdetect/include/opencv2/objdetect/aruco_board.hpp
View File

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

27
modules/objdetect/misc/python/test/test_objdetect_aruco.py
View File

@ -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()

80
modules/objdetect/src/aruco/aruco_board.cpp
View File

@ -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,
void Board::Impl::matchImagePoints(InputArrayOfArrays detectedCorners, InputArray detectedIds, OutputArray _objPoints,
OutputArray imgPoints) const {
CV_Assert(ids.size() == objPoints.size());
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();
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 = detectedIds.getMat().at<int>((int)i);
int pointId = detectedIdsMat.at<int>((int)i);
CV_Assert(pointId >= 0 && pointId < (int)chessboardCorners.size());
objPnts[i] = chessboardCorners[pointId];
imgPnts[i] = detectedCorners.getMat().at<Point2f>((int)i);
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);

12
modules/objdetect/src/aruco/charuco_detector.cpp
View File

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

27
modules/objdetect/test/test_charucodetection.cpp
View File

@ -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