Этот пример показывает вам, как отследить транспортные средства с помощью измерений от датчика лидара, смонтированного сверху автомобиля, оборудованного датчиком. Лоцируйте датчики, сообщают измерения как облако точек. Пример иллюстрирует рабочий процесс в MATLAB® для обработки облака точек и отслеживания объектов. Для версии Simulink® примера относитесь, чтобы Отследить Транспортные средства Используя Данные о Лидаре в данных о лидаре Simulink.The, используемых в этом примере, зарегистрирован от магистрали ведущий сценарий. В этом примере вы используете записанные данные, чтобы отследить транспортные средства со средством отслеживания объединенной вероятностной ассоциации данных (JPDA) и подходом взаимодействующей многоуровневой модели (IMM).
Из-за высоких разрешающих способностей датчика лидара, каждый скан от датчика содержит большое количество точек, обычно известных как облако точек. Это необработанные данные должно быть предварительно обработано, чтобы извлечь предметы интереса, такие как автомобили, велосипедисты и пешеходы. В этом примере вы используете классический алгоритм сегментации с помощью основанного на расстоянии алгоритма кластеризации. Для получения дополнительной информации о сегментации данных о лидаре в объекты, такие как наземная плоскость и препятствия, отошлите к Наземному Обнаружению Плоскости и Препятствия Используя Лидар (Automated Driving Toolbox) пример. Для рабочего процесса сегментации глубокого обучения обратитесь к Обнаруживанию, Классифицируйте, и Транспортные средства Дорожки Используя Лидар (Lidar Toolbox) пример. В этом примере облака точек, принадлежащие препятствиям, далее классифицируются в кластеры с помощью pcsegdist
функция и каждый кластер преобразованы в обнаружение ограничительной рамки со следующим форматом:
, и обратитесь к x-, y-и z-положениям ограничительной рамки, относится к ее углу рыскания и, и обратитесь к ее длине, ширине и высоте, соответственно. 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
Первый шаг в отслеживании объекта задает свое состояние и модели, которые задают переход состояния и соответствующее измерение. Эти две системы уравнений коллективно известны как модель в пространстве состояний цели. Чтобы смоделировать состояние транспортных средств для отслеживания лидара использования, этот пример использует модель кубоида со следующим соглашением:
относится к фрагменту состояния, которое управляет кинематикой центра движения и является углом рыскания. Длина, ширина, высота кубоида моделируется как константы, оценки которых развиваются вовремя во время этапов коррекции фильтра.
В этом примере вы используете две модели в пространстве состояний: постоянная скорость (cv) модель кубоида и постоянная угловая скорость вращения (ct) модель кубоида. Эти модели отличаются по способу, которым они задают кинематическую часть состояния, аналогичного описанному ниже:
Для получения информации об их изменении состояния обратитесь к 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);
Визуализация разделена на эти основные категории:
Лоцируйте Предварительную обработку и Отслеживание - Это отображение показывает необработанное облако точек, сегментированную землю и препятствия. Это также показывает получившиеся обнаружения из модели детектора и дорожек транспортных средств, сгенерированных средством отслеживания.
Отображение Автомобиля, оборудованного датчиком - Это отображение показывает 2D вид с высоты птичьего полета на сценарий. Это показывает облако точек препятствия, обнаружения ограничительной рамки и дорожки, сгенерированные средством отслеживания. Для ссылки это также отображает изображение, зарегистрированное от камеры, смонтированной на автомобиле, оборудованном датчиком и его поле зрения.
При отслеживании Деталей - Это отображение показывает сценарий, масштабируемый вокруг автомобиля, оборудованного датчиком. Это также показывает более прекрасные детали отслеживания, такие как ошибочная ковариация в предполагаемом положении каждой дорожки и ее вероятностей модели движения, обозначенных 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).