Halcon之计算抓取螺母的位姿

1，项目说明。

1. Robot标定使用的模式是eye-to-hand,即相机是固定静止，相机不随Robot运动。
2. BaseInCamPose、GripperInToolPose位姿由手眼标定事先标定完成。
3. 抓取螺母是基于2D场景而非3D点云场景，在2D场景下忽视螺母高度，视螺母高度为0。
4. 抓取螺母的姿态:工具坐标系位于Robot基础坐标系的位姿（ToolInBasePose）即发送给Robot的位姿数据。
5. Demo使用的文件，图片需从halcon安装文件中获取。

---

2，注意事项

• **相机的外参：**相机的姿态，世界坐标系在相机坐标系中位姿。

• xld_cont‌ 与 ‌xld_poly 的区别：

  **xld_cont‌ 与 ‌xld_poly**‌ 是两种不同的 XLD（扩展线描述）对象类型，其核心区别如下：

  ‌1. 数据结构差异‌

  • ‌**xld_cont（XLD 轮廓）**‌
    表示原始的轮廓数据，由一系列连续的、亚像素精度的点构成，通常通过边缘检测（如 edges_sub_pix）或区域转换（如 gen_contour_region_xld）生成。
    • 包含完整的几何细节，适用于需要高精度分析的任务（如曲率计算、亚像素测量）。
    • 支持开放或闭合的轮廓形态。
  • ‌**xld_poly（XLD 多边形）**‌
    是通过对 xld_cont 进行多边形近似后生成的简化表示，由折线段的关键顶点构成。
    • 通过算法（如 Ramer 算法）减少点数，保留主要形状特征。
    • 适用于几何特征提取、快速匹配等对效率要求较高的场景。

  ‌2. 生成方式与转换‌

  • ‌生成 xld_cont 的典型方法‌
    • 边缘检测算子（如 edges_sub_pix）直接输出 xld_cont。
    • 从区域转换而来（如 gen_contour_region_xld）。
  • ‌生成 xld_poly 的核心算子‌
    • 使用 gen_polygons_xld 对 xld_cont 进行多边形近似，需指定算法类型（如 'ramer'）和精度参数 Alpha。
    • 手动创建多边形轮廓时，可通过 gen_contour_polygon_xld 直接生成 xld_poly。

  ‌3. 应用场景对比‌

  ‌特征‌	‌**xld_cont**‌	‌**xld_poly**‌
  ‌精度‌	亚像素级，保留完整细节	近似表达，精度由参数控制
  ‌数据复杂度‌	点数多，存储密集	点数少，结构简化
  ‌典型用途‌	高精度测量、复杂形状分析	快速几何分析、模板匹配
  ‌操作灵活性‌	支持复杂几何变换与特征计算	便于提取线段端点或平行边
  包含的数据类型	基于Row，Column的坐标	基于Row，Column的坐标，长度，角度

  ---

  ‌4. 相互转换关系‌

  • ‌**xld_cont → xld_poly**‌
    使用 gen_polygons_xld 进行多边形近似，通过调整 Alpha 参数平衡精度与简化程度。
  • ‌**xld_poly → xld_cont**‌
    可通过 gen_contour_polygon_xld 反向生成轮廓，但会丢失原始细节，常用于自定义形状构建。

  ---

  ‌总结‌

  xld_cont 是原始的高精度轮廓数据，适用于细节敏感场景；xld_poly 是基于 xld_cont 的简化多边形，用于提升处理效率。两者可通过 Halcon 算子灵活转换，根据任务需求选择合适的数据类型。

  ---

3，关联的主要算子

3.1， gen_parallels_xld

是 Halcon 中用于提取平行 XLD（扩展线描述）多边形的关键算子，其功能及核心要点如下：

‌一、功能概述‌

该算子通过分析输入的 XLD 多边形（EdgePolygons），检测并输出满足平行条件的 XLD 轮廓（ParallelEdges）。其核心目标是识别图像中的平行结构，适用于工业检测、物体测量等场景。

---

‌二、核心参数解析‌

1. ‌EdgePolygons‌
   • 输入参数，需为 XLD 多边形对象，通常通过边缘检测或其他分割算子生成。
2. ‌ParallelEdges‌
   • 输出参数，存储检测到的平行 XLD 轮廓。
3. ‌Len‌
   • 线段的最小长度阈值，仅考虑长度超过此值的多边形段，用于过滤噪声或短小边缘1。
4. ‌Dist‌
   • 平行线段间的最大允许距离。若线段在角度平分线方向上的投影距离小于此值，则判定为平行1。
5. ‌Alpha‌
   • 平行线段间的最大角度差（弧度）。角度差超过此值时，不视为平行结构1。

---

‌三、算法原理‌

