[3D&Halcon] 三维点云匹配&无序抓取

• 📢博客主页：https://loewen.blog.csdn.net
• 📢欢迎点赞 👍 收藏 ⭐留言 📝 如有错误敬请指正！
• 📢本文由 丶布布原创，首发于 CSDN，转载注明出处🙉
• 📢现在的付出，都会是一种沉淀，只为让你成为更好的人✨

---

---

一. 3D无序抓取原理

通过3D成像系统（激光三角、结构光+单/双目等），对物体表面轮廓进行扫描，形成点云数据。选择其中一个物体的点云数据作为模板，去对其他物体的点云数据进行「三维点云匹配」，获取各个物体的姿态信息（x、y、z、Rx、Ry、Rz+1个旋转类型）。

根据物体所在的世界坐标系和机械手坐标系之间的「3D手眼标定」关系，将各个「物体坐标系下的姿态转换成机械手坐标系下的姿态」，从未完成机械手对物体的抓取。当然，抓取时还需要考虑避障、路径规划等。

针对上面无序抓取涉及到的三个核心点，本文主要讲解Halcon三维点云匹配：

⑴基于表面的三维点云匹配：参考案例 — `分类 — 方法 — 三维匹配（基于表面）`

⑵基于形状的三维点云匹配：参考案例 — `分类 — 方法 — 三维匹配（基于形状）`

---

二. 点云匹配核心算子

1、create_surface_model ( : : ObjectModel3D, RelSamplingDistance, GenParamName, GenParamValue : SurfaceModelID) — 创建3D点云模型
参数：
ObjectModel3D（in）：要读取的文件的文件名。
RelSamplingDistance（in）：代表采样距离在点云最小外界球体直径的比例。
GenParamName（in）：参数名称。
GenParamValue （in）：参数名称对应的值。
SurfaceModelID（out） ：3D点云模板的句柄。

重点解释一下第二个参数RelSamplingDistance：

曲面模型是通过以一定距离对三维对象点云模型进行采样来创建的，采样距离即为点云模型最小外界球体的直径D与比例参数RelSamplingDistance乘积，即D*RelSamplingDistance。

例如，如果RelSamplingDistance设置为0.05，而ObjectModel3D的直径为“10 cm”，则从对象曲面采样的点之间的距离将约为“5 mm”。

参数RelSamplingDistance越小，则创建点云模板所参与的坐标点越多，匹配精度提高但匹配速度会下降，反之亦然。采样点用于运算符find_surface_model中的近似匹配，可以使用值“sampled_model”使用操作符get_surface_model_param获得采样点。通过参数选取采样点的时候，应避免对象模型中的异常点，因为它们会破坏直径。

---

三. 点云匹配具体流程

3.1、读取硬币点云模型数据

如图所示：

read_object_model_3d ('C:/Users/Administrator/Desktop/2020-01-22-71816.om3', 'mm', [], [], ObjectModel3D, Status)
dev_open_window (0, 0, 512, 512, 'black', WindowHandle)
visualize_object_model_3d (WindowHandle, ObjectModel3D, [], [], ['lut','color_attrib'], ['color1','coord_z'], [], [], [], PoseOut)

3.2、创建硬币点云模板

如图所示：

* 提取出硬币的点云数据（类似二维的Blob分析）
get_object_model_3d_params (ObjectModel3D, 'point_coord_z', GenParamValue)
select_points_object_model_3d (ObjectModel3D, 'point_coord_z', 12, 14, ObjectModel3DThresholded)
connection_object_model_3d (ObjectModel3DThresholded, 'distance_3d', 1.5, ObjectModel3DConnected)
select_object_model_3d (ObjectModel3DConnected, 'num_points', 'and', 30000, 45000, ObjectModel3DSelected)
* 对硬币的点云数据进行三角曲面重建
triangulate_object_model_3d (ObjectModel3DSelected, 'greedy', [],[], TriangulatedObjectModel3D, Information)
visualize_object_model_3d (WindowHandle, TriangulatedObjectModel3D, [], [], ['lut','color_attrib'], ['color1','coord_z'], [], [], [], PoseOut)
* 创建硬币点云数据模板，SFM句柄
create_surface_model (TriangulatedObjectModel3D, 0.03, [], [], SFM)

3.3、对硬币点云数据进行三维匹配并显示

如图所示：

* 对整个场景三角曲面重建（因为是对经过三角曲面重建的硬币点云数据创建的模板）
triangulate_object_model_3d (ObjectModel3D, 'greedy', [],[], TriangulatedObjectModel3D1, Information)
* 三维点云匹配，获取当前匹配对象的Pose
find_surface_model (SFM, TriangulatedObjectModel3D1, 0.03, 0.5, 0.9, 'true', 'num_matches', 6, Pose, Score, SurfaceMatchingResultID)
* 获取场景信息和关键点
get_surface_matching_result (SurfaceMatchingResultID, 'sampled_scene', [], SampledScene)
get_surface_matching_result (SurfaceMatchingResultID, 'key_points', [], KeyPoints)
visualize_object_model_3d (WindowHandle, [ObjectModel3D,SampledScene,KeyPoints], [], [], ['color_' + [0,1,2],'point_size_' + [0,1,2]], ['gray','cyan','yellow',1.0,3.0,5.0], [], [], [], PoseOut)
ObjectModel3DResult:=[]
* 有可能会匹配到多个点云数据，所以遍历依次显示（本文只有一个）
for Index2 := 0 to |Score| - 1 by 1
    if (Score[Index2] < 0.11)
        continue
    endif
    CPose := Pose[Index2 * 7:Index2 * 7 + 6]
    * 当前被找到物体的点云数据 = 原始点云模板 * 匹配得到的当前物体姿态
    rigid_trans_object_model_3d (TriangulatedObjectModel3D, CPose, ObjectModel3DRigidTrans)
    ObjectModel3DResult := [ObjectModel3DResult,ObjectModel3DRigidTrans]
endfor
visualize_object_model_3d (WindowHandle, [ObjectModel3D,ObjectModel3DResult], [], [], ['color_' + [0,1],'point_size_0'], ['gray','red',1.0], [], [], [], PoseOut)

---

下雨天，最惬意的事莫过于躺在床上静静听雨，雨中入眠，连梦里也长出青苔。