使用RANSAC算法在空间点云中拟合平面

发表于 分类于 阅读次数：

最近在实现一篇论文时需要在3D空间点云中找出处在一个平面上的点，当共面的点超过30个时记为一个平面，尽量找出多个。

随机抽样一致性算法RANSAC(Random sample consensus)是一种迭代的方法来从一系列包含有离异值的数据中计算数学模型参数的方法。

RANSAC算法本质上由两步组成，不断进行循环：

  1. 从输入数据中随机选出能组成数学模型的最小数目的元素，使用这些元素计算出相应模型的参数。选出的这些元素数目是能决定模型参数的最少的。

  2. 检查所有数据中有哪些元素能符合第一步得到的模型。超过错误阈值的元素认为是离异值（outlier），小于错误阈值的元素认为是内部点（inlier）。

    这个过程重复多次，选出包含点最多的模型即得到最后的结果。

    5.png

具体到空间点云中拟合平面：

  1. 从点云中随机选取三个点。
  2. 由这三个点组成一个平面。
  3. 计算所有其他点到该平面的距离，如果小于阈值T，就认为是处在同一个平面的点。
  4. 如果处在同一个平面的点超过n个，就保存下这个平面，并将处在这个平面上的点都标记为已匹配。
  5. 终止的条件是迭代N次后找到的平面小于n个点，或者找不到三个未标记的点。

Matlab代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
plot3(data(:,1),data(:,2),data(:,3),'ro');     %显示全部数据,data中保存的是空间点云数据(x,y,z)
xlabel('x')
ylabel('y')
zlabel('z')

K=2000;         %设置最大迭代次数
sigma=0.01;     %设置拟合直线与数据距离的偏差
pretotal=0;     %符合拟合模型的数据的个数
planeNumber=10; %要寻找的平面个数
k=1;
i=0;
bestplane =zeros(planeNumber,4);
planeNo=1;
breakFlag = 0;
while planeNo <= planeNumber
    while pretotal < 30 &&  k< K      %有30数据符合拟合模型或达到最大迭代次数就可以退出了
        SampIndex=floor(1+(size(data,1)-1)*rand(3,1));  %产生三个随机索引，找样本用，floor向下取整
        sampFinderCounter = 0;
        while(sum(isnan([data(SampIndex(1),:) data(SampIndex(2),:) data(SampIndex(3),:)])) ~= 0 || length([data(SampIndex(1),:) data(SampIndex(2),:) data(SampIndex(3),:)]) ~= length(unique([data(SampIndex(1),:) data(SampIndex(2),:) data(SampIndex(3),:)])))
            SampIndex=floor(1+(size(data,1)-1)*rand(3,1));  %产生三个随机索引，找样本用，floor向下取整
            sampFinderCounter = sampFinderCounter +1;
            if(sampFinderCounter > 5000)
                breakFlag = 1;
                break;
            end
        end
        if breakFlag == 1
            break;
        end
        samp1=data(SampIndex(1),:);     %对原数据随机抽样两个样本
        samp2=data(SampIndex(2),:);
        samp3=data(SampIndex(3),:);

        plane = planeCreate([samp1;samp2;samp3]); %对三个数据拟合出平面
        %line=Mytls([samp1;samp2]);      %对两个数据拟合出直线，或其他变种拟合方法
        mask(planeNo,:)=abs(plane*[data ones(size(data,1),1)]');    %求每个数据到拟合直线的距离
        total=sum(mask(planeNo,:)<sigma);              %计算数据距离直线小于一定阈值的数据的个数

        if total>pretotal            %找到符合拟合直线数据最多的拟合直线
            pretotal=total;
            bestplane(planeNo,:)=plane;          %找到最好的拟合直线
        end
        k=k+1;
    end
    if pretotal<30 || breakFlag ~= 0
        disp(breakFlag);
        break;
    end
    %显示符合最佳拟合的数据
    mask(planeNo,:)=abs(bestplane(planeNo,:)*[data ones(size(data,1),1)]')<sigma;
    hold on;
    color(planeNo,:) = rand(1,3);
    disp(['画第' num2str(planeNo) '个平面']);
    for i=1:length(mask)
        if mask(planeNo, i)
            plot3(data(i,1),data(i,2),data(i,3),'o','color',color(planeNo,:));
            data(i,:) = NaN;
        end
    end
    planeNo = planeNo+1;
    pretotal = 0;
    k=1;
end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
function plane = planeCreate(data)
    point1 = data(1, :);%读取点
    point2 = data(2, :);
    point3 = data(3, :);

    line12 = point2 - point1;%计算两点之间的向量
    line13 = point3 - point1;
    n = cross(line12, line13); %计算平面的法向量

    a = n(1);%点法式生成平面参数
    b = n(2);
    c = n(3);
    d = n(1)*-point1(1) + n(2)*-point1(2) + n(3)*-point1(3);

    plane = [a b c d];
end

拟合平面后所有的点云数据：

1.jpg

拟合得到的平面单独显示:

2.jpg

3.jpg

4.jpg