LIDAR データを使用した車両の検出と追跡

この例では次を使用します。

この例では、自車の上に取り付けられた LIDAR センサーからの測定値を使用して車両を追跡する方法を示します。LIDAR センサーは測定値を点群としてレポートします。例では、点群を処理してオブジェクトを追跡する MATLAB® でのワークフローを説明します。Simulink® バージョンの例については、Track Vehicles Using Lidar Data in Simulink (Sensor Fusion and Tracking Toolbox)を参照してください。この例で使用する LIDAR データは、ハイウェイの運転シナリオから記録されています。この例では、記録されたデータを使用して、Joint Probabilistic Data Association (JPDA) トラッカーおよび Interacting Multiple Model (IMM) 法で車両を追跡します。

3 次元境界ボックス検出器モデル

LIDAR センサーの高解像度機能により、センサーからの各スキャンには一般に点群として知られている多数の点が含まれます。この生データは、自動車、自転車運転者、歩行者など、対象となるオブジェクトを抽出するために前処理しなければなりません。地面や障害物といったオブジェクトに対する LIDAR データのセグメンテーションの詳細については、LiDAR を使用した地面および障害物の検出 (Automated Driving Toolbox)の例を参照してください。この例では、障害物に属する点群は関数 pcsegdist を使用してさらにクラスターに分類され、各クラスターは次の形式をもつ境界ボックスの検出に変換されます。

$[x\ y\ z\ l\ w\ h]$

$x$ 、 $y$ および $z$ は境界ボックスの x 位置、y 位置、z 位置を参照し、 $l$ 、 $w$ および $h$ はその長さ、幅、高さをそれぞれ参照します。

境界ボックスは、各次元で点の最小および最大の座標を使用して各クラスターに収まります。検出器は、点群のセグメンテーション機能とクラスタリング機能をラップアラウンドするサポートクラス HelperBoundingBoxDetector によって実装されます。このクラスのオブジェクトは pointCloud 入力を受け入れ、境界ボックスの測定値をもつ objectDetection オブジェクトのリストを返します。

図は、境界ボックス検出器モデルに関連したプロセスと、各プロセスの実装に使用した Computer Vision Toolbox™ 関数を示します。また、各プロセスを制御するサポートクラスのプロパティも示します。

LIDAR データは、https://ssd.mathworks.com/supportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip から入手できます。

MATLAB の関数 tempdir によって指定された場所にある一時ディレクトリに、データをダウンロードします。ファイルを別のフォルダーに保存する場合は、後続の手順でディレクトリ名を変更してください。

% Load data if unavailable. The lidar data is stored as a cell array of
% pointCloud objects.
if ~exist('lidarData','var')
    dataURL = 'https://ssd.mathworks.com/supportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip';
    datasetFolder = fullfile(tempdir,'LidarExampleDataset');
    if ~exist(datasetFolder,'dir')
       unzip(dataURL,datasetFolder);
    end
    % Specify initial and final time for simulation.
    initTime = 0;
    finalTime = 35;
    [lidarData, imageData] = loadLidarAndImageData(datasetFolder,initTime,finalTime);
end

% Set random seed to generate reproducible results.
S = rng(2018);

% A bounding box detector model.
detectorModel = HelperBoundingBoxDetector(...
    'XLimits',[-50 75],...              % min-max
    'YLimits',[-5 5],...                % min-max
    'ZLimits',[-2 5],...                % min-max
    'SegmentationMinDistance',1.6,...   % minimum Euclidian distance
    'MinDetectionsPerCluster',1,...     % minimum points per cluster
    'MeasurementNoise',eye(6),...       % measurement noise in detection report
    'GroundMaxDistance',0.3);           % maximum distance of ground points from ground plane

ターゲットの状態およびセンサーの測定モデル

オブジェクトを追跡する最初のステップは、その状態、状態の遷移を定義するモデル、対応する測定を定義することです。これらの 2 つの方程式セットはまとめてターゲットの状態空間モデルと呼ばれます。LIDAR を使用して追跡する車両の状態をモデル化するために、この例では次の規則をもつ直方体モデルを使用します。

$x = [x_{kin}\ {\theta}\ l\ w\ h]$

