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

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

前面介绍过如何获取相机图片以及apriltag的识别，那么针对apriltag进行位置的获取作为应用，在这之前我们需要考虑相机标定，一般相机分为内参标定和外参标定。没有标定的相机对于位置的提取、识别等的计算都会带来极大的误差，所以标定是必须项。

内参则是相机的光学中心c、焦距f，像素倾斜系数s，畸变系数，一些成品相机厂家都提供了较为准确的值一般形式为3x3的矩阵进行表达

畸变系数（像一些鱼眼相机之类，畸变较严重，中心点和边缘差别很大）

详细的介绍可以看 「链接」 接下来我们将介绍利用 opencv 的标定方法标定mujoco的相机，本质mujoco相机是一个理想相机，其内参可以通过fov进行直接计算，但不妨碍进行对比

这里简化相机为固定实现，使用fixed_camera，可以提前在xml中定义，如下

<camera name="rgb_camera" pos="0.5 0 0.6" euler="0 0 0" fovy="60"/>

获取相机图片的方法通过封装，如下一行即可得到cv需要的bgr格式

image = self.getFixedCameraImage(fix_elevation=-90, show=True)

相机标定方法需要在视野下对于棋盘格checkboard进行前后左右上下以及姿态的旋转，这里可以用两种方式，一个是拖拽，另一个是mocap（前面有视频分享过）然后通过键盘进行控制

if key_states[keyboard.Key.up]:
            self.checker_board_z += 0.01
        if key_states[keyboard.Key.down]:
            self.checker_board_z -= 0.01
        if key_states[keyboard.Key.left]:
            self.checker_board_x -= 0.01
        if key_states[keyboard.Key.right]:
            self.checker_board_x += 0.01
        if key_states[keyboard.Key.page_up]:
            self.checker_board_y += 0.01
        if key_states[keyboard.Key.page_down]:
            self.checker_board_y -= 0.01
        
        self.setMocapPosition("calib_checkerboard", [self.checker_board_x, self.checker_board_y, self.checker_board_z])

通过空格进行采集图片识别保存数据，使用cv的方法

 def collect_calib_image(self, image: np.ndarray) -> bool:
        gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
        gray = cv2.GaussianBlur(gray, (3, 3), 0)  # 去抗锯齿噪声
        gray = cv2.Canny(gray, 50, 150)          # 提取边缘，强化角点
        
        # 查找棋盘格角点
        ret, corners = cv2.findChessboardCorners(
            gray, CALIB_BOARD_SIZE,
            # 允许更快检测，接受边缘模糊的角点
            cv2.CALIB_CB_ADAPTIVE_THRESH 
            + cv2.CALIB_CB_NORMALIZE_IMAGE 
            + cv2.CALIB_CB_FAST_CHECK  # 没有角点直接返回，提升速度
        )
        
        if ret:
            # 降低精度要求，加快速度
            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 20, 0.01)
            corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)
            self.calib_object_points.append(objp)
            self.calib_image_points.append(corners2)
            self.calib_images.append(image)
            img_with_corners = cv2.drawChessboardCorners(image.copy(), CALIB_BOARD_SIZE, corners2, ret)
            cv2.imshow('Calibration Image', img_with_corners)
            cv2.waitKey(300)
            print(f"成功采集第 {len(self.calib_images)} 张标定图像")
            return True
        else:
            # 调试：显示当前灰度图
            cv2.imshow('Calibration Image', gray)
            cv2.waitKey(100)
            print("未检测到角点，请调整棋盘格位置/视角")
            return False

收集的图片越多越好，这里设定大于5张，回车进行内参计算

def calibrate_camera(self) -> Tuple[np.ndarray, np.ndarray]:
        if len(self.calib_images) < 5:
            print(f"标定图像数量不足（当前{len(self.calib_images)}张，建议至少5张）")
            return None, None
        
        print("\n开始相机标定...")
        gray = cv2.cvtColor(self.calib_images[0], cv2.COLOR_RGB2GRAY)
        h, w = gray.shape[:2]
        
        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
            self.calib_object_points,
            self.calib_image_points,
            (w, h),
            None,
            None
        )
        
        if ret:
            new_mtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
            # 计算重投影误差
            total_error = 0
            for i in range(len(self.calib_object_points)):
                img_points2, _ = cv2.projectPoints(
                    self.calib_object_points[i], rvecs[i], tvecs[i], mtx, dist
                )
                error = cv2.norm(self.calib_image_points[i], img_points2, cv2.NORM_L2) / len(img_points2)
                total_error += error
            
            mean_error = total_error / len(self.calib_object_points)
            
            print("\n标定完成")
            print(f"平均重投影误差: {mean_error:.6f} (越小越好，建议小于<0.1)")
            print(f"相机内参矩阵:\n{mtx}")
            print(f"畸变系数:\n{dist}")
            print(f"优化后相机矩阵:\n{new_mtx}")
            
            self.camera_matrix = new_mtx
            self.dist_coeffs = dist
            self.calib_done = True
        
            return new_mtx, dist
        else:
            print("标定失败")
            return None, None

其中我们可以和直接用fov计算的对比

camera_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_CAMERA, "rgb_camera")
                fovy = self.model.cam_fovy[camera_id]
                width = 640
                height = 480
                f = 0.5 * height / math.tan(fovy * math.pi / 360)

                CAMERA_MATRIX = np.array([
                    [f, 0, width / 2],
                    [0, f, height / 2],
                    [0, 0, 1]
                ], dtype=np.float32)

                DIST_COEFFS = np.zeros(5, dtype=np.float32)  # 虚拟相机无畸变
                print("Mujoco FOV直接计算的内参：")
                print(CAMERA_MATRIX)

标定和直接计算的结果对比