最小二乘法通过最小化误差的平方和，使拟合对象越来越接近最终对象。
 拟合直线就是找到一条直线使所有数据点到拟合直线的欧式距离之和最小。
 拟合平面就是找到一个平面使得所有数据点到拟合平面的欧式距离最小
 设拟合平面方程为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的值，最小特征值对应的特征向量。
 具体代码如下：
 
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) / 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);
}
}

 
文章目录
 
最小二乘法拟合平面原理MATLAB&C++实现
最小二乘法拟合平面原理
MATLAB实现
c++实现
 

 
最小二乘法拟合平面原理MATLAB&C++实现
 
最小二乘法拟合平面原理
 
	最小二乘法是我们平时用的比较的多一种拟合算法，尤其是在直线拟合，平面拟合中，大量的工程实践验证了其具有简便好用的特点。但是，其抗噪声性差，对测量数据有较高的要求。下面我们探讨一下其基本原理和实现过程。
	首先，我们会用各种仪器测量得到多组数据。当然，至少要有不同的三组数据，且不能在同一条直线上取这些数据，这是确定一个平面的基本要求。
 
 
	我们选择平面方程为：
	ax + by + cz + d = 0
	为了方便构建误差函数，我们转换形式，我们可以用下面方程表示：
	z = ax + by + c
	我们构建最小二乘误差函数：
 
 
当误差函数取得最小值时，我们就得到了最佳拟合的平面。
 视a,b,c为自变量，我们对其求导，得到如下方程组：
 
 移项，整理可得：
 
 为了方便求解，我们用矩阵的形势来表示上述方程组，
 
 转变形势，得到a,b,c的表达式：
 
 
将测量数据带入上式，即可求出a,b,c的值。
 
MATLAB实现
 
clc;
 clear all;
 x = [1478.5,1190.5,2043];
 y = [968.5,1449.5,1432];
 z = [33202.83,33185.18,33168.82];
 a12 = sum(xy’);
 a13 = sum(x);
 a22 = sum(y.^2);
 a23 = sum(y);
 a33 = size(x,2);
 c11 = sum(xz’);
 c22 = sum(yz’);
 c33 = sum(z);
 A = [a11 a12 a13;
 a12 a22 a23;
 a13 a23 a33];
 B = inv(A);
 C = B[c11;c22;c33];
 
c++实现
 
int n = cloud_.size();
 Eigen::MatrixXd points;
 points.resize(n, 3);
 for (int i = 0; i < n; i++)
 {
 points(i, 0) = cloud_.at(i).x;
 points(i, 1) = cloud_.at(i).y;
 points(i, 2) = cloud_.at(i).z;
 }
 double A11 = 0, A12 = 0, A13 = 0;
 double A21 = 0, A22 = 0, A23 = 0;
 double A31 = 0, A32 = 0, A33 = 0;
 double B1 = 0, B2 = 0, B3 = 0;
 A11 = (points.array().col(0) * points.array().col(0)).sum();
 A12 = (points.array().col(0) * points.array().col(1)).sum();
 A13 = (points.array().col(0)).sum();
 A21 = A12;
 A22 = (points.array().col(1) * points.array().col(1)).sum();
 A23 = (points.array().col(1)).sum();
 A31 = A13; A32 = A23;
 A33 = n;
 B1 = (points.array().col(0) * points.array().col(2)).sum();
 B2 = (points.array().col(1) * points.array().col(2)).sum();
 B3 = (points.array().col(2)).sum();
 Eigen::MatrixXd A;
 A.resize(3, 3);
 A(0, 0) = A11; A(0, 1) = A12; A(0, 2) = A13;
 A(1, 0) = A21; A(1, 1) = A22; A(1, 2) = A23;
 A(2, 0) = A31; A(2, 1) = A32; A(2, 2) = A33;
 Eigen::MatrixXd B;
 B.resize(3, 1);
 B(0, 0) = B1; B(1, 0) = B2; B(2, 0) = B3;
 double C1 = 0, C2 = 0, C3 = 0;
 C1 = A.inverse()(0, 0) * B(0) + A.inverse()(0, 1) * B(1) + A.inverse()(0, 2) * B(2);
 C2 = A.inverse()(1, 0) * B(0) + A.inverse()(1, 1) * B(1) + A.inverse()(1, 2) * B(2);
 C3 = A.inverse()(2, 0) * B(0) + A.inverse()(2, 1) * B(1) + A.inverse()(2, 2) * B(2);

