无序抓取の3D点云匹配案例分享

无序抓取の3D点云匹配案例分享

如同2D模板匹配一样，3D点云也有模板匹配。2D模板匹配结果有3个值，他们分别是X,Y,U。3D点云匹配则有6个值，他们分别是X,Y,Z,RX,RY,RZ。他代表了物体在空间的位置和姿态。

— Edited By Hugo

# 1

如上图所示：

读取一个3D模型。模型数据怎么来呢，这里是通过3D传感器SDK二次开发后得到的点云数据，然后生成的点云模型。

读取的代码如下： 

*读取3D模型read_object_model_3d ('E:/PRODUCT/6.29/6.29.om3', 'mm', [], [], ObjectModel3D, Status)create_surface_model (ObjectModel3D, 0.03, [], [], SurfaceModelID)get_object_model_3d_params (ObjectModel3D, 'num_points', GenParamValue4) *重采样sample_object_model_3d (ObjectModel3D, 'fast', 0.05, [], [], SampledObjectModel3D)visualize_object_model_3d (WindowHandle, SampledObjectModel3D, [], [], ['intensity_red','intensity_green','intensity_blue','disp_pose'], ['coord_x','coord_y','coord_z','true'], '方块3D模型', [], [], PoseOut)disp_object_model_3d_safe (WindowHandle, SampledObjectModel3D, [], [], [], [])

# 2

如上图所示：

标注点云模型空间的抓取中心。为了更加直观的理解，这里使用一个球形元素作为抓取点显示出来。

同时给点云模型涂以及抓取点涂上不同颜色，方便观察它们的相对位置。

显示代码如下：

get_object_models_center (SampledObjectModel3D, Center)tp:=[Center[0], Center[1], Center[2],0,0,0,0]gen_sphere_object_model_3d (tp, 1, ObjectModel3D2) ObjectModel3DResult1 := [SampledObjectModel3D,ObjectModel3D2] NumResult := |ObjectModel3DResult1|tuple_gen_const (NumResult, 'green', Colors)Colors[0]:='gold'Indices := [1:NumResult] *显示点云模型dev_clear_window ()visualize_object_model_3d (WindowHandle, [ObjectModel3D,ObjectModel3DResult1], [], [], ['color_' + [0,Indices],'point_size_0'], ['gray',Colors,0.5], [], [], Instructions, PoseOut)

# 3

如上图所示：

这是一个实际拍摄的点云图，经过了坐标系纠正变换，更方便查看点云位置。里面有一块平面就是我们需要匹配的目标点云。

也可以为点云区域加上外框区域，更容易判断点云集合的方位，效果如下： 

如上图所示：

所有的点云集合被封装在一个盒子里面，并且显示整个点云集合的方向，方便进行点云筛选以及做相关的点云处理。

部分代码如下：

*显示坐标轴变换moments_object_model_3d (ObjectModel3D, 'principal_axes', PoseOut)pose_invert (PoseOut, PoseInvert)pose_to_hom_mat3d (PoseInvert, HomMat3D)affine_trans_object_model_3d (ObjectModel3D, HomMat3D, ObjectModel3DAffineTrans)*visualize_object_model_3d (WindowHandle, ObjectModel3DAffineTrans, [], [], ['intensity_red','intensity_green','intensity_blue','disp_pose'], ['coord_x','coord_y','coord_z','true'], '方块3D模型', [], [], PoseOut)triangulate_object_model_3d (ObjectModel3DAffineTrans, 'greedy', [], [], TriangulatedObjectModel3D, Information)visualize_object_model_3d (WindowHandle, TriangulatedObjectModel3D, [], [],  ['intensity_red','intensity_green','intensity_blue','disp_pose'], ['coord_x','coord_y','coord_z','true'], '方块3D模型', [], [], PoseOut)

# 4

如上图所示：

执行完点云匹配后，得出了匹配之后的目标抓取位置，图中白色小球即为目标平面的抓取位置。

匹配代码如下：

*执行点云匹配count_seconds (T0)find_surface_model (SurfaceModelID, UnionObjectModel3D, 0.05, 0.3, 0.6, 'true', 'num_matches', 10, Pose, Score, SurfaceMatchingResultID)count_seconds (T1)TimeForMatching := (T1 - T0) * 1000 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 (SampledObjectModel3D, CPose, ObjectModel3DRigidTrans) ObjectModel3DResult := [ObjectModel3DResult,ObjectModel3DRigidTrans]endfor

经过转换后可得目标平面的抓取的抓取姿态或者抓取平面法向量。效果如下： 

如上图所示：

在小白球抓取位置的地方求得平面的抓取方向并用三维坐标系显示出来，使得结果表示更加形象。

上述过程，演示了Halcon从读取点云模板，点云筛选，点云转换，点云匹配的过程。

案例讲解就先到这里了。