Hello everyone, I am trying to run this in my jetson Nano. I have installed this a

How to run pose estimation in python ? about apriltag HOT 3 CLOSED

shanmugamr1992 commented on August 27, 2024

How to run pose estimation in python ?

from apriltag.

Comments (3)

brmarkus commented on August 27, 2024

Would the note OpenCv's Pnp solver with SOLVEPNP_IPPE_SQUARE give a guide?
Some Google-hints:

https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html
https://stackoverflow.com/questions/57427636/camera-pose-estimation-with-solvepnp-and-solvepnp-ippe-square-method

from apriltag.

luccachiang commented on August 27, 2024

Does anyone know how to calculate 6dof pose with the following output of detector.detect?

({'hamming': 0, 'margin': 248.07064819335938, 'id': 0, 'center': array([657.39107487, 199.88093104]), 
'lb-rb-rt-lt': array([[570.875     , 286.875     ],
       [743.875     , 286.875     ],
       [743.875     , 112.91918945],
       [570.875     , 112.85452271]])},)

from apriltag.

christian-rauch commented on August 27, 2024

As noted above, you can use OpenCV's solvePnP to do the pose estimation from the 2D tag detection.

C++ code for this could look like this:

void
pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
{
    const std::vector<cv::Point3d> objectPoints{
        {-tagsize / 2, -tagsize / 2, 0},
        {+tagsize / 2, -tagsize / 2, 0},
        {+tagsize / 2, +tagsize / 2, 0},
        {-tagsize / 2, +tagsize / 2, 0},
    };

    const std::vector<cv::Point2d> imagePoints{
        {detection->p[0][0], detection->p[0][1]},
        {detection->p[1][0], detection->p[1][1]},
        {detection->p[2][0], detection->p[2][1]},
        {detection->p[3][0], detection->p[3][1]},
    };

    cv::Matx33d cameraMatrix;
    cameraMatrix(0, 0) = intr[0];// fx
    cameraMatrix(1, 1) = intr[1];// fy
    cameraMatrix(0, 2) = intr[2];// cx
    cameraMatrix(1, 2) = intr[3];// cy

    cv::Mat rvec, tvec;
    cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec);

    // 'rvec' and 'tvec' contain the orientation and position of the tag in 3D world
}