Halcon计算点到平面的距离没有那么简单

1. 一些基本概念

• 三维点：就是三维空间中的点（X，Y，Z）
• 平面：三维空间中的平面
• 平面方程：一般式：AX+BY+CZ+D=0
• 点到平面距离：过点做平面垂线，点到垂足的距离。（投影）

2. 浅谈有无符号的距离

2.1 无符号距离的用武之地

• 不带符号的距离相对简单，它不用理会平面方程法向量的方向，以及坐标系原点与平面之间的关系。
• Halcon中有很好用的算子，只需要设置合适的参数，就能非常方便的计算出三维点到平面的距离。
• 当然这个距离是没有符号的，也就是说你是无法通过这个距离来判断点与平面的位置关系的。点在平面上方， 还是在平面下方？（平面的上方下方这种表达并不准确，应该和平面法向量方向作比较）。那么这种计算距离的方法还有用武之地吗？当然，如果你确定你测量的点一定在基准面的一侧，那么你就可以放心大胆的使用此方法了，不是吗？
• 下图中，如果你想测量黑色元器件到PCB板的相对高度（距离），使用这种方法就非常合适，因为元器件上表面上的点到PCB的距离不可能小于0。
  

2.2 有符号距离的必要性

• 如下图，如果要测量元器件周围白色胶水到元件的高度呢？工艺要求是，胶水要比元件高，但是很有可能胶水点少了，胶高就低于元件了。这样如果还只是测量无符号距离的话，就很可能造成误判。
  

3. 无符号距离怎么算

3.1 创建一个无限延展的基准平面，对距离有什么影响？

Halcon代码

X:=[0,1,2,3,\
    0,1,2,3,\
    0,1,2,3,\
    0,1,2,3]
Y:=[0,0,0,0,\
    1,1,1,1,\
    2,2,2,2,\
    3,3,3,3]
Z:=[0.5,0,0,0,\
    0,0.1,0,0,\
    0,0.1,0,0,\
    0,0,0,0]
* 使用一组三维点创建三维点云物体模型
gen_object_model_3d_from_points (X, Y, Z, MeasurePoints)
* 创建一个基准平面
gen_plane_object_model_3d ([0,0,0,0,0,0,0], [], [], BasePlane)
* 计算三维点到基准平面的距离
distance_object_model_3d (MeasurePoints, BasePlane, [], 0, 'distance_to', 'primitive')
* 获取距离数据
get_object_model_3d_params (MeasurePoints, '&distance', distances)

图示

• 从图中可以看出，由于创建的平面是无限延展的，所以点云中的所有点都无法逃脱平面的掌控，这样计算距离是不会有问题的。
  

3.2 创建一个小小小的基准平面，对距离有什么影响？

halcon代码

• 在3.1的代码中修改一下基准的范围，如下图，现在基准平面完全hold不住所有点。

* 创建一个基准平面
gen_plane_object_model_3d ([0,0,0,0,0,0,0], [0,0,1,1], [0,1,1,0], BasePlane)

图示

• 通过观察上面的距离数据，你就会发现，这里计算点到平面距离，基准平面并不是默认为无限延展的，如果基准面不够大，那么没有在平面势力范围内的点就会找平面里离它最近的一个点。

3.3 直接使用点云拟合基准平面，可行吗？

halcon代码

• 你是说你使用点云上的所有点拟合成一个平面，这个平面作为基准平面，然后再计算点云里所有的点到基准平面的距离？ 这种方法当然是可行的。

* 创建一个基准平面
fit_primitives_object_model_3d (ObjectModel3D, ['primitive_type','fitting_algorithm'], \
                                ['plane','least_squares_tukey'], BasePlane)

gen_plane_object_model_3d ([0,0,0,0,0,0,0], [0,0,1,1], [0,1,1,0], BasePlane)

• 不用担心这个基准面不够大，经过我的验证，这个平面刚刚好，够用就行了，不是非要无限延展不可。

图示

4. 有符号距离怎么算

• 其实我写到这里已经累了，但是又不想半途而废，所以坚持写完吧。

4.1 诡异的平面法向量方向

4.1.1 下面的halcon代码展示了如何获得平面单位法向量，或者说如何获得平面方程一般式 A、B、C、D

* 计算带符号的三维点到平面的距离
X:=[1,2,3,4,\
    1,2,3,4,\
    1,2,3,4,\
    1,2,3,4]
Y:=[1,1,1,1,\
    2,2,2,2,\
    3,3,3,3,\
    4,4,4,4]

