最小二乘法拟合空间直线halcon实现（可以稍加修改为c/c++代码）

fit_line_3d

X1:=0
Y1:=0
Z1:=0
Z2:=0
X1Z1:=0
Y1Z1:=0

l_sum:=|x| - 1
for Index := 0 to l_sum by 1
    X1 := X1 + x[Index]
    Y1 := Y1 + y[Index]
    Z1 := Z1 + z[Index]
    Z2 := Z2 + z[Index] * z[Index]
    X1Z1 := X1Z1 + x[Index] * z[Index]
    Y1Z1 := Y1Z1 + y[Index] * y[Index]
endfor

K1 := (2*X1Z1 - X1*Z1)/(2*Z2 - Z1 *Z1)
b1 := (X1 - K1*Z1) / 2.0
K2 := (2*Y1Z1 - Y1*Z1)/(2*Z2 - Z1 *Z1)
b2 := (Y1 - K2*Z1) / 2.0

* gen_plane_object_model_3d (b2, b2, b2, ObjectModel3D)
return ()

x:=[-153.421, -141.301, -135.374, -129.426, -123.472, -111.455, -105.488, -99.3193, -93.2899, -75.441, -63.6699, -51.3573, -45.549, -15.649, 14.7569, 44.5297, 92.2409, 104.194, 110.571, 122.359, 134.46, 140.354, 158.496]
y:=[-0.0120591, 0.213574, -0.0494198, 0.206759, 0.16005, 0.263448, -0.00308438, 0.182695, 0.254049, 0.0747935, 0.180476, 0.094956, 0.0526892, -0.0291766, -0.190858, -0.131331, 0.21533, 0.419906, 0.253498, 0.461822, 0.576505, 0.341137, 0.224544]
z:=[-4.20976, -4.19573, -4.40216, -4.52007, -4.18796, -4.36231, -4.43388, -4.45376, -4.27497, -4.3173, -4.27856, -4.58745, -4.3552, -4.35169, -4.49092, -4.45811, -4.29285, -4.38076, -4.38355, -4.298, -4.51337, -4.07303, -4.47326]
fit_line_3d (x, y, z)

结果：