通过Apriltag码估计物体在相机坐标系下的位姿

下面的python脚本实现了通过Apriltag码（tag16h5）估计物体在相机坐标系下的位姿的功能。
世界坐标系原点为id=10的tag中心，物体坐标系各坐标轴朝向和世界坐标系相同，仅存在一个平移变换obj2world_T。

import numpy as np
import apriltag
import cv2


img = cv2.imread("color.png")
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag16h5'))
tags = at_detector.detect(gray)
#print("tags: {}\n".format(tags))

cam_params = np.array([612.345825, 613.81781, 318.473251, 237.981806]) #fx, fy, cx, cy
tag_len = 48    #tag的边长（单位：mm）
obj2world_T = np.array([20, 95, 50])    #物体中心在世界坐标系中的坐标值

for tag in tags:       
    if tag.tag_id == 10: #世界坐标系原点为id=10的tag中心
        M, e1, e2 = at_detector.detection_pose(tag, cam_params)
        M[:3,3:] *= tag_len

        obj2world = np.identity(4)
        obj2world[:3, 3] = obj2world_T
        world2camera = M
        obj2camera = world2camera.dot(obj2world)
        print("obj2camera:\n", obj2camera)

        for i in range(4):
            cv2.circle(img, tuple(tag.corners[i].astype(int)), 4, (255, 0, 0), 2)
        cv2.circle(img, tuple(tag.center.astype(int)), 4, (2, 180, 200), 4)
        cv2.imwrite("mark.png",img)

参考：Apriltag使用之二：方位估计(定位)