对aruco marker 进行debug所使用的一些函数

这里给出显示Aruco角点的函数、显示marker坐标系的函数和计算重投影误差的函数。

1. 显示角点

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()

3. 计算重投影误差

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))