Отслеживайте транспортные средства, используя лидар: от облака точек до списка треков

В этом примере показано, как отслеживать транспортные средства с помощью измерений с датчика лидара, установленного на верхнюю часть автомобиля , оборудованного датчиком. Датчики Lidar сообщают о измерениях как о облаке точек. Пример иллюстрирует рабочий процесс в MATLAB ® для обработки облака точек и отслеживания объектов. Для версии примера Simulink ® смотрите Track Vehicles Using Lidar Data in Simulink (Sensor Fusion and Tracking Toolbox). Данные лидара, используемые в этом примере, записываются из сценария движения по шоссе. В этом примере вы используете записанные данные для отслеживания транспортных средств с помощью совместного вероятностного трекера (JPDA) и подхода с несколькими моделями (IMM).

3-D модель детектора ограничивающих коробок

Из-за возможностей высокого разрешения датчика лидара каждый скан с датчика содержит большое число точек, обычно известных как облако точек. Эти необработанные данные должны быть предварительно обработаны для извлечения интересующих объектов, таких как автомобили, велосипедисты и пешеходы. В этом примере вы используете классический алгоритм сегментации, используя дистанционный алгоритм кластеризации. Для получения дополнительной информации о сегментации данных лидара на объекты, такие как наземная плоскость и препятствия, см. Пример обнаружения наземной плоскости и препятствий с использованием лидара (Automated Driving Toolbox). Рабочий процесс сегментации глубокого обучения см. в примере «Обнаружение, классификация и отслеживание транспортных средств» с использованием лидар. В этом примере облака точек, принадлежащие препятствиям, далее классифицируются в кластеры с помощью pcsegdist функция, и каждый кластер преобразуется в обнаружение ограничивающего прямоугольника со следующим форматом:

$[x\ y\ z\ {\theta}\ l\ w\ h]$

$x$,$y$ и$z$ относятся к положениям x -, y - и z - ограничивающего прямоугольника,${\theta}$ относятся к его углу рыскания и, $l$и $w$относятся$h$ к его длине, ширине и высоте, соответственно. The pcfitcuboid функция использует L-образный алгоритм аппроксимации, чтобы определить угол рыскания ограничивающего прямоугольника.

Детектор реализован поддерживающим классом HelperBoundingBoxDetector, который оборачивается вокруг функций сегментации облака точек и кластеризации. Объект этого класса принимает pointCloud введите и возвращает список objectDetection объекты с ограничивающими измерениями.

Схема показывает процессы, участвующие в модели детектора ограничивающих прямоугольников, и функции Lidar Toolbox™, используемые для реализации каждого процесса. Он также показывает свойства поддерживающего класса, которые управляют каждым процессом.

Данные лидара доступны в следующем месте: https://ssd.mathworks.com/supportfiles/lidar/data/TrackVehiclesUsingLidarExampleData.zip

Загрузите файлы данных в свою временную директорию, расположение которого задано tempdir MATLAB функция. Если вы хотите поместить файлы в другую папку, измените имя директории в последующих инструкциях.

% 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.8,...   % minimum Euclidian distance
    'MinDetectionsPerCluster',1,...     % minimum points per cluster
    'MeasurementNoise',blkdiag(0.25*eye(3),25,eye(3)),...       % measurement noise in detection report
    'GroundMaxDistance',0.3);           % maximum distance of ground points from ground plane

Целевое состояние и модель измерения датчика

Первым шагом в отслеживании объекта является определение его состояния и моделей, которые определяют переход состояния и соответствующее измерение. Эти два набора уравнений вместе известны как модель пространства состояний цели. Чтобы смоделировать состояние транспортных средств для отслеживания с помощью лидара, этот пример использует кубоидную модель со следующим соглашением:

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

$x_{kin}$ относится к фрагменту состояния, которая управляет кинематикой центра движения, и$\theta$ является углом рыскания. Длина, ширина, высота кубоида моделируются как константы, оценки которых эволюционируют во времени во время каскадов коррекции фильтра.

В этом примере вы используете две модели пространства состояний: кубоидную модель постоянной скорости (cv) и кубоидную модель постоянной скорости поворота (ct). Эти модели отличаются тем, как они определяют кинематическую часть состояния, как описано ниже:

$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 функции, используемые в этом примере.

The helperCvmeasCuboid и helperCtmeasCuboid модели измерения описывают, как датчик воспринимает постоянные состояния скорости и постоянной скорости поворота соответственно, и они возвращают ограничивающие прямоугольные измерения. Поскольку состояние содержит информацию о размере цели, модель измерения включает эффект смещения центральной точки и усадки ограничивающего прямоугольника, как воспринимается датчиком, из-за эффектов, таких как самоокклюзия [1]. Этот эффект моделируется коэффициентом усадки, который прямо пропорциональен расстоянию от отслеживаемого транспортного средства до датчика.

Изображение ниже демонстрирует модель измерения, работающую в различных выборках пространства состояний. Заметьте смоделированные эффекты усадки ограничивающего прямоугольника и смещения по центру, когда объекты перемещаются вокруг автомобиля , оборудованного датчиком.

