import numpy as np
import apriltag
import cv2
cam_params = np.array([794.1542154817092, 793.3728258493983, 695.2922336702912, 337.1454128919332])
K = np.array([[cam_params[0], 0, cam_params[2]], [0, cam_params[1], cam_params[3]], [0, 0, 1]])
dis_coeffs = np.array([-0.3875762756205275, 0.1316523135418939, -0.001223574701847069, -0.001407498199948943])
tag_len = 48
img = cv2.imread("1.bmp")
img_undistored = cv2.undistort(img, K, dis_coeffs)
gray = cv2.cvtColor(img_undistored, cv2.COLOR_BGR2GRAY)
at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag16h5'))
tags = at_detector.detect(gray)
print("tags: {}\n".format(tags))
points = np.array([])
for tag in tags:
coord = tag
#print(coord.center)
M,e1,e2=at_detector.detection_pose(coord, cam_params)
M[:3,3:] *= tag_len
#print(M) #world2camera
points = np.append(points, M[:3,3:] )
for i in range(4):
cv2.circle(img_undistored, tuple(tag.corners[i].astype(int)), 4, (255, 0, 0), 2)
cv2.circle(img_undistored, tuple(tag.center.astype(int)), 4, (2, 180, 200), 4)
cv2.imwrite("mark.png", img_undistored)
points = points.reshape(-1,3)
b = points[:,2]
A = points[:,:2]
ones = np.ones((A.shape[0], 1))
A = np.append(A, ones, axis=1)
A1 = np.dot(A.T, A)
A2 = np.linalg.inv(A1)
A3 = np.dot(A2, A.T)
X = np.dot(A3, b)
print(X) # 平面方程为z = X[0]*x + X[1]*y + X[2]
上面的程序是通过在平面上放置若干个(3个以上)Apriltag码,计算这些码中心坐标,再使用最小二乘法拟合在相机坐标系下的平面方程z = X[0]*x + X[1]*y + X[2],该平面的法向量为(X[0], X[1], -1)。如果只有Apriltag码个数少于3则无法通过最小二乘法求解,平面的法向量可以通过单个码的M[:3, 2]确定,又平面经过M[:3,3:] 点,也可以求出方程。