Halcon 用点来拟合平面
一、简介
项目要求用多个点来拟合一个平面，然后再用其他平面上的点来计算这个点到平面的距离，halcon 有现成的拟合函数。
MatLab 版本：Matlab 最小二乘法 拟合平面_Σίσυφος1900的博客-CSDN博客
二、算子解释
*输入点云数据然后生成3D模型
gen_object_model_3d_from_points(X, Y, Z, ObjectModel3D)
* X, Y, Z 分别是点x、y、z方向上的集合
* ObjectModel3D 是输出的3D模型*拟合成想要的平面
fit_primitives_object_model_3d (ObjectModel3D, ['primitive_type','fitting_algorithm'], ['plane','least_squares_tukey'], ObjectModel3DOut)
*fit_primitives_object_model_3d( : : ObjectModel3D, ParamName, ParamValue : ObjectModel3DOut)
*ObjectModel3D:输入模型
*ParamName：拟合的参数 ：fitting_algorithm, max_radius, min_radius, output_point_coord, output_xyz_mapping, primitive_type
*ParamValue:对应'primitive_type'------'cylinder'（圆柱体）, 'sphere'（球体）, 'plane'（平面）。对应'primitive_type'------'least_squares', 'least_squares_huber', 'least_squares_tukey'几种最小二乘法,这里选择plane和least_squares
*ObjectModel3DOut：输出的平面
三、代码演示
X:=[-0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04]
Y:=[-0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04]
Z:=[-0.0, -0.0, -0.0, -0.0, -0.1, -0.0, -0.0, -0.1, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0]
*输入点云数据然后生成3D模型
gen_object_model_3d_from_points(X, Y, Z, ObjectModel3D)
dev_open_window(0, 0, 512, 512, 'black', WindowHandle)
*拟合成想要的平面
fit_primitives_object_model_3d (ObjectModel3D, ['primitive_type','fitting_algorithm'], ['plane','least_squares_tukey'], ObjectModel3DOut)
*fit_primitives_object_model_3d( : : ObjectModel3D, ParamName, ParamValue : ObjectModel3DOut)
*ObjectModel3D:输入模型
*ParamName：拟合的参数 ：fitting_algorithm, max_radius, min_radius, output_point_coord, output_xyz_mapping, primitive_type
*ParamValue:对应'primitive_type'------'cylinder'（圆柱体）, 'sphere'（球体）, 'plane'（平面）。对应'primitive_type'------'least_squares', 'least_squares_huber', 'least_squares_tukey'几种最小二乘法,这里选择plane和least_squares
*ObjectModel3DOut：输出的平面
visualize_object_model_3d (WindowHandle,[ObjectModel3D,ObjectModel3DOut], [],[], \
['color_0','color_1','alpha_1','disp_pose'], ['green','gray',0.5,'true'],'RectBOX', [], [], Pose)
*获取法向量,Normal的前三个数值就是单位法向量
get_object_model_3d_params (ObjectModel3DOut, 'primitive_parameter', Normals)
平面方程：ax+by+cz+d=0;
gen_object_model_3d_from_points(X, Y, Z, ObjectModel3D)
paraName:=['primitive_type', 'fitting_algorithm']
paraVal:=['plane', 'least_squares_tukey']
fit_primitives_object_model_3d(ObjectModel3D, ['primitive_type', 'fitting_algorithm'], ['plane', 'least_squares_tukey'], ObjectModel3DOut)
get_object_model_3d_params(ObjectModel3DOut, 'primitive_parameter', plane)
* 计算平面方程(a,b,c,d)
A:= plane[0]
B:= plane[1]
C:= plane[2]
D:= plane[3]
那么计算点到平面的距离就是：
Distance:=a*X + b*Y + c*Z - d
X:=[-0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04, -0.04, -0.03, -0.02, -0.01, 0.0, 0.01, 0.02, 0.03, 0.04]
Y:=[-0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.04, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.03, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, -0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04, 0.04]
Z:=[-0.0, -0.0, -0.0, -0.0, -0.1, -0.0, -0.0, -0.1, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0]
gen_object_model_3d_from_points(X, Y, Z, ObjectModel3D)
paraName:=['primitive_type', 'fitting_algorithm']
paraVal:=['plane', 'least_squares_tukey']
fit_primitives_object_model_3d(ObjectModel3D, ['primitive_type', 'fitting_algorithm'], ['plane', 'least_squares_tukey'], ObjectModel3DOut)
get_object_model_3d_params(ObjectModel3DOut, 'primitive_parameter', plane)
* 计算平面方程(a,b,c,d)
A:= plane[0]
B:= plane[1]
C:= plane[2]
D:= plane[3]
x:=[-0.04, -0.03, -0.02, -0.01]
y:=[-0.04, -0.04, -0.04, -0.04]
z:=[-0.0, -1.0, 10.0, 30.0]
Distance:=A*x +B*y + C*z - D
四、用最小二乘法来拟合的平面
【MQ笔记】超简单的最小二乘法拟合平面（Python）_M＆Q的博客-CSDN博客_最小二乘法拟合平面
上述方程可以用
来表示。由于A是一个
的矩阵，因此我们先在等号两边分别乘以 A 的转置矩阵
，使系数矩阵变为
的方阵，之后，通过乘以系数矩阵的逆矩阵求解，也就是说，
。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 创建函数，用于生成不同属于一个平面的100个离散点
def not_all_in_plane(a, b, c):
x = np.random.uniform(-10, 10, size=100)
y = np.random.uniform(-10, 10, size=100)
z = (a * x + b * y + c) + np.random.normal(-1, 1, size=100)
return x, y, z
# 调用函数，生成离散点
x, y, z = not_all_in_plane(2, 5, 6)
# 创建系数矩阵A
a = 0
A = np.ones((100, 3))
for i in range(0, 100):
A[i, 0] = x[a]
A[i, 1] = y[a]
a = a + 1
# print(A)
# 创建矩阵b
b = np.zeros((100, 1))
a = 0
for i in range(0, 100):
b[i, 0] = z[a]
a = a + 1
# print(b)
# 通过X=(AT*A)-1*AT*b直接求解
A_T = A.T
A1 = np.dot(A_T, A)
A2 = np.linalg.inv(A1)
A3 = np.dot(A2, A_T)
X = np.dot(A3, b)
print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f' % (X[0, 0], X[1, 0], X[2, 0]))
# 计算方差
R = 0
for i in range(0, 100):
R = R + (X[0, 0] * x[i] + X[1, 0] * y[i] + X[2, 0] - z[i]) ** 2
print('方差为：%.*f' % (3, R))
# 展示图像
fig1 = plt.figure()
ax1 = fig1.add_subplot(111, projection='3d')
ax1.set_xlabel("x")
ax1.set_ylabel("y")
ax1.set_zlabel("z")
ax1.scatter(x, y, z, c='r', marker='o')
x_p = np.linspace(-10, 10, 100)
y_p = np.linspace(-10, 10, 100)
x_p, y_p = np.meshgrid(x_p, y_p)
z_p = X[0, 0] * x_p + X[1, 0] * y_p + X[2, 0]
ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10)
plt.show() 
利用最小二乘法公式求导
通过离散点拟合平面，也就是说，要找到一个平面（），使这平面到各个点的“距离”最近，根据最小二乘法，，也就是说我们要求得一组a,b,c,使得对于已有的离散点来说，S的值最小。
求解该恰定方程即可得到a，b，c。上述方程也可以用表示，该方程可以通过两边同时乘以系数矩阵的逆矩阵求得，即
。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 创建函数，用于生成不同属于一个平面的100个离散点
def not_all_in_plane(a, b, c):
x = np.random.uniform(-10, 10, size=100)
y = np.random.uniform(-10, 10, size=100)
z = (a * x + b * y + c) + np.random.normal(-1,1,size=100)
return x, y, z
# 调用函数，生成离散点
x2, y2, z2 = not_all_in_plane(2, 5, 6)
#创建系数矩阵A
A=np.zeros((3,3))
for i in range(0,100):
A[0,0]=A[0,0]+x2[i]**2
A[0,1]=A[0,1]+x2[i]*y2[i]
A[0,2]=A[0,2]+x2[i]
A[1,0]=A[0,1]
A[1,1]=A[1,1]+y2[i]**2
A[1,2]=A[1,2]+y2[i]
A[2, 0] = A[0,2]
A[2, 1] = A[1, 2]
A[2, 2] = 100
#print(A)
#创建b
b = np.zeros((3,1))
for i in range(0,100):
b[0,0]=b[0,0]+x2[i]*z2[i]
b[1,0]=b[1,0]+y2[i]*z2[i]
b[2,0]=b[2,0]+z2[i]
#print(b)
#求解X
A_inv=np.linalg.inv(A)
X = np.dot(A_inv, b)
print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f'%(X[0,0],X[1,0],X[2,0]))
#计算方差
R=0
for i in range(0,100):
R=R+(X[0, 0] * x2[i] + X[1, 0] * y2[i] + X[2, 0] - z2[i])**2
print ('方差为：%.*f'%(3,R))
# 展示图像
fig1 = plt.figure()
ax1 = fig1.add_subplot(111, projection='3d')
ax1.set_xlabel("x")
ax1.set_ylabel("y")
ax1.set_zlabel("z")
ax1.scatter(x2,y2,z2,c='r',marker='o')
x_p = np.linspace(-10, 10, 100)
y_p = np.linspace(-10, 10, 100)
x_p, y_p = np.meshgrid(x_p, y_p)
z_p = X[0, 0] * x_p + X[1, 0] * y_p + X[2, 0]
ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10)
plt.show() 