[SLAM]2D激光线特征提取

Nguyen, V., et al. (2007)."A comparison of line extraction algorithms using 2D range data for indoor mobile robotics." Autonomous Robots 23(2): 97-111.

论文提出了6中从二维激光扫描数据中提取线段的方法

1.分割合并算法

有的时候十分烦那些斜着的连线，实际不是想要的。

2.回归方法

先聚类，再回归

3.累积、区域生长算法

感觉对噪声数据真的没办法了，窝成一团的点，提取的线十分破碎而且乱...

 function [ lineSegCoord ] = extractLineSegment( model,normals,intervalPts,normalDelta,dThreshold)
 %EXTRACTLINESEGMENT Summary of this function goes here
 %   Detailed explanation goes here
 if (nargin == ) || isempty(model)
     lineSegCoord = [];
     return;
 end;
 ns = createns(model','NSMethod','kdtree')
 pts=size(model,);
 if (nargin == )
     normalDelta=0.9;
     dThreshold=0.5;
 end
 if isempty(normals)
     normals=zeros(,pts);
     for nor=:pts
         [idx, dist] = knnsearch(ns,model(:,nor)','k',2);
         data=model(:,idx);
         men=mean(data,);
         rep= repmat(men,,size(data,));
         data = data - rep;
         % Compute the MxM covariance matrix A
         A = cov(data');
         % Compute the eigenvector of A
         [V, LAMBDA] = eig(A);
         % Find the eigenvector corresponding to the minimum eigenvalue in A
         % This should always be the first column, but check just in case
         [~,idx] = min(diag(LAMBDA));
         % Normalize
         V = V(:,idx)./norm(V(:,idx));
         %定向
         normals(:,nor)=V;
     end
 end

     lineSeg=[;];
     newLineIdx=;
     for j=:pts-
         current=model(:,j);
         pre=model(:,j-);
         next=model(:,j+);
         curNormal=normals(:,j);
         preNormal=normals(:,j-);
         nextNormal=normals(:,j+);
         [d,vPt]=Dist2D_Point_to_Line(current,pre,next);
         dis=norm(current-pre);
         delta=dot(curNormal,preNormal)/(norm(curNormal)*norm(preNormal));
         if(delta>normalDelta&& d<dThreshold)  %注意两个阈值
             lineSeg(,newLineIdx)=lineSeg(,newLineIdx)+;%点数
         else
             newLineIdx=newLineIdx+;
             lineSeg=[lineSeg [; ]];
             lineSeg(,newLineIdx)=lineSeg(,newLineIdx-)+ lineSeg(,newLineIdx-);%起始点索引
         end
     end
     indexLs=;
     lineSegCoord=[];
     for k=:size(lineSeg,)
         from=lineSeg(,k);
         to=from+lineSeg(,k)-;
         if(lineSeg(,k) > intervalPts)
             try
                 pts= model(:,(from:to));
                 coef1 = polyfit(pts(,:),pts(,:),);
                 k2 = coef1();
                 b2 = coef1();
                 coef2 = robustfit(pts(,:),pts(,:),'welsch');
                 k2 = coef2();
                 b2 = coef2();
                 ML = true;
             catch
                 ML = false;
             end;
             [D,fPb]= Dist2D_Point_to_Line(model(:,from),[ b2]',[1 k2+b2]');
             [D,tPb]= Dist2D_Point_to_Line(model(:,to),[ b2]',[1 k2+b2]');
             interval=abs(model(,from) -model(,to));
             if(interval>0.05)
                 x = linspace(fPb() ,tPb(), );
                 if ML
                     y_ML = k2*x +b2;
                     lineSegCoord=[lineSegCoord [fPb() fPb() tPb() tPb()]'];
                     plot(x, y_ML, 'b-', 'LineWidth', );
                 end;
             else
                 y = linspace(fPb() ,tPb(), );
                 if ML
                     x_ML =(y-b2)/k2;
                     lineSegCoord=[lineSegCoord [fPb() fPb() tPb() tPb()]'];
                     plot(x_ML, y, 'b-', 'LineWidth', );
                 end;
             end;
 %             try
 %                 tmpPts= model(:,(from:to));
 %                 Theta_ML = estimate_line_ML(tmpPts,[], sigma, );
 %                 ML = true;
 %             catch
 %                 % probably the optimization toolbox is not installed
 %                 ML = false;
 %             end;
 %             interval=abs(model(,from) -model(,to));
 %             if(interval>)
 %                 x = linspace(model(,from) ,model(,to), );
 %                 if ML
 %                     y_ML = -Theta_ML()/Theta_ML()*x - Theta_ML()/Theta_ML();
 %                     lineSegCoord=[lineSegCoord [x() y_ML() x() y_ML()]'];
 %                     plot(x, y_ML, 'b-', 'LineWidth', );
 %                 end;
 %             else
 %                 y = linspace(model(,from) ,model(,to), );
 %                 if ML
 %                     x_ML = -Theta_ML()/Theta_ML()*y - Theta_ML()/Theta_ML();
 %                     lineSegCoord=[lineSegCoord [x_ML() y() x_ML() y()]'];
 %                     plot(x_ML, y, 'b-', 'LineWidth', );
 %                 end;
 %             end;
         end
     end
 end

4.Ransac方法

5.霍夫变换方法

6.EM方法