最小二乘法拟合空间直线

最小二乘法拟合空间直线

注：本文借鉴了 博客1 和 博客2 这两篇博客

空间直线的一种表达方式：（点向式）
$$\frac{x-x_0}{m}=\frac{y-y_0}{n}=\frac{z-z_0}{k}$$

方法一

可以将该公式转换为如下形式：
$$\{\begin{array}{rl}x& =x_0+\frac{m}{k}(z-z_0)\\ & \\ y& =y_0+\frac{n}{k}(z-z_0)\end{array}$$
令 方程为
$$\{\begin{array}{rl}x& =k_{1}z+b_1\\ y& =k_{2}z+b_2\end{array}$$
其中
$$\begin{array}{rlrl}{k}_1& =\frac{m}{k},& b_1& =x_0-\frac{m}{k}{z}_0\\ & & & \\ k_2& =\frac{n}{k},& b_2& =y_0-\frac{n}{k}{z}_0\end{array}$$
利用残差的平方和
$$\begin{gathered} Q_1=\sum _{i=1}^n(x_i-k_1{z}_i-b_1{)}^2 \\ {Q}_2=\sum _{i=1}^n(y_i-k_2{z}_i-b_2{)}^2 \end{gathered}$$
为使得$Q_1$,$Q_2$最小，则应满足：
$$\{\begin{array}{r}\frac{\partial Q_1}{\partial k_1}=0,\frac{\partial Q_1}{\partial b_1}=0\\ \frac{\partial Q_2}{\partial k_2}=0,\frac{\partial Q_2}{\partial b_2}=0\end{array}$$
即
$$\{\begin{array}{rl}& \sum _{i=1}^{n}2(x_i-k_1{z}_i-b_1)\times (-z_i)=0\\ & \sum _{i=1}^{n}2(x_i-k_1{z}_i-b_1)\times (-1)=0\\ & \sum _{i=1}^{n}2(y_i-k_2{z}_i-b_2)\times (-z_i)=0\\ & \sum _{i=1}^{n}2(y_i-k_2{z}_i-b_2)\times (-1)=0\end{array}$$
联立上面的方程，得：
$$\{\begin{array}{rl}& k_1=\frac{n\sum x_i{z}_i-\sum x_i\sum z_i}{n\sum z^2-\sum z_i\sum z_i}\\ & \\ & b_1=\frac{k1\sum z_i-\sum x_i}{n}\\ & \\ & k_2=\frac{n\sum y_i{z}_i-\sum y_i\sum z_i}{n\sum z^2-\sum z_i\sum z_i}\\ & \\ & b_2=\frac{k2\sum z_i-\sum y_i}{n}\end{array}$$

求解出$k_1,k_2,b_1,b_2$，即得出直线方程
当直线不平行于xoy平面时$($该直线过点($b_1$, $b_2$, 0)，法向量为($k_1$, $k_2$, 1)$)$

代码中的结构体

// (x-x0)/m = (y-y0)/n = (z-z0)/k
struct SpaceLine
{
    double x;
    double y;
    double z;
    double m;
    double n;
    double k;
    SpaceLine() = default;
        SpaceLine(double x, double y, double z, double m, double n, double k) : x(x), y(y), z(z), m(m), n(n), k(k) { }
};

struct Point3d
{
    double x;
    double y;
    double z;
    Point3d() = default;
    Point3d(double x, double y, double z) : x(x), y(y), z(z) { }
};

c++代码

using std::vector;

SpaceLine fit3dline(vector<Point3d> const & data)
{
	int n = data.size();
	double sum_xz = 0, sum_x = 0, sum_z = 0, sum_zz = 0, sum_yz = 0, sum_y = 0;

	for (Point3d const& p : data)
	{
		sum_xz += p.x * p.z;
		sum_x += p.x;
		sum_z += p.z;
		sum_zz += std::pow(p.z, 2);
		sum_yz += p.z * p.y;
		sum_y += p.y;
	}
	double k1 = (n * sum_xz - sum_x * sum_z) / (n * sum_zz - pow(sum_z, 2));
	double b1 = (sum_x - k1 * sum_z) / n;

	double k2 = (n * sum_yz - sum_y * sum_z) / (n * sum_zz - pow(sum_z, 2));
	double b2 = (sum_y - k2 * sum_z) / n;

	return SpaceLine(b1, b2, 0, k1, k2, 1);
}

方法二

当直线不平行于xoy平面时，令$z_0$ = 0，k =1，简化该方程为：
$$\frac{x-x_0}{m}=\frac{y-y_0}{n}=\frac{z}{1}$$
$$\{\begin{array}{rl}x& =mz+x_0\\ y& =nz+y_0\end{array}$$
表现成矩阵形式为：
$$\left[\begin{array}{cc}m& x_0\\ n& y_0\end{array}\right]\left[\begin{array}{c}z\\ 1\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]$$
当有n个点时第i个点的方程为：
$$\left[\begin{array}{cc}m& x_0\\ n& y_0\end{array}\right]\left[\begin{array}{c}{z}_i\\ 1\end{array}\right]=\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]$$
并联n个方程得到：
$$\left[\begin{array}{cc}m& x_0\\ n& y_0\end{array}\right]\left[\begin{array}{ccc}z& \cdots & z_n\\ 1& \cdots & 1\end{array}\right]=\left[\begin{array}{ccc}{x}_1& \cdots & x_n\\ y_1& \cdots & y_n\end{array}\right]$$
最小二乘拟合：
$$\left[\begin{array}{cc}m& x_0\\ n& y_0\end{array}\right]\left[\begin{array}{ccc}z& \cdots & z_n\\ 1& \cdots & 1\end{array}\right]\left[\begin{array}{cc}{z}_1& 1\\ ⋮& ⋮\\ z_n& 1\end{array}\right]=\left[\begin{array}{ccc}{x}_1& \cdots & x_n\\ y_1& \cdots & y_n\end{array}\right]\left[\begin{array}{cc}{z}_1& 1\\ ⋮& ⋮\\ z_n& 1\end{array}\right]$$
化简为：
$$\left[\begin{array}{cc}m& x_0\\ n& y_0\end{array}\right]=\left[\begin{array}{cc}\sum x_i{z}_i& \sum x_i\\ \sum y_i{z}_i& \sum y_i\end{array}\right]{\left[\begin{array}{cc}\sum z_i^2& \sum z_i\\ \sum z_i& n\end{array}\right]}^{-1}$$
求出$m,n,x_0,y_0$后 ，该直线过($x_0$,$y_0$, 0)，法向量为(m,n,1)

c++代码

using std::vector;
// 还运用了eigen库,进行矩阵的运算

SpaceLine fit3dline(vector<Point3d> const & data)
{
	int n = data.size();
	double sum_xz = 0, sum_x = 0, sum_yz = 0, sum_y = 0, sum_zz = 0, sum_z = 0;

	for (Point3d const & p : data)
	{
		sum_xz += p.x*p.z;
		sum_x += p.x;
		sum_yz += p.y*p.z;
		sum_y += p.y;
		sum_zz += pow(p.z, 2);
		sum_z += p.z;
	}

	Eigen::MatrixXf mat1(2, 2);
	mat1 <<
		sum_xz, sum_x,
		sum_yz, sum_y;

	Eigen::MatrixXf mat2(2, 2);
	mat2 <<
		sum_zz, sum_z,
		sum_z, n;

	Eigen::MatrixXf mat_result = mat1 * mat2.inverse();

	Eigen::MatrixXf &r = mat_result;

	return SpaceLine(r(0, 1), r(1, 1), 0, r(0, 0), r(1, 0), 1);
}