3D位姿定义解析（halcon）

综合 HALCON 3D 位姿定义解析

1. 位姿核心组成

• 参数形式：6维向量 [Tx, Ty, Tz, Rx, Ry, Rz]
• 平移分量（位置）：
  • Tx, Ty, Tz：物体原点在世界坐标系的偏移量
  • 单位：米（m）或毫米（mm），需统一
• 旋转分量（方向）：
  • Rx, Ry, Rz：绕坐标轴的旋转角度
  • 默认单位：弧度（rad），可用deg()转换角度
  • 示例：90° → deg(90) ≈ 1.5708 rad

2. 旋转表示规范

• 旋转类型：欧拉角（Euler angles）
• 旋转顺序（关键！）：
• 坐标系规则：
  • 右手坐标系：拇指=X，食指=Y，中指=Z
  • 旋转方向：逆时针为正（右手定则）

3. 坐标系定义

• 位姿本质：描述从物体坐标系到世界坐标系的刚体变换

  P_world = Pose * P_object
  

4. 齐次变换矩阵

位姿等效的 4×4 变换矩阵:
$$H=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& T_x\\ R_{21}& R_{22}& R_{23}& T_y\\ R_{31}& R_{32}& R_{33}& T_z\\ 0& 0& 0& 1\end{array}\right]$$

• R矩阵计算：

  # 伪代码：ZYX顺序的旋转矩阵计算
  Rz = [[cos(Rz), -sin(Rz), 0],
        [sin(Rz), cos(Rz),  0],
        [0,       0,        1]]
  
  Ry = [[cos(Ry),  0, sin(Ry)],
        [0,        1, 0      ],
        [-sin(Ry), 0, cos(Ry)]]
  
  Rx = [[1,  0,        0       ],
        [0, cos(Rx), -sin(Rx) ],
        [0, sin(Rx), cos(Rx)  ]]
  
  R = Rz @ Ry @ Rx  # 矩阵乘法顺序：Z→Y→X
  

5. 核心算子使用

* 创建位姿（指定旋转顺序）
create_pose(0.1, 0.2, 0.3,         // Tx, Ty, Tz
            0.0, HALCON_PI/2, 0.0, // Rx, Ry, Rz 
            'Rp+T', 'ZYX', 'point', // 变换类型/旋转顺序/投影类型
            Pose)                   // 输出位姿

* 位姿 ↔ 矩阵转换
pose_to_hom_mat3d(Pose, HomMat3D)  // 位姿→矩阵
hom_mat3d_to_pose(HomMat3D, Pose)  // 矩阵→位姿

* 位姿复合运算
pose_compose(Pose1, Pose2, PoseResult)  // PoseResult = Pose1 * Pose2
pose_invert(Pose, InvPose)              // 计算逆位姿

6. 关键注意事项

1. 旋转顺序敏感性：

   • ZYX（默认）≠ XYZ ≠ ZYZ
   • 万向节死锁：当Ry=±90°时，Rx和Rz自由度退化

2. 方向混淆陷阱：

   • 机器人工件坐标系 vs 相机坐标系
   • HALCON的Y轴向下（图像坐标惯例）

3. 应用黄金法则：
   

7. 实战调试技巧

• 可视化验证：

  * 创建3D模型
  create_shape_model_3d(..., ModelID)
  
  * 应用位姿投影
  project_shape_model_3d(Image, ModelID, Pose, ...)
  
  * 显示坐标框架（箭头指示方向）
  disp_3d_coord_system(WindowHandle, CamParam, Pose, 0.1)
  

• 错误排查：
  • 位置异常 → 检查平移单位（mm/m）
  • 方向颠倒 → 验证旋转顺序和坐标系定义
  • Z轴反向 → 确认相机成像方向（z_out或z_in）

最佳实践：在机器人应用中，使用hand_eye_calibration标定后，务必通过image_points_to_world_plane验证坐标转换链的正确性。

以上综合解析了HALCON 3D位姿的核心定义、数学原理和应用实践，理解这些概念对实现精准的3D视觉定位至关重要。