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

　　這樣你就可以看到連續的點雲處理界面。