$x_{kin}$ は動きの中心の運動学を制御する状態の部分を参照し、 $\theta$ はヨー角です。直方体の長さ、幅、および高さは、フィルターの補正段階中に推定が変化する定数としてモデル化されます。

この例では、等速度 (cv) の直方体モデルと一定の旋回率 (ct) の直方体モデルである 2 つの状態空間モデルを使用します。以下に説明するように、これらのモデルが状態の運動学部分を定義する方法は異なります。

$x_{cv} = [x\ {\dot{x}}\ y\ {\dot{y}}\ z\ {\dot{z}}\ {\theta}\ l\ w\ h]$

$x_{ct} = [x\ {\dot{x}}\ y\ {\dot{y}}\ {\dot{\theta}}\ z\ {\dot{z}}\ {\theta}\ l\ w\ h]$

これらの状態遷移の詳細については、この例で使用される関数 helperConstvelCuboid と関数 helperConstturnCuboid を参照してください。

helperCvmeasCuboid と helperCtmeasCuboid の測定モデルは、センサーが等速度と一定の旋回率の状態をそれぞれ認識し、境界ボックスの測定値を返す方法について説明しています。状態にはターゲットのサイズに関する情報が含まれるため、測定モデルには、自己オクルージョンのような影響によってセンサーが認識する、中心点のオフセットと境界ボックスの縮小による影響が含まれます [1]。この影響は追跡された車両からセンサーまでの距離に正比例する縮小係数でモデル化されます。

下図はさまざまな状態空間サンプルで動作する測定モデルを示します。オブジェクトが自車の周りを移動する際の境界ボックスの縮小と中心点のオフセットによるモデル化された影響に注目してください。

トラッカーと可視化の設定

下図は、pointCloud 入力から追跡物のリストを取得する完全なワークフローを示します。

次に、この例で使用されるトラッカーと可視化を設定します。

この例では、Joint Probabilistic Data Association トラッカー (trackerJPDA) を IMM フィルター (trackingIMM) と連動して使用し、オブジェクトを追跡します。IMM フィルターは等速度と一定の旋回率のモデルを使用し、この例に含まれているサポート関数 helperInitIMMFilter を使用して初期化されます。IMM 法は、追跡で運動モデル間を切り替え、操縦や車線変更などのイベント時に優れた推定精度を得るのに役立ちます。以下のアニメーションは、IMM フィルターの予測段階で等速度と一定の旋回率のモデルを組み合わせたことによる効果を示しています。

IMM フィルターは、各モデルがオブジェクトからの検出によって補正されると、各モデルの確率を更新します。以下のアニメーションは、車線変更イベント時の車両の推定軌跡と、それに対応する各モデルの推定確率を示しています。

トラッカーの HasDetectableTrackIDsInput プロパティを true として設定し、状態に依存する検出確率を指定できます。追跡の検出確率は、次の例の最後にリストされている関数 helperCalcDetectability で計算されます。

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

% IMM filter initialization function
filterInitFcn = @helperInitIMMFilter;

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

可視化は次の主なカテゴリに分かれています。

Lidar Preprocessing and Tracking - このディスプレイには、生の点群、セグメント化された地面、障害物が表示されます。また、検出器モデルから得られた結果の検出と、トラッカーで生成された車両の追跡物も表示されます。
Ego Vehicle Display - このディスプレイにはシナリオの 2 次元鳥観図ビューが表示されます。障害物の点群、境界ボックスの検出、トラッカーで生成された追跡物が表示されます。また、参考用に、自車に取り付けられたカメラから記録されたイメージとその視野も表示されます。
Tracking Details - このディスプレイには自車の周りをズームしたシナリオが表示されます。各追跡物の推定位置における誤差の共分散や、cv および ct で表されるその運動モデルの確率など、より細かい追跡の詳細も表示されます。

% Create display
displayObject = HelperLidarExampleDisplay(imageData{1},...
    'PositionIndex',[1 3 6],...
    'VelocityIndex',[2 4 7],...
    'DimensionIndex',[9 10 11],...
    'YawIndex',8,...
    'MovieName','',...  % Specify a movie name to record a movie.
    'RecordGIF',false); % Specify true to record new GIFs

データ内のループ

