平面方程：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 <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.csdn.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分解可能會不穩定，這點需要注意。