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）。其核心目标是识别图像中的平行结构，适用于工业检测、物体测量等场景。如果检查输出的xld轮廓存在非平行的轮廓，将导致后续get_parallels_xld计算出的Row,Column异常。

---

‌二、核心参数解析‌

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

---

‌三、算法原理‌

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

---

四、典型应用场景‌

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

---

‌五、调优建议‌

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

---

3.2 ，convert_pose_type

HALCON 机器视觉软件中的一个算子，主要用于‌改变 3D 位姿（Pose）的表示类型‌。
该算子的核心作用是将输入的 3D 位姿转换为具有不同数学表达形式（如不同的旋转顺序、变换顺序或视图定义）的输出位姿。需要注意的是，它‌仅改变位姿的表示方式，不改变位姿所描述的刚性变换本身的物理意义‌。

以下是关于该算子的详细说明：

1. 函数签名

convert_pose_type( : : PoseIn, OrderOfTransform, OrderOfRotation, ViewOfTransform : PoseOut)

2. 参数详解

• ‌PoseIn (输入)‌:原始的 3D 位姿。
  通常包含 7 个元素（具体取决于内部表示，但在 HALCON 标准 Pose 中通常对应平移和旋转参数）。

• ‌OrderOfTransform (输入)‌:定义旋转和平移的组合顺序。
  ‌默认值‌: ‘Rp+T’
  ‌建议值‌:
  Rp+T: 先旋转，后平移（Rotation then Translation）。create_pose 算子以及相机标定流程中最常用、也是默认的标准定义。
  R(p-T): 另一种变换顺序表示:先平移后旋转。除非机器人控制器或第三方算法明确要求使用 ‘R(p-T)’，否则一般不用此模式。

• OrderOfRotation (输入 - 字符串)‌: 定义旋转值的含义及旋转轴的顺序。
  ‌默认值‌: gba
  ‌建议值‌:
  gba: 绕 Z-Y-X 轴旋转（也常对应欧拉角的一种顺序）。
  abg: 绕 X-Y-Z 轴旋转（与 ‘gba’ 顺序相反）。
  rodriguez: 罗德里格斯向量（旋转向量）表示法。

• ‌ViewOfTransform (输入 - 字符串)‌:定义变换的视图类型，即变换是作用于点还是坐标系。
  ‌默认值‌: point
  ‌建议值‌:
  point: 变换作用于点（Point transformation）。
  coordinate_system: 变换作用于坐标系（Coordinate system transformation）。

• PoseOut (输出)‌:转换后的 3D 位姿，具有新的表示类型。

3. 主要用途与场景

• ‌旋转表示转换‌:
  在旋转向量（Rodrigues vector）、欧拉角（Euler angles，如 gba 或 abg）之间进行转换。
  例如，将默认的旋转向量表示转换为更易读的欧拉角表示：
  halcon
  convert_pose_type(PoseIn, 'Rp+T', 'gba', 'point', PoseOut)

• 适配不同算法需求‌:
  某些标定算法或手眼标定例程可能要求特定的旋转顺序或变换视图。使用此算子可以确保位姿格式符合后续算子（如 pose_compose, write_pose 等）的要求。

• 数据序列化与读取‌:
  在写入文件 (write_pose) 或从文件读取 (read_pose) 时，可能需要统一位姿的表示类型以确保兼容性。

4. 注意事项

• 物理不变性‌:
  转换前后，PoseIn 和 PoseOut 描述的是同一个刚性变换。如果在世界坐标系中有一个点，用 PoseIn 和 PoseOut 分别进行变换，结果应当是一致的（前提是正确理解并应用了对应的 ViewOfTransform）。
• ‌角度范围‌:
  当使用 rodriguez 时，旋转角的模长范围通常为 [0,π][0,π] 弧度。
  当转换为欧拉角（如 gba）时，俯仰角（Pitch）通常限制在 [−90∘,90∘] 以避免万向节锁问题，其他角度范围可能在 [−180∘,180∘]。
• 线程安全‌:
  该算子是重入的（reentrant），可以在多线程环境中并行调用，但自身内部不进行并行化处理。

5. 示例代码 (HDevelop)

* 创建一个初始位姿
create_pose (0.1, 0.1, 0.1, 90, 90, 90, 'Rp+T', 'gba', 'point', Pose)

