点云拟合—平面拟合

平面方程：Ax+By+Cz+D=0

方程本身不复杂，原理推导别人已经写得很明白了，我这里只贴地址了，不重复推导。

• 拟合方法一——最小二乘：

https://blog.csdn.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  

int Leastsquare::plane_fit_z(const double *pts, const int n, double *Para) {
	using namespace Eigen;
	using namespace