Этот пример показывает вам, как отследить расширенные объекты. Расширенные объекты являются объектами, размерности которых охватывают несколько ячеек разрешения датчика. В результате датчики сообщают о нескольких обнаружениях расширенных объектов на одном сканировании. В этом примере вы будете использовать различные методы, чтобы отследить расширенные объекты и оценить результаты их производительности отслеживания.
В обычных подходах отслеживания, таких как глобальный самый близкий сосед (multiObjectTracker, trackerGNN), объединенная вероятностная ассоциация данных (trackerJPDA) и мультигипотеза, отслеживающая (trackerTOMHT), отслеживаемые объекты приняты, чтобы возвратить одно обнаружение на сканирование датчика. С разработкой датчиков, которые имеют лучшее разрешение, такое как радар с высоким разрешением, датчики обычно возвращают больше чем одно обнаружение объекта. Например, изображение ниже изображает несколько обнаружений для одного автомобиля, который охватывает несколько радарных ячеек разрешения. В таких случаях метод, используемый, чтобы отследить объекты, известен как расширенный объект, отслеживающий [1].
Ключевое преимущество использования датчика с высоким разрешением получает больше информации об объекте, таком как его размерности и ориентация. Эта дополнительная информация может улучшить вероятность обнаружения и уменьшать ложный сигнальный уровень.
Расширенные объекты представляют собой новые проблемы к обычным средствам отслеживания, потому что эти средства отслеживания принимают одно обнаружение на объект на датчик. В некоторых случаях можно сегментировать данные о датчике, чтобы предоставить обычным средствам отслеживания одно обнаружение на объект. Однако путем выполнения так, преимущество использования датчика с высоким разрешением может быть потеряно.
Напротив, расширенные объектные средства отслеживания могут обработать несколько обнаружений на объект. Кроме того, эти средства отслеживания могут оценить не только кинематические состояния, такие как положение и скорость объекта, но также и размерности и ориентация объекта. В этом примере вы отслеживаете автомобили вокруг автомобиля, оборудованного датчиком с помощью следующих средств отслеживания:
Обычное мультиобъектное средство отслеживания с помощью целевой точкой модели, multiObjectTracker
GGIW-доктор-философии (Гэмма Госсиэн Инверс Уишарт PHD) средство отслеживания, trackerPHD
Прототип реализации Последовательного Монте-Карло (SMC) Пуассона мульти-Бернулли (PMB), helperExtendedObjectTracker.
Вы также оцените результаты отслеживания всех средств отслеживания с помощью trackErrorMetrics и trackAssignmentMetrics.
Сценарий
В этом примере, который воссоздает Fusion Датчика в качестве примера Используя Синтетические Данные о Радаре и Видении (Automated Driving Toolbox), существует автомобиль, оборудованный датчиком и три других автомобиля: автомобиль перед автомобилем, оборудованным датчиком в правильном маршруте, автомобиль позади автомобиля, оборудованного датчиком в правильном маршруте и автомобиль перехвата. Автомобиль перехвата начинает свое движение позади трех других автомобилей, перемещений к левому маршруту, чтобы передать их и концы в правильном маршруте перед всеми тремя автомобилями.
В этом примере вы моделируете автомобиль, оборудованный датчиком, который имеет 6 радарных датчиков и 2 датчика видения, покрывающие 360 полей зрения степени. Датчики имеют некоторое перекрытие и некоторый разрыв покрытия. Автомобиль, оборудованный датчиком оборудован радарным датчиком дальним и датчиком видения на обоих передняя и задняя часть автомобиля. Каждая сторона автомобиля имеет два ближних радарных датчика, каждый покрывающий 90 градусов. Один датчик на каждой стороне покрывает с середины автомобиля к спине. Другой датчик на каждой стороне покрывает с середины автомобиля вперед.
% Create the scenario exPath = fullfile(matlabroot,'examples','driving_fusion','main'); addpath(exPath) [scenario, egoVehicle, sensors] = helperCreateScenario; % Create the display and return a handle to the bird's-eye plot panels BEPS = helperCreateDemoDisplay(egoVehicle, sensors); % Create the Animation writer to record each frame of the figure for % animation writing. Set 'RecordGIF' to true to enable GIF writing. gifWriter = helperGIFWriter('Figure',BEPS{1}.Parent.Parent.Parent,... 'RecordGIF',false);
Метрики
В этом примере вы используете некоторые ключевые метрики, чтобы оценить производительность отслеживания каждого средства отслеживания. В частности, вы оцениваете средства отслеживания на основе их точности в оценке положений, скоростей, размерности (длина и ширина) и ориентации объектов. Эти метрики могут быть оценены с помощью класса trackErrorMetrics. Чтобы задать ошибку отслеженной цели от ее наземной истины, этот пример использует 'пользовательскую' функцию ошибок, extendedTargetError, перечисленный в конце этого примера.
Вы также оцените производительность на основе метрик, таких как количество ложных дорожек или избыточных дорожек. Эти метрики могут быть вычислены с помощью класса trackAssignmentMetrics. Чтобы задать расстояние между отслеженной целью и объектом истины, этот пример использует 'пользовательскую' функцию ошибок, extendedTargetDistance, перечисленный в конце этого примера. Функция задает метрику расстояния как Евклидово расстояние между положением дорожки и положением истины.
% actorProfiles to provide the error function size information about each % truth. aProfiles = actorProfiles(scenario); % Function to return the errors given track and truth. errorFcn = @(track,truth)extendedTargetError(track,truth,aProfiles); % Function to return a distance metric between track and truth. distFcn = @extendedTargetDistance; % Function to return the IDs from the ground truth. The default % identifier assumes that the truth is identified with PlatformID. In % drivingScenario, truth is identified with an ActorID. truthIdFcn = @(x)[x.ActorID]; % Create metrics object. tem = trackErrorMetrics(... 'ErrorFunctionFormat','custom',... 'EstimationErrorLabels',{'PositionError','VelocityError','DimensionsError','YawError'},... 'EstimationErrorFcn',errorFcn,... 'TruthIdentifierFcn',truthIdFcn); tam = trackAssignmentMetrics(... 'DistanceFunctionFormat','custom',... 'AssignmentDistanceFcn',distFcn,... 'DivergenceDistanceFcn',distFcn,... 'TruthIdentifierFcn',truthIdFcn,... 'AssignmentThreshold',5,... 'DivergenceThreshold',10);
Система multiObjectTracker object™ принимает одно обнаружение на объект на датчик и использует глобальный самый близкий соседний подход, чтобы сопоставить обнаружения к дорожкам. Это принимает, что каждый объект может быть обнаружен самое большее однажды датчиком на сканировании. В этом случае моделируемые радарные датчики имеют достаточно высокое разрешение, чтобы сгенерировать несколько обнаружений на объект. Если эти обнаружения не кластеризируются, средство отслеживания генерирует несколько дорожек на объект. Кластеризация возвращает одно обнаружение на кластер, за счет наличия большей ковариации неуверенности и потери информации об истинных объектных размерностях. Из-за кластеризации также сложно различать два объекта, когда они друг близко к другу, например, когда один автомобиль передает другой автомобиль.
trackerRunTimes = zeros(0,3); % Create a multiObjectTracker tracker = multiObjectTracker(... 'FilterInitializationFcn', @initSimDemoFilter, ... 'AssignmentThreshold', 30, ... 'ConfirmationParameters', [4 5], ... 'NumCoastingUpdates', 3); % Use 'Ego Cartesian' as the sensor coordinate frame for the point tracker for k = 1:8 sensors{k}.DetectionCoordinates = 'Ego Cartesian'; end % Reset the random number generator for repeatable results S = rng; rng(2018) timeStep = 1;
Запустите сценарий
while advance(scenario) && ishghandle(BEPS{1}.Parent) % Get the scenario time time = scenario.SimulationTime; % Get the poses of the other vehicles in ego vehicle coordinates ta = targetPoses(egoVehicle); % Collect detections from the ego vehicle sensors [detections,isValidTime] = helperDetect(sensors, ta, time); % Update the tracker if there are new detections if any(isValidTime) % Detections must be clustered first for the point tracker detectionClusters = helperClusterDetections(detections, egoVehicle.Length); % Update the tracker tic confirmedTracks = updateTracks(tracker, detectionClusters, time); t = toc; % Update the metrics tam(confirmedTracks,ta); [trackIDs,truthIDs] = currentAssignment(tam); tem(confirmedTracks,trackIDs,ta,truthIDs); % Update bird's-eye-plot helperUpdateDisplayPoint(BEPS, egoVehicle, sensors, detections, confirmedTracks); % Record tracker run times trackerRunTimes(timeStep,1) = t; timeStep = timeStep + 1; % Capture frames for animation gifWriter(); end end % Capture the cumulative track metrics. The error metrics show the averaged % value of the error over the simulation. assignmentMetricsMOT = tam.trackMetricsTable; errorMetricsMOT = tem.cumulativeTruthMetrics; % Write GIF if requested writeAnimation(gifWriter,'multiObjectTracking');
Эти результаты показывают, что с кластеризацией средство отслеживания может отслеживать объекты в сцене. Однако это также показывает, что дорожка сопоставила с автомобилем перехвата (желтые) перемещения от передней стороны автомобиля в начале сценария к задней части автомобиля в конце. В начале сценария автомобиль перехвата находится позади (синего) автомобиля, оборудованного датчиком, таким образом, радар и обнаружения видения сделаны из своей передней стороны. Когда автомобиль перехвата передает автомобиль, оборудованный датчиком, радарные обнаружения сделаны из стороны автомобиля перехвата и затем от его спины, и дорожка перемещается в заднюю часть автомобиля.
Можно также видеть, что кластеризация не совершенна. Когда проходящий мимо автомобиль передает автомобиль, который находится позади (фиолетового) автомобиля, оборудованного датчиком, обе дорожки немного переключены налево из-за несовершенной кластеризации. Точно так же в сценарии, кластеризации иногда не удается кластеризировать все радарные обнаружения от того же объекта до одного кластера. В результате средство отслеживания точки генерирует две дорожки для автомобиля перехвата.
В этом разделе вы используете средство отслеживания GGIW-PHD (trackerPHD с ggiwphd) к отслеживаемым объектам. В отличие от multiObjectTracker, который использует один фильтр на дорожку, GGIW-PHD является мультицелевым фильтром, который описывает плотность гипотезы вероятности (PHD) сценария. Чтобы смоделировать расширенную цель, GGIW-PHD использует следующие дистрибутивы:
\Gamma: Положительное значение, чтобы описать ожидаемое количество обнаружений.
Гауссов: Вектор состояния, чтобы описать кинематическое состояние цели
Обратный Уишарт: положительно-определенная матрица, чтобы описать эллиптическую степень.
Модель принимает, что каждое распределение независимо друг от друга. Таким образом плотность гипотезы вероятности (PHD) в фильтре GGIW-PHD описана взвешенной суммой функций плотности вероятности нескольких компонентов GGIW.
Средство отслеживания доктора философии требует вычисления обнаружительной способности каждого компонента в плотности. Вычисление обнаружительной способности требует настроек каждого датчика, используемого со средством отслеживания. Вы задаете эти настройки для trackerPHD с помощью класса trackingSensorConfiguration.
Настройте настройки датчика
% Allocate memory for configurations. sensorConfig = cell(numel(sensors),1); % Allocate memory for resolution noise. resolutionNoise = zeros(2,2,numel(sensors)); for i = 1:numel(sensors) % Configuration of each sensor. SensorLimits are defined by the % field of view and are the only parameters for detectability. sensorConfig{i} = trackingSensorConfiguration(sensors{i}.SensorIndex,... 'SensorLimits',[-1/2 1/2].*sensors{i}.FieldOfView',... 'FilterInitializationFcn',@initCVPHDFilter,'SensorTransformFcn',... @cvmeas); % origin of the sensor with respect to ego vehicle originPosition = [sensors{i}.SensorLocation(:);sensors{i}.Height]; % orientation of the sensor with respect to ego vehicle orientation = rotmat(quaternion([sensors{i}.Yaw sensors{i}.Pitch sensors{i}.Roll],'eulerd','ZYX','frame'),'frame'); % The SensorTransformParameters of the configuration requires % information about sensor's mounting location and orientation in the % tracking frame assembed in a struct in the following format: coordTransforms(1) = struct('Frame','Spherical',... 'OriginPosition',originPosition,... 'Orientation',orientation,... 'HasVelocity',false,... 'OriginVelocity',zeros(3,1),... 'HasRange',false); % Set the transform for each sensor sensorConfig{i}.SensorTransformParameters = coordTransforms; % All sensors are at the same update rate and the tracker is updated % at the same rate. Therefore, IsValidTime is true for all sensors. sensorConfig{i}.IsValidTime = true; if i <= 6 % A radar cannot report a detection with accuracy more than its % resolution for extended targets. Radars typically use a point % target model, which does not capture the correct uncertainty. resolutionNoise(:,:,i) = diag([sensors{i}.AzimuthResolution sensors{i}.RangeResolution].^2); % Clutter density for radar sensors sensorConfig{i}.ClutterDensity = sensors{i}.ClutterDensity; else % Vision sensors report 1 detection per object. This information % can be captured in the sensor configuration. This enables the % tracker to not generate possible partitions for those detections. sensorConfig{i}.MaxNumDetsPerObject = 1; % Clutter density for vision sensor sensorConfig{i}.ClutterDensity = sensors{i}.ClutterDensity; end end
Задайте средство отслеживания.
% The trackerPHD creates multiple possible partitions of a set of % detections and evaluate it against the current components in the PHD % filter. The 2 and 5 in the function below defines the lower and upper % Mahalanobis distance between detections. This is equivalent to defining % that each cluster of detection must be a minimum of 2 resolutions apart % and maximum of 5 resolutions apart from each other. partFcn = @(x)partitionDetections(x,2,5); tracker = trackerPHD('SensorConfigurations',sensorConfig,... 'PartitioningFcn',partFcn,... 'ConfirmationThreshold',0.75,...% Weight of a density component to be called a confirmed track. 'MergingThreshold',200,...% Threshold to merge components belonging to same track ID. 'AssignmentThreshold',50); % Minimum distance of a detection cell to give birth in the density.
Запустите симуляцию.
% The sensors report in sensor Cartesian frame to accurately set the effect % of resolution noise in the measurement noise. for k = 1:8 release(sensors{k}); if k <= 6 sensors{k}.DetectionCoordinates = 'Sensor Cartesian'; end end % Release and restart all objects. restart(scenario); release(tem); release(tam); gifWriter.pFrames = {}; for i = 1:numel(BEPS) clearPlotterData(BEPS{i}); end % Restore random seed. rng(2018) % First time step timeStep = 1; % Run the scenario while advance(scenario) && ishghandle(BEPS{1}.Parent) % Get the scenario time time = scenario.SimulationTime; % Get the poses of the other vehicles in ego vehicle coordinates ta = targetPoses(egoVehicle); % Collect detections from the ego vehicle sensors [detections,isValidTime] = helperDetect(sensors, ta, time, resolutionNoise); % Update the tracker if there are new detections if any(isValidTime) % Update the extended tracker with all the detections. Note that % there is no need to cluster the detections before passing them to % the tracker. tic [confirmedTracks,~,allTracks,info] = tracker(detections,time); t = toc; % Update the metrics tam(confirmedTracks,ta); [trackIDs,truthIDs] = currentAssignment(tam); tem(confirmedTracks,trackIDs,ta,truthIDs); % Update the bird's-eye plot helperUpdateDisplayExtended(BEPS, egoVehicle, sensors, detections, confirmedTracks); % Record tracker run times trackerRunTimes(timeStep,2) = t; timeStep = timeStep + 1; % Capture frames for GIF gifWriter(); end end % Capture the truth and track metrics tables assignmentMetricsPHD = tam.trackMetricsTable; errorMetricsPHD = tem.cumulativeTruthMetrics; % Write GIF if requested writeAnimation(gifWriter,'ggiwphdTracking');
Эти результаты показывают, что GGIW-PHD может обработать несколько обнаружений на объект на датчик без потребности кластеризировать эти обнаружения сначала. Кроме того, при помощи нескольких обнаружений, средство отслеживания оценивает положение, скорость, размерности и ориентацию каждого объекта. Пунктирная эллиптическая форма в фигуре демонстрирует ожидаемую степень цели.
Фильтр GGIW-PHD принимает, что обнаружения распределяются вокруг эллиптического центра цели. Поэтому дорожки имеют тенденцию следовать за заметными фрагментами автомобиля. Такие заметные фрагменты включают заднюю поверхность автомобиля, который является непосредственно перед автомобилем, оборудованным датчиком или передней стороной автомобиля непосредственно позади автомобиля, оборудованного датчиком, например, задней поверхности и передней стороны автомобиля непосредственно вперед и позади автомобиля, оборудованного датчиком соответственно. Напротив, длина и ширина проходящего мимо автомобиля полностью наблюдались во время симуляции. Поэтому его предполагаемый эллипс имеет лучшее перекрытие с фактической формой.
Чтобы создать прототип расширенное объектное средство отслеживания, сначала необходимо задать модель для расширенного объекта. Следующая модель задает расширенные объекты как прямоугольные с подобными размерностями к автомобилю, оборудованному датчиком. Каждый расширенный объект принят, чтобы заставить скоординированный обернуться свой центр, расположенный в центре задней оси.
model = extendedCoordinatedTurn( ... 'Pivot', 0.787, ... 'StdYawAcceleration', 0.1, ... 'MeanLength', egoVehicle.Length, ... 'MeanWidth', egoVehicle.Width, ... 'StdLength', 0.02, ... 'StdWidth', 0.01, ... 'CorrLengthWidth', 0.5);
Создайте объект средства отслеживания путем определения модели, используемой для отслеживания, количества частиц, используемых для отслеживаемых объектов и для необнаруженных объектов и алгоритма выборки. В этом примере вы используете Гиббс, выбирающий, чтобы сопоставить обнаружения с дорожками расширенного объекта.
sampler = gibbs; % Creates a Gibbs sampler tracker = helperExtendedObjectTracker( ... 'Model', model, ... 'NumTrackingParticles', 5000, ... 'MaxNumUndetectedParticles', 2e4, ... 'SamplingMethod', sampler);
Сбросьте сценарий, отображение и генератор случайных чисел
restart(scenario);
release(tem);
release(tam);
gifWriter.pFrames = {};
% The prototype uses "Identity" to define the Id of the track.
tem.TrackIdentifierFcn = @(x)[x.Identity];
tam.TrackIdentifierFcn = @(x)[x.Identity];
for i = 1:numel(BEPS)
clearPlotterData(BEPS{i});
end
rng(2018)
timeStep = 1;
% For this tracker, the sensors report in sensor coordinates
for k = 1:6
release(sensors{k});
sensors{k}.DetectionCoordinates = 'Sensor Spherical';
end
for k = 7:8
release(sensors{k});
sensors{k}.DetectionCoordinates = 'Sensor Cartesian';
end
Запустите сценарий
while advance(scenario) && ishghandle(BEPS{1}.Parent) % Get the scenario time time = scenario.SimulationTime; % Get the poses of the other vehicles in ego vehicle coordinates ta = targetPoses(egoVehicle); % Collect detections from the ego vehicle sensors [detections,isValidTime] = helperDetect(sensors, ta, time); % Update the tracker if there are new detections if any(isValidTime) % Update the extended tracker with all the detections. Note that % there is no need to cluster the detections before passing them to % the tracker. tic confirmedTracks = updateTracks(tracker, detections, time, egoVehicle, sensors); t = toc; % Coordinate transform the tracks output from scenario frame to ego % vehicle frame confirmedTracks = toEgoCoordinates(egoVehicle, confirmedTracks); % Update the bird's-eye plot helperUpdateDisplayExtended(BEPS, egoVehicle, sensors, detections, confirmedTracks); % Update the metrics tam(confirmedTracks,ta); [trackIDs,truthIDs] = currentAssignment(tam); tem(confirmedTracks,trackIDs,ta,truthIDs); % Record tracker run times trackerRunTimes(timeStep,3) = t; timeStep = timeStep + 1; % Capture frames for GIF gifWriter(); end end % Capture the truth and track metrics tables assignmentMetricsPrototype = tam.trackMetricsTable; errorMetricsPrototype = tem.cumulativeTruthMetrics; % Write GIF if requested writeAnimation(gifWriter,'prototypeTracking'); % Return the random number generator to its previous state rng(S) rmpath(exPath)
Эти результаты показывают, что прототип может также обработать несколько обнаружений на объект на датчик. Подобно GGIW-PHD это также оценивает размер и ориентацию объекта.
Можно заметить, что предполагаемые дорожки, которые моделируются как прямоугольники, имеют подходящий вариант для моделируемого наземного объекта истины, изображенного закрашенными фигурами сплошного цвета. В частности, дорожки могут правильно отследить форму автомобиля наряду с кинематическим центром.
Оцените производительность отслеживания каждого средства отслеживания с помощью количественных метрик, таких как ошибка оценки в положении, скорости, размерностях и ориентации. Также оцените присвоения дорожки с помощью метрик, таких как избыточные и ложные дорожки.
Метрики присвоения
helperPlotAssignmentMetrics(assignmentMetricsMOT, assignmentMetricsPHD, assignmentMetricsPrototype);
Метрики присвоения иллюстрируют, что две избыточных дорожки, Дорожка 11 и Дорожка 93, были инициализированы и подтверждены средством отслеживания точечного объекта. Избыточные дорожки заканчиваются из-за несовершенной кластеризации, где обнаружения, принадлежащие той же цели, кластеризировались больше чем в одно кластеризованное обнаружение. Кроме того, средство отслеживания точечного объекта, созданное и подтвержденное несколько ложных дорожек: Отследите 2, 18 и 66. Напротив, средство отслеживания GGIW-PHD и прототипное средство отслеживания поддерживают дорожки на всех трех целях, и не создавайте ложные или избыточные дорожки. Эти метрики показывают, что и расширенные объектные средства отслеживания правильно делят обнаружения и сопоставляют их с правильными дорожками.
Ошибочные метрики
helperPlotErrorMetrics(errorMetricsMOT, errorMetricsPHD, errorMetricsPrototype);
График показывает средние ошибки оценки для трех типов средств отслеживания, используемых в этом примере. Поскольку средство отслеживания точечного объекта не оценивает отклонение от курса и размерности объектов, их теперь показывают в графиках. Средство отслеживания точечного объекта может оценить кинематику объектов с разумной точностью. Ошибка положения автомобиля позади автомобиля, оборудованного датчиком выше, потому что это было перетащено налево, когда проходящий мимо автомобиль обгоняет этот автомобиль. Это - также артефакт кластеризации имперфекта, когда объекты друг близко к другу.
Как описано ранее, средство отслеживания GGIW-PHD принимает, что измерения распределяются вокруг степени объекта, которая приводит к центру дорожек на заметных частях автомобиля, Это может также быть замечено в ошибочных метриках положения для TruthID 2 и 4. Средство отслеживания может оценить размерности объекта приблизительно с 0,3-метровой точностью для автомобилей вперед и позади автомобиля, оборудованного датчиком. Из-за выше certainity заданный для размерностей автомобилей в функции initCVPHDFilter, средство отслеживания не сворачивает длину этих автомобилей, даже когда хорошо-подходящий эллипс имеет очень низкую длину. Когда проходящий мимо автомобиль (TruthID 3) наблюдался относительно всех размерностей, его размерности измеряются более точно, чем другие автомобили. Однако, когда проходящий мимо автомобиль маневрирует относительно автомобиля, оборудованного датчиком, ошибка в оценке отклонения от курса выше.
Прототип использует цель прямоугольной формы и использует пересечения лучом, чтобы оценить каждое обнаружение против предполагаемого состояния дорожки. Эта модель помогает средству отслеживания оценить форму и ориентацию более точно. Однако процесс пересечений лучом, объединенных с последовательными Методами Монте-Карло, является в вычислительном отношении более дорогим, чем использование дистрибутивов закрытой формы.
Ранее вы узнавали о различных методах, предположения, которые они делают о целевых моделях и получившейся производительности отслеживания. Теперь сравните время выполнения средств отслеживания. Заметьте, что фильтр GGIW-PHD предлагает значительные вычислительные преимущества перед прототипом, за счет немного уменьшенной производительности отслеживания.
f = figure; plot(trackerRunTimes(3:end,:)./trackerRunTimes(3:end,1),'LineWidth',2); legend('multiObjectTracker','trackerPHD','Prototype'); xlabel('Time step (k)'); ylabel('$$\frac{t_{tracker}}{t_{multiObjectTracker}}$$','interpreter','latex','fontsize',14);
extendedTargetError
Функция, чтобы задать ошибку между отслеженной целью и связанной наземной истиной.
function [posError,velError,dimError,yawError] = extendedTargetError(track,truth,aProfiles) % Errors as a function of target track and associated truth. % Get true information from the ground truth. truePos = truth.Position(1:2); trueVel = truth.Velocity(1:2); trueYaw = truth.Yaw(:); thisActor = truth.ActorID; thisProfile = aProfiles([aProfiles.ActorID] == thisActor); trueDims = [thisProfile.Length;thisProfile.Width]; % Get estimated value from track. % |trackerPHD| outputs a field 'Extent' for each track. % Prototype outputs the 'Orientation' and 'Extension' for each track. if ~isfield(track,'Extent') && ~isfield(track,'Position') % multiObjectTracker estPos = track.State(1:2:end); estVel = track.State(2:2:end); % No yaw or dimension information in multiObjectTracker. estYaw = nan; estDims = [nan;nan]; elseif isfield(track,'Extent') % trackerPHD estPos = track.State(1:2:end); estVel = track.State(2:2:end); [v,d] = eig(track.Extent); v = blkdiag(v,1); ypr = eulerd(quaternion(v,'rotmat','frame'),'ZYX','frame'); estYaw = ypr(1); dims = 2*sqrt(diag(d)); estDims = [max(dims);min(dims)]; else % Prototype estPos = track.Position(:); estVel = track.Velocity(:); A = [ ... track.Orientation(1), track.Orientation(2); ... -track.Orientation(2), track.Orientation(1) ... ]; extension = diag(A * track.Extension * A')'; estDims = [extension(1);extension(2)]; estYaw = atan2d(track.Orientation(2),track.Orientation(1)); end % Compute 2-norm of error for each attribute. posError = norm(truePos(:) - estPos(:)); velError = norm(trueVel(:) - estVel(:)); dimError = norm(trueDims(:) - estDims(:)); yawError = norm(trueYaw(:) - estYaw(:)); end
extendedTargetDistance
Функция, чтобы задать расстояние между дорожкой и наземной истиной.
function dist = extendedTargetDistance(track,truth) % Compute distance between track and a truth. % True position from ground truth. truePos = truth.Position(:); % Prototype uses Position and multiObjectTracker, trackerPHD provides % position as 1st, 2nd and 5th element for a constant velocity state. if isfield(track,'Extension') estPos = track.Position(:); else estPos = track.State(1:2:end); end % Error is defined as Euclidian distance error = truePos(1:2) - estPos(:); dist = norm(error); end
initCVPHDFilter
Функция, чтобы создать фильтр ggiwphd из ячейки обнаружения.
function phd = initCVPHDFilter(varargin) % Initialize a 2-dimensional constant velocity PHD filter. % The initcvggiwphd creates 3-D constant velocity filter. phd3d = initcvggiwphd(varargin{:}); % Create 2-D filter using 1st four states. % PositionIndex is [1 3] as constant velocity state is defined as [x;vx;y;vy]. % ProcessNoise is defined as the acceleration in x and y direction. Because % of time-dependence it is of non-additive nature. See constvel for more % information. phd = ggiwphd(phd3d.States(1:4,:),phd3d.StateCovariances(1:4,1:4,:),... 'StateTransitionFcn',@constvel,... 'StateTransitionJacobianFcn',@constveljac,... 'MeasurementFcn',@cvmeas,... 'MeasurementJacobianFcn',@cvmeasjac,... 'HasAdditiveMeasurementNoise',true,... 'HasAdditiveProcessNoise',false,... 'ProcessNoise',diag([0.1 0.1]),... 'MaxNumComponents',1000,... 'ExtentRotationFcn',@(x,dT)eye(2),... 'PositionIndex',[1 3]); % Set the sizes of the filter when created with a detection cell. Without % any input arguments, the function, initcvggiwphd, creates a filter with % no components. if nargin > 0 % A higher value on degree of freedom represents higher certainty in % the dimensions. The initial value is provided with that of a standard % passenger vehicle. A higher certainity also prevents collapsing of % length or width of the track when only one of the faces is visible. phd.DegreesOfFreedom = 1000; phd.ScaleMatrices = (1000-4)*diag([4.7/2 1.8/2].^2); phd.GammaForgettingFactors = 1.03; end end
Этот пример показал, как к отслеживаемым объектам, которые возвращают несколько обнаружений на одном сканировании датчика с помощью разных подходов. Эти подходы могут использоваться к отслеживаемым объектам с датчиками с высоким разрешением, такими как радар или лазерный датчик.
[1] Granström, Карл, Маркус Баум и Штефан Ройтер. "Расширенное объектное отслеживание: введение, обзор и приложения". Журнал усовершенствований в информационном Fusion. Издание 12, № 2, декабрь 2017.
[2] Granström, Карл, Леннарт Свенсон, Штефан Ройтер, Юйсюань Ся и Марьям Фэтеми. "Основанная на вероятности ассоциация данных для расширенного отслеживания объекта Используя выборку методов". Транзакции IEEE на интеллектуальных автомобилях. Издание 3, № 1, март 2018.
[3] Granström, Карл. "Расширенное целевое отслеживание с помощью фильтров PHD". 2012