这里给出显示Aruco角点的函数、显示marker坐标系的函数和计算重投影误差的函数。
- 显示角点
def showCorner(bgr_image,corners,ids):
# show corner
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.0001)
gray_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2GRAY)
for corner in corners:
cv2.cornerSubPix(gray_image, corner, winSize = (3,3), zeroZone = (-1,-1), criteria = criteria)
frame_markers = aruco.drawDetectedMarkers(bgr_image.copy(), corners, ids)
plt.figure()
plt.imshow(frame_markers, interpolation = "nearest")
plt.show()
2.显示坐标系
def showAxis(bgr_image,intrinsic_matrix,distort_matrix,rvecs,tvecs):
# show axis
intrinsic_matrix = np.array(intrinsic_matrix)
distort_matrix = np.array(distort_matrix)
size_of_marker = 0.1 # side lenght of the marker in meter
length_of_axis = 0.1
for i in range(len(tvecs)):
imaxis = aruco.drawAxis(bgr_image, intrinsic_matrix, distort_matrix, rvecs[i], tvecs[i], length_of_axis)
plt.figure()
plt.imshow(imaxis)
plt.grid()
plt.show()
- 计算重投影误差
def printReprojectError(corners,size_of_marker,intrinsic_matrix,distort_matrix,rvecs,tvecs):
reproject_err = 0
model_points = np.array([
(-size_of_marker/2,size_of_marker/2,0.0),
(size_of_marker/2,size_of_marker/2,0.0),
(size_of_marker / 2, -size_of_marker / 2, 0.0),
(-size_of_marker / 2, -size_of_marker / 2, 0.0) ])
for i, image_points in enumerate(corners):
image_points_reproj, _ = cv2.projectPoints(model_points, rvecs[i], tvecs[i], np.float32(intrinsic_matrix),np.float32(distort_matrix))
image_points_reproj = np.int32(image_points_reproj.reshape(4,2))
reproject_err += np.linalg.norm(image_points_reproj-image_points)
meanReprojectError = reproject_err/len(corners)
rospy.logwarn("平均反投影误差(像素):"+ str(meanReprojectError))