[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.累积、区域生长算法

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

  1 function [ lineSegCoord ] = extractLineSegment( model,normals,intervalPts,normalDelta,dThreshold)
  2 %EXTRACTLINESEGMENT Summary of this function goes here
  3 %   Detailed explanation goes here
  4 if (nargin == 0) || isempty(model)
  5     lineSegCoord = [];
  6     return;
  7 end;
  8 ns = createns(model','NSMethod','kdtree')
  9 pts=size(model,2);
 10 if (nargin == 3)
 11     normalDelta=0.9;
 12     dThreshold=0.5;
 13 end
 14 if isempty(normals) 
 15     normals=zeros(2,pts);
 16     for nor=1:pts
 17         [idx, dist] = knnsearch(ns,model(:,nor)','k',2);
 18         data=model(:,idx);
 19         men=mean(data,2);
 20         rep= repmat(men,1,size(data,2));
 21         data = data - rep;
 22         % Compute the MxM covariance matrix A
 23         A = cov(data');
 24         % Compute the eigenvector of A
 25         [V, LAMBDA] = eig(A);
 26         % Find the eigenvector corresponding to the minimum eigenvalue in A
 27         % This should always be the first column, but check just in case
 28         [~,idx] = min(diag(LAMBDA));
 29         % Normalize
 30         V = V(:,idx)./norm(V(:,idx));
 31         %定向    
 32         normals(:,nor)=V;
 33     end
 34 end
 35 
 36     lineSeg=[1;1];
 37     newLineIdx=1;
 38     for j=2:pts-1  
 39         current=model(:,j);
 40         pre=model(:,j-1);
 41         next=model(:,j+1); 
 42         curNormal=normals(:,j);
 43         preNormal=normals(:,j-1);
 44         nextNormal=normals(:,j+1);
 45         [d,vPt]=Dist2D_Point_to_Line(current,pre,next);
 46         dis=norm(current-pre);
 47         delta=dot(curNormal,preNormal)/(norm(curNormal)*norm(preNormal));
 48         if(delta>normalDelta&& d<dThreshold)  %注意两个阈值     
 49             lineSeg(2,newLineIdx)=lineSeg(2,newLineIdx)+1;%点数      
 50         else      
 51             newLineIdx=newLineIdx+1;
 52             lineSeg=[lineSeg [1; 1]];
 53             lineSeg(1,newLineIdx)=lineSeg(1,newLineIdx-1)+ lineSeg(2,newLineIdx-1);%起始点索引
 54         end
 55     end
 56     indexLs=1;
 57     lineSegCoord=[];
 58     for k=1:size(lineSeg,2)
 59         from=lineSeg(1,k);
 60         to=from+lineSeg(2,k)-1;
 61         if(lineSeg(2,k) > intervalPts)
 62             try
 63                 pts= model(:,(from:to));
 64                 coef1 = polyfit(pts(1,:),pts(2,:),1);
 65                 k2 = coef1(1);
 66                 b2 = coef1(2);
 67                 coef2 = robustfit(pts(1,:),pts(2,:),'welsch');
 68                 k2 = coef2(2);
 69                 b2 = coef2(1);
 70                 ML = true;
 71             catch
 72                 ML = false;
 73             end;
 74             [D,fPb]= Dist2D_Point_to_Line(model(:,from),[0 b2]',[1 k2+b2]');
 75             [D,tPb]= Dist2D_Point_to_Line(model(:,to),[0 b2]',[1 k2+b2]');
 76             interval=abs(model(1,from) -model(1,to));
 77             if(interval>0.05)
 78                 x = linspace(fPb(1) ,tPb(1), 5);
 79                 if ML
 80                     y_ML = k2*x +b2;
 81                     lineSegCoord=[lineSegCoord [fPb(1) fPb(2) tPb(1) tPb(2)]'];
 82                     plot(x, y_ML, 'b-', 'LineWidth', 1);
 83                 end;
 84             else
 85                 y = linspace(fPb(2) ,tPb(2), 5);
 86                 if ML
 87                     x_ML =(y-b2)/k2;
 88                     lineSegCoord=[lineSegCoord [fPb(1) fPb(2) tPb(1) tPb(2)]'];
 89                     plot(x_ML, y, 'b-', 'LineWidth', 1);
 90                 end;
 91             end;           
 92 %             try
 93 %                 tmpPts= model(:,(from:to));
 94 %                 Theta_ML = estimate_line_ML(tmpPts,[], sigma, 0);
 95 %                 ML = true;
 96 %             catch
 97 %                 % probably the optimization toolbox is not installed
 98 %                 ML = false;
 99 %             end;
100 %             interval=abs(model(1,from) -model(1,to));
101 %             if(interval>10)
102 %                 x = linspace(model(1,from) ,model(1,to), 5);
103 %                 if ML
104 %                     y_ML = -Theta_ML(1)/Theta_ML(2)*x - Theta_ML(3)/Theta_ML(2);
105 %                     lineSegCoord=[lineSegCoord [x(1) y_ML(1) x(5) y_ML(5)]'];
106 %                     plot(x, y_ML, 'b-', 'LineWidth', 1);
107 %                 end;
108 %             else
109 %                 y = linspace(model(2,from) ,model(2,to), 5);
110 %                 if ML
111 %                     x_ML = -Theta_ML(2)/Theta_ML(1)*y - Theta_ML(3)/Theta_ML(1);
112 %                     lineSegCoord=[lineSegCoord [x_ML(1) y(1) x_ML(5) y(5)]'];
113 %                     plot(x_ML, y, 'b-', 'LineWidth', 1);
114 %                 end;
115 %             end;           
116         end
117     end
118 end

View Code

4.Ransac方法

5.霍夫变换方法

6.EM方法