前言:
 
    最近要实现一个算法，“对一系列点拟合出一条线，且区分出不属于该线的点”。在网上找了许多资料，用数学公式解释原理以及用matlab实现的居多，本文章主要解释用最小二乘法的进行点拟合成线，matlab 和 c++两个版本的代码实现。
 
 
 
使用矩阵实现:
 
     根据公式A = (X'*X)-1*X'*Y（这个公式可以拟合一条最接近点的曲线，而不仅仅是直线，具体请参照链接），便得到了系数矩阵A，A是一个2行1列的矩阵，其中的两个值分别是直线的k、b值。
 
测试样例点：(27 39) (8 5) (8 9) (16 22) (44 71) (35 44) (43 57) (19 24) (27 39) (37 52)
 
matlab：
 
 
 
x =[
27,1;
8,1;
7,1;
16,1;
44,1;
35,1;
43,1;
19,1;
27,1;
37,1]
xt = x.'
y = [39;5;9;22;71;44;57;24;39;52]
xt * x
inv(xt * x)
b = inv(xt * x) * xt * y

 
 
 
结果：
 
 
 
 
为了使用实现对矩阵的操作，分别选用了Eigen库 和 OpenCv库两个版本：
 
EIgen库:
 
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;  
using namespace Eigen::internal;  
using namespace Eigen::Architecture; 
int main(){

	MatrixXd x(10,2);
	for(int i = 0; i<10 ; ++i ) x(i,1) = 1;
	
	x(0,0) = 27;
	x(1,0) = 8;
	x(2,0) = 7;
	x(3,0) = 16;
	x(4,0) = 44;
	x(5,0) = 35;
	x(6,0) = 43;
	x(7,0) = 19;
	x(8,0) = 27;
	x(9,0) = 37;
	MatrixXd xt = x.transpose();

	MatrixXd y(10,1);
	y(0,0) = 39;
	y(1,0) = 5;
	y(2,0) = 9;
	y(3,0) = 22;
	y(4,0) = 71;
	y(5,0) = 44;
	y(6,0) = 57;
	y(7,0) = 24;
	y(8,0) = 39;
	y(9,0) = 52;
	
	MatrixXd b(2,1);

	b = (xt * x).inverse() * xt * y; //主要公式

	for(int i = 0 ; i<2 ;++i){
		for(int j = 0 ; j<2 ;++j){
			std::cout<<(xt * x)(i,j)<<" ";
		}
		std::cout<<std::endl;
	}

	for(int i = 0 ; i<2 ;++i){
		for(int j = 0 ; j<2 ;++j){
			std::cout<<(xt * x).inverse()(i,j)<<" ";
		}
		std::cout<<std::endl;
	}
	std::cout<<b(0,0)<<","<<b(1,0)<<std::endl;
	system("pause");
	return 0;
}
 
结果：
 

  
 
Opencv Mat库：
 
 
 
 
 
 
 
#include <iostream>
#include<opencv2/highgui/highgui.hpp>


using namespace cv;
int main(){

	double x[10][2] = {
		27,1,
		8,1,
		7,1,
		16,1,
		44,1,
		35,1,
		43,1,
		19,1,
		27,1,
		37,1
	};
	CvMat vx = cvMat(10,2,CV_64FC1,x);

	double y[10][1]={
		39,
		5,
		9,
		22,
		71,
		44,
		57,
		24,
		39,
		52
	};
	CvMat vy = cvMat(10,1,CV_64FC1,y);

	double temp[2][2]={0,0,0,0};   //用于对矩阵初始化
	double temp2[2][2]={0,0,0,0};
	double temp3[2][10]={0,0,0,0};
	double temp4[2][1]={0,0};
	CvMat xt_multiply_x = cvMat(2,2,CV_64FC1,temp);
	CvMat x_inverse = cvMat(2,2,CV_64FC1,temp2);
	CvMat multiply1 = cvMat(2,10,CV_64FC1,temp3);
	CvMat b = cvMat(2,1,CV_64FC1,temp4);

	cvGEMM(&vx,&vx,1,&xt_multiply_x,1,&xt_multiply_x,1); //函数解释请参考链接
	//cvCrossProduct(&xt, &x, &xt_multiply_x);
	cvInvert(&xt_multiply_x,&x_inverse);
	cvGEMM(&x_inverse, &vx,1, &multiply1,1,&multiply1,2);
	cvGEMM(&multiply1, &vy,1, &b,1,&b);


	for(int i = 0 ; i<2 ;++i){
		for(int j = 0 ; j<2 ;++j){
			std::cout<<cvmGet(&xt_multiply_x,i,j)<<" ";
		}
		std::cout<<std::endl;
	}

	for(int i = 0 ; i<2 ;++i){
		for(int j = 0 ; j<2 ;++j){
			std::cout<<cvmGet(&x_inverse,i,j)<<" ";
		}
		std::cout<<std::endl;
	}	
	std::cout<<cvmGet(&b,0,0)<<","<<cvmGet(&b,1,0)<<std::endl;
	system("pause");
	return 0;
}
 
 
 
 
 
 
 
使用最小二乘公式实现：
 
 
 
根据公式得到的a,b分别为直线的k、b值。这个公式的原理是：
 
1. 要求的方程为y=ax+b。
 
2.假设残差为e，yi=a*xi+b+ei  ->  ei = yi - a*xi - b。
 
3.只要求  使∑ei^2(残差平方和)最小   的a b的值。
 
4.设Q =  ∑ei^2，要使Q最小，只需要分别对 a b 求导 使得求导后的两方程为0，联立求出a b。
 
 
 
#include <iostream>
#include <algorithm>
#include <valarray>
#include <vector>

struct Point{
	int x,y;
};
int main()
{

	double x[]={27,8,7,16,44,35,43,19,27,37,};
	double y[]={39,5,9,22,71,44,57,24,39,52};


    std::valarray<double> data_x(x,10);
    std::valarray<double> data_y(y,10);

    float A = 0.0;
    float B = 0.0;
    float C = 0.0;
    float D = 0.0;
    A = (data_x*data_x).sum();
    B = data_x.sum();
    C = (data_x*data_y).sum();
    D = data_y.sum();
    float k, b, tmp = 0;
    if (tmp = (A*data_x.size() - B*B))
    {
        k = (C*data_x.size() - B*D) / tmp;
        b = (A*D - C*B) / tmp;
    }
    else
    {
        k = 1;
        b = 0;
    }
	std::cout<<k<<","<<b<<std::endl;
	system("pause");
	
	return 0;
}
 

 结果：
 
 
 
 
 
 
实现前言中提到的算法：
 
 
 
不知道为什么，要编两次才会出结果，第一次跑opencv的界面不会出现，没有搭建opencv环境的小伙伴可以把关于画图的部分注释掉。
 
 
 
#include <iostream>
#include <time.h>
#include <algorithm>
#include <Eigen/Dense>
#include <valarray>
#include<opencv2/highgui/highgui.hpp>
#define random(x) ((rand())%x)

using namespace cv;
using namespace Eigen;  
using namespace Eigen::internal;  
using namespace Eigen::Architecture; 

//输入一系列点的坐标，输出这些点所拟合的线的k b值
std::vector<double> leastSquareFitting(std::vector<Point> &rPoints){
	std::vector<double > resLine(2);
    int num_points = rPoints.size();
    std::valarray<float> data_x(num_points);
    std::valarray<float> data_y(num_points);
    for (int i = 0; i < num_points; i++)
    {
        data_x[i] = rPoints[i].x;
        data_y[i] = rPoints[i].y;
    }
    float A = 0.0;
    float B = 0.0;
    float C = 0.0;
    float D = 0.0;
    A = (data_x*data_x).sum();
    B = data_x.sum();
    C = (data_x*data_y).sum();
    D = data_y.sum();
    float k, b, tmp = 0;
    if (tmp = (A*data_x.size() - B*B))
    {
        k = (C*data_x.size() - B*D) / tmp;
        b = (A*D - C*B) / tmp;
    }
    else
    {
        k = 1;
        b = 0;
    }
    resLine[0] = k;
    resLine[1] = b;

    return resLine;
}

//第一个参数为系列点，第二个参数为在一条线上的点，第三个参数为在这条线之外的点
void RegressionPoints(const std::vector<Point> &rPoints,std::vector<Point> &rInlinePoints,std::vector<Point>&rOutlinePoints){
	int length = rPoints.size();
	std::vector<bool > is_inline;
	for(int i = 0 ;i<length;++i) is_inline.push_back(true);

	double avg = 0.0;
	std::vector<double> kb;
	bool find_new_points = false;
	while(!find_new_points){
		//线内点
		std::vector<int> InlinePointsIndex;
		rInlinePoints.clear();
		for(int i = 0 ; i < length ;++i){
			if(is_inline[i]){
				rInlinePoints.push_back(rPoints[i]);
				InlinePointsIndex.push_back(i);
			}
		}

		//线的k b值
		kb = leastSquareFitting(rInlinePoints);

		//残差
		std::vector<double > def;
		for(int i = 0 ;i < rInlinePoints.size();++i){
			def.push_back(abs(kb[0] * rInlinePoints[i].x + kb[1] - rInlinePoints[i].y));
			avg += def[i];
		}
		//判断是否有新的线外点
		avg  =  avg / rInlinePoints.size();	
		find_new_points = false;
		for(int i = 0 ; i < rInlinePoints.size();++i){
			if(2.0 * avg < def[i]){
				is_inline[InlinePointsIndex[i]] = false;
				find_new_points = true;
			}
		}

	}

	//线外点
	rOutlinePoints.clear();
	for(int i = 0 ; i < length ;++i){
		if(!is_inline[i]) rOutlinePoints.push_back(rPoints[i]);
	}
	
	//---------------------------------------用opencv在窗口中画出，便于观察
	//画图
	cv::Mat cdst = cv::Mat::zeros(480, 720, CV_8UC3);
	//画出线内点
	for (int i = 0; i < rInlinePoints.size(); i++){
		cv::Point center = cv::Point((int)rInlinePoints[i].x , (int)rInlinePoints[i].y);
		circle(cdst, center, 3, cv::Scalar(0, 255, 255), -1);
	}
	//画出线外点
	for (int i = 0; i < rOutlinePoints.size(); i++){
		cv::Point center = cv::Point((int)rOutlinePoints[i].x , (int)rOutlinePoints[i].y);
		circle(cdst, center, 3, cv::Scalar(255, 0, 255), -1);
	}
	//画线
	cv::Point pt1, pt2;
	pt1.x = 0;
	pt1.y = kb[0]*pt1.x + kb[1];
	pt2.x = 400;
	pt2.y = kb[0]*pt2.x + kb[1];
	line(cdst, pt1, pt2, cv::Scalar(255, 255/2,255/2), 2, 8);

	imshow("LinesCluster", cdst);
	cv::namedWindow("LinesCluster", 1);
	cv::waitKey(1000);

	//----------------------------输出线内点、线外点、线的kb值
	for(auto w:rInlinePoints) std::cout<<w.x<<","<<w.y<<std::endl;
	std::cout<<std::endl;
	for(auto w:rOutlinePoints) std::cout<<w.x<<","<<w.y<<std::endl;
	std::cout<<std::endl;
	std::cout<<kb[0]<<","<<kb[1]<<std::endl;
}


int main()
{
	std::vector<Point> temp(10),in_line_points,rOutlinePoints;
	Point crd;
	srand(unsigned(time(0)));
	for(int i = 0;i<10;++i){
		temp[i].x = random(50) * 5;
		temp[i].y = 1.3 * temp[i].x + random(100) - 50;
	}

	RegressionPoints(temp,in_line_points,rOutlinePoints);

	system("pause");
	
	return 0;
}

 

 结果：
 
 
 
 
 
 
参考链接：
 
Opencv矩阵操作
 
Opencv cvGEMM函数
 
Eigen的使用
 
最小二乘法公式
 
最小二乘法矩阵
 
介绍一个好用的绘制函数图像的工具Graph，下载链接秘密：dvww

 
最小二乘法拟合平面 -- 代码实现（C++, Eigen3）
 
说明
作者[ 哈哈kls ]的推导
C++实现
 

 
说明
 
本文基于博主 哈哈kls 的博文:最小二乘法拟合平面 的公式推导，用C++进行实现。
 文章出处：https://blog.csdn.net/konglingshneg/article/details/82585868
 
作者[ 哈哈kls ]的推导
 

 注：上图最后一句应该是：$$z=a_{0}x+a_{1}y+a2$$
 
C++实现
 
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>

using namespace std;
using namespace Eigen;

/*
* From: https://blog.csdn.net/konglingshneg/article/details/82585868
* Author: JARK006(JARK006@QQ.COM)
* z = a0x + a1y + a2
* a0*x + a1*y - z + a2 = 0
* 入参：points --待拟合平面的点集
* 返回：拟合平面法向量 { a0, a1, -1 }
*/
vector<double> FittingPlaneEigen(vector<vector<double>>& points) {
    vector<vector<double>> mat3x4(3, vector<double>(4, 0));
    for (const auto& p : points) {
        // 3x3 方程组参数   p[0]:x, p[1]:y, p[2]:z
        // 对角线对称，只需处理5个
        mat3x4[0][0] += p[0] * p[0];
        mat3x4[0][1] += p[0] * p[1];
        mat3x4[0][2] += p[0];
        mat3x4[1][1] += p[1] * p[1];
        mat3x4[1][2] += p[1];

        // 3x1 方程组值
        mat3x4[0][3] += p[0] * p[2];
        mat3x4[1][3] += p[1] * p[2];
        mat3x4[2][3] += p[2];
    }

    MatrixXd A(3, 3);
    A <<
        mat3x4[0][0], mat3x4[0][1], mat3x4[0][2],
        mat3x4[0][1], mat3x4[1][1], mat3x4[1][2],
        mat3x4[0][2], mat3x4[1][2], (double)points.size();
    MatrixXd B(3, 1);
    B << mat3x4[0][3], mat3x4[1][3], mat3x4[2][3];

    //以下三个方式前14位有效数字基本一致，任选一个
    //来源：https://eigen.tuxfamily.org/dox/group__LeastSquares.html
    MatrixXd a = A.bdcSvd(ComputeThinU | ComputeThinV).solve(B);
    //MatrixXd a = A.colPivHouseholderQr().solve(B);
    //MatrixXd a = A.lu().solve(B);

    // a0*x + a1*y - z + a2 = 0 法向量为：a(0), a(1), -1
    return { a(0), a(1), -1.0 };
    
    // 若需 A*x + B*y + C*z + D = 0 形式,则A,B,C,D为{ a(0), a(1), -1.0, a(2) }
    // return { a(0), a(1), -1.0, a(2) };
}