通过Apritag码估计平面方程

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:] 点，也可以求出方程。