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

合集 - SLAM(55)

1.个人资源索引2018-03-032.SLAM学习笔记（1）基本概念2015-08-063.SLAM学习笔记（2）SLAM算法2015-08-214.[ROS]2 尝试编译OrbSLAM2016-01-185.[ROS]1 小乌龟2016-01-186.SLAM最近的工作2015-11-057.SLAM学习笔记（3）相关概念2015-09-16

8.[SLAM]2D激光线特征提取2016-07-16

9.[SLAM] GMapping SLAM源码阅读（草稿）2016-07-0110.[QGLViewer]First Demo2016-06-2911.SLAM数据集2016-06-2312.[SLAM]2D激光扫描匹配方法2016-06-1513.[g2o]一个备忘2016-04-2114.[ROS] slam_gmapping2016-01-2415.[ROS]3 Linux编程练习2016-01-2016.Linux学习和ROS安装（1）2017-12-1917.小豆包的学习之旅：机器人定位2017-05-0218.小豆包的学习之旅：传感器观测模型2017-04-2119.Kinect2.0相机标定2017-04-1020.小豆包的学习之旅：里程计运动模型2017-04-0321.小豆包的学习之旅：入门篇2017-04-0222.Kinect2.0点云数据获取2017-03-3023.[硬件]Robot运动控制2017-03-2224.[硬件]Urg_viewer数据读取2017-03-1425.[硬件]三维点云数据获取2017-03-0426.Kinect2.0获取数据2017-02-1527.ndt histogram_direction2016-09-0828.rplidar & hector slam without odometry2016-08-2629.rplidar测试2016-08-1230.[原创]NDT方法在SLAM中的应用2016-08-0831.[SLAM]Karto SLAM算法学习（草稿）2016-07-2232.ROS学习备忘2018-07-1833.[ROS]激光驱动安装2018-07-1634.OrbSLAM2采集点云数据2018-03-2635.Cartographer源码阅读(8)：imu_tracker2018-03-2136.[硬件]点云数据采集22018-03-1937.Cartographer源码阅读(9)：图优化的前端——闭环检测2018-03-0838.Cartographer源码阅读(7)：轨迹推算和位姿推算的原理2018-03-0839.Ethzasl MSF源码阅读(3)：MSF_Core和PoseMeasurement2018-03-0740.Ethzasl MSF源码阅读(2)：百川汇海2018-03-0641.Ethzasl MSF源码阅读(1)：程序入口和主题订阅2018-03-0442.Cartographer源码阅读(6)：LocalTrajectoryBuilder和PoseExtrapolator2018-02-2543.Cartographer源码阅读(5)：PoseGraph位姿图2018-02-2444.Cartographer源码阅读(4)：Node和MapBuilder对象22018-01-1045.Cartographer源码阅读(3)：程序逻辑结构2017-12-2946.Cartographer源码阅读(2)：Node和MapBuilder对象2017-12-2847.Cartographer源码阅读(1)：程序入口2017-12-2848.实时Cartographer测试(1) - rplidar2017-12-2149.Cartographer安装2017-12-2050.ROS安装（2）2017-12-2051.[ROS]一些传感器数据读取融合问题的思考2017-05-0652.小豆包的学习之旅：占用概率栅格地图和cost-map2017-05-0253.小豆包的学习之旅：开发记录2017-04-2954.[概述]移动机器人自主探索2017-04-1855.MRPT编译2017-04-14

收起

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 == 0) || isempty(model)
    lineSegCoord = [];
    return;
end;
ns = createns(model','NSMethod','kdtree')
pts=size(model,2);
if (nargin == 3)
    normalDelta=0.9;
    dThreshold=0.5;
end
if isempty(normals) 
    normals=zeros(2,pts);
    for nor=1:pts
        [idx, dist] = knnsearch(ns,model(:,nor)','k',2);
        data=model(:,idx);
        men=mean(data,2);
        rep= repmat(men,1,size(data,2));
        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=[1;1];
    newLineIdx=1;
    for j=2:pts-1  
        current=model(:,j);
        pre=model(:,j-1);
        next=model(:,j+1); 
        curNormal=normals(:,j);
        preNormal=normals(:,j-1);
        nextNormal=normals(:,j+1);
        [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(2,newLineIdx)=lineSeg(2,newLineIdx)+1;%点数      
        else      
            newLineIdx=newLineIdx+1;
            lineSeg=[lineSeg [1; 1]];
            lineSeg(1,newLineIdx)=lineSeg(1,newLineIdx-1)+ lineSeg(2,newLineIdx-1);%起始点索引
        end
    end
    indexLs=1;
    lineSegCoord=[];
    for k=1:size(lineSeg,2)
        from=lineSeg(1,k);
        to=from+lineSeg(2,k)-1;
        if(lineSeg(2,k) > intervalPts)
            try
                pts= model(:,(from:to));
                coef1 = polyfit(pts(1,:),pts(2,:),1);
                k2 = coef1(1);
                b2 = coef1(2);
                coef2 = robustfit(pts(1,:),pts(2,:),'welsch');
                k2 = coef2(2);
                b2 = coef2(1);
                ML = true;
            catch
                ML = false;
            end;
            [D,fPb]= Dist2D_Point_to_Line(model(:,from),[0 b2]',[1 k2+b2]');
            [D,tPb]= Dist2D_Point_to_Line(model(:,to),[0 b2]',[1 k2+b2]');
            interval=abs(model(1,from) -model(1,to));
            if(interval>0.05)
                x = linspace(fPb(1) ,tPb(1), 5);
                if ML
                    y_ML = k2*x +b2;
                    lineSegCoord=[lineSegCoord [fPb(1) fPb(2) tPb(1) tPb(2)]'];
                    plot(x, y_ML, 'b-', 'LineWidth', 1);
                end;
            else
                y = linspace(fPb(2) ,tPb(2), 5);
                if ML
                    x_ML =(y-b2)/k2;
                    lineSegCoord=[lineSegCoord [fPb(1) fPb(2) tPb(1) tPb(2)]'];
                    plot(x_ML, y, 'b-', 'LineWidth', 1);
                end;
            end;           
%             try
%                 tmpPts= model(:,(from:to));
%                 Theta_ML = estimate_line_ML(tmpPts,[], sigma, 0);
%                 ML = true;
%             catch
%                 % probably the optimization toolbox is not installed
%                 ML = false;
%             end;
%             interval=abs(model(1,from) -model(1,to));
%             if(interval>10)
%                 x = linspace(model(1,from) ,model(1,to), 5);
%                 if ML
%                     y_ML = -Theta_ML(1)/Theta_ML(2)*x - Theta_ML(3)/Theta_ML(2);
%                     lineSegCoord=[lineSegCoord [x(1) y_ML(1) x(5) y_ML(5)]'];
%                     plot(x, y_ML, 'b-', 'LineWidth', 1);
%                 end;
%             else
%                 y = linspace(model(2,from) ,model(2,to), 5);
%                 if ML
%                     x_ML = -Theta_ML(2)/Theta_ML(1)*y - Theta_ML(3)/Theta_ML(1);
%                     lineSegCoord=[lineSegCoord [x_ML(1) y(1) x_ML(5) y(5)]'];
%                     plot(x_ML, y, 'b-', 'LineWidth', 1);
%                 end;
%             end;           
        end
    end
end

 

4.Ransac方法

 

5.霍夫变换方法

 

6.EM方法