* 将位姿转换为不同的旋转顺序表示 (例如从 'gba' 转为 'abg')
convert_pose_type (Pose, 'Rp+T', 'abg', 'point', PoseConverted)

* 此时 PoseConverted 与 Pose 描述相同的物理变换，但内部旋转参数不同

通过合理使用 convert_pose_type，可以更灵活地处理 3D 空间中的姿态数据，特别是在涉及不同坐标系定义或与其他软件/库交互时。

4，程序流程。

5，代码

特别注意事项：

1. image_points_to_world_plane( : : CameraParam, WorldPose, Rows, Cols, Scale : X, Y)
WorldPose 参数：

• 描述世界坐标系相对于相机坐标系的姿态（即外参）。格式通常为[Tx, Ty, Tz, Rx, Ry, Rz]或类似结构， WorldPose 和CameraParam 必须通过严格的相机标定流程（如使用 calibrate_cameras）获得。标定板的放置姿态应覆盖实际工作视野和深度范围。如果WorldPose 内参数顺序或定义方式错误，会导致坐标转换结果完全错误（如位置偏移、方向颠倒）。
• 传入的 WorldPose 必须与 CameraParam（相机内参）在标定时的旋转顺序保持一致,默认为'gba'即位姿的类型参数为0， 如若不一致将导致计算出的视线（Line of Sight）与世界坐标系（z=0平面）的交点发生严重偏移，最终导致转换出的 X 和 Y 坐标完全错误。
• 默认的 'gba' 顺序（从右向左读）对应 ZYX 旋转顺序（即先绕 Z 轴，再绕旧 Y 轴，最后绕旧 X 轴，这被称为 Yaw-Pitch-Roll）。'abg'顺序，对应的其实是 XYZ 旋转顺序（即 Roll-Pitch-Yaw）。

* 例如需要将顺序'abg'的位姿[-0.173048,-0.157195,179.482,359.738,59.2524,2]
 变换为默认'gba`的位姿[0.574653,-0.173048,-0.157195,179.961,0.579277,300.749,0]
* 位姿最后一个类型参数0：表示'gba' 顺序；2：表示'abg' 顺序

2. 标定板位姿问题。
   使用calibrate_cameras (CalibDataID, Errors)后通过get_calib_data_observ_pose获得的标定板在相机的位姿常常以为是标定后的位姿，其实不是，而是标定前的位姿，与其配合使用的也需是标定前的内参。
   如果使用标定后的内参进行配套，则出现错位。如下

calibrate_cameras (CalibDataID, Errors)
Index := 19
get_calib_data_observ_pose (CalibDataID, 0, 0, Index, ObjInCameraPose)
get_calib_data (CalibDataID, 'camera', 0, 'params', CameraParameters)
disp_3d_coord_system (WindowHandle, CameraParameters, CalTabObservPose, 0.01)

与标定前的内参配套使用则正常。

calibrate_cameras (CalibDataID, Errors)
Index := 19
get_calib_data_observ_pose (CalibDataID, 0, 0, Index, ObjInCameraPose)
get_calib_data (CalibDataID, 'camera', 0, 'params', CameraParameters)
disp_3d_coord_system (WindowHandle, StartParameters, CalTabObservPose, 0.01)

那么如何获得标定后的相机外参呢？
通过get_calib_data (CalibDataID, 'calib_obj_pose', [0,Index], 'pose', CalTabPose)即可获得标定后外参，其中[0,index]中的0表示表示标定板编号，index表示位姿索引。

calibrate_cameras (CalibDataID, Errors)
Index := 19
get_calib_data (CalibDataID, 'camera', 0, 'params', CameraParameters)
get_calib_data (CalibDataID, 'calib_obj_pose', [0,Index], 'pose', CalTabPose)
disp_3d_coord_system (WindowHandle, StartParameters, CalTabPose, 0.01)

* 参考案例库：handeye_stationarycam_grasp_nut.hdev

* 目的：
* 计算robot抓取螺母(不是3D点云是2D)的姿态（即ToolInRootBasePose）,该数据可直接发送给Robot

