無序抓取の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從讀取點云模板，點云篩選，點云轉換，點云匹配的過程。

案例講解就先到這里了。