matlab 三维激光雷达点云的地面与障碍物检测

基于激光雷达的地面与障碍物检测

这个例子告诉我们如何去检测地平面并且找到三维LIDAR数据中与车相近的障碍物。

这个过程能够方便我们对汽车导航的可行驶区域规划。

注：每一帧的雷达属于都被存储为三维的雷达点云。为了能够高效的处理这些数据。快速的指出与搜索能力是需要的。通过kd-tree结构处理数据。周围平面通过RANSAC算法来拟合（RANSAC算法是一个稳健的模型拟合方法）。这个例子也展示了如何使用点云来实现多帧点云的动画过程。

选择要显示的点云区域

首先, 在车辆周围选择一个目标区域, 并配置pcplayer以显示它。

1.加载点云序列。

load('01_city_c2s_fcw_10s_Lidar.mat');
pcloud = d.LidarPointCloud;

2.为了规定车辆周围的环境, 在左右边20米(自由配置，此处选20）左右的车辆, 和50米(自由配置，此处选50）的前面和后面的车辆被纳入检测范围。

%% 设置第一帧点云格式
pc = pcloud(1).ptCloud;

%设置目标区域.
xBound = 50;
yBound = 20;
xlimits = [-xBound, xBound];
ylimits = [-yBound, yBound];
zlimits = pc.ZLimits;

player = pcplayer(xlimits, ylimits, zlimits);

% 将点云数据导入目标区域，不合规的点云将不会被导入
indices = find(pc.Location(:, 2) >= -yBound ...
& pc.Location(:, 2) <= yBound ...
& pc.Location(:, 1) >= -xBound ...
& pc.Location(:, 1) <= xBound);

% 选择并图形化精简后的点云
pc = select(pc, indices);
view(player, pc)

分离地面平面与障碍物

找到地面平面并移出地面平面。使用RANSAC算法来找到并拟合地平面。正常平面方向应该粗略指向Z轴。所有内收的点都应该在地面平面的20cm内。

maxDistance = 0.2; % 0.2米
referenceVector = [0, 0, 1];
[~, inPlanePointIndices, outliers] = pcfitplane(pc, maxDistance, referenceVector);

　　

为点云中所有的点附着一种颜色标签。使用绿色来表示地平面，红色来表示10米内LIDAR传感器能找到的障碍物。

labelSize   = [pc.Count, 1];
colorLabels = zeros(labelSize, 'single');

% 设置颜色谱来描述不同的点云
colors = [0 0 1; ...  %蓝色作为未被标签化的点云
          0 1 0; ...  % 绿色作为地面点云
          1 0 0; ...  % 红色作为障碍物点云
          0 0 0];     % 汽车点云

blueIdx  = 0; % 将所有点云初始化为蓝色
greenIdx = 1;
redIdx   = 2;
blackIdx = 3;

% 标签化地面点云
colorLabels(inPlanePointIndices) = greenIdx;

% 选择出不是地面点云的点云
pcWithoutGround = select(pc, outliers);

　　重新获得的点云在10米半径以内的障碍物

sensorLocation   = [0,0,0]; % 将LIDAR传感器放在车的中心坐标
radius           = 10;    

nearIndices  = findNeighborsInRadius(pcWithoutGround, sensorLocation, radius);
nearPointIndices = outliers(nearIndices);

% 将障碍物点云标签化
colorLabels(nearPointIndices) = redIdx;

　　分离本车雷达点云

因为雷达安装在车的本身，所有的点云数据会包括雷达他本身，比如车顶或者车身。这些点云离车最近却不是障碍物。重新获得这些包围着汽车的点云。使用这些点云来形成一个三维边界立方体来代表本车。

radius      = 3;
nearIndices = findNeighborsInRadius(pcWithoutGround, sensorLocation, radius);

vehiclePointIndices = outliers(nearIndices);
pcVehicle           = select(pcWithoutGround, nearIndices);

　　形成一个三维立方体并标签化本车点云

delta = 0.1;
selfCube = [pcVehicle.XLimits(1)-delta, pcVehicle.XLimits(2)+delta ...
            pcVehicle.YLimits(1)-delta, pcVehicle.YLimits(2)+delta ...
            pcVehicle.ZLimits(1)-delta, pcVehicle.ZLimits(2)+delta];

colorLabels(vehiclePointIndices) = blackIdx;

　　显示所有被标签化的点云进入点云播放器，使用我们之前设定的数字化的标签。

colormap(player.Axes, colors)
view(player, pc.Location, colorLabels);
title(player.Axes, 'Segmented Point Cloud');

　　

处理点云序列（注：形成点云播放器）

现在我们有了点云播放器，并已经配置好它并且已经处理好标签化点云过程。现在开始处理整个点云序列。

for k = 2:length(pcloud)
    pc = pcloud(k).ptCloud;

    % 将下一帧的点云数据导入
    indices = find(pc.Location(:, 2) >= -yBound ...
                 & pc.Location(:, 2) <=  yBound ...
                 & pc.Location(:, 1) >= -xBound ...
                 & pc.Location(:, 1) <=  xBound);
    pc = select(pc, indices);

    colorLabels = zeros(pc.Count, 1, 'single'); % 创造标签阵列

    % 拟合地面平面
    [~, inPlanePointIndices, outliers] = pcfitplane(pc, maxDistance, referenceVector);
    colorLabels(inPlanePointIndices) = greenIdx;

    pcWithoutGround = select(pc, outliers);

    % 找到与障碍物相关的点
    radius           = 10;
    nearIndices      = findNeighborsInRadius(pcWithoutGround, sensorLocation, radius);
    nearPointIndices = outliers(nearIndices);

    colorLabels(nearPointIndices) = redIdx;

    %找到与本车相关的点
    nearIndices         = findPointsInROI(pcWithoutGround, selfCube);
    vehiclePointIndices = outliers(nearIndices);

    colorLabels(vehiclePointIndices) = blackIdx;

    % 显示结果
    view(player, pc.Location, colorLabels);
end

　　这样你就可以看到连续的点云处理界面。