Vision/scripts/calibrate/标定与解算流程.md

激光点位置解算详细说明

本方案旨在通过视觉PnP算法解算相机位姿，结合预先标定的“相机-激光器”刚体关系，计算激光点在矩形平面（世界坐标系）上的精确位置。

1. 前置准备：相机内参标定

首先对相机进行内参标定，获取相机内参矩阵 K 和畸变系数。这是后续所有像素坐标转空间坐标的基础。

2. 第一阶段：激光器与相机的联合标定（离线）

由于激光器与相机刚性绑定，激光束在相机坐标系下表现为一条固定的空间直线。我们需要通过以下步骤求出这条直线的方程（起点与方向）。

1. 数据采集：

   • 开启激光器，将激光点投射在平面标定板（如棋盘格）上。
   • 保持相机与激光器相对静止，改变整体姿态（距离、角度），拍摄多组（建议10组以上）包含标定板和激光点的图像。

2. 解算相机坐标系下的激光点：

   • 外参获取：对每张图像使用PnP算法，根据标定板角点计算相机相对于标定板的外参矩阵 $T_{wc}$（即世界到相机的变换）。
   • 反投影求交：
     • 提取激光点的像素坐标 $(u, v)$。
     • 利用 K^{-1} 将其转换为归一化射线方向。
     • 关键步骤：利用标定板在相机坐标系下的平面方程与该射线求交，计算出激光点在相机坐标系下的三维坐标 $P_{c_i}$。

3. 拟合激光线方程：

   • 收集所有图像对应的相机坐标系下的激光点 ${P_{c_1}, P_{c_2}, ...}$。
   • 使用最小二乘法或PCA（主成分分析）拟合这些点，得到相机坐标系下的激光直线方程。
   • 记录参数：
     • 方向向量 $\vec{d}_{cam}$：拟合直线的单位方向向量。
     • 基准起点 $O_{cam}$：选取该直线与相机坐标系 Z=0 平面的交点作为参考原点。

3. 第二阶段：在线测量与位置解算

标定完成后，即可在实际应用中实时计算激光点位置。

1. 获取当前相机位姿：

   • 拍摄包含目标矩形平面的图像。
   • 根据成像原理：$\text{像素坐标} \approx K \cdot [\text{归一化平面}] (T_{wc} \cdot \text{世界坐标})$。
   • 已知像素坐标、内参 K 以及矩形平面的特征点世界坐标，利用PnP算法解算出当前的外参矩阵 $T_{wc}$（包含旋转矩阵 R 和平移向量 $t$）。

2. 坐标变换至世界坐标系： 将标定阶段得到的激光线参数从相机坐标系变换到世界坐标系：

   • 变换方向向量（仅受旋转影响）： \vec{d}_{world} = R^T \cdot \vec{d}_{cam}
   • 变换基准起点（受旋转和平移共同影响，即乘以 T_{wc} 的逆矩阵）： O_{world} = R^T \cdot (O_{cam} - t)

3. 求解世界坐标：

   • 构建世界坐标系下的激光射线方程：$L(s) = O_{world} + s \cdot \vec{d}_{world}$。
   • 平面约束：已知目标平面在世界坐标系中满足 $Z_{world} = 0$。
   • 联立求解：令射线的Z分量等于0，解出参数 $s$，代回方程求得 $X, Y$。
   • 最终输出：激光点在矩形平面上的世界坐标 $(x, y, 0)$。