作业3 模型拟合 生成数据点集 基于 RANSAC 的内点集获取 平面的最小二乘拟合

clc
clear all
%生成数据点集
a=2;
b=3;
c=2;
x = 1:30;
y = 1:30;
for i=1:30
    for j=1:30
        z((i-1)*30+j,1)=i;                    %组成一个900*3的矩阵，第一列是30个1，30个2，循环的30个30
        z((i-1)*30+j,2)=j;                    %第二列，1-30轮流循环，这样就组成900个点，其值是1-30，无重复
        z((i-1)*30+j,3)=a*x(i)+b*y(j)+c;      %第三列是a,b,c三个参数来组成平面
    end
end
z=z+wgn(900,3,20);    % wgn(m,n,p) 产生一个m行n列的高斯白噪声的矩阵，p以dBW为单位指定输出噪声的强度

n=randi([1 30],100,3);    %生成100*3的300个外点，范围是1-30
for i=901:1000              %将外点加上
    for j=1:3
        z(i,j)=n(i-900,j);
    end
end
save('points.mat','z');    %使用 save 将生成的数据点集保存在“points.mat”文件;此时的z是1000*3 double

%基于 RANSAC 的内点集获取
s = 3;  %一次采样的点数
e = 0.1; %外点的概率,1000中有100个外点
p = 0.99; % 99%可能至少获得一次没有外点的采样
N=ceil(log(1-p)/log(1-(1-e)^s));   %采样次数=3.5271,用ceil取整，N=4
d=20;  %设置阈值
r=0;
for n=1:N
    point = z(randi(1000,1,3),:);   %在z中随机选择三个不同的点
    A=[point(:,1:2) ones(3,1)];
    B=point(:,3);
    mb_estimate = A\B; %参数估计   得到3*1的矩阵（也就是估计出来的a,b,c）
    Y=[z(:,1:2) ones(1000,1)];    %1000*1的矩阵
    Z=Y*mb_estimate;   %1000*3 的矩阵乘以3*1的矩阵，得1000*1，相当于line 11行进行的处理，等价与将原来的1000*3的第一二列，再根据z((i-1)*30+j,3)=a*x(i)+b*y(j)+c; 其实就是在输入x，y，[1:30],然后与估计出来的参数进行相同运算，得到结果，在于第一次的相减对比
    if (r < sum(abs(Z- z(:,3))<d));
      r=sum(abs(Z- z(:,3))<d);
      ipoint = repmat(((abs(Z- z(:,3))<d)),1,3).*z;    %(abs(Z- z(:,3))<d))输出是，满足条件是1，不满足是0，通过repmat将会得到一个abs(Z- z(:,3))<d)中所有1求和大小为行，3列的矩阵，每行要么111，要么000，然后再点乘原来z，就可以将内点完美保留
      ipoint(all(ipoint==0,2),:) = [];  %将全零的行向量赋值为空
    end
     f(n)=sum(abs(Z- z(:,3))<d); %记录下内点个数
     fprintf('第%d次,其中在阈值内的内点个数是%d个\n',n,f(n));
end

%平面的最小二乘拟合
C = [ipoint(:,1:2) ones(r,1)];
D = ipoint(:,3);
estimate =C\D;    %最小二乘法求解平面参数
error = estimate - [a; b; c];  %计算绝对误差
fprintf('a的绝对误差是:');
disp(error(1));
fprintf('b的绝对误差是:');
disp(error(2));
fprintf('c的绝对误差是:');
disp(error(3));