1. ‌输入处理‌
   接收 XLD 多边形，并基于参数过滤不符合长度要求（Len）的线段。
2. ‌平行性判定‌
   • ‌距离约束‌：计算线段在角度平分线方向上的投影距离，若小于 Dist 则保留。
   • ‌角度约束‌：比较线段间的夹角，若小于 Alpha 则判定为平行1。
3. ‌输出生成‌
   将满足条件的平行线对合并为 XLD 轮廓，输出至 ParallelEdges。

---

四、典型应用场景‌

• ‌工业零件检测‌：如提取机械零件的平行边缘，用于尺寸测量或装配验证。
• ‌电子元件定位‌：识别 PCB 板上的平行引脚或线路。
• ‌文本区域分割‌：提取文档图像中的平行文本行。

---

‌五、调优建议‌

• ‌Len 设置‌：过小可能引入噪声，过大可能遗漏细节，需根据目标尺寸调整。
• ‌Dist 与 Alpha 平衡‌：较小的 Dist 和 Alpha 可提高精度，但可能漏检；增大参数可提升召回率，但可能包含误检1。
• ‌结合后处理‌：可联动 mod_parallels_xld 进一步筛选基于灰度均质性或质量因子的平行线。

---

3.2 ，convert_pose_type

用于转换 3D 位姿（Pose）的表示形式，涉及旋转和平移分量的数学表达切换。以下从参数列表、转换类型、数学原理及注意事项展开说明：

---

‌1. 参数列表与功能‌

‌参数‌	‌类型‌	‌说明‌
PoseIn	输入	输入位姿，格式由 FromType 定义（如 'Rp+T' 表示旋转矩阵+平移向量）
FromType	输入	输入位姿类型（支持 'Rp+T'、'rodriguez'、'abg' 等）
ToType	输入	目标位姿类型（如 'abg' 表示欧拉角，需配合 ToOrder 指定旋转顺序）
ToOrder	输入	旋转顺序（如 'ZYX' 表示绕 Z→Y→X 轴旋转）
PoseOut	输出	转换后的位姿

---

‌2. 核心参数解析‌

1. ‌**FromType 与 ToType 类型**‌
   • ‌**'Rp+T'**‌：旋转矩阵（3×3）与平移向量（3×1）的组合；
   • ‌**'rodriguez'**‌：罗德里格斯向量（3×1，旋转轴归一化后乘旋转角度）；
   • ‌**'abg'**‌：欧拉角（绕指定顺序的轴旋转，如 'ZYX' 对应 γ、β、α）。
2. ‌**ToOrder 旋转顺序**‌
   • 决定欧拉角分解方式：
     • 'ZYX'：绕 Z 轴旋转 γ → Y 轴旋转 β → X 轴旋转 α；
     • 'gba'（等价于 'ZYX'）：按 Roll-Pitch-Yaw 顺序。
   • ‌万向节死锁‌：当 β ≈ ±90° 时，分解结果不唯一，需谨慎处理。
3. ‌坐标系与应用视角‌
   位姿转换需明确视角类型（ViewOfTransform）：
   • ‌点变换‌（'point'）：直接应用旋转平移矩阵（P_new = R * P_old + T）8；
   • ‌坐标系变换‌（'coordinate_system'）：需取逆矩阵描述坐标系关系（P_old = R * P_new + T）8。

---

‌3. 典型转换示例‌

1. ‌旋转矩阵 → 欧拉角‌

• 输入：旋转矩阵（Rp+T）
  create_pose(0.1, 0.2, 0.3, 0.0, 0.0, 1.5708, ‘Rp+T’, ‘ZYX’, PoseIn)
• 转换
  convert_pose_type(PoseIn, ‘Rp+T’, ‘abg’, ‘ZYX’, PoseOut)
• 输出：PoseOut = [0.1, 0.2, 0.3, 0.0, 0.0, 1.5708] （Tx,Ty,Tz,α,β,γ）
• 2. ‌罗德里格斯向量 → 四元数‌

* 输入：罗德里格斯向量（Rodriguez）  
create_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5708, 'rodriguez', 'ZYX', PoseIn)  
* 转换  
convert_pose_type(PoseIn, 'rodriguez', 'quaternion', '', PoseOut)  
* 输出：PoseOut = [0.0, 0.0, 0.0, 0.7071, 0.0, 0.0, 0.7071] （Tx,Ty,Tz,qx,qy,qz,qw）  

---

‌4. 注意事项‌

• ‌单位一致性‌：平移量单位需与场景物理单位一致（如毫米或米），旋转角度默认弧度；
• ‌逆矩阵处理‌：若涉及坐标系变换（'coordinate_system'），需显式调用 hom_mat3d_invert 取逆；
• ‌数据类型匹配‌：字符串参数需严格匹配预设值（如 'quaternion' 不可简写为 'quat'）。