記録した LIDAR データ内をループし、検出器モデルを使用して現在の点群から検出を生成した後、トラッカーを使用して検出を処理します。

time = 0;       % Start time
dT = 0.1;       % Time step

% Initiate all tracks.
allTracks = struct([]);

% Initiate variables for comparing MATLAB and MEX simulation.
numTracks = zeros(numel(lidarData),2);

% Loop through the data
for i = 1:numel(lidarData)
    % Update time
    time = time + dT;

    % Get current lidar scan
    currentLidar = lidarData{i};

    % Generator detections from lidar scan.
    [detections,obstacleIndices,groundIndices,croppedIndices] = detectorModel(currentLidar,time);

    % Calculate detectability of each track.
    detectableTracksInput = helperCalcDetectability(allTracks,[1 3 6]);

    % Pass detections to track.
    [confirmedTracks,tentativeTracks,allTracks] = tracker(detections,time,detectableTracksInput);
    numTracks(i,1) = numel(confirmedTracks);

    % Get model probabilities from IMM filter of each track using
    % getTrackFilterProperties function of the tracker.
    modelProbs = zeros(2,numel(confirmedTracks));
    for k = 1:numel(confirmedTracks)
        c1 = getTrackFilterProperties(tracker,confirmedTracks(k).TrackID,'ModelProbabilities');
        modelProbs(:,k) = c1{1};
    end

    % Update display
    if isvalid(displayObject.PointCloudProcessingDisplay.ObstaclePlotter)
        % Get current image scan for reference image
        currentImage = imageData{i};

        % Update display object
        displayObject(detections,confirmedTracks,currentLidar,obstacleIndices,...
            groundIndices,croppedIndices,currentImage,modelProbs);
    end

    % Snap a figure at time = 18
    if abs(time - 18) < dT/2
        snapnow(displayObject);
    end
end

% Write movie if requested
if ~isempty(displayObject.MovieName)
    writeMovie(displayObject);
end

% Write new GIFs if requested.
if displayObject.RecordGIF
    % second input is start frame, third input is end frame and last input
    % is a character vector specifying the panel to record.
    writeAnimatedGIF(displayObject,10,170,'trackMaintenance','ego');
    writeAnimatedGIF(displayObject,310,330,'jpda','processing');
    writeAnimatedGIF(displayObject,150,180,'imm','details');
end

上記の図に、時間 = 18 秒の 3 つのディスプレイを示します。追跡物は緑の境界ボックスとして表されます。境界ボックスの検出はオレンジの境界ボックスとして表されます。検出の内部には、障害物としてセグメント化された点群を表すオレンジの点もあります。セグメント化された地面は紫で示されます。トリミングまたは破棄された点群は青で示されます。

C コードを生成する

MATLAB Coder™ を使用して、MATLAB® コードから追跡および前処理アルゴリズム用の C コードを生成できます。C コード生成により、シミュレーション用に MATLAB コードを高速化できます。C コードを生成するには、アルゴリズムを MEX ファイルまたは共有ライブラリにコンパイルできる MATLAB 関数として再構成しなければなりません。この目的で、点群処理アルゴリズムと追跡アルゴリズムは MATLAB 関数 mexLidarTracker に再構成されます。一部の変数は、関数に対する複数の呼び出し間で状態を維持するように persistent として定義されます (persistentを参照)。関数の入力と出力は、この例の最後で「サポート対象のファイル」の節に記載されている関数の説明で確認できます。

MATLAB Coder では、すべての入力引数のプロパティを指定しなければなりません。これを簡単に行うには、コマンドラインで -args オプションを使用して、入力プロパティを例で定義します。詳細については、コマンドラインでの例による入力プロパティの定義 (MATLAB Coder)を参照してください。最上位の入力引数は handle クラスのオブジェクトにできないことに注意してください。したがって、関数は点群の x、y および z の位置を入力として受け入れます。保存された点群から、pointCloud オブジェクトの Location プロパティを使用してこの情報を抽出できます。この情報も LIDAR センサーからの生データとして直接使用できます。

% Input lists
inputExample = {lidarData{1}.Location, 0};

% Create configuration for MEX generation
cfg = coder.config('mex');

