pick_and_place_with_2d_matching_moving_cam.hdev *眼在手上 2D匹配，3D抓取【案例解析】

prepare_poses_and_rectification_data_moving_cam函数解析

这段代码的核心价值

功能	说明
位姿准备	构建与图像对齐的匹配平面，并考虑物体高度
Z轴方向校正	确保平面法向量朝向合理（避免背面）
自适应分辨率	通过投影点拟合自动确定校正尺度
高效校正	预生成 RectificationMap，避免重复计算透视变换
格式兼容	输入/输出保持机器人原有位姿格式
模块化输出	通过字典传递数据，便于后续流程使用

这段 HALCON/HDevelop 脚本代码用于 为机器人抓取任务准备位姿（poses）和图像校正映射（rectification map），核心目标是：

在已知物体高度的前提下，构建一个与相机图像对齐的"匹配平面"，并生成可用于快速重投影的校正映射（map），以便后续进行模板匹配或抓取规划。

下面逐部分详细解析。

一、输入读取与校验

read_dict_tuple (HandEyeCalibData, 'CamParam', CamParam)
read_dict_tuple (HandEyeCalibData, 'ToolInCamPose', ToolInCamPose)
read_dict_tuple (HandEyeCalibData, 'PlaneInBasePose0', PlaneInBasePose0)

• 从手眼标定数据中读取：

  • 相机参数 CamParam

  • 工具（末端）在相机坐标系中的位姿 ToolInCamPose

  • 初始参考平面在机器人基坐标系中的位姿 PlaneInBasePose0

    if (ObjectHeight < 0.0) throw(...)
    if (CamParam[0] =～ 'line_scan') throw(...)

• 校验：物体高度不能为负；不支持线扫相机。

二、统一所有位姿表示格式

get_pose_type (ToolInBasePose, OrderOfTransform, ...)
convert_pose_type (..., 'Rp+T', 'gba', 'point', ...)

• 获取当前机器人使用的位姿格式（旋转顺序、平移/旋转顺序等）
• 将所有位姿转换为内部统一格式：'Rp+T'（旋转+平移）、'gba'（ZYX欧拉角）、'point'
• 最后会再转回原始格式输出，保证兼容性

---

三、构建"对齐图像"的参考平面（PlaneInCamPose）

1. 计算参考平面在相机坐标系中的初始位姿

pose_invert (ToolInBasePose, BaseInToolPose)
pose_compose (ToolInCamPose, BaseInToolPose, BaseInCamPose)
pose_compose (BaseInCamPose, PlaneInBasePose0, PlaneInCamPose0)

• 得到 PlaneInCamPose0：参考平面在相机坐标系中的位姿

2. 确保平面 Z 轴指向远离相机方向

affine_trans_point_3d (..., 0,0,1, ..., CosAngleBetweenZAxis)
if (CosAngleBetweenZAxis < 0)
    // 绕X轴旋转180°，翻转Z方向
    pose_compose (PlaneInCamPose0, SwitchZDirection, PlaneInCamPose1)
endif

• 避免平面"背面朝向"相机，影响后续投影

3. 对齐图像坐标系（消除绕Z轴旋转）

**PlaneInCamPose：**参考平面在相机坐标系中的位姿

PlaneInCamPose := PlaneInCamPose0
PlaneInCamPose[5] := 0.0  // 第6个元素是绕Z轴的旋转角（gamma），设为0

• 使得平面在图像中"正对"，X/Y 与图像行列方向一致（无偏航）

4. 更新该平面对应的基坐标系位姿

pose_invert (BaseInCamPose, CamInBasePose)
pose_compose (CamInBasePose, PlaneInCamPose, PlaneInBasePose)

✅ 此时 PlaneInCamPose 是一个与当前图像对齐、Z轴朝外的标准参考平面

四、创建"匹配平面"（考虑物体高度）

create_pose (0, 0, -ObjectHeight, 0, 0, 0, ..., MatchingPlaneInPlanePose)
pose_compose (PlaneInBasePose, ..., MatchingPlaneInBasePose)
pose_compose (PlaneInCamPose, ..., MatchingPlaneInCamPose)

• 在参考平面上向下偏移 ObjectHeight（因为物体表面高于参考平面？或反之，取决于约定）
• 得到实际用于匹配的平面位姿：MatchingPlaneInCamPose 和 MatchingPlaneInBasePose

💡 假设 PlaneInBasePose0 是工作台平面，而物体有一定高度，则匹配应在物体顶面进行。

五、根据 RectifyImage 模式处理校正

情况 A：'no_rectification'

• 直接使用 MatchingPlaneInCamPose
• ScaleRectification 留空

情况 B：'only_rectify' 或 'align_and_rectify'

步骤 1：自动计算校正尺度（分辨率）

gen_grid_region (RegionGrid, 20, 20, 'points', Width, Height)
contour_to_world_plane_xld (..., MatchingPlaneInCamPose, ...)
fit_ellipse_contour_xld (...)
ScaleRectification := min(Radius2)

