OpenCV Get distance to rotated fiducial marker (Aruco)

Question

(In iOS) I need to determine the distance to a fiducial marker which may be rotated.

I'm using the Aruco library and I have so far implemented marker detection and pose estimation in iOS, but what I really need is the distance to that marker from the camera.

I've seen some examples where you use the real size of the marker, focal length of the camera and the size on screen of the marker to compute distance, but this doesn't take into account any rotation applied to the marker.

As I have the pose estimation working I'm "guessing" that it should be possible to un-rotate the corner points of the marker then use the bounding box of those points, along with the real size and camera focal length. Though I'm not entirely sure that is correct, or how to implement it.

This is my first time using OpenCV so I'm really just guessing at the moment.

Any and all help is very much appreciated.

Many thanks

If you've got pose estimation working, then you already have the distance from the marker to the camera -- it's the length of the tvec. But beware that doing pose estimation using a single marker is likely to be inaccurate in many cases -- see [this answer](https://stackoverflow.com/questions/51709522/unstable-values-in-aruco-pose-estimation). It's better to use more markers. — Chungzuwalla, Aug 14 '18 at 00:30

score 0 · Answer 1 · edited May 07 '21 at 13:09

I use the code below in my project to detect Z distance between camera and marker. I'm not sure whether it's completely correct or not, but it gives acceptable results.

Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
detectorParams->doCornerRefinement = true;

Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(kMarkerDictionaryID));

Mat camMatrix, distCoeffs;

if (!readCameraParameters(kCameraParametersFile, camMatrix, distCoeffs)) {
    
    // ...
    
    return;
}

VideoCapture inputVideo;
inputVideo.open(kCameraID);

while (inputVideo.grab()) {
    
    Mat image;
    
    inputVideo.retrieve(image);
    
    vector< int > ids;
    vector< vector< Point2f > > corners;
    vector< Vec3d > rvecs, tvecs;
    
    aruco::detectMarkers(image, dictionary, corners, ids, detectorParams);
    
    if (ids.size() > 0) {
        
        aruco::estimatePoseSingleMarkers(corners, kMarkerLengthInMeters, camMatrix, distCoeffs, rvecs, tvecs);
        
        for (unsigned int i = 0; i < ids.size(); i++) {
            
            // if (ids[i] != kMarkerID)
            //     continue;
                            
            // Calc camera pose
            Mat R;
            Rodrigues(rvecs[i], R);
            Mat cameraPose = -R.t() * (Mat)tvecs[i];

            double x = cameraPose.at<double>(0,0);
            double y = cameraPose.at<double>(0,1);
            double z = cameraPose.at<double>(0,2);

            // So z is the distance between camera and marker 

            // Or if you need rotation invariant offsets
            // x = tvecs[i][0];
            // y = tvecs[i][0];
            // z = tvecs[i][0];

            cout << "X: " << x << " Y: " << y << " Z: " << z << endl;

            aruco::drawAxis(image, camMatrix, distCoeffs, rvecs[i], tvecs[i], kMarkerLengthInMeters * 0.5f);
        }

        aruco::drawDetectedMarkers(image, corners, resultIds);
    }
    
    resize(image, image, Size(image.cols / 2, image.rows / 2));
    imshow("out", image);
    char key = (char)waitKey(kWaitTimeInmS);
    if (key == 27) break;
}

OpenCV Get distance to rotated fiducial marker (Aruco)

1 Answers1