First version of aruco_pose nodelet

2026-05-28 14:09:33 +00:00 · 2017-11-23 05:59:25 +03:00
parent 6b4777063b
commit e975213a13
6 changed files with 827 additions and 0 deletions
--- a/aruco_pose/src/fix.cpp
+++ b/aruco_pose/src/fix.cpp
@@ -0,0 +1,61 @@
+using namespace cv;
+
+// Temporal fix!
+// TODO: remove
+// fix strange bug in our OpenCV version
+
+void _getBoardObjectAndImagePoints(const Ptr<aruco::Board> &board, InputArrayOfArrays detectedCorners,
+    InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) {
+
+    CV_Assert(board->ids.size() == board->objPoints.size());
+    CV_Assert(detectedIds.total() == detectedCorners.total());
+
+    size_t nDetectedMarkers = detectedIds.total();
+
+    std::vector< Point3f > objPnts;
+    objPnts.reserve(nDetectedMarkers);
+
+    std::vector< Point2f > imgPnts;
+    imgPnts.reserve(nDetectedMarkers);
+
+    // look for detected markers that belong to the board and get their information
+    for(unsigned int i = 0; i < nDetectedMarkers; i++) {
+        int currentId = detectedIds.getMat().ptr< int >(0)[i];
+        for(unsigned int j = 0; j < board->ids.size(); j++) {
+            if(currentId == board->ids[j]) {
+                for(int p = 0; p < 4; p++) {
+                    objPnts.push_back(board->objPoints[j][p]);
+                    imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]);
+                }
+            }
+        }
+    }
+
+    // create output
+    Mat(objPnts).copyTo(objPoints);
+    Mat(imgPnts).copyTo(imgPoints);
+}
+
+int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<aruco::Board> &board,
+                      InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec,
+                      OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) {
+
+    CV_Assert(_corners.total() == _ids.total());
+
+    // get object and image points for the solvePnP function
+    Mat /*objPoints, */imgPoints;
+    _getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints);
+
+    CV_Assert(imgPoints.total() == objPoints.total());
+
+    if(objPoints.total() == 0) // 0 of the detected markers in board
+        return 0;
+
+//    std::cout << "objPoints: " << objPoints << std::endl;
+//    std::cout << "imgPoints: " << imgPoints << std::endl;
+
+    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;
+}