% Replace cfg with the following to generate static library and perform
% software-in-the-loop simulation. This requires Embedded Coder license.
%
% cfg = coder.config('lib'); % Static library
% cfg.VerificationMode = 'SIL'; % Software-in-the-loop

% Generate code if file does not exist.
if ~exist('mexLidarTracker_mex','file')
    h = msgbox({'Generating code. This may take a few minutes...';'This message box will close when done.'},'Codegen Message');
    % -config allows specifying the codegen configuration
    % -o allows specifying the name of the output file
    codegen -config cfg -o mexLidarTracker_mex mexLidarTracker -args inputExample
    close(h);
else
    clear mexLidarTracker_mex;
end

MEX コードによるシミュレーションの再実行

生成された MEX コード mexLidarTracker_mex を使用してシミュレーションを再実行します。

% Reset time
time = 0;

for i = 1:numel(lidarData)
    time = time + dT;

    currentLidar = lidarData{i};

    [detectionsMex,obstacleIndicesMex,groundIndicesMex,croppedIndicesMex,...
        confirmedTracksMex, modelProbsMex] = mexLidarTracker_mex(currentLidar.Location,time);

    % Record data for comparison with MATLAB execution.
    numTracks(i,2) = numel(confirmedTracksMex);
end

MATLAB と MEX 実行間の結果を比較する

disp(isequal(numTracks(:,1),numTracks(:,2)));

確認された追跡物の数が MATLAB と MEX のコード実行で同じであることに注目してください。これにより、LIDAR の前処理および追跡アルゴリズムから、生成された C コードと MATLAB コードで同じ結果が返されるようになります。

結果

今度は、シナリオでさまざまなイベントを解析し、LIDAR 測定モデル、Joint Probabilistic Data Association、Interacting Multiple Model フィルターの組み合わせが車両追跡を適切に推測するのにどのように役立つかを理解します。

追跡物の維持

上のアニメーションは時間 = 3 秒と時間 = 16 秒間のシミュレーションを示します。T10 と T6 などの追跡物は、時間範囲の間、その ID と軌跡を維持することに注目してください。ただし、追跡された車両がセンサーによって長時間見つからなかった (検出されなかった) ため、追跡物 T9 は失われます。また、車両の表示可能な部分に検出を配置すると、追跡されたオブジェクトがその形状と運動学的な中心を維持できることに注目してください。たとえば、追跡物 T7 が前進すると、境界ボックスの検出は表示可能な後部の部分にかかり始め、追跡物は車両の実際のサイズを維持します。これは、測定関数でモデル化されたオフセットと縮小の影響を示しています。

操縦のキャプチャ

このアニメーションは、IMM フィルターを使用するとトラッカーが操縦車両上で追跡物を維持しやすくなることを示しています。T4 で追跡される車両が自車の後ろで車線を変更することに注目してください。トラッカーはこの操縦イベント中に車両の追跡物を維持できます。また、ディスプレイ内で、ct で表される次の一定の旋回のモデルの確率が車線変更操縦中に増加することにも注目してください。

Joint Probabilistic Data Association

このアニメーションは、Joint Probabilistic Data Association トラッカーを使用すると、あいまいな状況で追跡物を維持しやすくなることを示しています。ここで、T24 と T62 で追跡される車両はセンサーからの距離が大きいため、低い検出確率をもちます。トラッカーは、いずれか 1 つの車両が検出されない場合、イベント中に追跡物を維持できることに注目してください。イベント中、追跡物は最初に結合し (JPDA の既知の現象)、車両が再度検出されるとすぐに分離します。

まとめ

この例では、JPDA トラッカーを IMM フィルターと共に使用し、LIDAR センサーを使用してオブジェクトを追跡する方法を示しました。生の点群を前処理し、各センサースキャンあたりのオブジェクトごとに 1 つの検出を想定する従来のトラッカーに対して検出を生成する方法を学習しました。また、直方体モデルを定義して、JPDA トラッカーで追跡される拡張オブジェクトの運動学、次元、測定値を記述する方法についても学習しました。さらに、アルゴリズムから C コードを生成し、MATLAB シミュレーションでその実行結果を検証しました。

サポート対象のファイル

helperLidarModel

