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

前景概述

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

数学原理

最小二乘拟合直线方程：

\[\frac {x - x_0}m = \frac {y - y_0}n = \frac {z - z_0}p \]

等价转换后：

\[x = \frac mp(z - z_0) + x_0 = k_1z + b_1 \]

\[y = \frac np(z - z_0) + y_0 = k_2z + b_2 \]

其中：

\[k_1 = \frac mp,b_1 = x_0 - \frac mpz_0, k_2 = \frac np, b_2 = y_0 - \frac npz_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;

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