---

5. 应用场景‌

1. ‌机器人抓取‌：将视觉检测的 'Rp+T' 位姿转换为机器人控制器支持的欧拉角格式；
2. ‌多传感器标定‌：统一不同设备的位姿表示（如相机与雷达的 'rodriguez' → 'quaternion'）；
3. ‌运动规划‌：适配规划库（如 ROS MoveIt）的四元数输入要求。

4，程序流程。

5，代码

* 参考案例库：handeye_stationarycam_grasp_nut.hdev* 目的：
* 计算robot抓取螺母(不是3D点云是2D)的姿态（即ToolInRootBasePose）,该数据可直接发送给Robot* 注意事项：
* 本案例是eye to hand模式即相机为固定静止dev_update_off ()
dev_close_window ()
* Directories with calibration images and data files
ImageNameStart := '3d_machine_vision/handeye/stationarycam_'
DataNameStart := 'C:/Users/Public/Documents/MVTec/HALCON-12.0/examples/solution_guide/3d_vision/handeye/stationarycam_'
read_image (Image, ImageNameStart + 'nut12_square')
dev_close_window ()
get_image_size (Image, Width, Height)
dev_open_window_fit_image (Image, 0, 0, Width, Height, WindowHandle)
dev_set_draw ('margin')
dev_set_line_width (2)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
dev_display (Image)
disp_message (WindowHandle, 'Object to grasp', 'window', 12, 12, 'black', 'true')* ---------------1，读取相机内参read_cam_par (DataNameStart + 'final_campar.dat', CamParam)* ---------------2，读取 BaseInCamPoseread_pose (DataNameStart + 'final_pose_cam_base.dat', BaseInCamPose)
pose_to_hom_mat3d (BaseInCamPose, cam_H_base)
read_pose (DataNameStart + 'final_pose_tool_calplate.dat', CalplateInToolPose)
pose_to_hom_mat3d (CalplateInToolPose, tool_H_calplate)* ---------------3，读取 夹具在工具坐标系的姿态read_pose (DataNameStart + 'pose_tool_gripper.dat', GripperInToolPose)
pose_to_hom_mat3d (GripperInToolPose, tool_H_gripper)
stop ()*-----------------4，获取相机的外参* 相机的外参：世界坐标系在相机坐标系的位姿
* caltab_30mm.descr： Distance between mark centers : 0.00375m
CalplateFile := 'caltab_30mm.descr'
read_image (calpImage, ImageNameStart +'calib3cm_00')
* 从标定板描述文件中读取 Mark的 X，Y，Z值
caltab_points (CalplateFile, X, Y, Z)
* 
* parameter settings for find_caltab and find_marks_and_pose
SizeGauss := 3
MarkThresh := 100
MinDiamMarks := 5
StartThresh := 128
DeltaThresh := 10
MinThresh := 18
Alpha := 0.9
MinContLength := 15
MaxDiamMarks := 100
* 获取标定板区域
find_caltab (calpImage, Caltab, CalplateFile, SizeGauss, MarkThresh, MinDiamMarks)
* 获取Mark点 坐标 与 相机外参： 标定板在相机中的位姿（标定板代表世界坐标系）
find_marks_and_pose (calpImage, Caltab, CalplateFile, CamParam, StartThresh, DeltaThresh, MinThresh, Alpha, MinContLength, MaxDiamMarks, RCoord, CCoord, PoseRef)
* disp_cross (3600, RCoord, CCoord, 6, 0)
* 绘制坐标系
disp_3d_coord_system (WindowHandle, CamParam, PoseRef, 0.01)*--------------------5，获取螺母在世界坐标系中的位姿
*5.1，提取出螺母的轮廓
threshold (Image, Regions, 60, 255)fill_up (Regions, RegionFillUp)
connection (RegionFillUp, ConnectedRegions)
select_shape_std (ConnectedRegions, SelectedRegions, 'max_area', 0)
gen_contour_region_xld (SelectedRegions, Contours, 'border')
segment_contours_xld (Contours, ContoursSplit, 'lines_circles', 3, 6, 4)
*5.2,进行直线的拟合
count_obj (ContoursSplit, Number)
gen_empty_obj (lines)
for Index := 1 to Number by 1select_obj (ContoursSplit, ObjectSelected, Index)fit_line_contour_xld (ObjectSelected, 'tukey', -1, 0, 5, 2, RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist)gen_contour_polygon_xld (line,[ RowBegin,RowEnd], [ColBegin,ColEnd])concat_obj (lines, line, lines)
endfor
* 5.3,找出螺母平行的两条边
gen_polygons_xld (lines, Polygons, 'ramer', 2)
gen_parallels_xld (Polygons, Parallels, 50, 130, rad(10), 'false')
* 5.4, 获取平行线起点终点的坐标
get_parallels_xld (Parallels, Row1, Col1, Length1, Phi1, Row2, Col2, Length2, Phi2)
CornersRow := [Row1[0],Row1[1],Row2[0],Row2[1]]
CornersCol := [Col1[0],Col1[1],Col2[0],Col2[1]]
* 5.5, 将坐标转换到世界坐标系
image_points_to_world_plane (CamParam, PoseRef, CornersRow, CornersCol, 'm', CornersX_Ref, CornersY_Ref)
* 5.7, 计算在世界坐标系中螺母平行边的中心点与中点,角度
*CenterPointY_ref，CenterPointX_ref，GraspPointsX_ref，GraspPointsY_ref，GraspPhiZ_ref
CenterPointX_ref:=mean( CornersX_Ref)
CenterPointY_ref:=mean( CornersY_Ref)
* 抓取点的世界坐标
GraspPointsX_ref:=[mean([CornersX_Ref[0],CornersX_Ref[1]]),mean([CornersX_Ref[2],CornersX_Ref[3]])]
GraspPointsY_ref:=[mean([CornersY_Ref[0],CornersY_Ref[1]]),mean([CornersY_Ref[2],CornersY_Ref[3]])]
* 在世界坐标中螺母的旋转角度,atan返回的值为弧度
GraspPhiZ_ref:=atan((CornersY_Ref[1]-CornersY_Ref[0])/(CornersX_Ref[1]-CornersX_Ref[0]))
* 5.8，计算螺母在世界坐标系中的矩阵
hom_mat3d_identity (HomMat3DIdentity)
*绕Z axis 旋转
hom_mat3d_rotate (HomMat3DIdentity, GraspPhiZ_ref, 'z', 0, 0, 0, HomMat3DRotate)
* 平移
hom_mat3d_translate (HomMat3DRotate, CenterPointX_ref, CenterPointY_ref, 0, ref_H_grasp)* 5.9，计算出螺母在世界坐标系中的位姿
hom_mat3d_to_pose (ref_H_grasp, graspInRefPose)*------------------------6，计算螺母在相机坐标系中的位姿，显示螺母的抓取点
* 方法1：
pose_compose (graspInRefPose, PoseRef, graspInCamPose)* 方法2：
vector_to_pose (CornersX_Ref,CornersY_Ref, gen_tuple_const(|CornersX_Ref|,0), CornersRow, CornersCol, CamParam, 'iterative', 'error', graspInCamPose2, Quality)*螺母在世界坐标的抓取点仿射，投影出在相机坐标系中(用于在相机坐标系中展示抓取点）
pose_to_hom_mat3d (PoseRef, Cam_H_Ref)
affine_trans_point_3d (Cam_H_Ref, GraspPointsX_ref, GraspPointsY_ref, [0,0], Qx, Qy, Qz)
* 投影
project_3d_point (Qx, Qy, Qz, CamParam, Row, Column)
* 绘制夹取点
gen_cross_contour_xld (Cross, Row, Column, 12, 0)
* 显示坐标系
disp_3d_coord_system (WindowHandle, CamParam, PoseRef, 0.01)
* 显示螺母的平行边、夹取点
dev_display (Parallels)
dev_set_color ('green')
dev_set_line_width (3)
* 显示夹取点
dev_display (Cross)
disp_message (WindowHandle, 'G1', 'window', Row[0]-10, Column[0]+10, 'yellow', 'false')
disp_message (WindowHandle, 'G2', 'window', Row[1]-10, Column[1]-20, 'yellow', 'false')
dev_set_colored (12)
dev_set_line_width (1)*-----------------------7,计算最终的姿态：ToolInBasePose
pose_invert (BaseInCamPose,CamInBasePose)
pose_compose ( graspInCamPose,CamInBasePose, GripperInBasePose)
pose_invert (GripperInToolPose, ToolInGripperPose)
pose_compose (ToolInGripperPose, GripperInBasePose, ToolInBasePose)*-----------------------8,转换为发送给Robot的位姿
* Rp+T:先旋转后平移；R(p-T)：先平移后旋转
* gba:以欧拉角（Euler angles）‌ 的 γ-β-α 顺序描述旋转，
* abg:以欧拉角，的 ‌α-β-γ 顺序旋转convert_pose_type (ToolInBasePose, 'Rp+T', 'abg', 'point', PoseOut)
*弹出位姿窗口
dev_inspect_ctrl (PoseOut)
disp_continue_message (WindowHandle, 'black', 'true')
stop()
*关闭位姿窗口
dev_close_inspect_ctrl (PoseOut)

效果显示：

6，Demo链接。

Halcon之Robot抓取螺母姿态计算Demo