この関数は、LIDAR モデルを定義して境界ボックスの測定値の縮小と中心点のオフセットをシミュレートする LIDAR モデルを定義します。この関数を関数 helperCvmeasCuboid と関数 helperCtmeasCuboid で使用して、状態から境界ボックスの測定値を取得します。

function meas = helperLidarModel(pos,dim,yaw)
% This function returns the expected bounding box measurement given an
% object's position, dimension, and yaw angle.

% Copyright 2019 The MathWorks, Inc.

% Get x,y and z.
x = pos(1,:);
y = pos(2,:);
z = pos(3,:) - 2; % lidar mounted at height = 2 meters.

% Get spherical measurement.
[az,~,r] = cart2sph(x,y,z);

% Shrink rate
s = 3/50; % 3 meters radial length at 50 meters.
sz = 2/50; % 2 meters height at 50 meters.

% Get length, width and height.
L = dim(1,:);
W = dim(2,:);
H = dim(3,:);

az = az - deg2rad(yaw);

% Shrink length along radial direction.
Lshrink = min(L,abs(s*r.*(cos(az))));
Ls = L - Lshrink;

% Shrink width along radial direction.
Wshrink = min(W,abs(s*r.*(sin(az))));
Ws = W - Wshrink;

% Shrink height.
Hshrink = min(H,sz*r);
Hs = H - Hshrink;

% Measurement is given by a min-max detector hence length and width must be
% projected along x and y.
Lmeas = Ls.*cosd(yaw) + Ws.*sind(yaw);
Wmeas = Ls.*sind(yaw) + Ws.*cosd(yaw);

% Similar shift is for x and y directions.
shiftX = Lshrink.*cosd(yaw) + Wshrink.*sind(yaw);
shiftY = Lshrink.*sind(yaw) + Wshrink.*cosd(yaw);
shiftZ = Hshrink;

% Modeling the affect of box origin offset
x = x - sign(x).*shiftX/2;
y = y - sign(y).*shiftY/2;
z = z + shiftZ/2 + 2;

% Measurement format
meas = [x;y;z;Lmeas;Wmeas;Hs];

end

helperInverseLidarModel

この関数は、境界ボックスの測定値を使用してトラッキングフィルターを開始する逆 LIDAR モデルを定義します。この関数を関数 helperInitIMMFilter で使用して、境界ボックスの測定値から状態推定値を取得します。

function [pos,posCov,dim,dimCov,yaw,yawCov] = helperInverseLidarModel(meas,measCov)
% This function returns the position, dimension, yaw using a bounding
% box measurement.

% Copyright 2019 The MathWorks, Inc.

% Shrink rate.
s = 3/50;
sz = 2/50;

% x,y and z of measurement
x = meas(1,:);
y = meas(2,:);
z = meas(3,:);

[az,~,r] = cart2sph(x,y,z);

% Shift x and y position.
Lshrink = abs(s*r.*(cos(az)));
Wshrink = abs(s*r.*(sin(az)));
Hshrink = sz*r;

shiftX = Lshrink;
shiftY = Wshrink;
shiftZ = Hshrink;

x = x + sign(x).*shiftX/2;
y = y + sign(y).*shiftY/2;
z = z + sign(z).*shiftZ/2;

pos = [x;y;z];
posCov = measCov(1:3,1:3,:);

yaw = zeros(1,numel(x),'like',x);
yawCov = ones(1,1,numel(x),'like',x);

% Dimensions are initialized for a standard passenger car with low
% uncertainity.
dim = [4.7;1.8;1.4];
dimCov = 0.01*eye(3);
end

HelperBoundingBoxDetector

これは、点群入力を受け入れて objectDetection のリストを返すサポートクラス HelperBoundingBoxDetector です

