無序抓取の3D點云匹配案例分享

無序抓取の3D點云匹配案例分享

如同2D模板匹配一樣，3D點云也有模板匹配。2D模板匹配結(jié)果有3個值，他們分別是X,Y,U。3D點云匹配則有6個值，他們分別是X,Y,Z,RX,RY,RZ。他代表了物體在空間的位置和姿態(tài)。

—Edited By Hugo

# 1

如上圖所示：

讀取一個3D模型。模型數(shù)據(jù)怎么來呢，這里是通過3D傳感器SDK二次開發(fā)后得到的點云數(shù)據(jù)，然后生成的點云模型。

讀取的代碼如下：

*讀取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

如上圖所示：

標(biāo)注點云模型空間的抓取中心。為了更加直觀的理解，這里使用一個球形元素作為抓取點顯示出來。

同時給點云模型涂以及抓取點涂上不同顏色，方便觀察它們的相對位置。

顯示代碼如下：

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

如上圖所示：

這是一個實際拍攝的點云圖，經(jīng)過了坐標(biāo)系糾正變換，更方便查看點云位置。里面有一塊平面就是我們需要匹配的目標(biāo)點云。

也可以為點云區(qū)域加上外框區(qū)域，更容易判斷點云集合的方位，效果如下：

如上圖所示：

所有的點云集合被封裝在一個盒子里面，并且顯示整個點云集合的方向，方便進(jìn)行點云篩選以及做相關(guān)的點云處理。

部分代碼如下：

*顯示坐標(biāo)軸變換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

如上圖所示：

執(zhí)行完點云匹配后，得出了匹配之后的目標(biāo)抓取位置，圖中白色小球即為目標(biāo)平面的抓取位置。

匹配代碼如下：

*執(zhí)行點云匹配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

經(jīng)過轉(zhuǎn)換后可得目標(biāo)平面的抓取的抓取姿態(tài)或者抓取平面法向量。效果如下：

如上圖所示：

在小白球抓取位置的地方求得平面的抓取方向并用三維坐標(biāo)系顯示出來，使得結(jié)果表示更加形象。

上述過程，演示了Halcon從讀取點云模板，點云篩選，點云轉(zhuǎn)換，點云匹配的過程。

案例講解就先到這里了。