dev01
fd3d6b7ab5
- Integrate PnP rectangle pose estimation + laser world coordinate computation from scripts/calibrate/laser_pnp.py - Publish normalized measurement (laser/RECT) and reference (rect center) via Float32MultiArray - Update gimbal 0xA5 frame to 18-byte format: measure_x, ref_x, measure_y, ref_y - Add camera calibration, laser line params, and RECT_L/W constants - Add corner ordering (TL/BL/BR/TR), SOLVEPNP_IPPE, and pose/laser visualization overlay - Remove standalone vision_example.py (superseded by ROS2 nodes) - Add scripts/calibrate/ with calibration and PnP tools
50 lines
3.3 KiB
Markdown
50 lines
3.3 KiB
Markdown
|
||
### 激光点位置解算详细说明
|
||
|
||
本方案旨在通过视觉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)$。
|