[Matlab] RANSAC平面擬合例子

RANSAC原理

輸入：①數據 ②抽樣次數N ③距離閾值t ④數量閾值T
輸出：最終估計的模型

程序流程：

1. data ：數據
2. 取樣本 ：確定模型參數p所需要的最小數據數n，隨機取n個數據作為一個樣本J
3. 建模型：根據樣本J建立模型Mp(J)。
4. 判斷距離：根據模型Mp（J）判斷所有數據點到模型的距離。
5. 記錄：記錄 距離小於t的個數total 和 距離小於t的點的索引。
6. 判斷：　若total>數量閾值T ：則用距離小於t的點重新估計模型 重復3-5一次。
　　　　   若total<數量閾值T：則跳出。
7. 記錄最大total和此時的模型作為最佳模型。
8. 循環N次。
9.輸出

---

 

函數ransac_fitplane

function [a,b,c,d]=ransac_fitplane(data,N,t,T)
figure;plot3(data(1,:),data(2,:),data(3,:),'o');hold on; % 顯示數據
iter = N; %抽樣次數N
number = size(data,2); % 總數
maxNum=0;     %符合擬合模型的數據的個數
for i=1:iter %循環次數
     sampleidx = randperm(number); sampleidx =sampleidx(1,1:3);
     sample = data(:,sampleidx); %取樣本
     [a1,a2,a3,a4]=get_nice_plane(sample);%擬合直線方程 z=ax+by+c
     plane = [-a1/a3,-a2/a3,-1,-a4/a3];%建模型
     mask=abs(plane*[data; ones(1,size(data,2))]);    %求每個數據到擬合平面的距離
     total=sum(mask<t); %計算數據距離平面小於一定閾值的數據的個數
     index= mask<t;
     if total>T
         nsample=data(:,index);
         [a1,a2,a3,a4]=get_nice_plane(nsample);
         plane = [-a1/a3,-a2/a3,-1,-a4/a3];  %z=ax+by+c
         mask=abs(plane*[data; ones(1,size(data,2))]);
         total=sum(mask<t);              %計算數據距離平面小於一定閾值的數據的個數
     end;
     if total>maxNum   %記錄最大total
         maxNum=total;
         bestplane=plane;%最好的擬合平面
         bestindex=index;
         bestplane2=[a1,a2,a3,a4];
    end  
end
%顯示符合最佳擬合的數據
maxNum %最大一致集
avgerror=abs(bestplane*[data; ones(1,size(data,2))]);
avgerror=sum(avgerror(find(avgerror<t)))/maxNum  %計算一致集內平均誤差
a=bestplane2(1);b=bestplane2(2);c=bestplane2(3);d=bestplane2(4);
% 圖形繪制
temp1=data(1,bestindex);
temp2=data(2,bestindex);
xfit = min(temp1):0.2:max(temp1);
yfit = min(temp2):0.2:max(temp2);
[XFIT,YFIT]= meshgrid (xfit,yfit);
ZFIT = bestplane(1)*XFIT+bestplane(2)*YFIT+bestplane(4);
mesh(XFIT,YFIT,ZFIT);grid on;
xlabel('X');
ylabel('Y');
end

函數get_nice_plane

function [a,b,c,d]=get_nice_plane(data)
planeData=data';
% 協方差矩陣的SVD變換中，最小奇異值對應的奇異向量就是平面的方向
xyz0=mean(planeData,1);
centeredPlane=bsxfun(@minus,planeData,xyz0);
[~,~,V]=svd(centeredPlane);
a=V(1,3);
b=V(2,3);
c=V(3,3);
d=-dot([a b c],xyz0);
end　　

主程序

mu=[0 0 0];
S=[2 0 4;0 4 0;4 0 8]; 
data1=mvnrnd(mu,S,400);
%外點
mu=[2 2 2];
S=[8 1 4;1 8 2;4 2 8];
data2=mvnrnd(mu,S,500);
data=[data1',data2'];%% 相當於原始數據
[a,b,c,d]=ransac_fitplane(data,3000,0.5,300)

　　

實驗結果