classdef HelperBoundingBoxDetector < matlab.System
    % HelperBoundingBoxDetector A helper class to segment the point cloud
    % into bounding box detections.
    % The step call to the object does the following things:
    %
    % 1. Removes point cloud outside the limits.
    % 2. From the survived point cloud, segments out ground
    % 3. From the obstacle point cloud, forms clusters and puts bounding
    %    box on each cluster.
    
    % Cropping properties
    properties
        % XLimits XLimits for the scene
        XLimits = [-70 70];
        % YLimits YLimits for the scene
        YLimits = [-6 6];
        % ZLimits ZLimits fot the scene
        ZLimits = [-2 10];
    end
   
    % Ground Segmentation Properties
    properties
        % GroundMaxDistance Maximum distance of point to the ground plane
        GroundMaxDistance = 0.3;
        % GroundReferenceVector Reference vector of ground plane
        GroundReferenceVector = [0 0 1];
        % GroundMaxAngularDistance Maximum angular distance of point to reference vector
        GroundMaxAngularDistance = 5;
    end
    
    % Bounding box Segmentation properties
    properties
        % SegmentationMinDistance Distance threshold for segmentation
        SegmentationMinDistance = 1.6;
        % MinDetectionsPerCluster Minimum number of detections per cluster
        MinDetectionsPerCluster = 2;
        % MaxZDistanceCluster Maximum Z-coordinate of cluster
        MaxZDistanceCluster = 3;
        % MinZDistanceCluster Minimum Z-coordinate of cluster
        MinZDistanceCluster = -3;
    end
    
    % Ego vehicle radius to remove ego vehicle point cloud.
    properties
        % EgoVehicleRadius Radius of ego vehicle
        EgoVehicleRadius = 3;
    end
    
    properties
        % MeasurementNoise Measurement noise for the bounding box detection
        MeasurementNoise = blkdiag(eye(3),eye(3));
    end
    
    properties (Nontunable)
        MeasurementParameters = struct.empty(0,1);
    end
    
    methods 
        function obj = HelperBoundingBoxDetector(varargin)
            setProperties(obj,nargin,varargin{:})
        end
    end
    
    methods (Access = protected)
        function [bboxDets,obstacleIndices,groundIndices,croppedIndices] = stepImpl(obj,currentPointCloud,time)
            % Crop point cloud
            [pcSurvived,survivedIndices,croppedIndices] = cropPointCloud(currentPointCloud,obj.XLimits,obj.YLimits,obj.ZLimits,obj.EgoVehicleRadius);
            % Remove ground plane
            [pcObstacles,obstacleIndices,groundIndices] = removeGroundPlane(pcSurvived,obj.GroundMaxDistance,obj.GroundReferenceVector,obj.GroundMaxAngularDistance,survivedIndices);
            % Form clusters and get bounding boxes
            detBBoxes = getBoundingBoxes(pcObstacles,obj.SegmentationMinDistance,obj.MinDetectionsPerCluster,obj.MaxZDistanceCluster,obj.MinZDistanceCluster);
            % Assemble detections
            if isempty(obj.MeasurementParameters)
                measParams = {};
            else
                measParams = obj.MeasurementParameters;
            end
            bboxDets = assembleDetections(detBBoxes,obj.MeasurementNoise,measParams,time);
        end
    end
end
    
function detections = assembleDetections(bboxes,measNoise,measParams,time)
% This method assembles the detections in objectDetection format.
numBoxes = size(bboxes,2);
detections = cell(numBoxes,1);
for i = 1:numBoxes
    detections{i} = objectDetection(time,cast(bboxes(:,i),'double'),...
        'MeasurementNoise',double(measNoise),'ObjectAttributes',struct,...
        'MeasurementParameters',measParams);
end
end

function bboxes = getBoundingBoxes(ptCloud,minDistance,minDetsPerCluster,maxZDistance,minZDistance)
    % This method fits bounding boxes on each cluster with some basic
    % rules.
    % Cluster must have atleast minDetsPerCluster points.
    % Its mean z must be between maxZDistance and minZDistance.
    % length, width and height are calculated using min and max from each
    % dimension.
    [labels,numClusters] = pcsegdist(ptCloud,minDistance);
    pointData = ptCloud.Location;
    bboxes = nan(6,numClusters,'like',pointData);
    isValidCluster = false(1,numClusters);
    for i = 1:numClusters
        thisPointData = pointData(labels == i,:);
        meanPoint = mean(thisPointData,1);
        if size(thisPointData,1) > minDetsPerCluster && ...
                meanPoint(3) < maxZDistance && meanPoint(3) > minZDistance
            xMin = min(thisPointData(:,1));
            xMax = max(thisPointData(:,1));
            yMin = min(thisPointData(:,2));
            yMax = max(thisPointData(:,2));
            zMin = min(thisPointData(:,3));
            zMax = max(thisPointData(:,3));
            l = (xMax - xMin);
            w = (yMax - yMin);
            h = (zMax - zMin);
            x = (xMin + xMax)/2;
            y = (yMin + yMax)/2;
            z = (zMin + zMax)/2;
            bboxes(:,i) = [x y z l w h]';
            isValidCluster(i) = l < 20; % max length of 20 meters
        end
    end
    bboxes = bboxes(:,isValidCluster);
