直线提取和RANSAC分割

       直线提取可以用来在已经存在的地图上构建地图。使用特征代替原始数据显得更加简洁，并且可以反映物理或抽象的对象，同时包含着丰富的信息量，还可以对特征的精度进行评估。

       分割：有多少条直线，哪些数据点属于哪些直线。

       直线提取：已知哪些点属于哪些直线，如何去估计曲线的参数。

一.直线提取

       给定激光扫描仪的测量矢量，确定直线的参数。

       matlab源码如下：

clc; clear;

T=100;
cov=50;
X=zeros(T,1);
Y=zeros(T,1);
W=ones(T,1);    %unweighted least squares solution
                %also compatible with weighted least squares solution if having gaussian measurement model

for i=1:T
    X(i)=i;
    Y(i)=-X(i)+T+randn*sqrt(cov);
end

figure(1);
set(gcf,'Color','white');
plot(X,Y,'go','MarkerSize',6,'MarkerFaceColor','g');
xlabel('x'); ylabel('y');
grid on;
hold on;

Pos=zeros(T,2);
for i=1:T
    Pos(i,1)=sqrt(X(i)^2+Y(i)^2);
    Pos(i,2)=atan2(Y(i),X(i));
end

den=0;
for i=1:T
    den=den+W(i);
end

Mem=zeros(1,2);
Den=zeros(1,2);
for i=1:T
    Mem(1)=Mem(1)+W(i)*Pos(i,1)^2*sin(2*Pos(i,2));
    Den(1)=Den(1)+W(i)*Pos(i,1)^2*cos(2*Pos(i,2));
    for j=1:T
        Mem(2)=Mem(2)+W(i)*W(j)*Pos(i,1)*Pos(j,1)*cos(Pos(i,2))*sin(Pos(j,2));
        Den(2)=Den(2)+W(i)*W(j)*Pos(i,1)*Pos(j,1)*cos(Pos(i,2)+Pos(j,2));
    end
end
Mem(2)=-Mem(2)*2/den;
Den(2)=-Den(2)/den;

alpha=atan((Mem(1)+Mem(2))/(Den(1)+Den(2)))/2;

mem=0;
for i=1:T
    mem=mem+W(i)*Pos(i,1)*cos(Pos(i,2)-alpha);
end
r=mem/den
alpha

L=zeros(T,1);
for i=1:T
    L(i)=-i/tan(alpha)+r*(sin(alpha)+cos(alpha)/tan(alpha));
end

plot(X,L,'r-','LineWidth',2);
hold on
plot(r*cos(alpha),r*sin(alpha),'bo','MarkerSize',8,'MarkerFaceColor','b');
legend('measurent','extracted line','closest point');
title('Line Extraction');

        matlab仿真结果如下：

       绿色点模拟激光扫描仪的采样位置，红线为采样点拟合的直线。思想和最小二乘法一样都是计算采样点到拟合直线的距离的最小均方误差对应的参数。不同的是采样点到拟合直线的距离并不垂直于x轴，而是垂直于拟合的直线。

二.分割

（1）Split and Merge

       Split and Merge首先以所有数据点为数据集拟合直线，但有时候仅拟合1条直线的误差会很大，这时会剔除距离上次拟合的直线最远的点（异常点），将上次拟合的直线以异常点为中心下分成2个数据集合，然后子数据集合分别拟合出1条直线。如果子数据集合拟合的直线依然大于误差阈值，将当前的子数据集合以异常点为中心裁剪出新的2个子数据集合，然后继续拟合，直到所有的数据集合拟合的直线误差都小于误差阈值为止。如下图所示：

（2）RANSAC

       RANSAC是以一定的概率去排除异常点的来实现直线拟合的非确定手段。计算机视觉相关博客有太多原理说明，不在此对原理赘述。具体迭代过程如下图所示：

       matlab仿真代码如下：

<span style="font-size:14px;">clc; clear;

T=200;         %num of raw data
cov=160;       %covariance of raw data
X=zeros(T,1);  
Y=zeros(T,1);

for i=1:2:T
    X(i)=i;
    Y(i)=X(i)+randn*sqrt(cov);  %generate random raw data from line
end

ymax=max(Y);
for i=2:2:T
    X(i)=rand*T;
    Y(i)=rand*ymax; %generate random raw noise
end

figure(1);
set(gcf,'Color','white');
plot(X,Y,'go','MarkerSize',5,'MarkerFaceColor','g');    %plot raw data
axis([1 T min(Y) max(Y)]);
xlabel('x'); ylabel('y');
grid on;
hold on;

%error initialized
minerr=inf;
mink=0;
minb=0;
N=10;
n=2;
minS=zeros(n,2);

for itr=1:N
    %random sample of 2 points
    S=zeros(n,2);   %sample
    SI=zeros(n,1);  %sample index
    for i=1:n
        SI(i)=round(rand*T)+1;
        if SI(i)==0
            SI(i)=1;
        else if SI(i)>T
                SI(i)=T;
            end
        end
        S(i,1)=X(SI(i));
        S(i,2)=Y(SI(i));
    end

    %line parameters that fit the sample data
    k=(S(2,2)-S(1,2))/(S(2,1)-S(1,1));
    b=S(1,2)-k*S(1,1);

    %error function for each data point
    err=0;
    A=k; B=-1; C=b;
    Dist=zeros(T,1);
    for i=1:T
        den=sqrt(A^2+B^2);
        if den<10e-4
            den=10e-4;
        end
        Dist(i)=abs(A*X(i)+B*Y(i)+C)/den;
        err=err+Dist(i)^2;
    end
    err

    if err<minerr
        minerr=err;
        mink=k;
        minb=b;
    else
        continue;
    end

    %data that support current hypothesis
    thrs=10;        %threshold of distance
    I=zeros(T,2);   %qualified data
    idx=1;
    for i=1:T
        if Dist(i)<thrs
            I(idx,1)=X(i);
            I(idx,2)=Y(i);
            for i=1:n
                minS(i,1)=X(SI(i));
                minS(i,2)=Y(SI(i));
            end
            idx=idx+1;
        end
    end
end

Xl=zeros(T,1);  %points in line given 2 points
Yl=zeros(T,1);
for i=1:T
    Xl(i)=i;
    Yl(i)=mink*Xl(i)+minb;
end
plot(Xl,Yl,'-');
hold on;

i=1:idx;
plot(I(i,1),I(i,2),'ro','MarkerSize',5,'MarkerFaceColor','r');
hold on;

i=1:2;
plot(minS(i,1),minS(i,2),'bo','MarkerSize',7,'MarkerFaceColor','b'); 
title('RANSAC');
hold on;

minerr</span>

       matlab仿真结果如下：

       上图显示的是迭代10次后拟合误差最小的情况。绿点模拟构建地图时采集乱七八糟的原始数据点，蓝点为随机采样的2个点，蓝色直线为随机采样点对应的直线，红点为小于距离误差阈值的符合要求的数据点。由于每次运行随机采样的点会变化，所以每次运行和迭代都会出现不同的结果。

注：详细内容和推导参见《Introductionto Autonomous Mobile Robots》的4.3节内容“Feature Extraction”和Chris Clark的《Autonomous Robot Navigation》感知部分课件。