Halcon 表面法向量 pcl

一、Halcon  

halcon 案例：

find_surface_model_noisy_data.hdev

思路步骤：

1、读取图像

2、拆通道

3、通过Z通道选出比较合适做匹配的模板

4、通过Z  x y 生成一个模型xyz_to_object_model_3d

5、计算表面法向量并生成表面的模型，这个模型就是匹配的模板 SFM

surface_normals_object_model_3d   create_surface_model

6、将待匹配的图像读入，1 2 3 4

7、类似2D的模板匹配，    find_surface_model (SFM, ObjectModel3DSceneReduced, 0.05, 0.3, 0.65, 'true', ['num_matches','scene_normal_computation','pose_ref_scoring_dist_rel'], [3,'mls',0.03], PoseMLS, ScoreMLS, SurfaceMatchingResultIDMLS)

mls：移动最小二乘法；

8、仿射变换并显示

halcon 代码：

* ***********************************************************************
* Example for surface-based 3D matching with very noisy data
* 
* This example shows the surface-based 3D matching on data taken with a
* time-of-flight camera (TOF/PMD) with very noisy data.
* The model for the matching is created from a reference view of the
* object, i.e., no CAD model is used.
* The created model is then searched for in a number of 3D scenes.
* 
* This example demonstrates how surface-based matching can be made more
* robust with highly noisy data using the 'scene_normal_computation'
* parameter.
* 
* ***********************************************************************
* 
* Initialization
dev_update_off ()
gen_empty_obj (EmptyObject)
ImagePath := 'time_of_flight/'
dev_close_window ()
* 
* ***********************************************************************
* Offline-Phase: Model generation from a reference XYZ Image
* ***********************************************************************
* 
* Load the XYZ images with the reference object
read_image (Image, ImagePath + 'engine_cover_noisy_xyz_01')
decompose3 (Image, Xm, Ym, Zm)
* Remove invalid parts. The sensor returns Z=0 if no measurement is available
threshold (Zm, RegionGood, 1e-30, 1)
reduce_domain (Zm, RegionGood, Zm)
* Smooth Z image
median_image (Zm, ZmMedian, 'circle', 2.5, 'mirrored')
* Remove the background plane and select the ROI containing the model
threshold (ZmMedian, RegionsModel, 0.422, 0.529)
threshold (ZmMedian, RegionsModel, 0.422, 0.529)
connection (RegionsModel, ConnectedModel)
select_shape (ConnectedModel, ModelROI, ['area','holes_num'], 'and', [1500,1], [1e30,1])
* Create the ROI
reduce_domain (Xm, ModelROI, Xm)
* 
* Display model image and ROI
get_image_size (Zm, Width, Height)
dev_open_window (0, 0, Width * 4, Height * 4, 'black', WindowHandle)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
dev_display (Zm)
dev_set_line_width (2)
dev_set_draw ('margin')
dev_set_color ('green')
dev_display (ModelROI)
disp_message (WindowHandle, 'Create surface model from XYZ image region', 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
dev_clear_window ()
* 
* Create the surface model from the reference view
xyz_to_object_model_3d (Xm, Ym, Zm, ObjectModel3DModel)
* Remove isolated points
connection_object_model_3d (ObjectModel3DModel, 'distance_3d', 0.005, ObjectModel3DConnected)
select_object_model_3d (ObjectModel3DConnected, 'num_points', 'and', 1000, 1e30, ObjectModel3DSelected)
* robust computation of surface normals
surface_normals_object_model_3d (ObjectModel3DSelected, 'mls', [], [], ObjectModel3DModelNormal)
* Create surface model
* mls 移动最小二乘法 
create_surface_model (ObjectModel3DModelNormal, 0.03, 'model_invert_normals', 'true', SFM)
* 
* Display the model
Instructions[0] := 'Rotate: Left button'
Instructions[1] := 'Zoom:   Shift + left button'
Instructions[2] := 'Move:   Ctrl  + left button'
* 
Message := 'Surface model'
visualize_object_model_3d (WindowHandle, ObjectModel3DModelNormal, [], [], ['point_size','disp_normals'], [2,'true'], Message, [], Instructions, PoseOut)
* 
* ***********************************************************************
* Online-Phase: Match the reference object in 3D scenes
* ***********************************************************************
* 
ContainsNum := [2,2,1,3,3,2]
for Index := 2 to 7 by 1
    * ***************************************
    * Acquire scene
    * ***************************************
    * Load the XYZ Images
    read_image (Image, ImagePath + 'engine_cover_noisy_xyz_' + Index$'02')
    decompose5 (Image, X, Y, Z, Confidence, Intensity)
    * 
    * Remove points where the sensor returned invalid data
    threshold (Confidence, Region1, 0, 16000)
    reduce_domain (Confidence, Region1, Confidence)
    reduce_domain (Intensity, Region1, Intensity)
    * 
    threshold (Z, Region2, 0.001, 0.65)
    intersection (Region1, Region2, Region)
    reduce_domain (X, Region, X)
    reduce_domain (Y, Region, Y)
    reduce_domain (Z, Region, Z)
    xyz_to_object_model_3d (X, Y, Z, ObjectModel3DScene)
    * 
    if (2 == Index)
        * Visualize the scene to give an impression of the noise
        Message := 'Example scene with noise from time-of-flight sensor'
        visualize_object_model_3d (WindowHandle, ObjectModel3DScene, [], [], 'point_size', 2, Message, [], [], PoseOut1)
    endif
    * 
    * Remove background plane
    threshold (Z, Region, 0.001, 0.55)
    reduce_domain (X, Region, XR)
    reduce_domain (Y, Region, YR)
    reduce_domain (Z, Region, ZR)
    xyz_to_object_model_3d (XR, YR, ZR, ObjectModel3DSceneReduced)
    * 
    * ***************************************
    * Match: Find the reference model in the
    *        3D scene
    * ***************************************
    * 
    * 'scene_normal_computation'    controls how the scene normals are computed
    * 'pose_ref_scoring_dist_rel'   is modified according to the sensor noise
    * 
    count_seconds (T0)
    find_surface_model (SFM, ObjectModel3DSceneReduced, 0.05, 0.3, 0.65, 'true', ['num_matches','scene_normal_computation','pose_ref_scoring_dist_rel'], [3,'mls',0.03], PoseMLS, ScoreMLS, SurfaceMatchingResultIDMLS)
    count_seconds (T1)
    TimeForMatchingMLS := (T1 - T0) * 1000
    * 
    count_seconds (T0)
    find_surface_model (SFM, ObjectModel3DSceneReduced, 0.05, 0.3, 0.65, 'true', ['num_matches','scene_normal_computation','pose_ref_scoring_dist_rel'], [3,'fast',0.03], PoseFast, ScoreFast, SurfaceMatchingResultIDFast)
    count_seconds (T1)
    TimeForMatchingFast := (T1 - T0) * 1000
    stop()
    * 
    * ***************************************
    * Display: Visualize the result(s)
    * ***************************************
    * 
    if (2 == Index)
        Message := 'Normal vectors of default method (\'fast\') vs. accurate method'
        Message[1] := '(\'mls\'). Notice how the normals are noisy and rather incorrect'
        Message[2] := 'for \'fast\', while they are approximately perpendicular to the'
        Message[3] := 'surface for \'mls\'.'
        get_surface_matching_result (SurfaceMatchingResultIDFast, 'sampled_scene', 0, SampledSceneFast)
        get_surface_matching_result (SurfaceMatchingResultIDMLS, 'sampled_scene', 0, SampledSceneMLS)
        * Select scene parts close to the found model
        distance_object_model_3d (SampledSceneFast, ObjectModel3DModel, PoseMLS[7:13], 0, 'invert_pose', 'true')
        distance_object_model_3d (SampledSceneMLS, ObjectModel3DModel, PoseMLS[7:13], 0, 'invert_pose', 'true')
        select_points_object_model_3d (SampledSceneFast, '&distance', 0, 0.01, SceneOnModelFast)
        select_points_object_model_3d (SampledSceneMLS, '&distance', 0, 0.01, SceneOnModelMLS)
        max_diameter_object_model_3d (ObjectModel3DModel, Diameter)
        * Visualize scenes next to each other
        rigid_trans_object_model_3d (SceneOnModelMLS, [0,Diameter,0,0,0,0,0], ObjectModel3DRigidTrans)
        create_pose (-0.2, 0.07, 3.0, 3.8, 2.0, 270, 'Rp+T', 'gba', 'point', Pose)
        visualize_object_model_3d (WindowHandle, [SceneOnModelFast,ObjectModel3DRigidTrans], [], Pose, ['point_size','disp_normals','color_0','color_1'], [2,'true','red','green'], Message, ['fast','mls'], [], PoseOut1)
    endif
    * 
    * Prepare the visualization of the result(s)
    rigid_trans_object_model_3d (ObjectModel3DModel, PoseFast, ObjectModel3DResultFast)
    rigid_trans_object_model_3d (ObjectModel3DModel, PoseMLS, ObjectModel3DResultMLS)
    * 
    * Visualize result(s)
    NumGT := ContainsNum[Index - 2]
    Message := 'Scene: ' + Index + ' (contains ' + NumGT + ' object' + ['s',''][1 == NumGT] + ')'
    if (|ScoreFast| > 0)
        Message[1] := 'Fast (yellow): Found ' + |ObjectModel3DResultFast| + ' object(s) in ' + TimeForMatchingFast$'.3' + ' ms'
        ScoreString := sum(ScoreFast$'.2f' + ' / ')
        Message[2] := '  Score(s): ' + ScoreString{0:strlen(ScoreString) - 4}
    else
        Message[1] := 'Fast (yellow): No objects found'
        Message[2] := ''
    endif
    Message[3] := 'MLS (green): Found ' + |ObjectModel3DResultMLS| + ' object(s) in ' + TimeForMatchingMLS$'.3' + ' ms'
    ScoreString := '' + sum(ScoreMLS$'.2f' + ' / ')
    Message[4] := '  Score(s): ' + ScoreString{0:strlen(ScoreString) - 4}
    * 
    CYellow := gen_tuple_const(|ObjectModel3DResultFast|,'yellow')
    CGreen := gen_tuple_const(|ObjectModel3DResultMLS|,'green')
    NYellow := [0:|ObjectModel3DResultFast| - 1] + 1
    NGreen := [0:|ObjectModel3DResultMLS| - 1] + 1 + |ObjectModel3DResultFast|
    * 
    dev_clear_window ()
    visualize_object_model_3d (WindowHandle, [ObjectModel3DScene,ObjectModel3DResultFast,ObjectModel3DResultMLS], [], [], ['point_size_0','color_0','color_' + NYellow,'color_' + NGreen], [2,'gray',CYellow,CGreen], Message, [], Instructions, PoseOut)
endfor

 

二、生成表面法向量Halcon 

surface_normals_object_model_3d

surface_normals_object_model_3d (ObjectModel3DSelected, 'mls', [], [], ObjectModel3DModelNormal)

ObjectModel3D (input_control)  

Method (input_control)
Default value: 'mls'
List of values: 'mls', 'triangles', 'xyz_mapping'

GenParamName (input_control) 
Default value: []
List of values: 'mls_abs_sigma', 'mls_force_inwards', 'mls_kNN', 'mls_order', 'mls_relative_sigma'


GenParamValue 
Default value: []
Suggested values: 10, 20, 40, 60, 0.1, 0.5, 1.0, 2.0, 0, 1, 2, 'true', 'false'

ObjectModel3DNormals (output_control) 

halcon 计算两个向量之间的夹角

vec1 := [Normals[0],Normals[1], Normals[2]] 
vec1 := [1,0,0] 
vec2 := [0, 0, 1.0]
dop_r:=vec1[0]*vec2[0] +vec1[1]*vec2[1]+vec1[2]*vec2[2]
squre_v1:=sqrt(vec1[0]*vec1[0] +vec1[1]*vec1[1]+vec1[2]*vec1[2])
squre_v2:=sqrt(vec2[0]*vec2[0] +vec2[1]*vec2[1]+vec2[2]*vec2[2])
s:=number(dop_r/(squre_v1*squre_v2))
tuple_acos(s,selta)