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

Этот пример показывает вам, как отследить транспортные средства с помощью измерений от датчика лидара, смонтированного сверху автомобиля, оборудованного датчиком. Лоцируйте датчики, сообщают измерения как облако точек. Пример иллюстрирует рабочий процесс в MATLAB® для обработки облака точек и отслеживания объектов. Для версии Simulink® примера относитесь, чтобы Отследить Транспортные средства Используя Данные о Лидаре в Simulink (Sensor Fusion and Tracking Toolbox).The, данные о лидаре, используемые в этом примере, зарегистрированы от магистрали ведущий сценарий. В этом примере вы используете записанные данные, чтобы отследить транспортные средства со средством отслеживания объединенной вероятностной ассоциации данных (JPDA) и подходом взаимодействующей многоуровневой модели (IMM).

3-D модель детектора ограничительной рамки

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

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

$x$,$y$ и$z$ обратитесь к x-, y-и z-положениям ограничительной рамки,${\theta}$ относится к ее углу рыскания и$l$,$w$ и$h$ обратитесь к ее длине, ширине и высоте, соответственно. pcfitcuboid (Lidar Toolbox) функция использует алгоритм подбора 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 функции используются в этом примере.

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

  2. Отображение Автомобиля, оборудованного датчиком - Это отображение показывает 2D вид с высоты птичьего полета на сценарий. Это показывает облако точек препятствия, обнаружения ограничительной рамки и дорожки, сгенерированные средством отслеживания. Для ссылки это также отображает изображение, зарегистрированное от камеры, смонтированной на автомобиле, оборудованном датчиком и его поле зрения.

  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). Вводы и выводы функции могут наблюдаться в функциональном описании, предоставленном в разделе "Supporting Files" в конце этого примера.

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

Сравните результаты между Выполнением MEX и MATLAB

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

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

Результаты

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

Отследите обслуживание

Анимация выше показов симуляция между временем = 3 секунды и временем = 16 секунд. Заметьте, что дорожки, такие как T10 и T6 обеспечивают свои идентификаторы и траекторию во время отрезка времени. Однако отследите T9, потерян, потому что гусеничная машина ушлась (не обнаруженный) в течение долгого времени датчиком. Кроме того, заметьте, что отслеживаемые объекты могут обеспечить свою форму и кинематический центр путем расположения обнаружений на видимые фрагменты транспортных средств. Например, как Дорожка 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

Лидар стежков и данные о Камере для обработки начальной буквы использования и итоговое время заданы.

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 много отслеживание объекта для автономного управления автомобилем: мультицелевое обнаружение и отслеживающий под Урбан-Роуд Ансертэйнтис". (2017).

Смотрите также

| (Sensor Fusion and Tracking Toolbox) | (Sensor Fusion and Tracking Toolbox)

Похожие темы

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