平面方程：Ax+By+Cz+D=0
方程本身不复杂，原理推导别人已经写得很明白了，我这里只贴地址了，不重复推导。
拟合方法一——最小二乘：
https://blog.****.net/konglingshneg/article/details/82585868
构建系数矩阵后，利用最小二乘即可 求解：
Ax=b
x=(ATA)-1ATb
% matlab
inv(A'*A)*A'*b
实际使用过程中，遇到一些问题。
参考原博客提出的方法，当用于拟合系数 c=0的平面，结果有时候对，有时候不对，一开始我怀疑系数矩阵超出double类型的数据范围，后来采用了8万多个点计算发现，并没有出现数据类型超限的情况，那么剩下可能就是在 C≠0 的情况下推导出的模型不适用于所有情况，于是我假设 A≠0，同样的方法重新推导了系数矩阵，代入点云计算发现拟合出来的平面是正确的。
总结：方法一可以用来拟合平面，但是如果平面与推导的系数矩阵假设条件（A≠0 or B≠0 or C≠0 ）不符，拟合容易失败（有时候也可以拟合出来）。如果结合先验知识提前知道（A≠0 or B≠0 or C≠0 ）则可以对应采用相应的模型进行拟合，会稍微麻烦一些。
贴个简单例子：
z= a0*x+a1*y+a2 ( C≠0 )
#include <Eigen/SVD>
int Leastsquare::plane_fit_z(const double *pts, const int n, double *Para) {
using namespace Eigen;
using namespace std;
double xx, xy, yy, x, y;
double xz, yz, z;
xx = xy = yy = x = y = 0.0;
xz = yz = z = 0.0;
// comfirm step for sampling according to n
int step = 1;
//step += 10 * n / 10000;
////int realNo = 0;
for (int i = 0; i < n; i = i + step) {
double tem_x = pts[3 * i];
double tem_y = pts[3 * i + 1];
double tem_z = pts[3 * i + 2];
xx += tem_x * tem_x;
yy += tem_y * tem_y;
xy += tem_x * tem_y;
x += tem_x;
y += tem_y;
xz += tem_x * tem_z;
yz += tem_y * tem_z;
z += tem_z;
////realNo++;
}
Matrix3d A;
Vector3d b;
A << xx, xy, x, \
xy, yy, y, \
x, y, n;
b << xz, yz, z;
cout << "Here is the matrix A:\n" << A << endl;
cout << "Here is the vector b:\n" << b << endl;
Vector3d res = A.colPivHouseholderQr().solve(b);
cout << "The solution is:\n" << res << endl;
Para[0] = res[0];
Para[1] = res[1];
Para[2] = res[2];
return 0;
}
拟合方法二 ——SVD分解：
空间中的离散点得到拟合平面，其实这就是一个最优化的过程。即求这些点到某个平面距离最小和的问题。由此，我们知道一个先验消息，那就是该平面一定会过众散点的平均值。接着我们需要做的工作就是求这个平面的法向量。
根据协方差矩阵的SVD变换，最小奇异值对应的奇异向量就是平面的方向。
注意：这个方法是直接的计算方法，没办法解决数值计算遇到的病态矩阵问题.在公式转化代码之前必须对空间点坐标进行近似归一化！
原理推导，建议看这个（实际是一个 带条件的最优化问题）：https://blog.****.net/oChenWen/article/details/84373582
SVD相关资料：http://www.cnblogs.com/LeftNotEasy/archive/2011/01/19/svd-and-applications.html
算法步骤如下：
求点云的平均值(x0,y0,z0)；
所有点减去平均值，构建矩阵A;
对矩阵A做SVD分解，A = U * S * VT；
V最后一列对应为(A,B,C);
D=-(A*x0+B*y0+C*z0)
matlab版本：
clc;clear;
pcCloud=pcread('data/out/01/3.pcd');
pc=pcCloud.Location;
[m n]=size(pc);
% if m>4000
% m=round(m/2)
% end
x=pc(1:m,1);
y=pc(1:m,2);
z=pc(1:m,3);
scatter3(x,y,z,'filled')
hold on;
planeData=[x,y,z];
%SVD
xyz0=mean(planeData,1);
centeredPlane=bsxfun(@minus,planeData,xyz0);
[U,S,V]=svd(centeredPlane);
a=V(1,3);
b=V(2,3);
c=V(3,3);
d=-dot([a b c],xyz0);
fprintf('%f %f %f %f ',a,b,c,d)
% 图形绘制
xfit = min(x):0.1:max(x);
yfit = min(y):0.1:max(y);
[XFIT,YFIT]= meshgrid (xfit,yfit);
ZFIT = -(d + a * XFIT + b * YFIT)/c;
mesh(XFIT,YFIT,ZFIT);
点数太多，A矩阵太大，会导致matlab计算时候报错，这个时候需要降采样。
c++， eigen版本
#include <Eigen/SVD>
// pts 中的点按照 xi yi zi的顺序存储，共n个点
int plane_fitSVD(const double *pts, const int n, double *Para) {
using namespace Eigen;
// comfirm step for sampling according to n
int step = 1;
//step += 10 * n / 10000;
double ave_x, ave_y, ave_z;
ave_x = ave_y = ave_z = 0.0;
int realNo = 0;
for (int i = 0; i < n; i = i + step) {
double tem_x = pts[3 * i];
double tem_y = pts[3 * i + 1];
double tem_z = pts[3 * i + 2];
ave_x += tem_x;
ave_y += tem_y;
ave_z += tem_z;
realNo++;
}
ave_x /= realNo;
ave_y /= realNo;
ave_z /= realNo;
MatrixXd A(realNo, 3);
for (int i = 0, j = 0; i < n; i = i + step, j++) {
double tem_x = pts[3 * i];
double tem_y = pts[3 * i + 1];
double tem_z = pts[3 * i + 2];
A(j, 0) = tem_x - ave_x;
A(j, 1) = tem_y - ave_y;
A(j, 2) = tem_z - ave_z;
}
JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
Matrix3d V = svd.matrixV();
std::cout << "V :\n" << V << std::endl;
Para[0] = V(0,2);
Para[1] = V(1,2);
Para[2] = V(2,2);
Para[3] = -(Para[0] * ave_x + Para[1] * ave_y + Para[2] * ave_z);
std::cout << "Params are :\n" << Para[0] << "\t" << Para[1] << "\t" << Para[2] << "\t" << Para[3] << "\n";
return 0;
}
结果：
调用示意图：
总结：SVD分解拟合平面方程对平面没有要求，可以用于拟合所有的平面。当参与拟合点云的数量较多的时候，svd分解可能会不稳定，这点需要注意。