一、概述

        基于Halcon开发环境下基于提前知道标准模型的几何参数，来构建的标准3D模型建立标准工件 3D 模板，通过3D 曲面匹配在在现场点云场景中搜索对应工件，根据3D点云配准的结果输出精准 6D 位姿：X/Y/Z 平移 + Rx/Ry/Rz 旋转，直接下发给机械手来完成视觉引导抓取。

二、标准3D模型范围

主要包括四大基础 3D 几何建模算子

1）算子：gen_cylinder_object_model_3d

     功能：生成标准圆柱体 3D 模型；

     用途：圆柱物料、轴类、管材、滚轮类工件等建模；

     参数：基准位姿、半径、轴线起止延伸长度

这里重点介绍该算子的主要参数：gen_cylinder_object_model_3d(Pose, Radius, MinExtent, MaxExtent, ObjectModel3D)

Pose —— 圆柱的基准位姿，作用是定义圆柱在 3D 空间里放在哪里、朝哪个方向。Pose := [0, 0, 0, 0, 0, 0, 0]表示圆柱中心点在世界原点 (0,0,0)，圆柱轴线沿着 Z 轴向上 。

注意：Pose 将来就是机械手抓取的中心位姿。

Radius —— 圆柱半径

MinExtent —— 圆柱轴线负方向长度

MaxExtent —— 圆柱轴线正方向长度

MinExtent和MaxExtent这两个参数一起决定圆柱总长度。

         总长度 = MaxExtent - MinExtent

ObjectModel3D —— 输出的圆柱 3D 模型。

2）算子：gen_sphere_object_model_3d

     功能：生成球体3D 模型；

     用途：钢球、球形零件、球面工件等建模；

     参数：基准位姿、球体半径

3）算子：gen_plane_object_model_3d

     功能：生成三维平面 3D 模型；

     用途：工件平面贴合、基面定位、平整度检测、相机标定基准面、物料放置平台定位等建模；

     参数：平面位姿、平面长宽范围；

4）算子：gen_box_object_model_3d

     功能：生成长方体 / 立方体 3D 模型；

     用途：方块料、箱体、方形零件、电芯、纸箱、矩形工件等建模；    

     参数：基准位姿、X/Y/Z 三个方向边长。

三、配准实现代码（经过测试）

dev_close_window ()
dev_update_off ()

* ===================== 1. 自定义圆柱参数 =====================
Cylinder_Diameter := 50.0   // 物料圆柱直径
Cylinder_Length   := 80.0   // 物料圆柱总长度
Radius := Cylinder_Diameter / 2.0
* 圆柱轴向范围：居中分布
MinExtent := -Cylinder_Length / 2.0
MaxExtent :=  Cylinder_Length / 2.0
* 圆柱初始位姿：原点、沿Z轴摆放
PoseInit := [0, 0, 0, 0, 0, 0, 0]
* ===================== 2. Halcon25 创建圆柱3D模板模型 =====================
gen_cylinder_object_model_3d (PoseInit, Radius, MinExtent, MaxExtent, ObjectModel3D)

* ==============================
* 2. 关键：采样点云 + 计算法线（必须做！）
* ==============================
* 采样密度（根据你的精度调整，越小越精确）
SamplingDist := 0.5
* 从圆柱模型采样点云!!!!
sample_object_model_3d (ObjectModel3D, 'fast', SamplingDist, [], [], ObjectModel3DSampled)
* 计算点云法线（表面匹配 必须要有法线）
* 3. 计算法线（25 标准算子）
NormalRadius := 1.0
surface_normals_object_model_3d (ObjectModel3DSampled, 'mls', [], [], ObjectModel3DWithNormals)

* 4. 创建表面匹配模型（现在有“点+法线”，合法）
RelSampling := 0.05
create_surface_model (ObjectModel3DWithNormals, RelSampling, [], [], SurfaceModelID)
* ==============================


* ===================== 3. 读取现场物料点云/深度图 (替换成你的相机采集) =====================
* 方式1：读取已有点云
read_object_model_3d('mvtec_bunny_normals', 'm', [], [], ObjectModel3D1, Status)

* 方式2：如果是深度图+内参，转点云
* read_image (ImageDepth, 'depth.png')
* image_to_object_model_3d (ImageDepth, CameraParam, 0, 0, -1, -1, SceneModel3D)

* ===================== 4. 3D曲面匹配 找圆柱物料位姿 =====================
find_surface_model (SurfaceModelID, ObjectModel3D1, 0.05, 0.2, 0, 'false', [], [], Pose, Score, SurfaceMatchingResultID)

* ===================== 5. 输出抓取位姿 给机械手 =====================
* PoseResult: [X,Y,Z,Rx,Ry,Rz,0] 圆柱中心位姿
* 可直接发给机器人/机械手作为抓取基准