📚 课程目录第3章相机模型与标定

3. 相机模型与标定

在RGB-D机器人三维视觉工程中，相机模型与标定是连接“像素世界”与“物理世界”的桥梁。没有准确的相机模型，深度图无法反投影为点云，RGB与深度无法对齐，机器人抓取时也无法把图像坐标映射到机械臂基座坐标。本章围绕针孔相机模型、内参外参、畸变模型、RGB与深度对齐、张正友标定法以及机器人手眼标定六个核心知识点展开，帮助读者建立从成像几何到工程标定的完整链路。

一、针孔相机模型：三维到二维的投影

针孔相机模型是最常用的成像几何模型。它假设光线从物体表面出发，穿过一个理想的小孔，在像平面上形成倒立的像。设相机坐标系下一点为 P_c = (X_c, Y_c, Z_c)，其像素坐标为 (u, v)，则投影关系为：

u = f_x * X_c / Z_c + c_x
v = f_y * Y_c / Z_c + c_y

其中 f_x、f_y 是以像素为单位的焦距，c_x、c_y 是主点坐标。写成矩阵形式即内参矩阵 K：

K = [ f_x   0   c_x
       0   f_y  c_y
       0    0    1 ]

针孔模型忽略了镜头畸变和景深，但在大多数工业相机和RGB-D相机中，经过畸变校正后，该模型足以支撑三维重建与机器人定位。

二、内参、外参与畸变模型

内参描述相机自身的成像属性，包括焦距、主点、像素偏斜等；外参描述相机相对于世界坐标系或机器人基座坐标系的位姿，通常由旋转矩阵 R 和平移向量 t 组成。世界点 P_w 到像素点的完整投影链为：

P_c = R * P_w + t
pixel = K * P_c / Z_c

真实镜头存在径向畸变和切向畸变。径向畸变使直线在图像边缘弯曲，常用 k1、k2、k3 描述；切向畸变由镜头与成像平面不平行引起，常用 p1、p2 描述。OpenCV中通常使用5参数或8参数畸变模型：

distCoeffs = [k1, k2, p1, p2, k3]

工程提示：RGB-D相机的彩色相机和深度相机通常有各自的内参和畸变系数。深度图若未校正，反投影点云会出现“碗状”弯曲，抓取时表现为深度方向系统性偏差。

三、RGB与深度对齐：让颜色和距离落在同一像素

RGB-D相机中，彩色相机与深度传感器物理位置不同，因此同一物体在彩色图和深度图中的像素坐标不一致。RGB与深度对齐的目标是：把深度图重投影到彩色相机坐标系，使每个彩色像素都能查询到对应深度值。典型流程如下：

读取深度图与彩色图，获取各自内参和畸变系数。
将深度像素反投影到深度相机坐标系，得到三维点。
利用深度相机到彩色相机的外参 R, t，把三维点变换到彩色相机坐标系。
再投影到彩色图像平面，生成与彩色图对齐的深度图。

对齐后，彩色像素 (u, v) 与深度值 Z 一一对应，后续点云生成、目标检测和抓取位姿计算才能在同一坐标系下进行。

四、张正友标定法：从棋盘格到内参

张正友标定法利用平面棋盘格在不同位姿下的多张图像，通过角点检测和单应矩阵求解相机内参。其核心步骤为：

打印棋盘格标定板，固定在世界坐标系中，拍摄10~20张不同角度图像。
检测每张图像中的棋盘格角点，得到像素坐标。
利用平面约束构造单应矩阵 H，建立像素坐标与棋盘格物理坐标的对应关系。
由多个单应矩阵求解内参矩阵 K，再求解每张图像的外参 R, t。
使用最小二乘法优化内参、外参和畸变系数，降低重投影误差。

# OpenCV 张正友标定核心调用
ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(
    objectPoints, imagePoints, imageSize, None, None
)
print("重投影误差:", ret)
print("内参 K:\n", K)
print("畸变系数:", dist.ravel())

工程中常用重投影误差评价标定质量。若误差大于0.5像素，通常说明角点检测不准、标定板不平或图像位姿覆盖不足。

五、机器人手眼标定：把相机坐标接入机器人

手眼标定解决的是相机坐标系与机器人坐标系之间的变换关系。根据相机安装方式不同，分为“眼在手上”和“眼在手外”两类。眼在手上时，相机固定在机械臂末端，标定目标是求相机到末端的变换 T_cam_tool；眼在手外时，相机固定在外部支架，标定目标是求相机到机器人基座的变换 T_cam_base。

经典手眼标定方程为 AX = XB，其中 A 为机械臂末端两次运动的相对位姿，B 为相机两次观测的相对位姿，X 为待求的手眼矩阵。常用求解方法包括Tsai-Lenz、Park-Martin和OpenCV的 calibrateHandEye。

# OpenCV 手眼标定示例
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,
    R_target2cam, t_target2cam,
    method=cv2.CALIB_HAND_EYE_TSAI
)

常见误区：只做相机内参标定，不做手眼标定，导致点云在相机坐标系中看起来正确，但机械臂抓取时始终偏移。另一个误区是标定板位姿变化过小，导致外参退化，手眼矩阵不稳定。

六、工程案例与复盘清单

以一台搭载RGB-D相机的六轴机械臂抓取为例：先对彩色相机和深度相机分别标定内参和畸变，完成RGB与深度对齐；再用张正友标定法获得彩色相机内参；最后控制机械臂带动标定板运动，采集多组末端位姿与相机观测位姿，求解手眼矩阵。抓取时，目标检测得到像素坐标，结合对齐深度反投影为相机坐标系点云，再经手眼矩阵变换到机器人基座坐标系，生成抓取位姿。

标定环节	输入	输出	典型工具	关键指标
相机内参标定	棋盘格多视角图像	K、畸变系数	OpenCV calibrateCamera	重投影误差 < 0.5 px
RGB-深度对齐	RGB图、深度图、双相机内外参	对齐深度图	OpenCV reprojectImageTo3D	边缘对齐偏差 < 2 px
手眼标定	末端位姿、标定板观测位姿	T_cam_tool 或 T_cam_base	calibrateHandEye	抓取重复定位误差 < 2 mm

复盘时重点检查：标定板是否平整、角点检测是否稳定、图像位姿是否覆盖相机视场、深度图是否已校正、手眼标定运动是否包含足够旋转。只有把相机模型、畸变模型、RGB-深度对齐和手眼标定串成一条完整链路，RGB-D机器人才能在三维空间中稳定地“看得见、算得准、抓得住”。