LiDAR を使用した車両の検出、分類、および追跡

この例では、エゴ ビークルに取り付けられている LiDAR センサーによって取得された LiDAR 点群データを使用して、車両を検出、分類、追跡する方法を説明します。この例で使用する LiDAR データは、ハイウェイ走行シナリオから記録されています。この例では、PointSeg ネットワークを使用して、オブジェクトのクラスを特定するために点群データをセグメント化します。Interactive Multiple Model フィルターを含む Joint Probabilistic Data Association (JPDA) トラッカーを使用して、検出された車両を追跡します。

概要

ADAS システムを搭載した車両の完全な自律運転を達成する上で、知覚モジュールは重要な役割を果たします。LiDAR とカメラは、知覚ワークフローに不可欠なセンサーです。LiDAR はオブジェクトの正確な深さ情報の抽出に適している一方で、カメラはオブジェクトの分類に便利な環境の豊富かつ詳細な情報を生成します。

この例に含まれる主なパートは次のとおりです。

• 地面のセグメント化

• セマンティック セグメンテーション

• 有向境界ボックスの当てはめ

• 有向境界ボックスの追跡

次のフローチャートにシステム全体の概要を示します。

データの読み込み

LiDAR センサーは、オーガナイズド形式またはアンオーガナイズド形式で点群データを生成します。この例で使用するデータは、Ouster OS1 LiDAR センサーを使用して収集されています。この LiDAR は、64 本の水平走査線でオーガナイズド点群を生成します。点群データは、点の "x" 座標、"y" 座標、"z" 座標を表す 3 つのチャネルで構成されます。各チャネルのサイズは 64 x 1024 です。補助関数 helperDownloadData を使用してデータをダウンロードし、MATLAB® ワークスペースに読み込みます。

メモ: このダウンロードには数分かかることがあります。

[ptClouds,pretrainedModel] = helperDownloadData;

地面のセグメント化

この例では、関数segmentGroundFromLidarDataおよびpcfitplaneを使用するハイブリッド アプローチを採用します。まず、関数 segmentGroundFromLidarData を使用して、地面のパラメーターを推定します。推定された地面は、平面に当てはめるために、車両の方向に沿ってストリップに分割されます。その際、各ストリップで関数 pcfitplane を使用します。このハイブリッド アプローチでは、地面を区分的な方法でロバストに当てはめ、点群内のバリエーションを処理します。

% Load point cloud
ptCloud = ptClouds{1};
% Define ROI for cropping point cloud
xLimit = [-30,30];
yLimit = [-12,12];
zLimit = [-3,15];

roi = [xLimit,yLimit,zLimit];
% Extract ground plane
[nonGround,ground] = helperExtractGround(ptCloud,roi);
figure;
pcshowpair(nonGround,ground);
axis on;
legend({'\color{white} Nonground','\color{white} Ground'},'Location','northeastoutside');

セマンティック セグメンテーション

この例では、事前学習済みの PointSeg ネットワーク モデルを使用します。PointSeg は、自動車、トラック、背景などのオブジェクト クラス用に学習させた、エンドツーエンドのリアルタイム セマンティック セグメンテーション ネットワークです。このネットワークからの出力は、各ピクセルにクラスごとのラベルが付けられたマスク イメージです。このマスクを使用して、点群内のさまざまなタイプのオブジェクトがフィルター処理されます。ネットワークへの入力は、"x、y、z、強度、範囲" の 5 チャネルのイメージです。ネットワークの詳細について、またネットワークに学習させる方法については、Lidar Point Cloud Semantic Segmentation Using PointSeg Deep Learning Networkの例を参照してください。

入力データの準備

関数 helperPrepareData は、読み込んだ点群データから 5 チャネルのデータを生成します。

% Load and visualize a sample frame
frame = helperPrepareData(ptCloud);
figure;
subplot(5,1,1);
imagesc(frame(:,:,1));
title('X channel');

subplot(5,1,2);
imagesc(frame(:,:,2));
title('Y channel');

