AprilRobotics / apriltag

AprilTag is a visual fiducial system popular for robotics research.
https://april.eecs.umich.edu/software/apriltag
Other
1.56k stars 535 forks source link

Apriltag Built in Pose vastly superior to SolvePnp? #212

Open henryjliu opened 2 years ago

henryjliu commented 2 years ago

I did some tests and here is some basic python code using dt_apriltags in python. I believe the solvepnp to be equivalent eg uses the center of the tag and IPPE_SQUARE but it gives terrible results compared to the builtin AprilTag estimatepose function. Is there an implementation that mimicks the behavior of the Apriltag function but can be used with multiple tags / grid of tags? I need to dig deeper into the code of apriltag_pose.c but again I'm not sure why results are so different than the below code which uses exact same corners and tag parameters. Any ideas?

Snippet:

cameraMatrix = mtx #need mtx from calibration
camera_params = ( cameraMatrix[0,0], cameraMatrix[1,1], cameraMatrix[0,2], cameraMatrix[1,2] )
print(cameraMatrix)
img = cv2.imread(ab_path, cv2.IMREAD_GRAYSCALE) #needs grayscale
#cv2.imshow('img',img);

tags = at_detector.detect(img, True, camera_params, .001944)

print("tap[0] ", tags[0].pose_t)
print("tap[1] ", tags[1].pose_t)
#print(tags[0].pose_R)
#print(tags[1].pose_R)

ts=.01944/2
worldpoints=np.array([[-ts, ts, 0],
                    [ts,ts,0],
                    [ts,-ts,0],
                    [-ts,-ts,0]],dtype=numpy.float)
#check tag corners
SOLVEPNP_IPPE_SQUARE=7
#compare to solve pnp    
(_, rvec, tvec)=cv2.solvePnP(worldpoints, tags[0].corners, mtx, dist, useExtrinsicGuess=tru, flags=SOLVEPNP_IPPE_SQUARE)
(_, rvec2, tvec2)=cv2.solvePnP(worldpoints, tags[1].corners, mtx, dist, useExtrinsicGuess=False, flags=SOLVEPNP_IPPE_SQUARE)
print("tvec[0] ", tvec)
print("tvec[1] " , tvec2)
jrepp commented 2 years ago

What are these extra parameters you're passing to detect()? Is the request to get access to the estimate pose functionality of apriltag from python? I don't see those APIs exposed currently to python.