feat: add PnP laser solving and update 0xA5 frame to 4-float protocol

- 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

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

@@ -0,0 +1,49 @@
+
+### 激光点位置解算详细说明
+
+本方案旨在通过视觉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)$。