subplot(5,1,3);
imagesc(frame(:,:,3));
title('Z channel');

subplot(5,1,4);
imagesc(frame(:,:,4));
title('Intensity channel');

subplot(5,1,5);
imagesc(frame(:,:,5));
title('Range channel');

読み込んだ事前学習済みのネットワークの 1 つのフレームで前向き推論を実行します。

if ~exist('net','var')
    net = pretrainedModel.net;
end

% Define classes
classes = ["background","car","truck"];

% Define color map
lidarColorMap = [
            0.98  0.98   0.00  % unknown
            0.01  0.98   0.01  % green color for car
            0.01  0.01   0.98  % blue color for motorcycle
            ];

% Run forward pass
pxdsResults = semanticseg(frame,net);

% Overlay intensity image with segmented output
segmentedImage = labeloverlay(uint8(frame(:,:,4)),pxdsResults,'Colormap',lidarColorMap,'Transparency',0.5);

% Display results
figure;
imshow(segmentedImage);
helperPixelLabelColorbar(lidarColorMap,classes);

生成されたセマンティック マスクを使用して、トラックを含む点群をフィルター処理します。同様に、その他のクラスの点群をフィルター処理します。

truckIndices = pxdsResults == 'truck';
truckPointCloud = select(nonGround,truckIndices,'OutputSize','full');

% Crop point cloud for better display
croppedPtCloud = select(ptCloud,findPointsInROI(ptCloud,roi));
croppedTruckPtCloud = select(truckPointCloud,findPointsInROI(truckPointCloud,roi));

% Display ground and nonground points
figure;
pcshowpair(croppedPtCloud,croppedTruckPtCloud);
axis on;
legend({'\color{white} Nonvehicle','\color{white} Vehicle'},'Location','northeastoutside');

クラスタリングと境界ボックスの当てはめ

さまざまなオブジェクト クラスの点群を抽出後に、オブジェクトは、関数pcsegdistを使用してユークリッド クラスタリングを適用することによってクラスター化されます。単一のクラスターに属するすべての点をグループ化するために、クラスターとして取得された点群が、地面以外の点で領域を拡張するためのシード点として使用されます。領域を拡張するには、関数findNearestNeighborsを使用して、すべての点をループ処理します。抽出されたクラスターは、関数pcfitcuboidを使用して、L 字型の境界ボックスに当てはめられます。これらの車両のクラスターは、上から見ると L の字の形状に似ています。この特徴は、車両の向きを推定する際に役立ちます。有向境界ボックスの当てはめは、オブジェクトの向首角を推定する際に役立ちます。これは、パス計画や交通処理などの用途で便利です。

クラスターの直方体境界は、各方向の最小、最大の空間範囲を求めることによっても計算できます。ただし、この方法では、検出された車両の向きを推定することはできません。2 つの方法の違いを次の図に示します。

[labels,numClusters] = pcsegdist(croppedTruckPtCloud,1);

% Define cuboid parameters
params = zeros(0,9);

for clusterIndex = 1:numClusters
    ptsInCluster = labels == clusterIndex;
        
    pc = select(croppedTruckPtCloud,ptsInCluster);
    location = pc.Location;
    
    xl = (max(location(:,1)) - min(location(:,1)));
    yl = (max(location(:,2)) - min(location(:,2)));
    zl = (max(location(:,3)) - min(location(:,3)));
    
    % Filter small bounding boxes
    if size(location,1)*size(location,2) > 20 && any(any(pc.Location)) && xl > 1 && yl > 1
        indices = zeros(0,1);
        objectPtCloud = pointCloud(location);        
        for i = 1:size(location,1)
            seedPoint = location(i,:);
            indices(end+1) = findNearestNeighbors(nonGround,seedPoint,1);
        end
        
        % Remove overlapping indices        
        indices = unique(indices);
        
        % Fit oriented bounding box
        model = pcfitcuboid(select(nonGround,indices));
        params(end+1,:) = model.Parameters;
    end
