with openCV / aruco, express rvecs and tvecs in a specific tag coordinate system

Question

I am finding aruco tag in image, then do a pos estimation on a raspberry pi.

minimal code might look like this :

#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>

Ptr<aruco::DetectorParameters> aruco_param() {
  Ptr<aruco::DetectorParameters> p = aruco::DetectorParameters::create();
  p->maxMarkerPerimeterRate = 1.0;
  p->minMarkerPerimeterRate = 0.04;
  p->cornerRefinementMethod = aruco::CORNER_REFINE_SUBPIX;
  return p;
}

int main(int argc, char *argv[]) {
  Ptr<aruco::DetectorParameters> param = aruco_param();

  Ptr<aruco::Dictionary> dict =
      aruco::getPredefinedDictionary(aruco::DICT_4X4_100);

  Mat mtx = (Mat_<double>(3, 3) << 1.23424578e+03, 0.00000000e+00,
             1.27563061e+03, 0.00000000e+00, 1.23424578e+03, 1.02267452e+03,
             0.00000000e+00, 0.00000000e+00, 1.00000000e+00);

  Mat dist = (Mat_<double>(1, 14) << 2.48388910e-01, 8.96432397e-03,
              4.60522564e-04, 2.62749266e-04, 9.94878947e-04, 5.96768987e-01,
              2.89157221e-02, 4.36330487e-03, 0.00000000e+00, 0.00000000e+00,
              0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
              0.00000000e+00);

  cv::VideoCapture cap(0);
  cap.set(CAP_PROP_FRAME_WIDTH, 2592);
  cap.set(CAP_PROP_FRAME_HEIGHT, 1944);
  cap.set(CAP_PROP_AUTO_EXPOSURE, 0.25);
  cap.set(CAP_PROP_EXPOSURE, 80);
  usleep(1 * 1000 * 1000);


  while (true) {
    Mat image;
    cap >> image;
        
    cvtColor(image, image, COLOR_BGR2GRAY);
    extractChannel(image, image, 0);

    vector<int> ids;
    vector<vector<Point2f>> corners, rejected;
    vector<Vec3d> rvecs, tvecs;
    aruco::detectMarkers(image, dict, corners, ids, param, rejected);

    if(ids.size() > 0){
        aruco::estimatePoseSingleMarkers(corners, 0.05f, mtx, dist, rvecs, tvecs);
        for (int i = 0; i < ids.size(); i++)
        {
                cout << ids[i] << ";" << tvecs[i] << ";" << rvecs[i] << endl;
        }
    }
}

And it mostly works. Now, tvec (and rvecs) are in the camera coordinate system. (tvecs unit is meter). And I don't really care about the camera coordinate system, instead, I want to use the tag 0 as the origin of my coordinate (I want to express all points in the tag0 coordinate system) (to be honest, I don't care about the tag 0 itself, but a point in the same plane, at (x,y) = (-0.58, -0.58), but I think I could figure it by myself)

I didn't do any work with matrix for a while, and I'm a bit confused by the rvecs format.

I think I can get the rotation matrix using Rodriguez, and my translation vector should not be hard to compute

cv::Mat R, t;
cv::Rodrigues(rvecs[0], R);
R = R.t();
t = -tvecs[0];

new_pos = R*Mat(tvecs[0]) + t // false

Could someone help me out on that last part ?

(you can assume that tvecs[0] contains information about tag 0 (even if it is not the case))

opencv has functions to compose/handle "rtvec" pairs. I'd ditch all that, convert them into pose matrices (4x4) and compose/invert those. so much easier to deal with. when you're done with the operation, you can convert that 4x4 matrix back to "rtvec" -- I would strongly recommend prototyping in Python. C++ is assembly with classes. its refcounting isn't even a syntactic feature (`shared_ptr` all over the place) — Christoph Rackwitz, May 16 '23 at 09:27

score 0 · Answer 1 · answered May 16 '23 at 12:23

I added 2 helper function :

Mat rtvec_to_mat(cv::Vec3f tvec, cv::Vec3f rvec){
        cv::Mat R, t;
        cv::Rodrigues(rvec, R);
        cv::Mat T = cv::Mat::eye(4, 4, CV_32FC1);
        cv::Mat T_roi = T(cv::Rect(0, 0, 3, 3));
        R.copyTo(T_roi);
        T.at<float>(0, 3) = tvec[0];
        T.at<float>(1, 3) = tvec[1];
        T.at<float>(2, 3) = tvec[2];
  return T;
}

Vec3f mat_to_vec(Mat m){
        cv::Vec3f p(m.at<float>(0, 3), m.at<float>(1, 3),  m.at<float>(2, 3));
        return p;
}

rtvec_to_mat convert tvec / rvec to a 44 matrix. mat_to_vec extract position from a 44 matrix. I should still add at least one function to extract the rotation of the z axis (as all my aruco should be on the same plane, or at least share other rotation)

Mat T = rtvec_to_mat(tvecs[0], rvecs[0]); // actually, it is the aruco with tag 0, not the first element of tvecs/rvecs

Mat Tinv = T.inv();


for (int i=0; i<dds_ids.size(); i++){
    cout << ids[i]  << " --- "  << mat_to_vec(Tinv*rtvec_to_mat(tvecs[i], rvecs[i])) << endl;
}

and now I have what I wanted :)

`cv::Rodrigues()` can do both directions, so a `mat_to_rtvec()` is about the same code. rvecs are axis-angle encodings (magnitude = rotation angle, vector = rotational axis), i.e. not "euler angles" of any kind (everyone avoids euler angles like the plague because there's no decent convention for their order, also gimbal lock) — Christoph Rackwitz, May 16 '23 at 14:15

with openCV / aruco, express rvecs and tvecs in a specific tag coordinate system

1 Answers1