Z:=[0,0,0,0,\
    0,1,1,0,\
    0,1,1,0,\
    0,0,0,0]

dev_get_window (WindowHandle)

gen_object_model_3d_from_points (X, Y, Z, ObjectModel3D)
fit_primitives_object_model_3d (ObjectModel3D, ['primitive_type','fitting_algorithm'], \
                                ['plane','least_squares_tukey'], ObjectModel3DOut)
visualize_object_model_3d (WindowHandle, [ObjectModel3DOut,ObjectModel3D], \
                           [], [], ['disp_pose','color_0','color_1'], ['true','gray','blue'], [], [], [], PoseOut)

get_object_model_3d_params (ObjectModel3DOut, 'primitive_parameter', GenParamValue) //GenParamValue里面有四个参数，就是平面方程的A、B、C、D

4.1.2 Halcon如何解释 A、B、C、D

• 但是halcon对于A、B、C、D的解释并非我们想的那样自然，以下是get_object_model_3d_params 的参数解释，以及豆包的翻译。
  

4.1.3 我们如何理解A、B、C、D

• 先不说别的，我给出两组数据，观察一下结果：
  
  
• 我们观察到，当平面高于 Z=0 平面时，单位法向量 （A，B，C） 指向与Z轴的正方向相反；
• 而当平面低于 Z=0 平面时，单位法向量 （A，B，C） 指向与Z轴的正方向相同；
• 为了观察的更仔细，我再给两组数据，
  
  
• 从这两组数据可以看出，平面单位法向量的方向和平面与Z轴的交点有关，如果交点在原点上方，那么法向量的方向与Z轴正方向所成的夹角是钝角，或者简单说成法向量指向下方。反之，法向量指向上方。
• 我为什么这么在意法向量指向那一侧呢？因为这能帮助我们判断一个三维点在平面的哪一侧。如果我们想一个办法，可以让平面的法向量永远向上指（法向量与Z轴夹角为锐角），那这个问题就可以画上句号了。

4.1.4 如何修改法向量

• 如果 （A，B，C） 是平面 Plane 的单位法向量，那么 （-A，-B，-C） 也是它的法向量，只不过和前面的那个法向量方向相反。
• 从4.1.2中我们了解到，可以通过 D 的符号来判断原点位于平面的哪一侧，确定了这件事情后，我们也就知道平面法向量指向哪一侧了。
• 然后，我们通过一些处理，让法向量永远指向我们想要的方向。
• 不想再解释了，现在真的累了，直接给代码吧。

4.2 封装计算带符号距离的方法

GetPointToPlaneDistance (PlaneA, PlaneB, PlaneC, PlaneD, X, Y, Z, Distance)

if (PlaneC<0)
    PlaneA:=-PlaneA
    PlaneB:=-PlaneB
    PlaneC:=-PlaneC
else
    PlaneD:=-PlaneD
endif

NumPoints := |X|
Distance := []

for I := 0 to NumPoints-1 by 1
    Numerator := PlaneA * X[I] + PlaneB * Y[I] + PlaneC * Z[I] + PlaneD
    SignedDistance := Numerator
    Distance := [Distance, SignedDistance]
endfor
return ()

4.3 全部测试代码

* 计算带符号的三维点到平面的距离
X:=[1,2,3,4,\
    1,2,3,4,\
    1,2,3,4,\
    1,2,3,4]
Y:=[1,1,1,1,\
    2,2,2,2,\
    3,3,3,3,\
    4,4,4,4]

Z:=[1,0,0,0,\
    0,0,0,0,\
    0,0,0,0,\
    0,0,0,0]


dev_get_window (WindowHandle)

gen_object_model_3d_from_points (X, Y, Z, ObjectModel3D)
fit_primitives_object_model_3d (ObjectModel3D, ['primitive_type','fitting_algorithm'], \
                                ['plane','least_squares_tukey'], ObjectModel3DOut)

get_object_model_3d_params (ObjectModel3DOut, 'primitive_parameter', GenParamValue)

PlaneA:=GenParamValue[0]
PlaneB:=GenParamValue[1]
PlaneC:=GenParamValue[2]
PlaneD:=GenParamValue[3]

GetPointToPlaneDistance (PlaneA, PlaneB, PlaneC, PlaneD, X, Y, Z, Distance)

* visualize_object_model_3d (WindowHandle, [ObjectModel3DOut,ObjectModel3D], \
                           [], [], ['disp_pose','color_0','color_1'], ['true','gray','blue'], [], [], [], PoseOut)