end

% Display point cloud and detected bounding box
figure;
pcshow(croppedPtCloud.Location,croppedPtCloud.Location(:,3));
showShape('cuboid',params,"Color","red","Label","Truck");
axis on;

可視化の設定

helperLidarObjectDetectionDisplay クラスを使用して、完全なワークフローを 1 つのウィンドウで可視化します。可視化ウィンドウのレイアウトは次のセクションに分割されます。

1. Lidar Range Image: レンジ イメージとしての 2 次元の点群イメージ

2. Segmented Image: 強度イメージ、つまりデータの 4 番目のチャネルを重ね合わせたセマンティック セグメンテーション ネットワークから生成された検出ラベル

3. Oriented Bounding Box Detection: 有向境界ボックスを含む 3 次元点群

4. Top View: 有向境界ボックスを含む点群の上面ビュー

display = helperLidarObjectDetectionDisplay;

データ内のループ

helperLidarObjectDetection クラスは、これまでの節で説明したセグメンテーション、クラスタリング、境界ボックスの当てはめのステップをすべてカプセル化するラッパーです。関数 findDetections を使用して、検出されたオブジェクトを抽出します。

% Initialize lidar object detector
lidarDetector = helperLidarObjecDetector('Model',net,'XLimits',xLimit,...
    'YLimit',yLimit,'ZLimit',zLimit);

% Prepare 5-D lidar data
inputData = helperPrepareData(ptClouds);

% Set random number generator for reproducible results
S = rng(2018);

% Initialize the display
initializeDisplay(display);

numFrames = numel(inputData);
for count = 1:numFrames
    
    % Get current data
    input = inputData{count};
    
    rangeImage = input(:,:,5);
    
    % Extact bounding boxes from lidar data
    [boundingBox,coloredPtCloud,pointLabels] = detectBbox(lidarDetector,input);
            
    % Update display with colored point cloud
    updatePointCloud(display,coloredPtCloud);
    
    % Update bounding boxes
    updateBoundingBox(display,boundingBox);
    
    % Update segmented image 
    updateSegmentedImage(display,pointLabels,rangeImage);
    
    drawnow('limitrate');
end

有向境界ボックスの追跡

この例では、Joint Probabilistic Data Association (JPDA) トラッカーを使用します。データセットが 10 Hz で取得されるため、タイム ステップ dt は 0.1 秒に設定されています。トラッカーで使用する状態空間モデルは、パラメーター $\left[\mathit{x},\mathit{y},\mathit{z},\varphi ,\mathit{l},\mathit{w},\mathit{h}\right]$ をもつ直方体モデルに基づいています。LiDAR データで境界ボックスを追跡する方法の詳細については、Track Vehicles Using Lidar: From Point Cloud to Track List (Sensor Fusion and Tracking Toolbox)の例を参照してください。この例では、クラス情報は objectDetection object の ObjectAttributes プロパティを使用して提供されます。新しい追跡を作成するとき、補助関数 helperMultiClassInitIMMFilter を使用して定義されるフィルター初期化関数は、検出のクラスを使用してオブジェクトの初期次元を設定します。これにより、トラッカーは適切な追跡の次元で境界ボックス測定モデルを調整できます。

次のパラメーターを指定して JPDA トラッカー オブジェクトを設定します。

assignmentGate = [10 100]; % Assignment threshold;
confThreshold = [7 10];    % Confirmation threshold for history logi
delThreshold = [2 3];     % Deletion threshold for history logic
Kc = 1e-5;                 % False-alarm rate per unit volume

% IMM filter initialization function
filterInitFcn = @helperMultiClassInitIMMFilter;

% A joint probabilistic data association tracker with IMM filter
tracker = trackerJPDA('FilterInitializationFcn',filterInitFcn,...
    'TrackLogic','History',...
    'AssignmentThreshold',assignmentGate,...
    'ClutterDensity',Kc,...
    'ConfirmationThreshold',confThreshold,...
    'DeletionThreshold',delThreshold,'InitializationThreshold',0);