• 在原始图像上撒点（20x20网格）
• 将这些1*1的像素点投影到匹配平面，拟合椭圆
• 取短轴半径 min(Radius2) 作为物理尺寸对应1像素的尺度（单位：米/像素）
• 目的：保证校正后图像分辨率不低于原图

📌 这是一种自适应分辨率策略，避免过度缩放导致信息丢失或计算浪费。

步骤 2：确定校正图像应覆盖的物理范围

gen_rectangle1 (ImageArea, 0, 0, Height-1, Width-1)
boundary → get_region_points → image_points_to_world_plane

• 将原始图像四边边界点反投影到匹配平面，得到 (BorderX, BorderY)
• 计算 X/Y 方向的物理范围：[min, max]

步骤 3：构建校正用的位姿和图像尺寸

create_pose (min(BorderX), min(BorderY), 0, ...)  // 新原点在左下角
pose_compose → MatchingPlaneRectifiedPartInCamPose

WidthRect  = (ΔX) / Scale + 0.5
HeightRect = (ΔY) / Scale + 0.5

步骤 4：生成校正映射（Map）而非直接校正图像

gen_image_to_world_plane_map (RectificationMap, CamParam, MatchingPlaneInCamPose, 
                              Width, Height, WidthRect, HeightRect, ScaleRectification, 'bilinear')

• ✅ 关键优化 ：不每次都调用 image_to_world_plane，而是预计算一个映射表（map）
• 后续只需 map_image(Image, RectificationMap, ImageRectified) 即可快速校正
• 适用于需要多次校正同一视角图像的场景（如实时抓取）

六、恢复位姿格式并输出结果

convert_pose_type (..., original_format, ...)

• 将所有内部计算的位姿转回机器人原始使用的格式

  create_pose (0, 0, ObjectHeight, 180, 0, 0, ..., ModelInPlanePose)
  pose_invert (ModelInPlanePose, PlaneInModelPose)

• 定义模型坐标系相对于匹配平面的变换（用于后续将匹配结果转为抓取位姿）

七、打包输出到字典

1. Poses 字典（供匹配和抓取使用）

set_dict_tuple (Poses, 'PlaneInCamPose', ...)
set_dict_tuple (Poses, 'MatchingPlaneInBasePose', ...)
set_dict_tuple (Poses, 'PlaneInModelPose', ...)  // 关键：模型到平面的变换

2. RectificationData 字典（供图像校正使用）

set_dict_tuple (RectificationData, 'RectifyImage', ...)
if (need rectify)
    set_dict_tuple (..., 'ScaleRectification', ...)
    set_dict_object (RectificationMap, ...)  // 存储映射对象
endif
set_dict_tuple (..., 'MatchingPlaneRectifiedPartInCamPose', ...)

obtain_3d_pose_of_match_moving_cam函数解析

核心功能是：

根据 2D 形状匹配结果（Row, Column, Angle）和系统标定数据，计算物体模型在机器人基坐标系中的 3D 位姿（ModelInBasePose）。

这是 Eye-in-Hand（相机装在机械臂上） 场景下实现 6D 物体定位 的关键步骤。下面逐段详细解析。

一、输入数据读取

read_dict_tuple (HandEyeCalibData, 'CamParam', CamParam)
read_dict_tuple (HandEyeCalibData, 'ToolInCamPose', ToolInCamPose)
read_dict_tuple (Poses, 'PlaneInModelPose', PlaneInModelPose)
read_dict_tuple (RectificationData, 'RectifyImage', RectifyImage)
if (RectifyImage != 'no_rectification')
    read_dict_tuple (RectificationData, 'ScaleRectification', ScaleRectification)
endif
read_dict_tuple (RectificationData, 'MatchingPlaneRectifiedPartInCamPose', MatchingPlaneRectifiedPartInCamPose)

关键输入说明：

变量	含义
CamParam	相机内参（含畸变）
ToolInCamPose	工具（末端）在相机坐标系中的位姿（来自手眼标定）
PlaneInModelPose	匹配平面 在物体模型坐标系中的位姿（通常为已知，如模型底面）
RectifyImage	是否启用图像校正（'no_rectification', 'only_rectify', 'align_and_rectify'）
ScaleRectification	校正图像的物理尺度（米/像素）
MatchingPlaneRectifiedPartInCamPose	用于校正的匹配平面 在相机坐标系中的位姿（由前序流程生成）

💡 注意：MatchingPlaneRectifiedPartInCamPose 是一个"对齐且可能平移过原点"的平面位姿，专用于校正。

二、统一所有位姿表示格式

get_pose_type (ToolInBasePose, OrderOfTransform, OrderOfRotation, ViewOfTransform)
convert_pose_type (..., 'Rp+T', 'gba', 'point', ...)

• 获取当前机器人使用的位姿格式（用于最后输出时还原）
• 将所有内部位姿转换为统一格式：
  • 'Rp+T'：旋转 + 平移
  • 'gba'：ZYX 欧拉角（gamma-beta-alpha）
  • 'point'：位姿描述的是一个点（非方向？或约定）

三、核心逻辑：从 2D 匹配 → 3D 位姿

前提：仅处理单个匹配结果

if (|Row| == 1)
    ...
