网上三维点拟合空间直线的代码很多 大多数都是python或者matlab 这里贴一个C++的代码 原目的是拟合出直线之后任取两个点手动计算一下斜率
数学原理最小二乘拟合直线方程：
x
−
x
0
m
=
y
−
y
0
n
=
z
−
z
0
p
frac {x - x_0}m = frac {y - y_0}n = frac {z - z_0}p
mx−x0=ny−y0=pz−z0
等价转换后：
x
=
m
p
(
z
−
z
0
)
+
x
0
=
k
1
z
+
b
1
x = frac mp(z - z_0) + x_0 = k_1z + b_1
x=pm(z−z0)+x0=k1z+b1
y
=
n
p
(
z
−
z
0
)
+
y
0
=
k
2
z
+
b
2
y = frac np(z - z_0) + y_0 = k_2z + b_2
y=pn(z−z0)+y0=k2z+b2
其中：
k
1
=
m
p
,
b
1
=
x
0
−
m
p
z
0
,
k
2
=
n
p
,
b
2
=
y
0
−
n
p
z
0
k_1 = frac mp,b_1 = x_0 - frac mpz_0, k_2 = frac np, b_2 = y_0 - frac npz_0
k1=pm,b1=x0−pmz0,k2=pn,b2=y0−pnz0
求出以上四个值就可以指定z值计算出x和y了
double sum_x = 0.;
double sum_y = 0.;
double sum_z = 0.;
double sum_xz = 0.;
double sum_yz = 0.;
double sum_z2 = 0.;
for (size_t i = 0; i < point3_vector.size(); ++i)
{
sum_x += point3_vector[i].x;
sum_y += point3_vector[i].y;
sum_z += point3_vector[i].z;
sum_xz += point3_vector[i].x * point3_vector[i].z;
sum_yz += point3_vector[i].y * point3_vector[i].z;
sum_z2 += point3_vector[i].z * point3_vector[i].z;
}
size_t n = point3_vector.size();
double den = n * sum_z2 - sum_z * sum_z;
double k1 = (n * sum_xz - sum_x * sum_z) / den;
double b1 = (sum_x - k1 * sum_z) / n;
double k2 = (n * sum_yz - sum_y * sum_z) / den;
double b2 = (sum_y - k2 * sum_z) / n;
清楚明了 就不注释了 亲测准确放心使用
