前景概述
网上三维点拟合空间直线的代码很多 大多数都是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;
清楚明了 就不注释了 亲测准确放心使用
标签:vector,frac,Ctrl,double,sum,C++,point3,拟合,CV From: https://www.cnblogs.com/linglingdog/p/17059838.html