一、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)
