最小二乘法拟合空间直线的原理及实现

空间直线的表达式可以表示为：
$$\frac{x-b}{a}=\frac{y-d}{c}=\frac{z}{1}$$
通过变形可得：
$$\{\begin{array}{l}x=a\times z+b\\ y=c\times z+d\end{array}$$
用矩阵可表示为：
$$\begin{array}{c}\left[\begin{array}{cc}a& b\\ c& d\end{array}\right]\left[\begin{array}{c}z\\ 1\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]\end{array}$$
对于直线上的多个点，可表示为：
$$\begin{array}{c}\left[\begin{array}{cc}a& b\\ c& d\end{array}\right]\left[\begin{array}{ccc}{z}_1& \dots & z_n\\ 1& \dots & 1\end{array}\right]=\left[\begin{array}{ccc}{x}_1& \dots & x_n\\ y_1& \dots & y_n\end{array}\right]\end{array}$$
等号两边的矩阵右边同时乘以：
$$\begin{array}{c}\left[\begin{array}{cc}{z}_1& 1\\ \dots & \dots \\ z_n& 1\end{array}\right]\end{array}$$
则有：
$$\begin{array}{c}\left[\begin{array}{cc}a& b\\ c& d\end{array}\right]\left[\begin{array}{cc}{\sum }_{i=1}^n{z}_i^2& {\sum }_{i=1}^n{z}_i\\ {\sum }_{i=1}^n{z}_i& n\end{array}\right]=\left[\begin{array}{cc}{\sum }_{i=1}^n{x}_i{z}_i& {\sum }_{i=1}^n{x}_i\\ {\sum }_{i=1}^n{y}_i{z}_i& {\sum }_{i=1}^n{y}_i\end{array}\right]\end{array}$$
最后可得：
$$\begin{array}{c}\left[\begin{array}{cc}a& b\\ c& d\end{array}\right]=\left[\begin{array}{cc}{\sum }_{i=1}^n{x}_i{z}_i& {\sum }_{i=1}^n{x}_i\\ {\sum }_{i=1}^n{y}_i{z}_i& {\sum }_{i=1}^n{y}_i\end{array}\right]{\left[\begin{array}{cc}{\sum }_{i=1}^n{z}_i^2& {\sum }_{i=1}^n{z}_i\\ {\sum }_{i=1}^n{z}_i& n\end{array}\right]}^{-1}\end{array}$$

C++程序为：

#include<iostream>
#include<math.h>

/*
设空间直线方程为：
x-b   y-d    z
--- = --- = ---
 a     c     1
 
a = e[0][0]
b = e[0][1]
c = e[1][0]
d = e[1][1]

*/ 

using namespace std;
void matrix_inverse(double(*a)[2], double(*b)[2]);

int main()
{
	int i, j, M, N;
	double f[][3] = {
						{0,1,2},
						{1,2,3},
						{2,3,4},
						{3,4,5},
						{4,5,6},
						{5,6,7},
						{6,7,8},
						{7,8,9},
						{8,9,10}
					};
	M = sizeof(f) / sizeof(f[0]);
	
	double a[M][3] = { 0 };
	for (i = 0; i < M; i++)
	{
		for(j = 0; j < 3; j++)
			a[i][j] = f[i][j];
	}
	
	double b[2][2] = { 0 };
	double c[2][2] = { 0 };
	double d[2][2] = { 0 };
	double e[2][2] = { 0 };
	
	for ( i = 0; i < M; i++ )
	{
		b[0][0] = b[0][0] + ( a[i][0] * a[i][2] );
		b[0][1] = b[0][1] + a[i][0];
		b[1][0] = b[1][0] + ( a[i][1] * a[i][2] );
		b[1][1] = b[1][1] + a[i][1];
	}
	
	for ( i = 0; i < M; i++ )
	{
		c[0][0] = c[0][0] + pow( a[i][2] , 2 );
		c[0][1] = c[0][1] + a[i][2];
		c[1][0] = c[1][0] + a[i][2];
		c[1][1] = M;
	}
	
	matrix_inverse(c, d);  //求逆函数
	
	e[0][0] = (b[0][0] * d[0][0]) + (b[0][1] * d[1][0]);	// a
	e[0][1] = (b[0][0] * d[0][1]) + (b[0][1] * d[1][1]);	// b
	e[1][0] = (b[1][0] * d[0][0]) + (b[1][1] * d[1][0]);	// c
	e[1][1] = (b[1][0] * d[0][1]) + (b[1][1] * d[1][1]);	// d
	
//	for (i = 0; i < 2; i++)	//显示系数 
//	{
//		for (j = 0; j < 2; j++)
//			cout << " " << e[i][j] << "\t";
//		cout << "\n";
//	}
	return 0;
 }
 
 void matrix_inverse(double(*a)[2], double(*b)[2])
{
	using namespace std;
	int i, j, k;
	double max, temp;
	// 定义一个临时矩阵t
	double t[2][2];
	// 将a矩阵临时存放在矩阵t[n][n]中
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			t[i][j] = a[i][j];
	// 初始化B矩阵为单位矩阵
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			b[i][j] = (i == j) ? (double)1 : 0;
	// 进行列主消元，找到每一列的主元
	for (i = 0; i < 2; i++)
	{
		max = t[i][i];
		// 用于记录每一列中的第几个元素为主元
		k = i;
		// 寻找每一列中的主元元素
		for (j = i + 1; j < 2; j++)
		{
			if (fabs(t[j][i]) > fabs(max))
			{
				max = t[j][i];
				k = j;
			}
		}
		//cout<<"the max number is "<<max<<endl;
		// 如果主元所在的行不是第i行，则进行行交换
		if (k != i)
		{
			// 进行行交换
			for (j = 0; j < 2; j++)
			{
				temp = t[i][j];
				t[i][j] = t[k][j];
				t[k][j] = temp;
				// 伴随矩阵B也要进行行交换
				temp = b[i][j];
				b[i][j] = b[k][j];
				b[k][j] = temp;
			}
		}
		if (t[i][i] == 0)
		{
			cout << "\nthe matrix does not exist inverse matrix\n";
			break;
		}
		// 获取列主元素
		temp = t[i][i];
		// 将主元所在的行进行单位化处理
		//cout<<"\nthe temp is "<<temp<<endl;
		for (j = 0; j < 2; j++)
		{
			t[i][j] = t[i][j] / temp;
			b[i][j] = b[i][j] / temp;
		}
		for (j = 0; j < 2; j++)
		{
			if (j != i)
			{
				temp = t[j][i];
				//消去该列的其他元素
				for (k = 0; k < 2; k++)
				{
					t[j][k] = t[j][k] - temp * t[i][k];
					b[j][k] = b[j][k] - temp * b[i][k];
				}
			}
		}
	}
}