Closed shanmugamr1992 closed 5 months ago
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
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]])},)
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
}
Hello everyone, I am trying to run this in my jetson Nano. I have installed this and am able to get the detection results in python (Center, decision margin, hamming , corner points etc) I want to do pose estimation ? I see an easy way to do it in C, but I want to do this in python. Some help would be appreciated. Thanks