Настройка трекера и визуализации

На изображении ниже показан полный рабочий процесс получения списка треков из входа pointCloud.

Теперь настройте трекер и визуализацию, использованную в примере.

Совместный вероятностный трекер ассоциации данных (trackerJPDA) в сочетании с IMM-фильтром (trackingIMM) используется для отслеживания объектов в этом примере. IMM-фильтр использует модель постоянной скорости и постоянной скорости поворота и инициализируется с помощью вспомогательной функции, helperInitIMMFilter, включенный в этот пример. Подход IMM помогает дорожке переключаться между моделями движения и, таким образом, достигать хорошей точности оценки во время таких событий, как маневрирование или изменение маршрута. Анимация ниже показывает эффект смешения модели постоянной скорости и постоянной скорости поворота во время этапов предсказания IMM-фильтра.

IMM-фильтр обновляет вероятность каждой модели, когда она исправлена с обнаружениями от объекта. Анимация ниже показывает предполагаемую траекторию транспортного средства во время события изменения маршрута и соответствующие оценочные вероятности каждой модели.

Установите HasDetectableTrackIDsInput свойство трекера как true, что позволяет вам задать зависимую от состояния вероятность обнаружения. Вероятность обнаружения дорожки вычисляется helperCalcDetectability функции, перечисленной в конце этого примера.

assignmentGate = [75 1000]; % Assignment threshold;
confThreshold = [7 10];    % Confirmation threshold for history logic
delThreshold = [8 10];     % Deletion threshold for history logic
Kc = 1e-9;                 % 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,...
    'HitMissThreshold',0.1);

Визуализация разделена на следующие основные категории:

  1. Lidar Preprocessing and Tracking - Это отображение показывает необработанное облако точек, сегментированную землю и препятствия. Также показаны получившиеся обнаружения от модели детектора и дорожки транспортных средств, сгенерированных трекером.

  2. Ego Vehicle Display - На этом отображении показан 2-D вид сценария на птичий глаз. Он показывает облако точек препятствий, обнаружение ограничивающих прямоугольников и треки, сгенерированные трекером. Для ссылки он также отображает изображение, записанное с камеры, установленной на автомобиль , оборудованный датчиком, и его поле зрения.

  3. Детали отслеживания - Это отображение показывает сценарий с масштабированием вокруг автомобиля , оборудованного датчиком. Это также показывает более мелкие детали отслеживания, такие как ковариация ошибок в предполагаемом положении каждой дорожки и ее вероятности модели движения, обозначенные 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

Цикл через данные

Цикл через записанные данные лидара, сгенерировать обнаружения из текущего облака точек с помощью модели детектора и затем обработать обнаружения с помощью трекера.

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,info] = 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,120,140,'imm','details');
end

Рисунок выше показывает три отображения во время = 18 секунд. Дорожки представлены зелеными ограничивающими рамками. Обнаружение ограничивающих прямоугольников представлено оранжевыми ограничивающими прямоугольниками. Обнаружения также имеют оранжевые точки внутри них, представляющие облако точек, сегментированное как препятствия. Сегментированный грунт показан фиолетовым цветом. Обрезанное или сброшенное облако точек показано синим цветом.

Сгенерируйте код С

Можно сгенерировать код С из кода MATLAB ® для алгоритма отслеживания и предварительной обработки с помощью MATLAB Coder™. Генерация кода C позволяет вам ускорить код MATLAB для симуляции. Чтобы сгенерировать код С, алгоритм должен быть реструктурирован как функция MATLAB, которая может быть скомпилирована в файл MEX или общую библиотеку. С этой целью алгоритм обработки облака точек и алгоритм отслеживания реструктуризируются в функцию MATLAB, mexLidarTracker. Некоторые переменные заданы как persistent сохранение их состояния между несколькими вызовами функции (см persistent). Входные и выходные параметры функции можно наблюдать в описании функции, представленном в разделе «Вспомогательные файлы» в конце этого примера.

MATLAB Coder требует определения свойств всех входных аргументов. Легкий способ сделать это - определить входные свойства по примеру в командной строке с помощью -args опция. Для получения дополнительной информации смотрите Задать входные свойства по примеру в командной строке (MATLAB Coder). Обратите внимание, что входные параметры верхнего уровня не могут быть объектами handle класс. Поэтому функция принимает положения x, y и z облака точек как вход. Из сохраненного облака точек эта информация может быть извлечена с помощью Location свойство pointCloud объект. Эта информация также непосредственно доступна в качестве необработанных данных с датчика лидара.

% 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. Сброс времени

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 Execution

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

Заметьте, что количество подтвержденных треков одинаковое для выполнения кода MATLAB и MEX. Это гарантирует, что алгоритм предварительной обработки и отслеживания лидара возвращает те же результаты с сгенерированным кодом C, что и с кодом MATLAB.

Результаты

Теперь анализируйте различные события в сценарии и понимайте, как комбинация модели измерения лидара, совместной вероятностной ассоциации данных и взаимодействия фильтра нескольких моделей помогает достичь хорошей оценки трасс транспортного средства.