end

function [ptCloudOut,obstacleIndices,groundIndices] = removeGroundPlane(ptCloudIn,maxGroundDist,referenceVector,maxAngularDist,currentIndices)
    % This method removes the ground plane from point cloud using
    % pcfitplane.
    [~,groundIndices,outliers] = pcfitplane(ptCloudIn,maxGroundDist,referenceVector,maxAngularDist);
    ptCloudOut = select(ptCloudIn,outliers);
    obstacleIndices = currentIndices(outliers);
    groundIndices = currentIndices(groundIndices);
end

function [ptCloudOut,indices,croppedIndices] = cropPointCloud(ptCloudIn,xLim,yLim,zLim,egoVehicleRadius)
    % This method selects the point cloud within limits and removes the
    % ego vehicle point cloud using findNeighborsInRadius
    locations = ptCloudIn.Location;
    locations = reshape(locations,[],3);
    insideX = locations(:,1) < xLim(2) & locations(:,1) > xLim(1);
    insideY = locations(:,2) < yLim(2) & locations(:,2) > yLim(1);
    insideZ = locations(:,3) < zLim(2) & locations(:,3) > zLim(1);
    inside = insideX & insideY & insideZ;
    
    % Remove ego vehicle
    nearIndices = findNeighborsInRadius(ptCloudIn,[0 0 0],egoVehicleRadius);
    nonEgoIndices = true(ptCloudIn.Count,1);
    nonEgoIndices(nearIndices) = false;
    validIndices = inside & nonEgoIndices;
    indices = find(validIndices);
    croppedIndices = find(~validIndices);
    ptCloudOut = select(ptCloudIn,indices);
end

mexLidarTracker

この関数は、コード生成用の関数型インターフェイスを使用して、点群の前処理ディスプレイと追跡アルゴリズムを実装します。

function [detections,obstacleIndices,groundIndices,croppedIndices,...
    confirmedTracks, modelProbs] = mexLidarTracker(ptCloudLocations,time)

persistent detectorModel tracker detectableTracksInput currentNumTracks


if isempty(detectorModel) || isempty(tracker) || isempty(detectableTracksInput) || isempty(currentNumTracks)
    
    % Use the same starting seed as MATLAB to reproduce results in SIL
    % simulation.
    rng(2018);
    
    % A bounding box detector model.
    detectorModel = HelperBoundingBoxDetector(...
                    'XLimits',[-50 75],...              % min-max
                    'YLimits',[-5 5],...                % min-max
                    'ZLimits',[-2 5],...                % min-max
                    'SegmentationMinDistance',1.6,...   % minimum Euclidian distance
                    'MinDetectionsPerCluster',1,...     % minimum points per cluster
                    'MeasurementNoise',eye(6),...       % measurement noise in detection report.
                    'GroundMaxDistance',0.3);           % maximum distance of ground points from ground plane
    
    assignmentGate = [50 100]; % Assignment threshold;
    confThreshold = [7 10];    % Confirmation threshold for history logic
    delThreshold = [8 10];     % Deletion threshold for history logic
    Kc = 1e-5;                 % False-alarm rate per unit volume
    
    filterInitFcn = @helperInitIMMFilter;
    
    tracker = trackerJPDA('FilterInitializationFcn',filterInitFcn,...
                      'TrackLogic','History',...
                      'AssignmentThreshold',assignmentGate,...
                      'ClutterDensity',Kc,...
                      'ConfirmationThreshold',confThreshold,...
                      'DeletionThreshold',delThreshold,...
                      'HasDetectableTrackIDsInput',true,...
                      'InitializationThreshold',0,...
                      'MaxNumTracks',30);
    
    detectableTracksInput = zeros(tracker.MaxNumTracks,2);
    
    currentNumTracks = 0;