allTracks = struct([]);
time = 0;
dt = 0.1;

% Define Measurement Noise
measNoise = blkdiag(0.25*eye(3),25,eye(3));

numTracks = zeros(numFrames,2);

検出されたオブジェクトは、関数 helperAssembleDetections を使用して、objectDetection (Automated Driving Toolbox)オブジェクトの cell 配列として組み立てられます。

display = helperLidarObjectDetectionDisplay;
initializeDisplay(display);

for count = 1:numFrames
    time = time + dt;
    % Get current data
    input = inputData{count};
    
    rangeImage = input(:,:,5);
    
    % Extact bounding boxes from lidar data
    [boundingBox,coloredPtCloud,pointLabels] = detectBbox(lidarDetector,input);
    
    % Assemble bounding boxes into objectDetections
    detections = helperAssembleDetections(boundingBox,measNoise,time);
    
    % Pass detections to tracker
    if ~isempty(detections)
        % Update the tracker
         [confirmedTracks,tentativeTracks,allTracks,info] = tracker(detections,time);
         numTracks(count,1) = numel(confirmedTracks);
    end
    
    % Update display with colored point cloud
    updatePointCloud(display,coloredPtCloud);
            
    % Update segmented image 
    updateSegmentedImage(display,pointLabels,rangeImage);
    
    % Update the display if the tracks are not empty
     if ~isempty(confirmedTracks)
        updateTracks(display,confirmedTracks);
     end
     
    drawnow('limitrate');
end

まとめ

この例では、LiDAR データで有向境界ボックスに当てはめられた車両を検出して分類する方法を説明しました。また、IMM フィルターを使用して複数のクラス情報でオブジェクトを追跡する方法も学習しました。セマンティック セグメンテーションの結果は、より多くの学習データを追加することによって、さらに改善できます。

サポート関数

helperPrepareData

function multiChannelData = helperPrepareData(input)
% Create 5-channel data as x, y, z, intensity and range
% of size 64-by-1024-by-5 from pointCloud.

if isa(input, 'cell')
    numFrames = numel(input);
    multiChannelData = cell(1, numFrames);
    for i = 1:numFrames
        inputData = input{i};
        
        x = inputData.Location(:,:,1);
        y = inputData.Location(:,:,2);
        z = inputData.Location(:,:,3);
        
        intensity = inputData.Intensity;
        range = sqrt(x.^2 + y.^2 + z.^2);
        
        multiChannelData{i} = cat(3, x, y, z, intensity, range);
    end
else
    x = input.Location(:,:,1);
    y = input.Location(:,:,2);
    z = input.Location(:,:,3);
    
    intensity = input.Intensity;
    range = sqrt(x.^2 + y.^2 + z.^2);
    
    multiChannelData = cat(3, x, y, z, intensity, range);
end
end

pixelLabelColorbar

function helperPixelLabelColorbar(cmap, classNames)
% Add a colorbar to the current axis. The colorbar is formatted
% to display the class names with the color.

colormap(gca,cmap)

% Add colorbar to current figure.
c = colorbar('peer', gca);

% Use class names for tick marks.
c.TickLabels = classNames;
numClasses = size(cmap,1);

% Center tick labels.
c.Ticks = 1/(numClasses*2):1/numClasses:1;

% Remove tick mark.
c.TickLength = 0;
end

helperExtractGround

function [ptCloudNonGround,ptCloudGround] = helperExtractGround(ptCloudIn,roi)
% Crop the point cloud

idx = findPointsInROI(ptCloudIn,roi);
pc = select(ptCloudIn,idx,'OutputSize','full');

% Get the ground plane the indices using piecewise plane fitting
[ptCloudGround,idx] = piecewisePlaneFitting(pc,roi);

nonGroundIdx = true(size(pc.Location,[1,2]));
nonGroundIdx(idx) = false;
ptCloudNonGround = select(pc,nonGroundIdx,'OutputSize','full');
end