* 注意事项：
* 本案例是eye to hand模式即相机为固定静止
ImageNameStart := '3d_machine_vision/handeye/stationarycam_'
DataNameStart := 'handeye/stationarycam_'
* Define reference coordinate system and display it
CalplateFile := 'caltab_30mm.descr'
NumImages:=17
* 从文件中读取已标定的参数，不再重新标定
ReadParam:=false
* 保存标定数据
SaveParam:=true
* 作为世界坐标系的标定板索引
RefCalibPlateIndex:=0
dev_close_window ()
dev_open_window (0, 0, 512, 512, 'black', WindowHandle)
dev_set_line_width (2)
dev_update_off ()
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
if(ReadParam==false)
   
    * ------------1,进行标定，获得相应位姿
    create_calib_data ('hand_eye_stationary_cam', 1, 1, CalibDataID)
    * 读取预设的内参
    read_cam_par (DataNameStart+'start_campar.dat', StartCameraParam)
    * 设置相机模式与参数
    set_calib_data_cam_param (CalibDataID, 0, 'area_scan_division', StartCameraParam)
    * 设置描述文件
    set_calib_data_calib_object (CalibDataID, 0, CalplateFile)
    for i := 0 to NumImages-1 by 1
        read_image (Image, ImageNameStart+'calib3cm_'+i$'02')
        get_image_size (Image, Width, Height)
        dev_set_window_extents (-1, -1, Width, Height)
        dev_display (Image)
        find_calib_object (Image, CalibDataID, 0, 0, i, [], [])
        * 读取当前的robot位姿
        read_pose (DataNameStart+'robot_pose_'+i$'02'+'.dat', ToolInBasePose)
        set_calib_data (CalibDataID, 'tool', i, 'tool_in_base_pose', ToolInBasePose)
        * 获得当前的标定板轮廓
        get_calib_data_observ_contours (Contours, CalibDataID, 'caltab', 0, 0, i)
        * 获得当前的标定点,已经标定板在相机坐标系中的位姿，即相机外参
        get_calib_data_observ_points (CalibDataID, 0, 0, i, Row, Column, Index, RefInCamPose)
        * 绘制标定点
        gen_cross_contour_xld (MarkCross, Row, Column, 6, 0)
        dev_set_color ('green')
        dev_display (Contours)
        dev_set_color ('red')
        dev_display (MarkCross)
        * 在图像坐标系中显示世界坐标系，注意此次世界坐标系的中心点是标定板的中心（3,3）Mark点
        dev_set_colored (12)
        * 注意这里与get_calib_data_observ_points 获得的RefInCamPose搭配使用的
        * 是标定前的内参 StartCameraParam不是标定后的内参CameraParams，这也说明RefInCamPose不是标定后的标定板位姿
        disp_3d_coord_system (WindowHandle, StartCameraParam, RefInCamPose, 0.01)
        disp_message (WindowHandle, '标定中：Image '+(i+1)+' of '+NumImages, 'window', 12, 12, 'black', 'true')
        
    endfor
    * 标定
    calibrate_hand_eye (CalibDataID, Errors)
    * --------------2,获得标定后的参数
    * 内参
    get_calib_data (CalibDataID, 'camera', 0, 'params', CameraParams)
    * 基座在相机的位姿
    get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPose)
    * 标定板在Tool的位姿
    get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', ObjInToolPose)
    * 查询观察的索引
    query_calib_data_observ_indices (CalibDataID, 'calib_obj', 0, CameraIndices, PoseIndices)
    dev_set_line_width (2)
    for j := 0 to |PoseIndices|-1 by 1
        get_calib_data (CalibDataID, 'tool', PoseIndices[j], 'tool_in_base_pose', ToolInBasePose)
        read_image (Image, ImageNameStart+'calib3cm_'+j$'02')
        dev_display (Image)
        * 验证各位姿的正确性,即计算出ObjInCameraPose   
        pose_compose (ToolInBasePose,  ObjInToolPose, ObjInBasePose)
        pose_compose ( BaseInCamPose, ObjInBasePose, ObjInCamPose)
        * 显示坐标系
        dev_set_colored (12)
        * 注意：这里与计算所得ObjInCamPose（标定板在相机的位姿）搭配使用的标定后的CameraParams
        * 若想直接获得指定PoseIndex标定板的位姿可通过
        * get_calib_data (CalibDataID, 'calib_obj_pose',[0,j], 'pose', DataValue)
        * [0,j]0：表示标定板编号，j表示位姿索引 
        disp_3d_coord_system (WindowHandle, CameraParams, ObjInCamPose, 0.01)
        
        disp_message (WindowHandle, '验证标定中心:Image '+(j+1)+' of '+|PoseIndices|, 'window', 12, 12, 'black', 'true')
        wait_seconds (0.5)
    endfor
    
    * -------------3，标定完成
    
    if(SaveParam)
        * 保存标定后的参数
        * 保存相机内参
        write_cam_par (CameraParams, 'final_camera_params.dat')
        * 保存objInToolPose
        write_pose (ObjInToolPose, 'final_obj_in_tool_pose.dat')
        * 保存BaseInCamPose
        write_pose (BaseInCamPose, 'final_base_in_cam_pose.dat')
    endif
    * 取stationarycam_calib3cm_00.png图像的标定板标标定的坐标系作为后续参考坐标系
    get_calib_data (CalibDataID, 'tool', RefCalibPlateIndex, 'tool_in_base_pose', ToolInBasePose)
    
    pose_compose (ToolInBasePose,  ObjInToolPose, ObjInBasePose)
    * RefInCamPose为stationarycam_calib3cm_00.png图像的标定板在相机坐标系中的位姿
    pose_compose ( BaseInCamPose, ObjInBasePose, RefInCamPose)
    * 
    clear_calib_data (CalibDataID)
    dev_display (Image)
    disp_message (WindowHandle, '标定完成', 'Image', 12, 12, 'black', 'true')
    stop()