end

ptCloud = pointCloud(ptCloudLocations);

% Detector model
[detections,obstacleIndices,groundIndices,croppedIndices] = detectorModel(ptCloud,time);

% Call tracker
[confirmedTracks,~,allTracks] = tracker(detections,time,detectableTracksInput(1:currentNumTracks,:));
% Update the detectability input
currentNumTracks = numel(allTracks);
detectableTracksInput(1:currentNumTracks,:) = helperCalcDetectability(allTracks,[1 3 6]);    

% Get model probabilities
modelProbs = zeros(2,numel(confirmedTracks));
if isLocked(tracker)
    for k = 1:numel(confirmedTracks)
        c1 = getTrackFilterProperties(tracker,confirmedTracks(k).TrackID,'ModelProbabilities');
        probs = c1{1};
        modelProbs(1,k) = probs(1);
        modelProbs(2,k) = probs(2);
    end
end

end

helperCalcDetectability

関数は各追跡物の検出確率を計算します。この関数は trackerJPDA の "DetectableTracksIDs" 入力を生成するために使用されます。

function detectableTracksInput = helperCalcDetectability(tracks,posIndices)
% This is a helper function to calculate the detection probability of
% tracks for the lidar tracking example. It may be removed in a future
% release.

% Copyright 2019 The MathWorks, Inc.

% The bounding box detector has low probability of segmenting point clouds
% into bounding boxes are distances greater than 40 meters. This function
% models this effect using a state-dependent probability of detection for
% each tracker. After a maximum range, the Pd is set to a high value to
% enable deletion of track at a faster rate.
if isempty(tracks)
    detectableTracksInput = zeros(0,2);
    return;
end
rMax = 75;
rAmbig = 40;
stateSize = numel(tracks(1).State);
posSelector = zeros(3,stateSize);
posSelector(1,posIndices(1)) = 1;
posSelector(2,posIndices(2)) = 1;
posSelector(3,posIndices(3)) = 1;
pos = getTrackPositions(tracks,posSelector);
if coder.target('MATLAB')
    trackIDs = [tracks.TrackID];
else
    trackIDs = zeros(1,numel(tracks),'uint32');
    for i = 1:numel(tracks)
        trackIDs(i) = tracks(i).TrackID;
    end
end
[~,~,r] = cart2sph(pos(:,1),pos(:,2),pos(:,3));
probDetection = 0.9*ones(numel(tracks),1);
probDetection(r > rAmbig) = 0.4;
probDetection(r > rMax) = 0.99;
detectableTracksInput = [double(trackIDs(:)) probDetection(:)];
end

loadLidarAndImageData

指定された初期時刻と最終時刻を使用する処理用に LIDAR とカメラデータを繋ぎ合わせます。

function [lidarData,imageData] = loadLidarAndImageData(datasetFolder,initTime,finalTime)
initFrame = max(1,floor(initTime*10));
lastFrame = min(350,ceil(finalTime*10));
load (fullfile(datasetFolder,'imageData_35seconds.mat'),'allImageData');
imageData = allImageData(initFrame:lastFrame);

numFrames = lastFrame - initFrame + 1;
lidarData = cell(numFrames,1);

% Each file contains 70 frames.
initFileIndex = floor(initFrame/70) + 1;
lastFileIndex = ceil(lastFrame/70);

frameIndices = [1:70:numFrames numFrames + 1];

counter = 1;
for i = initFileIndex:lastFileIndex
    startFrame = frameIndices(counter);
    endFrame = frameIndices(counter + 1) - 1;
    load(fullfile(datasetFolder,['lidarData_',num2str(i)]),'currentLidarData');
    lidarData(startFrame:endFrame) = currentLidarData(1:(endFrame + 1 - startFrame));
    counter = counter + 1;
end
end

参考文献

[1] Arya Senna Abdul Rachman, Arya."3D-LIDAR Multi Object Tracking for Autonomous Driving: Multi-target Detection and Tracking under Urban Road Uncertainties." (2017).