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 位置を参照し、
、
および
はその長さ、幅、高さをそれぞれ参照します。
境界ボックスは、各次元で点の最小および最大の座標を使用して各クラスターに収まります。検出器は、点群のセグメンテーション機能とクラスタリング機能をラップ アラウンドするサポート クラス 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 を使用して追跡する車両の状態をモデル化するために、この例では次の規則をもつ直方体モデルを使用します。
は動きの中心の運動学を制御する状態の部分を参照し、
はヨー角です。直方体の長さ、幅、および高さは、フィルターの補正段階中に推定が変化する定数としてモデル化されます。
この例では、等速度 (cv) の直方体モデルと一定の旋回率 (ct) の直方体モデルである 2 つの状態空間モデルを使用します。以下に説明するように、これらのモデルが状態の運動学部分を定義する方法は異なります。
これらの状態遷移の詳細については、この例で使用される関数 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)));
1
確認された追跡物の数が 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).
関連するトピック
- Track Vehicles Using Lidar: From Point Cloud to Track List (Lidar Toolbox)
- Detect, Classify, and Track Vehicles Using Lidar (Lidar Toolbox)