Mujoco 仿真相机下 SolvePnp 获得 Apriltag 位姿

视频讲解：https://www.bilibili.com/video/BV1PTwTzUEq5/?vd_source=5ba34935b7845cd15c65ef62c64ba82f

代码仓库：https://github.com/LitchiCheng/mujoco-learning

上期视频《Mujoco 仿真棋格盘标定相机内参方法》中我们得到了相机的内参，结合之前Apriltag的标签获取，今天介绍一下在Mujoco的相机视野下通过solvepnp计算得到apriltag的位姿。

参考 https://docs.opencv.ac.cn/4.11.0/d5/d1f/calib3d_solvePnP.html 使用opencv的solvepnp，首先需要传入内参，畸变参数，tag的大小

原理：得到在相机坐标系下的位姿和在世界坐标系下的位姿，一般情况下是无法得到两个坐标系的变换关系，有无穷种解，但当描述的物体是同一个或已知物体之间的变换干系，那么就可以根据如下

安装cv2，uv pip install cv2 即可

复制代码

TAG_SIZE = 0.1 
# 格式：[[fx, 0, cx], [0, fy, cy], [0, 0, 1]]
# fx/fy: 焦距, cx/cy: 主点坐标
CAMERA_MATRIX = np.array([
    [415.019, 0.0, 318.59934938],
    [0.0, 415.294660431, 242.1195262],
    [0.0, 0.0, 1.0] 
], dtype=np.float32)
DIST_COEFFS = np.array([-0.00201 , 0.00547 ,-0.0003586  ,-0.00020906 ,-0.00284597], dtype=np.float32)
self.spnp = solvepnp.SolvePnp(TAG_SIZE, CAMERA_MATRIX, DIST_COEFFS)

然后通过getFixedCameraImage就得到相机视野下的bgr图，传入给solvepnp的计算方法中，循环中已经配置了键盘的控制，可以通过移动观察计算出的位置是否基本相近

复制代码

self.setMocapPosition("apriltag_0", [self.apriltag_pos_x, self.apriltag_pos_y, self.apriltag_pos_z])
image = self.getFixedCameraImage(fix_elevation=-90)
# print(self.model.cam_ipd)
if image is None:
    pass
else:
    _,_,_,transform = self.spnp.compute(image, 0)
    self.spnp.show()
    print(transform)

当然pnp的方法遇到的情况和使用的具体算法有很多，代码中只用到了一种情况做了封装

复制代码

# SOLVEPNP_IPPE_SQUARE 适合正方形标签，精度更高
success, rvec, tvec = cv2.solvePnP(
    self.obj_points, 
    img_points, 
    self.camera_intrinsic, 
    self.camera_distort,
    flags=cv2.SOLVEPNP_IPPE_SQUARE
)

SOLVEPNP_ITERATIVE：基于Levenberg-Marquardt（LM）迭代优化，至少 4 个有效点，重投影误差迭代
SOLVEPNP_EPNP：支持 ≥4 个点，适合多标签 / 多特征点的场景，解算效率高
SOLVEPNP_SQPNP：支持 ≥3 个点即可解算，无初值要求，适合标签被遮挡、仅提取到 3 个角点的场景

最终输出为apriltag在相机坐标系下的4*4的齐次变换矩阵

如图，apriltag位置相对于相机，x=0.00176，y=-0.00255，z=0.512，和mujoco 中配置的位置基本一致