【C++】【Ctrl+CV即可食用】三维点拟合空间直线

前景概述

网上三维点拟合空间直线的代码很多 大多数都是python或者matlab 这里贴一个C++的代码 原目的是拟合出直线之后任取两个点手动计算一下斜率

数学原理

最小二乘拟合直线方程：
$$\frac{x-x_0}{m}=\frac{y-y_0}{n}=\frac{z-z_0}{p}$$
等价转换后：
$$x=\frac{m}{p}(z-z_0)+x_0=k_{1}z+b_1$$
$$y=\frac{n}{p}(z-z_0)+y_0=k_{2}z+b_2$$
其中：
$$k_1=\frac{m}{p},b_1=x_0-\frac{m}{p}{z}_0,k_2=\frac{n}{p},b_2=y_0-\frac{n}{p}{z}_0$$
求出以上四个值就可以指定z值计算出x和y了

C++代码

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;

清楚明了 就不注释了 亲测准确放心使用