【Halcon】点云到平面的距离计算

在Halcon中简单直接计算点云到平面的方法是：

distance_object_model_3d (ObjectModel3DPoints, ObjectModel3DPlane, [], 0, [], [])
get_object_model_3d_params (ObjectModel3DPoints, '&distance', Distance)

但是这样存在一个问题：在点云投影到平面的方向看过去上，平面的大小必须大于点云的边界，否则这个算子计算出来的结果不正确。
所以自己写了一个函数计算，使用基本的点到平面距离公式：

get_object_model_3d_params (ObjectModel3DPoints, 'point_coord_x', X)
get_object_model_3d_params (ObjectModel3DPoints, 'point_coord_y', Y)
get_object_model_3d_params (ObjectModel3DPoints, 'point_coord_z', Z)
get_object_model_3d_params (ObjectModel3DPlane, 'primitive_parameter', Normals)
***创建点云矩阵P
create_matrix (|X|, 1, X, MatrixX)
create_matrix (|Y|, 1, Y, MatrixY)
create_matrix (|Z|, 1, Z, MatrixZ)
create_matrix (|Z|, 3, 0, MatrixP)
set_sub_matrix (MatrixP, MatrixX, 0, 0)
set_sub_matrix (MatrixP, MatrixY, 0, 1)
set_sub_matrix (MatrixP, MatrixZ, 0, 2)
***平面的法向量
N := Normals[0:2]
create_matrix (3, 1, N, MatrixN)
***法向量的模长，其实默认都是1
NLength := sqrt(sum(N*N))
create_matrix (1, 1, NLength, MatrixNL)
repeat_matrix (MatrixNL, |Z|, 1, MatrixNL)
D := -Normals[3] //平面的偏移量需要取反
create_matrix (1, 1, D, MatrixD)
repeat_matrix (MatrixD, |Z|, 1, MatrixD)
***计算投影距离
mult_matrix (MatrixP, MatrixN, 'AB', MatrixPN)
***添加偏移量
add_matrix (MatrixPN, MatrixD, MatrixPN_D)
abs_matrix (MatrixPN_D, MatrixPN_D)
***计算最终距离
div_element_matrix (MatrixPN_D, MatrixNL, Matrix_PN_D_Div_NL)
***返回矩阵的元素
get_full_matrix (Matrix_PN_D_Div_NL, Distance)
return ()