最小二乘法拟合平面

最小二乘法通过最小化误差的平方和，使拟合对象越来越接近最终对象。
拟合直线就是找到一条直线使所有数据点到拟合直线的欧式距离之和最小。
拟合平面就是找到一个平面使得所有数据点到拟合平面的欧式距离最小
设拟合平面方程为ax+by+cz+d=0，则约束条件为a平方+b平方+c平方=1
构建方程ax’+by’+cz’+d=0,则可以得到如下方程：
a(x-x’)+b(y-y’)+c(z-z’)=0
主要是将方程组变为齐次方程组，用于奇异值分解。
接下来将方程转为矩阵形式，使得min||WX||，约束条件||X||=1，运用SVD分解求出使||WX||最小且||X||=1的X值。
SVD后得到V的最后一列就是a,b,c,d的值，最小特征值对应的特征向量。
具体代码如下：

//cv::Mat points = cv::Mat(objPoints.size(),3,CV_32FC1);
void fitPlane(cv::Mat &points,cv::Mat &plane){
cv::Mat centor = cv::Mat::zeros(1,points.cols,CV_32FC1);
for(int i =0;i < points.cols;++i){
	for(int j =0;j < points.rows;++j){
		centor.at<float>(0,i) = centor.at<float>(0.i) + points.at<float>(j,i);  
	}
	centor.at<float>(0.i) = centor.at<float>(0.i) / points.rows;
}
cv::Mat pointC = cv::Mat::ones(points.rows,points.cols,CV_32FC1);
for(int i =0;i < points.cols;++i){
	for(int j =0;j < points.rows;++j){
		pointC.at<float>(j,i) =  points.at<float>(j,i) - centor.at<float>(0.i);
	}
}
//SVD分解
cv::Mat A,W,U,V;
//构建奇异值矩阵(gemm矩阵相乘)
cv::gemm(pointC,points,1,NULL,0,A,CV_GEMM_A_T);
SVD::compute(A,W,U,V);
plane = cv::Mat::zeros(4,1,CV_32FC1);
for(int i =0;i < points.cols;++i){
	plane.at<float>(i,0) = V.at<float>(points.cols-1,i);
	plane.at<float>(points.cols,0) -= plane.at<float>(i,0)*centor.at<float>(0.i);
}
}