else
    * 读取相机内参   
    read_cam_par ('final_camera_params.dat',CameraParams)
    * 读取objInToolPose   
    read_pose ('final_obj_in_tool_pose.dat', ObjInToolPose)
    * 读取BaseInCamPose    
    read_pose ('final_base_in_cam_pose.dat', BaseInCamPose)
    * 读取stationarycam_calib3cm_00.png图像的ToolInBasePose
    read_pose (DataNameStart+'robot_pose_'+RefCalibPlateIndex$'02'+'.dat', ToolInBasePose)
    * 将stationarycam_calib3cm_00.png图像的ToolInBasePose标定板在相机坐标系中的位姿作为RefInCamPose
    pose_compose (ToolInBasePose,  ObjInToolPose, ObjInBasePose)
    pose_compose ( BaseInCamPose, ObjInBasePose, RefInCamPose)
endif

*
* --------------------4，计算螺母的夹取点
read_image (Image, ImageNameStart +'nut12_square')
dev_display (Image)
* 显示参考坐标系
disp_3d_coord_system (WindowHandle, CameraParams, RefInCamPose, 0.01)
disp_message (WindowHandle, '显示参考坐标系', 'window', 12, 12, 'black', 'true')
threshold (Image, Region, 60, 255)
fill_up (Region, RegionFillUp)
connection (RegionFillUp, ConnectedRegions)
select_shape_std (ConnectedRegions, SelectedRegions, 'max_area', 0)
gen_contour_region_xld (SelectedRegions, NutContours, 'border')
* 分割轮廓片段
segment_contours_xld (NutContours, ContoursSplit, 'lines', 3, 4, 3)
fit_line_contour_xld (ContoursSplit, 'tukey', -1, 0, 5, 2, RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist)
gen_empty_obj (Lines)
for i := 0 to |RowBegin|-1 by 1
    gen_contour_polygon_xld (Line, [RowBegin[i],RowEnd[i]], [ColBegin[i],ColEnd[i]])
    concat_obj (Lines, Line, Lines)