Отслеживайте техническое обслуживание

Анимация выше показывает симуляцию между временем = 3 секунды и временем = 16 секунд. Заметьте, что такие треки, как T10 и T6, поддерживают свои идентификаторы и траекторию в течение временного интервала. Однако T9 пути теряется, потому что отслеживаемое транспортное средство был пропускаемо (не обнаружен) в течение длительного времени датчиком. Кроме того, заметьте, что отслеживаемые объекты способны поддерживать свою форму и кинематический центр путем позиционирования обнаружений на видимых фрагментах транспортных средств. Для примера, когда Track T7 движется вперед, обнаружение ограничивающего прямоугольника начинает падать на его видимый задний фрагмент, и дорожка поддерживает фактический размер транспортного средства. Это иллюстрирует эффект смещения и усадки, смоделированный в функциях измерения.

Захват маневров

Анимация показывает, что использование IMM-фильтра помогает трекеру поддерживать треки на маневрирующих транспортных средствах. Заметьте, что отслеживаемое T4 транспортное средство изменяет полосы движения позади автомобиля , оборудованного датчиком. Трекер способен поддерживать трек на транспортное средство во время этого события маневрирования. Также заметьте на отображении, что его вероятность следования модели постоянного поворота, обозначенной ct, увеличивается во время маневра изменения маршрута.

Объединение вероятностных данных

Эта анимация показывает, что использование совместного вероятностного трекера ассоциации данных помогает в поддержании треков во время неоднозначных ситуаций. Здесь, транспортные средства отслеживаются T43 и T73, имеют низкую вероятность обнаружения из-за их большого расстояния от датчика. Заметьте, что трекер способен поддерживать треки во время событий, когда один из транспортных средств не обнаружен. Во время события дорожки сначала объединяются, что является известным явлением в JPDA, а затем отделяются, как только транспортное средство была обнаруживаемо снова.

Сводные данные

Этот пример показал, как использовать JPDA-трекер с IMM-фильтром для отслеживания объектов с помощью датчика лидара. Вы узнали, как необработанное облако точек может быть предварительно обработано, чтобы сгенерировать обнаружения для обычных трекеров, которые предполагают одно обнаружение на объект в каждом скане датчика. Вы также научились определять кубоидную модель для описания кинематики, размерностей и измерений расширенных объектов, отслеживаемых трекером JPDA. В сложение вы сгенерированными кодами C от алгоритма и проверили его результаты выполнения с помощью симуляции MATLAB.

Вспомогательные файлы

helperLidarModel

Эта функция задает модель лидара, чтобы симулировать усадку измерения ограничивающего прямоугольника и смещения центральной точки. Эта функция используется в 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;

% 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;yaw;Ls;Ws;Hs];

end

helperInverseLidarModel

Эта функция задает обратную модель лидара, чтобы инициировать фильтр отслеживания с помощью измерения ограничивающего прямоугольника лидара. Эта функция используется в 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 - shiftZ/2;

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

yaw = meas(4,:);
yawCov = measCov(4,4,:);

% 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

Это вспомогательный класс HelperBoundingBoxDetector чтобы принять вход облака точек и вернуть список objectDetection

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),10,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 at least 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(7,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
            cuboid = pcfitcuboid(pointCloud(thisPointData));
            yaw = cuboid.Orientation(3);
            L = cuboid.Dimensions(1);
            W = cuboid.Dimensions(2);
            H = cuboid.Dimensions(3);
            if abs(yaw) > 45
                possibles = yaw + [-90;90];
                [~,toChoose] = min(abs(possibles));
                yaw = possibles(toChoose);
                temp = L;
                L = W;
                W = temp;
            end
            bboxes(:,i) = [cuboid.Center yaw L W H]';
            isValidCluster(i) = L < 20 & W < 20;
        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.8,...   % minimum Euclidian distance
                    'MinDetectionsPerCluster',1,...     % minimum points per cluster
                    'MeasurementNoise',blkdiag(0.25*eye(3),25,eye(3)),...       % measurement noise in detection report.
                    'GroundMaxDistance',0.3);           % maximum distance of ground points from ground plane
    
    assignmentGate = [75 1000]; % Assignment threshold;
    confThreshold = [7 10];    % Confirmation threshold for history logic
    delThreshold = [8 10];     % Deletion threshold for history logic
    Kc = 1e-9;                 % 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,...
                      'HitMissThreshold',0.1);
    
    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

Функция вычисляет вероятность обнаружения для каждой дорожки. Эта функция используется, чтобы сгенерировать вход «DetectableTracksIDs» для trackerJPDA.

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

Сшивает данные лидар и Camera для обработки с использованием начального и последнего указанного времени.

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] Арья Сенна Абдул Рахман, Арья. 3D-LIDAR Multi Object Tracking for Автономное Управление Автомобилем: Multi-Target Detection and Tracking Universities (неопр.) (недоступная ссылка). (2017).

Для просмотра документации необходимо авторизоваться на сайте