else
    throw ('Exactly one match should be given as input')
endif

---

步骤 1：构建 2D 刚性变换矩阵

vector_angle_to_rigid (0, 0, 0, Row, Column, Angle, HomMat2DObject)

• 将匹配结果 (Row, Column, Angle) 转换为 2D 齐次变换矩阵 HomMat2DObject
• 表示：从模型坐标系（原点在模型中心）到图像像素坐标的变换
• 注意 HALCON 中：Col = x, Row = y

四、分两种情况处理：是否图像校正

✅ 情况 A：RectifyImage == 'no_rectification'（无校正）

affine_trans_pixel (HomMat2DObject, 0, 0, RowObject, ColObject)
image_points_to_world_plane (CamParam, MatchingPlaneRectifiedPartInCamPose, RowObject, ColObject, 'm', PXM, PYM)

• 将模型原点 (0,0) 变换到图像像素 (ColObject, RowObject)

• 再通过 image_points_to_world_plane 将该像素点反投影到匹配平面 ，得到物理坐标 (PXM, PYM)

  HomMat3DObject := [ ... ] // 构造 3x4 齐次矩阵
  hom_mat3d_to_pose (HomMat3DObject, ModelToMatchInPlanePose)

• 手动构造 3D 变换矩阵（Z=0，因为假设物体在平面上）

• 转为位姿 ModelToMatchInPlanePose：模型原点在匹配平面坐标系中的位姿

  pose_compose (ModelToMatchInPlanePose, PlaneInModelPose, ModelInPlanePose)
  pose_compose (MatchingPlaneRectifiedPartInCamPose, ModelInPlanePose, ModelInCamPose)

• PlaneInModelPose 是平面在模型坐标系中的位姿 ，其逆即为模型在平面坐标系中的位姿

• 但此处直接 compose(ModelToMatchInPlanePose, PlaneInModelPose) 实际等价于：

ModelInPlane = ModelToMatchInPlane ∘ PlaneInModelModelInPlane

= ModelToMatchInPlane ∘ PlaneInModel

⚠️ 这里逻辑需注意：通常应为 inv(PlaneInModelPose)，但若 PlaneInModelPose 定义为"从模型到平面"，则正确。

• 最终得到 ModelInCamPose：模型在相机坐标系中的位姿

✅ 情况 B：RectifyImage == 'only_rectify' or 'align_and_rectify'（有校正）

HomMat3DObject := [
    HomMat2DObject[4], HomMat2DObject[3], 0, HomMat2DObject[5] * ScaleRectification,
    HomMat2DObject[1], HomMat2DObject[0], 0, HomMat2DObject[2] * ScaleRectification,
    0, 0, 1, 0
]

• 关键区别 ：在校正图像中，像素已直接对应物理坐标（乘以 ScaleRectification）

• HomMat2DObject[5] 和 [2] 是平移项（x, y），乘以 ScaleRectification（米/像素）得到物理平移（米）

• 旋转部分直接填入（因校正后无透视，2D 旋转 ≈ 3D 绕Z旋转）

  hom_mat3d_to_pose (..., ModelToMatchInPlanePartRectPose)
  pose_compose (..., PlaneInModelPose, ModelInMatchingPlaneRectifiedPartPose)
  pose_compose (MatchingPlaneRectifiedPartInCamPose, ..., ModelInCamPose)

• 同样组合得到 ModelInCamPose

✅ 优势：避免调用 image_points_to_world_plane，计算更快（因校正图已是度量空间）

五、将模型位姿转换到机器人基坐标系

pose_invert (ToolInBasePose, BaseInToolPose)
pose_compose (ToolInCamPose, BaseInToolPose, BaseInCamPose)
pose_invert (BaseInCamPose, CamInBasePose)
pose_compose (CamInBasePose, ModelInCamPose, ModelInBasePose)

推导链：

1. ToolInBasePose：工具在基坐标系
2. BaseInToolPose = inv(ToolInBasePose)
3. BaseInCamPose = ToolInCamPose ∘ BaseInToolPose → 基坐标系在相机中的位姿
4. CamInBasePose = inv(BaseInCamPose) → 相机在基坐标系中的位姿
5. ModelInBasePose = CamInBasePose ∘ ModelInCamPose → 最终输出：模型在基坐标系中的位姿

🎯 这就是机器人控制器需要的抓取目标位姿！

---

🔹 六、恢复原始位姿格式并返回

convert_pose_type (ModelInBasePose, OrderOfTransform, OrderOfRotation, ViewOfTransform, ModelInBasePose)

• 将内部计算的 'Rp+T', 'gba' 格式转回机器人实际使用的格式（如 'Rp+T', 'xyz' 或 'T+Rp' 等）
• 确保与机器人通信兼容

总结：函数输入/输出

输入	说明
Row, Column, Angle	2D 形状匹配结果（单个）
ToolInBasePose	拍摄图像时，工具在基坐标系中的位姿
HandEyeCalibData	手眼标定结果（ToolInCamPose + CamParam）
RectificationData	图像校正参数
Poses.PlaneInModelPose	匹配平面在模型坐标系中的定义