endfor
gen_polygons_xld (Lines, Polygons, 'ramer', 2)
* 从输入的轮廓中提取平行的xld
* 注意得到Parallels需要为平行的xld，如果存在非平行的xld将导致
* get_parallels_xld计算出的坐标异常
gen_parallels_xld (Polygons, Parallels, 50, 100, rad(10), 'true')
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]]
dev_clear_window ()
* 将坐标从像素坐标系变换到世界坐标系（参照坐标系）
* 需要特别注意的是：传入的 WorldPose 必须与 CameraParam（相机内参）在标定时的旋转顺序保持一致,默认为'gba'即位姿的类型参数为0
* 如若不一致将导致计算出的视线（Line of Sight）与世界坐标系（z=0平面）的交点发生严重偏移，最终导致转换出的 X 和 Y 坐标完全错误。
* 默认的 'gba' 顺序（从右向左读）对应 ZYX 旋转顺序（即先绕 Z 轴，再绕旧 Y 轴，最后绕旧 X 轴，这被称为 Yaw-Pitch-Roll）。
* 'abg' 顺序，对应的其实是 XYZ 旋转顺序（即 Roll-Pitch-Yaw）。
* 例如需要将顺序'abg'的位姿[-0.173048,-0.157195,179.482,359.738,59.2524,2]
* 变换为默认'gba'的位姿[0.574653,-0.173048,-0.157195,179.961,0.579277,300.749,0]
* 位姿最后一个类型参数0：表示'gba' 顺序；2：表示'abg' 顺序
convert_pose_type (RefInCamPose, 'Rp+T', 'gba', 'point', RefInCamPose)

* 方法1：通过参考坐标系推导出GraspCenter在像素坐标系的位姿
image_points_to_world_plane (CameraParams, RefInCamPose, CornersRow, CornersCol, 'm', CornersX_ref, CornersY_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]])]
* 计算抓取角度
GraspPhiZ_ref :=atan((GraspPointsY_ref[1] - GraspPointsY_ref[0]) / (GraspPointsX_ref[1] - GraspPointsX_ref[0]))

