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