В этом примере показано, как сформировать список дорожек на уровне объекта из измерений радара и лидарного датчика и далее сплавить их с помощью схемы слияния на уровне дорожек. Радиолокационные измерения обрабатываются с использованием расширенного трекера объектов и лидарных измерений с использованием совместного трекера вероятностных данных (JPDA). Эти дорожки далее сплавляются с помощью схемы слияния на уровне дорожек. Схема рабочего процесса показана ниже.
Сценарий, используемый в этом примере, создается с помощью drivingScenario (Автоматическая панель инструментов вождения). Данные от РЛС и лидарных датчиков моделируются с помощью drivingRadarDataGenerator (Автоматизированная панель инструментов вождения) и lidarPointCloudGenerator (Automated Driving Toolbox) соответственно. Создание сценария и моделей датчиков переносится в вспомогательную функцию. helperCreateRadarLidarScenario. Дополнительные сведения о сценарии и создании синтетических данных см. в разделе Программное создание сценария вождения (Automated Driving Toolbox).
% For reproducible results rng(2019); % Create scenario, ego vehicle and get radars and lidar sensor [scenario, egoVehicle, radars, lidar] = helperCreateRadarLidarScenario;
Транспортное средство Эго установлено с четырьмя 2-D радиолокационными датчиками. Передние и задние радиолокационные датчики имеют поле зрения 45 градусов. Левый и правый радиолокационные датчики имеют поле зрения 150 градусов. Каждая РЛС имеет разрешение 6 градусов по азимуту и 2,5 метра по дальности. Ego также смонтирован с одним 3-D лидарным датчиком с полем зрения 360 градусов по азимуту и 40 градусов по высоте. Лидар имеет разрешение 0,2 градуса по азимуту и 1,25 градуса по отметке (32 канала возвышения). Визуализируйте конфигурацию датчиков и смоделированные данные датчиков в анимации ниже. Обратите внимание, что радары имеют более высокое разрешение, чем объекты, и поэтому возвращают несколько измерений на объект. Также обратите внимание, что лидар взаимодействует с низкополюсной сеткой актера, а также с дорожным покрытием для возврата нескольких точек от этих объектов.
Как уже упоминалось, радары имеют более высокое разрешение, чем объекты, и возвращают множество обнаружений на объект. Обычные трекеры, такие как Global Nearest Neighbor (GNN) и Joint Probabilistic Data Association (JPDA), предполагают, что датчики возвращают максимум одно обнаружение на объект на сканирование. Поэтому обнаружения от датчиков высокого разрешения должны быть либо сгруппированы перед их обработкой обычными трекерами, либо должны быть обработаны с использованием расширенных трекеров объектов. Расширенные объектные трекеры не требуют предварительной кластеризации обнаружений и обычно оценивают как кинематические состояния (например, положение и скорость), так и протяженность объектов. Для более подробного сравнения обычных трекеров и расширенных трекеров объектов см. пример расширенного отслеживания объектов автотранспортных средств с помощью радара и камеры (Sensor Fusion and Tracking Toolbox).
В общем, расширенные объектные трекеры предлагают лучшую оценку объектов, поскольку они обрабатывают кластеризацию и ассоциацию данных одновременно с использованием временной истории дорожек. В этом примере радиолокационные обнаружения обрабатываются с использованием трекера плотности гипотезы вероятности гауссовой смеси (GM-PHD) (trackerPHD(панель инструментов слияния и отслеживания датчиков) и gmphd (Sensor Fusion and Tracking Toolbox)) с прямоугольной целевой моделью. Дополнительные сведения о настройке трекера см. в разделе «GM-PHD Rectural Object Tracker» в примере расширенного отслеживания объектов автотранспортных средств с помощью радара и камеры (Sensor Fusion and Tracking Toolbox).
Алгоритм слежения за объектами с помощью радиолокационных измерений заключен внутри класса помощника, helperRadarTrackingAlgorithm, реализованный как системный object™. Этот класс выводит массив objectTrack (Sensor Fusion and Tracking Toolbox) объекты и определите их состояние в соответствии со следующим соглашением:
![$[x\ y\ s\ {\theta}\ {\omega}\ L\ W]$](../../examples/shared_driving_fusion_lidar/win64/RadarAndLidarTrackFusionExample_eq16091761654656236408.png)
radarTrackingAlgorithm = helperRadarTrackingAlgorithm(radars);
Аналогично радарам, лидарный датчик также возвращает множество измерений на объект. Кроме того, датчик возвращает большое количество точек от дороги, которые должны быть удалены до использования в качестве входных данных для алгоритма отслеживания объектов. В то время как лидарные данные от препятствий могут непосредственно обрабатываться с помощью расширенного алгоритма отслеживания объектов, обычные алгоритмы отслеживания все еще более распространены для отслеживания с использованием лидарных данных. Первая причина этой тенденции в основном наблюдается из-за более высокой вычислительной сложности расширенных объектных трекеров для больших наборов данных. Вторая причина - инвестиции в усовершенствованные детекторы на основе Deep Learning, такие, как, в частности, StartStolps [1], VoxelNet [2] и PIXOR [3], которые могут сегментировать облако точек и возвращать обнаружения ограничивающих коробок для транспортных средств. Эти детекторы могут помочь в преодолении снижения производительности обычных трекеров из-за неправильной кластеризации.
В этом примере лидарные данные обрабатываются с использованием обычного трекера ассоциации совместных вероятностных данных (JPDA), сконфигурированного с взаимодействующим фильтром множественной модели (IMM). Предварительная обработка лидарных данных для удаления облака точек выполняется с использованием алгоритма подгонки плоскости на основе RANSAC, а ограничивающие рамки формируются путем выполнения алгоритма кластеризации расстояний на основе евклидова. Дополнительные сведения об алгоритме см. в примере Track Vehicles Using Lidar: From Point Cloud to Track List (Sensor Fusion and Tracking Toolbox). По сравнению со связанным примером отслеживание выполняется в кадре сценария, и трекер настраивается по-разному для отслеживания объектов различных размеров. Кроме того, состояния переменных определяются по-разному для ограничения движения дорожек в направлении их расчетного угла курса.
Алгоритм отслеживания объектов с использованием данных лидара заключен в класс помощника, helperLidarTrackingAlgorithm реализован как системный объект. Этот класс выводит массив objectTrack (Sensor Fusion and Tracking Toolbox) объекты и определяет их состояние в соответствии со следующим соглашением:
![$[x\ y\ s\ {\theta}\ {\omega}\ z\ {\dot{z}}\ L\ W\ H]$](../../examples/shared_driving_fusion_lidar/win64/RadarAndLidarTrackFusionExample_eq17720683028359187391.png)
Состояния, общие для радиолокационного алгоритма, определяются аналогично. Кроме того, в качестве датчика 3-D лидарный трекер выдает три дополнительных состояния, 
и, 
которые относятся к координате z (m), скорости z (m/s) и высоте (m) отслеживаемого объекта соответственно.
lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(lidar);
Fuser
Далее вы настроите алгоритм слияния для слияния списка дорожек с радаров и лидар-трекеров. Аналогично другим алгоритмам слежения, первым шагом к настройке алгоритма слияния на уровне дорожки является определение выбора вектора состояния (или состояния-пространства) для слитых или центральных дорожек. В этом случае состояние-пространство для конденсированных дорожек выбирается таким же, как у лидара. После выбора центрального состояния дорожки-пространства определяется преобразование центрального состояния дорожки в локальное состояние дорожки. При этом локальное путевое состояние-пространство относится к состояниям радиолокационных и лидарных путей. Для этого используется fuserSourceConfiguration(Панель инструментов слияния и отслеживания датчиков).
Определите конфигурацию радиолокационного источника. helperRadarTrackingAlgorithm выходные дорожки с SourceIndex задайте значение 1. SourceIndex предоставляется в качестве свойства на каждом трекере для его уникальной идентификации и позволяет алгоритму слияния различать дорожки из разных источников. Поэтому необходимо установить SourceIndex свойство конфигурации РЛС аналогично свойствам траекторий РЛС. Вы установили IsInitializingCentralTracks кому true позволить этим неназначенным радиолокационным трассам инициировать новые центральные трассы. Далее определяется преобразование дорожки в центральном состоянии-пространстве в радиолокационное состояние-пространство и наоборот. Вспомогательные функции central2radar и radar2central выполните два преобразования и включите их в конец этого примера.
radarConfig = fuserSourceConfiguration('SourceIndex',1,... 'IsInitializingCentralTracks',true,... 'CentralToLocalTransformFcn',@central2radar,... 'LocalToCentralTransformFcn',@radar2central);
Определите конфигурацию источника лидара. Поскольку состояние-пространство лидарной дорожки совпадает с состоянием центральной дорожки, преобразования не определяются.
lidarConfig = fuserSourceConfiguration('SourceIndex',2,... 'IsInitializingCentralTracks',true);
Следующим шагом является определение алгоритма слияния состояний. Алгоритм слияния состояний принимает множество состояний и ковариаций состояний в центральном пространстве состояний в качестве входных данных и возвращает слитую оценку состояния и ковариаций. В этом примере используется алгоритм ковариационного пересечения, предоставляемый вспомогательной функцией. helperRadarLidarFusionFcn. Общий алгоритм ковариационного пересечения для двух гауссовых оценок со средним
и ковариационным
можно определить согласно следующим уравнениям:
где
и
являются конденсированным состоянием и ковариацией и
и являются
коэффициентами смешения из каждой оценки. Обычно эти коэффициенты смешения оценивают путем минимизации детерминанта или следа конденсированной ковариации. В этом примере весовые коэффициенты смешения оцениваются путем минимизации детерминанта позиционной ковариации каждой оценки. Кроме того, поскольку радар не оценивает 3-D состояния, 3-D состояния сплавляются только с лидарами. Для получения дополнительной информации см. helperRadarLidarFusionFcn в конце этого сценария отображается функция.
Затем вы соберете всю информацию с помощью trackFuser объект.
% The state-space of central tracks is same as the tracks from the lidar, % therefore you use the same state transition function. The function is % defined inside the helperLidarTrackingAlgorithm class. f = lidarTrackingAlgorithm.StateTransitionFcn; % Create a trackFuser object fuser = trackFuser('SourceConfigurations',{radarConfig;lidarConfig},... 'StateTransitionFcn',f,... 'ProcessNoise',diag([1 3 1]),... 'HasAdditiveProcessNoise',false,... 'AssignmentThreshold',[250 inf],... 'ConfirmationThreshold',[3 5],... 'DeletionThreshold',[5 5],... 'StateFusion','Custom',... 'CustomStateFusionFcn',@helperRadarLidarFusionFcn);
Метрики
В этом примере оценивается производительность каждого алгоритма с помощью метрики Обобщенная оптимальная метрика назначения субпоследовательности (GOSPA). Три отдельные метрики настраиваются с помощью trackGOSPAMetric (Sensor Fusion and Tracking Toolbox) для каждого трекера. Метрика GOSPA направлена на оценку производительности системы отслеживания путем предоставления скалярной стоимости. Меньшее значение метрики указывает на лучшую производительность алгоритма отслеживания.
Чтобы использовать метрику GOSPA с пользовательскими моделями движения, такими как использованная в этом примере, необходимо установить Distance свойство 'custom' и определяет функцию расстояния между дорожкой и связанной с ней истинностью грунта. Эти функции расстояния, показанные в конце этого примера: helperRadarDistance, и helperLidarDistance.
% Radar GOSPA gospaRadar = trackGOSPAMetric('Distance','custom',... 'DistanceFcn',@helperRadarDistance,... 'CutoffDistance',25); % Lidar GOSPA gospaLidar = trackGOSPAMetric('Distance','custom',... 'DistanceFcn',@helperLidarDistance,... 'CutoffDistance',25); % Central/Fused GOSPA gospaCentral = trackGOSPAMetric('Distance','custom',... 'DistanceFcn',@helperLidarDistance,...% State-space is same as lidar 'CutoffDistance',25);
Визуализация
Визуализация для этого примера реализована с помощью класса помощника helperLidarRadarTrackFusionDisplay. Дисплей разделен на 4 панели. На дисплее отображаются измерения и дорожки от каждого датчика, а также оценки конденсированных дорожек. Легенда для отображения показана ниже. Кроме того, дорожки аннотируются своей уникальной идентичностью (TrackID), а также префикс. Префиксы «R», «L» и «F» обозначают соответственно оценку радара, лидара и взрывателя.
% Create a display. % FollowActorID controls the actor shown in the close-up % display display = helperLidarRadarTrackFusionDisplay('FollowActorID',3); % Show persistent legend showLegend(display,scenario);
Далее выполняется продвижение сценария, создание синтетических данных из всех датчиков и их обработка для создания дорожек из каждой системы. Также вычисляется метрика для каждого трекера с использованием базовой истинности, доступной из сценария.
% Initialzie GOSPA metric and its components for all tracking algorithms. gospa = zeros(3,0); missTarget = zeros(3,0); falseTracks = zeros(3,0); % Initialize fusedTracks fusedTracks = objectTrack.empty(0,1); % A counter for time steps elapsed for storing gospa metrics. idx = 1; % Ground truth for metrics. This variable updates every time-step % automatically being a handle to the actors. groundTruth = scenario.Actors(2:end); while advance(scenario) % Current time time = scenario.SimulationTime; % Collect radar and lidar measurements and ego pose to track in % scenario frame. See helperCollectSensorData below. [radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle, radars, lidar, time); % Generate radar tracks radarTracks = radarTrackingAlgorithm(egoPose, radarDetections, time); % Generate lidar tracks and analysis information like bounding box % detections and point cloud segmentation information [lidarTracks, lidarDetections, segmentationInfo] = ... lidarTrackingAlgorithm(egoPose, ptCloud, time); % Concatenate radar and lidar tracks localTracks = [radarTracks;lidarTracks]; % Update the fuser. First call must contain one local track if ~(isempty(localTracks) && ~isLocked(fuser)) fusedTracks = fuser(localTracks,time); end % Capture GOSPA and its components for all trackers [gospa(1,idx),~,~,~,missTarget(1,idx),falseTracks(1,idx)] = gospaRadar(radarTracks, groundTruth); [gospa(2,idx),~,~,~,missTarget(2,idx),falseTracks(2,idx)] = gospaLidar(lidarTracks, groundTruth); [gospa(3,idx),~,~,~,missTarget(3,idx),falseTracks(3,idx)] = gospaCentral(fusedTracks, groundTruth); % Update the display display(scenario, radars, radarDetections, radarTracks, ... lidar, ptCloud, lidarDetections, segmentationInfo, lidarTracks,... fusedTracks); % Update the index for storing GOSPA metrics idx = idx + 1; end % Update example animations updateExampleAnimations(display);
Оцените производительность каждого трекера с помощью визуализации, а также количественных показателей. Анализ различных событий в сценарии и понимание того, как схема слияния на уровне пути помогает добиться лучшей оценки состояния транспортного средства.
Техническое обслуживание пути
Анимация ниже показывает весь прогон каждые три шага времени. Отметим, что каждая из трех систем слежения - радар, лидар и слияние на уровне трассы - смогли отследить все четыре машины по сценарию, и никаких ложных дорожек подтверждено не было.
Можно также количественно измерить этот аспект производительности с помощью компонентов «пропущенная цель» и «ложная дорожка» метрики GOSPA. Обратите внимание на рисунки ниже, что пропущенный целевой компонент начинается с более высокого значения из-за задержки установления и опускается до нуля примерно за 5-10 шагов для каждой системы слежения. Также обратите внимание, что компонент ложной дорожки равен нулю для всех систем, что указывает на то, что ложные дорожки не были подтверждены.
% Plot missed target component figure; plot(missTarget','LineWidth',2); legend('Radar','Lidar','Fused'); title("Missed Target Metric"); xlabel('Time step'); ylabel('Metric'); grid on; % Plot false track component figure; plot(falseTracks','LineWidth',2); legend('Radar','Lidar','Fused'); title("False Track Metric"); xlabel('Time step'); ylabel('Metric'); grid on;
Точность на уровне отслеживания
Точность отслеживания или локализации каждого трекера также может количественно оцениваться метрикой GOSPA на каждом временном шаге. Меньшее значение указывает на лучшую точность отслеживания. Поскольку не было пропущенных целей или ложных дорожек, метрика фиксирует ошибки локализации, возникающие в результате оценки состояния каждого транспортного средства.
Следует отметить, что метрика GOSPA для слитых оценок ниже метрики для отдельного датчика, что указывает на то, что точность дорожки увеличивается после слияния оценок дорожки от каждого датчика.
% Plot GOSPA figure; plot(gospa','LineWidth',2); legend('Radar','Lidar','Fused'); title("GOSPA Metric"); xlabel('Time step'); ylabel('Metric'); grid on;
Близкорасположенные цели
Как упоминалось выше, в этом примере используется кластеризация на основе евклидова расстояния и подгонка ограничивающей рамки для подачи данных лидара в обычный алгоритм слежения. Алгоритмы кластеризации обычно страдают, когда объекты расположены близко друг к другу. При использовании конфигурации детектора в этом примере, когда движущееся транспортное средство приближается к транспортному средству перед эго-транспортным средством, детектор группирует облако точек из каждого транспортного средства в большую ограничивающую рамку. В анимации ниже можно заметить, что трасса отходит от центра транспортного средства. Поскольку дорожка была представлена с более высокой достоверностью в своей оценке за несколько шагов, первоначально также была затронута оценка слитого. Однако по мере увеличения неопределенности ее связь со слитой оценкой становится слабее. Это происходит потому, что алгоритм ковариационного пересечения выбирает вес смешения для каждой назначенной дорожки на основе достоверности каждой оценки.
Этот эффект также фиксируется в метрике GOSPA. На графике метрики GOSPA выше можно заметить, что метрика лидара показывает пик около 65-го шага времени.
Радиолокационные трассы не затрагиваются во время этого события по двум основным причинам. Во-первых, радиолокационный датчик выдает информацию о дальности при каждом обнаружении, которая отличается от уровней шума для проезжающего автомобиля по сравнению с более медленным движущимся автомобилем. Это приводит к увеличению статистического расстояния между обнаружениями от отдельных автомобилей. Во-вторых, расширенные объектные трекеры оценивают множественную возможную гипотезу кластеризации против предсказанных дорожек, что приводит к отклонению неправильных кластеров и принятию правильных кластеров. Обратите внимание, что для того, чтобы расширенные средства отслеживания объектов правильно выбирали лучшие кластеры, фильтр для дорожки должен быть надежным до такой степени, чтобы можно было зафиксировать разницу между двумя кластерами. Например, дорожка с высоким технологическим шумом и очень неопределенными размерами может быть не в состоянии должным образом претендовать на кластер из-за его преждевременного возраста и более высокой гибкости для учета неопределенных событий.
Цели на большой дальности
Когда цели отходят от радиолокационных датчиков, точность измерений ухудшается из-за уменьшенного отношения сигнал/шум на детекторе и ограниченного разрешения датчика. Это приводит к высокой неопределенности в измерениях, что, в свою очередь, снижает точность дорожки. Обратите внимание на крупным планом ниже, что оценка пути от радара находится дальше от наземной истины для радиолокационного датчика и сообщается с более высокой неопределенностью. Однако лидарный датчик сообщает о достаточном количестве измерений в облаке точек для создания «усаженной» ограничивающей рамки. Эффект усадки, смоделированный в модели измерения для алгоритма отслеживания лидара, позволяет трекеру поддерживать дорожку с правильными размерами. В таких ситуациях вес смешивания лидара выше, чем у радара, и позволяет более точно оценить слитый заряд, чем у радара.
В этом примере вы научились настраивать алгоритм слияния на уровне дорожки для сплавления дорожек от радаров и лидарных датчиков. Также вы научились оценивать алгоритм отслеживания с помощью обобщенной оптимальной метрики податтерна и связанных с ней компонентов.
collectSensorData
Функция формирования радиолокационных и лидарных измерений на текущем шаге времени.
function [radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle, radars, lidar, time) % Current poses of targets with respect to ego vehicle tgtPoses = targetPoses(egoVehicle); radarDetections = cell(0,1); for i = 1:numel(radars) thisRadarDetections = step(radars{i},tgtPoses,time); radarDetections = [radarDetections;thisRadarDetections]; %#ok<AGROW> end % Generate point cloud from lidar rdMesh = roadMesh(egoVehicle); ptCloud = step(lidar, tgtPoses, rdMesh, time); % Compute pose of ego vehicle to track in scenario frame. Typically % obtained using an INS system. If unavailable, this can be set to % "origin" to track in Ego vehicle's frame. egoPose = pose(egoVehicle); end
radar2cental
Функция преобразования дорожки в радиолокационном пространстве в дорожку в центральном пространстве.
function centralTrack = radar2central(radarTrack) % Initialize a track of the correct state size centralTrack = objectTrack('State',zeros(10,1),... 'StateCovariance',eye(10)); % Sync properties of radarTrack except State and StateCovariance with % radarTrack See syncTrack defined below. centralTrack = syncTrack(centralTrack,radarTrack); xRadar = radarTrack.State; PRadar = radarTrack.StateCovariance; H = zeros(10,7); % Radar to central linear transformation matrix H(1,1) = 1; H(2,2) = 1; H(3,3) = 1; H(4,4) = 1; H(5,5) = 1; H(8,6) = 1; H(9,7) = 1; xCentral = H*xRadar; % Linear state transformation PCentral = H*PRadar*H'; % Linear covariance transformation PCentral([6 7 10],[6 7 10]) = eye(3); % Unobserved states % Set state and covariance of central track centralTrack.State = xCentral; centralTrack.StateCovariance = PCentral; end
central2radar
Функция преобразования дорожки в центральном пространстве состояний в дорожку в радиолокационном пространстве состояний.
function radarTrack = central2radar(centralTrack) % Initialize a track of the correct state size radarTrack = objectTrack('State',zeros(7,1),... 'StateCovariance',eye(7)); % Sync properties of centralTrack except State and StateCovariance with % radarTrack See syncTrack defined below. radarTrack = syncTrack(radarTrack,centralTrack); xCentral = centralTrack.State; PCentral = centralTrack.StateCovariance; H = zeros(7,10); % Central to radar linear transformation matrix H(1,1) = 1; H(2,2) = 1; H(3,3) = 1; H(4,4) = 1; H(5,5) = 1; H(6,8) = 1; H(7,9) = 1; xRadar = H*xCentral; % Linear state transformation PRadar = H*PCentral*H'; % Linear covariance transformation % Set state and covariance of radar track radarTrack.State = xRadar; radarTrack.StateCovariance = PRadar; end
syncTrack
Функция для синхронизации свойств одной дорожки с другой, за исключением свойств «State» и «StateCovariance»
function tr1 = syncTrack(tr1,tr2) props = properties(tr1); notState = ~strcmpi(props,'State'); notCov = ~strcmpi(props,'StateCovariance'); props = props(notState & notCov); for i = 1:numel(props) tr1.(props{i}) = tr2.(props{i}); end end
pose
Функция возврата позы эго-транспортного средства как структуры.
function egoPose = pose(egoVehicle) egoPose.Position = egoVehicle.Position; egoPose.Velocity = egoVehicle.Velocity; egoPose.Yaw = egoVehicle.Yaw; egoPose.Pitch = egoVehicle.Pitch; egoPose.Roll = egoVehicle.Roll; end
helperLidarDistance
Функция для вычисления нормированного расстояния между оценкой пути в радиолокационном состоянии-пространстве и назначенной истинностью земли.
function dist = helperLidarDistance(track, truth) % Calculate the actual values of the states estimated by the tracker % Center is different than origin and the trackers estimate the center rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2]; rot = quaternion([truth.Yaw truth.Pitch truth.Roll],'eulerd','ZYX','frame'); actPos = truth.Position(:) + rotatepoint(rot,rOriginToCenter')'; % Actual speed and z-rate actVel = [norm(truth.Velocity(1:2));truth.Velocity(3)]; % Actual yaw actYaw = truth.Yaw; % Actual dimensions. actDim = [truth.Length;truth.Width;truth.Height]; % Actual yaw rate actYawRate = truth.AngularVelocity(3); % Calculate error in each estimate weighted by the "requirements" of the % system. The distance specified using Mahalanobis distance in each aspect % of the estimate, where covariance is defined by the "requirements". This % helps to avoid skewed distances when tracks under/over report their % uncertainty because of inaccuracies in state/measurement models. % Positional error. estPos = track.State([1 2 6]); reqPosCov = 0.1*eye(3); e = estPos - actPos; d1 = sqrt(e'/reqPosCov*e); % Velocity error estVel = track.State([3 7]); reqVelCov = 5*eye(2); e = estVel - actVel; d2 = sqrt(e'/reqVelCov*e); % Yaw error estYaw = track.State(4); reqYawCov = 5; e = estYaw - actYaw; d3 = sqrt(e'/reqYawCov*e); % Yaw-rate error estYawRate = track.State(5); reqYawRateCov = 1; e = estYawRate - actYawRate; d4 = sqrt(e'/reqYawRateCov*e); % Dimension error estDim = track.State([8 9 10]); reqDimCov = eye(3); e = estDim - actDim; d5 = sqrt(e'/reqDimCov*e); % Total distance dist = d1 + d2 + d3 + d4 + d5; end
helperRadarDistance
Функция для вычисления нормированного расстояния между оценкой пути в радиолокационном состоянии-пространстве и назначенной истинностью земли.
function dist = helperRadarDistance(track, truth) % Calculate the actual values of the states estimated by the tracker % Center is different than origin and the trackers estimate the center rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2]; rot = quaternion([truth.Yaw truth.Pitch truth.Roll],'eulerd','ZYX','frame'); actPos = truth.Position(:) + rotatepoint(rot,rOriginToCenter')'; actPos = actPos(1:2); % Only 2-D % Actual speed actVel = norm(truth.Velocity(1:2)); % Actual yaw actYaw = truth.Yaw; % Actual dimensions. Only 2-D for radar actDim = [truth.Length;truth.Width]; % Actual yaw rate actYawRate = truth.AngularVelocity(3); % Calculate error in each estimate weighted by the "requirements" of the % system. The distance specified using Mahalanobis distance in each aspect % of the estimate, where covariance is defined by the "requirements". This % helps to avoid skewed distances when tracks under/over report their % uncertainty because of inaccuracies in state/measurement models. % Positional error estPos = track.State([1 2]); reqPosCov = 0.1*eye(2); e = estPos - actPos; d1 = sqrt(e'/reqPosCov*e); % Speed error estVel = track.State(3); reqVelCov = 5; e = estVel - actVel; d2 = sqrt(e'/reqVelCov*e); % Yaw error estYaw = track.State(4); reqYawCov = 5; e = estYaw - actYaw; d3 = sqrt(e'/reqYawCov*e); % Yaw-rate error estYawRate = track.State(5); reqYawRateCov = 1; e = estYawRate - actYawRate; d4 = sqrt(e'/reqYawRateCov*e); % Dimension error estDim = track.State([6 7]); reqDimCov = eye(2); e = estDim - actDim; d5 = sqrt(e'/reqDimCov*e); % Total distance dist = d1 + d2 + d3 + d4 + d5; % A constant penality for not measuring 3-D state dist = dist + 3; end
helperRadarLidarFusionFcn
Функция слияния состояний и ковариаций состояний в состоянии-пространстве центральной дорожки
function [x,P] = helperRadarLidarFusionFcn(xAll,PAll) n = size(xAll,2); dets = zeros(n,1); % Initialize x and P x = xAll(:,1); P = PAll(:,:,1); onlyLidarStates = false(10,1); onlyLidarStates([6 7 10]) = true; % Only fuse this information with lidar xOnlyLidar = xAll(onlyLidarStates,:); POnlyLidar = PAll(onlyLidarStates,onlyLidarStates,:); % States and covariances for intersection with radar and lidar both xToFuse = xAll(~onlyLidarStates,:); PToFuse = PAll(~onlyLidarStates,~onlyLidarStates,:); % Sorted order of determinants. This helps to sequentially build the % covariance with comparable determinations. For example, two large % covariances may intersect to a smaller covariance, which is comparable to % the third smallest covariance. for i = 1:n dets(i) = det(PToFuse(1:2,1:2,i)); end [~,idx] = sort(dets,'descend'); xToFuse = xToFuse(:,idx); PToFuse = PToFuse(:,:,idx); % Initialize fused estimate thisX = xToFuse(:,1); thisP = PToFuse(:,:,1); % Sequential fusion for i = 2:n [thisX,thisP] = fusecovintUsingPos(thisX, thisP, xToFuse(:,i), PToFuse(:,:,i)); end % Assign fused states from all sources x(~onlyLidarStates) = thisX; P(~onlyLidarStates,~onlyLidarStates,:) = thisP; % Fuse some states only with lidar source valid = any(abs(xOnlyLidar) > 1e-6,1); xMerge = xOnlyLidar(:,valid); PMerge = POnlyLidar(:,:,valid); if sum(valid) > 1 [xL,PL] = fusecovint(xMerge,PMerge); elseif sum(valid) == 1 xL = xMerge; PL = PMerge; else xL = zeros(3,1); PL = eye(3); end x(onlyLidarStates) = xL; P(onlyLidarStates,onlyLidarStates) = PL; end function [x,P] = fusecovintUsingPos(x1,P1,x2,P2) % Covariance intersection in general is employed by the following % equations: % P^-1 = w1*P1^-1 + w2*P2^-1 % x = P*(w1*P1^-1*x1 + w2*P2^-1*x2); % where w1 + w2 = 1 % Usually a scalar representative of the covariance matrix like "det" or % "trace" of P is minimized to compute w. This is offered by the function % "fusecovint". However. in this case, the w are chosen by minimizing the % determinants of "positional" covariances only. n = size(x1,1); idx = [1 2]; detP1pos = det(P1(idx,idx)); detP2pos = det(P2(idx,idx)); w1 = detP2pos/(detP1pos + detP2pos); w2 = detP1pos/(detP1pos + detP2pos); I = eye(n); P1inv = I/P1; P2inv = I/P2; Pinv = w1*P1inv + w2*P2inv; P = I/Pinv; x = P*(w1*P1inv*x1 + w2*P2inv*x2); end
[1] Лэнг, Алекс Х., и др. «StartStolps: быстрые кодеры для обнаружения объектов из облаков точек». Материалы Конференции IEEE по компьютерному зрению и распознаванию образов. 2019.
[2] Чжоу, Инь и Онкель Тузель. «Voxelnet: Сквозное обучение для обнаружения трехмерных объектов на основе облака точек». Материалы Конференции IEEE по компьютерному зрению и распознаванию образов. 2018.
[3] Ян, Бин, Вэньцзе Луо и Ракель Уртасун. «Пиксор: обнаружение 3D-объектов в реальном времени из облаков точек». Материалы конференции IEEE по компьютерному зрению и распознаванию образов. 2018.