* 计算出GraspCenter在相机坐标系的位姿
* 'Rp+T'：先旋转后平移，create_pose 算子以及相机标定流程中最常用、也是默认的标准定义。
* 'R(p-T)'：先平移后旋转，除非机器人控制器或第三方算法明确要求使用 'R(p-T)'，否则一般不用此模式。
create_pose ( CenterPointX_ref,CenterPointY_ref, 0, 0, 0,deg(GraspPhiZ_ref), 'Rp+T', 'gba', 'point', GraspInRefPose) 
pose_compose (RefInCamPose, GraspInRefPose, GraspInCamPose)
dev_display (Image)
* 显示以螺母为中心的坐标系
disp_3d_coord_system (WindowHandle, CameraParams, GraspInCamPose, 0.01)
disp_message (WindowHandle, '方法1：通过参考坐标系推导出GraspCenter在像素坐标系的位姿', 'image', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop()
dev_display (Image)
* 方法2：九点标定（最少需要3个点）算出GraspCenter在像素坐标系的位姿
* 以GraspCenter为原点测量四点坐标值
NX := [0.009,0.009,-0.009,-0.009]
NY := [-0.009,0.009,0.009,-0.009]
NZ := [0,0,0,0]
*NX:=CornersX_ref-CenterPointX_ref 
*NY:=CornersY_ref-CenterPointY_ref
NZ:=gen_tuple_const(|NX|,0)
vector_to_pose (NX, NY, NZ, CornersRow, CornersCol, CameraParams, 'iterative', 'error', CamPoseNut, Quality)
* 显示以螺母为中心的坐标系
disp_3d_coord_system (WindowHandle, CameraParams, CamPoseNut, 0.01)
disp_message (WindowHandle, '方法2：九点标定（最少需要3个点）算出GraspCenter在像素坐标系的位姿', 'image', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop()

* 夹取点G1,G2从世界坐标系（参考坐标系）变换到像素坐标系
* 仿射变换从世界坐标系变换到相机坐标系
pose_to_hom_mat3d (RefInCamPose, Cam_H_Ref)
affine_trans_point_3d (Cam_H_Ref, GraspPointsX_ref, GraspPointsY_ref, gen_tuple_const(|GraspPointsY_ref|,0), CamPointsX, CamPointsY, CamPointsZ)
* 相机坐标系投影到图像坐标系,投影过程丢失深度信息
project_3d_point (CamPointsX, CamPointsY, CamPointsZ, CameraParams, GraspRow, GraspCol)
* 绘制夹取点
gen_cross_contour_xld (GraspCross, GraspRow, GraspCol, 6, 0)
* ----------------------5,显示抓取效果
dev_clear_window ()
dev_display (Image)
* 显示平行线
dev_set_color ('yellow')
dev_display (Parallels)
* 显示抓取点
dev_set_color ('cyan')
dev_display (GraspCross)
* 显示抓取点编号
if(GraspCol[0]>mean(CornersCol))
    * 位于中心点右边
    set_tposition (WindowHandle, GraspRow[0]-10, GraspCol[0]+6)
    write_string (WindowHandle, 'G1')
else   
 * 位于中心点左边
    set_tposition (WindowHandle, GraspRow[0]-10, GraspCol[0]-20)
    write_string (WindowHandle, 'G1')   
endif
if(GraspCol[1]>mean(CornersCol))
    * 位于中心点右边
    set_tposition (WindowHandle, GraspRow[1]-10, GraspCol[1]+6)
    write_string (WindowHandle, 'G2')
else   
 * 位于中心点左边
    set_tposition (WindowHandle, GraspRow[1]-10, GraspCol[1]-25)
    write_string (WindowHandle, 'G2')   
endif
dev_set_colored (12)
* 显示标定板坐标系
disp_3d_coord_system (WindowHandle, CameraParams, RefInCamPose, 0.01)
* 显示以螺母中心为原点的坐标系（其实就是抓取点）
disp_3d_coord_system (WindowHandle, CameraParams, GraspInCamPose, 0.01)

* -------------------6,计算出发送至Robot的Pose
create_pose (0, 0, 0.145, 0, 0, 0, 'Rp+T', 'gba', 'point', GripperInToolPose)
* 方法1：通过齐次矩阵计算RobotInToolPose
* 'Rp+T'：先旋转后平移，create_pose 算子以及相机标定流程中最常用、也是默认的标准定义。
* 'R(p-T)'：先平移后旋转，除非机器人控制器或第三方算法明确要求使用 'R(p-T)'，否则一般不用此模式。
hom_mat3d_identity (HomMat3DIdentity)
* 首先旋转
hom_mat3d_rotate (HomMat3DIdentity, GraspPhiZ_ref, 'z', 0, 0, 0, HomMat3DRotate)
* 平移
hom_mat3d_translate (HomMat3DRotate, CenterPointX_ref, CenterPointY_ref, gen_tuple_const(|CenterPointX_ref|,0), Ref_H_Gripper)
* Cam_H_Ref:表示Cam矩阵到Ref矩阵，H：表示齐次矩阵
pose_to_hom_mat3d (RefInCamPose, Cam_H_Ref)
hom_mat3d_compose (Cam_H_Ref, Ref_H_Gripper, Cam_H_Gripper)
pose_to_hom_mat3d (GripperInToolPose, Tool_H_Gripper)
pose_to_hom_mat3d (BaseInCamPose, Cam_H_Base)
hom_mat3d_invert (Cam_H_Gripper, Gripper_H_Cam)
hom_mat3d_compose (Tool_H_Gripper, Gripper_H_Cam, Tool_H_Cam)
hom_mat3d_compose (Tool_H_Cam, Cam_H_Base, Tool_H_Base)
hom_mat3d_invert (Tool_H_Base, Base_H_Tool)
hom_mat3d_to_pose (Base_H_Tool, ToolInBasePose)
* 方法2，直接通过位姿进行计算
* GraspInCamPose,GripperInToolPose,BaseInCamPose
pose_invert (GripperInToolPose, ToolInGripperPose)
*GripperInBasePose
pose_invert (BaseInCamPose, CamInBasePose)
pose_compose (CamInBasePose, GraspInCamPose, GripperInBasePose)
pose_compose (GripperInBasePose, ToolInGripperPose, ToolInBasePose2)
* 将'gba'顺序（ZYX 旋转顺序,即先绕 Z 轴，再绕旧 Y 轴，最后绕 X 轴）转换为Robot使用的
* 'abg' 顺序(XYZ 旋转顺序)
convert_pose_type (ToolInBasePose2, 'Rp+T', 'abg', 'point', PoseOut)

* 显示发送给Robot的ToolInBasePose
dev_inspect_ctrl (PoseOut)
stop()
dev_close_inspect_ctrl (PoseOut)

效果显示：

6 ，Demo链接。

https://download.csdn.net/download/lingxiao16888/90804259