function [groundPlane,idx] = piecewisePlaneFitting(ptCloudIn,roi)
groundPtsIdx = ...
    segmentGroundFromLidarData(ptCloudIn, ...
    'ElevationAngleDelta',5,'InitialElevationAngle',15);
groundPC = select(ptCloudIn,groundPtsIdx,'OutputSize','full');

% Divide x-axis in 3 regions
segmentLength = (roi(2) - roi(1))/3;

x1 = [roi(1),roi(1) + segmentLength];
x2 = [x1(2),x1(2) + segmentLength];
x3 = [x2(2),x2(2) + segmentLength];

roi1 = [x1,roi(3:end)];
roi2 = [x2,roi(3:end)];
roi3 = [x3,roi(3:end)];

idxBack = findPointsInROI(groundPC,roi1);
idxCenter = findPointsInROI(groundPC,roi2);
idxForward = findPointsInROI(groundPC,roi3);

% Break the point clouds in front and back
ptBack = select(groundPC,idxBack,'OutputSize','full');

ptForward = select(groundPC,idxForward,'OutputSize','full');

[~,inliersForward] = planeFit(ptForward);
[~,inliersBack] = planeFit(ptBack);
idx = [inliersForward; idxCenter; inliersBack];
groundPlane = select(ptCloudIn, idx,'OutputSize','full');
end

function [plane,inlinersIdx] = planeFit(ptCloudIn)
[~,inlinersIdx, ~] = pcfitplane(ptCloudIn,1,[0, 0, 1]);
plane = select(ptCloudIn,inlinersIdx,'OutputSize','full');
end

helperAssembleDetections

function mydetections = helperAssembleDetections(bboxes,measNoise,timestamp)
% Assemble bounding boxes as cell array of objectDetection

mydetections = cell(size(bboxes,1),1);
for i = 1:size(bboxes,1)
    classid = bboxes(i,end);
    lidarModel = [bboxes(i,1:3), bboxes(i,end-1), bboxes(i,4:6)];
    % To avoid direct confirmation by the tracker, the ClassID is passed as
    % ObjectAttributes.
    mydetections{i} = objectDetection(timestamp, ...
        lidarModel','MeasurementNoise',...
        measNoise,'ObjectAttributes',struct('ClassID',classid));
end
end

helperDownloadData

function [lidarData, pretrainedModel] = helperDownloadData
outputFolder = fullfile(tempdir,'WPI');
url = 'https://ssd.mathworks.com/supportfiles/lidar/data/lidarSegmentationAndTrackingData.tar.gz';
lidarDataTarFile = fullfile(outputFolder,'lidarSegmentationAndTrackingData.tar.gz');
if ~exist(lidarDataTarFile,'file')
    mkdir(outputFolder);
    websave(lidarDataTarFile,url);
    untar(lidarDataTarFile,outputFolder);
end
% Check if tar.gz file is downloaded, but not uncompressed
if ~exist(fullfile(outputFolder,'highwayData.mat'),'file')
    untar(lidarDataTarFile,outputFolder);
end
% Load lidar data
data = load(fullfile(outputFolder,'highwayData.mat'));
lidarData = data.ptCloudData;

% Download pretrained model
url = 'https://ssd.mathworks.com/supportfiles/lidar/data/pretrainedPointSegModel.mat';
modelFile = fullfile(outputFolder,'pretrainedPointSegModel.mat');
if ~exist(modelFile,'file')
    websave(modelFile,url);
end
pretrainedModel = load(fullfile(outputFolder,'pretrainedPointSegModel.mat'));
end

参考文献

[1] Xiao Zhang, Wenda Xu, Chiyu Dong and John M. Dolan, "Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners", IEEE Intelligent Vehicles Symposium, June 2017

[2] Y. Wang, T. Shi, P. Yun, L. Tai, and M. Liu, “Pointseg: Real-time semantic segmentation based on 3d lidar point cloud,” arXiv preprint arXiv:1807.06288, 2018.