Разработайте визуальный алгоритм SLAM Используя нереальную симуляцию Engine

Этот пример использует:

В этом примере показано, как разработать визуальный алгоритм Одновременной локализации и картографии (SLAM) с помощью данных изображения, полученных из Нереальной среды симуляции Engine®.

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

Этот пример демонстрирует использование Нереальной симуляции Engine, чтобы разработать визуальный алгоритм SLAM или для монокуляра или для стереофотоаппарата в сценарии парковки. Для получения дополнительной информации о реализации визуальных трубопроводов SLAM, смотрите Монокулярный Визуальный Одновременный пример Локализации и картографии и Визуальный Одновременный пример Локализации и картографии Стерео.

Настройте сценарий в среде симуляции

Используйте Симуляцию 3D Блок Configuration Сцены, чтобы настроить среду симуляции. Выберите встроенную Большую сцену Парковки, которая содержит несколько припаркованных транспортных средств. Визуальный алгоритм SLAM совпадает с функциями через последовательные изображения. Чтобы увеличить число потенциальных соответствий функции, можно использовать Припаркованную подсистему Транспортных средств, чтобы добавить более припаркованные транспортные средства в сцену. Чтобы задать положения парковки транспортных средств, используйте helperAddParkedVehicle функция. Если вы выбираете более естественную сцену, присутствие дополнительных транспортных средств не необходимо. Естественные сцены обычно имеют достаточно структуры и показывают разнообразие, подходящее для соответствия функции.

Можно последовать Выбрать примеру Waypoints for Unreal Engine Simulation (Automated Driving Toolbox), чтобы в интерактивном режиме выбрать последовательность парковки местоположений. Можно использовать тот же подход, чтобы выбрать последовательность waypoints и сгенерировать ссылочную траекторию для автомобиля, оборудованного датчиком. Этот пример использует записанную ссылочную траекторию и припаркованные местоположения транспортного средства.

% Load reference path
data = load('parkingLotReferenceData.mat');

% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;

% Set poses of the parked vehicles
parkedPoses = data.parkedPoses;

% Display the reference path and the parked vehicle locations
sceneName = 'LargeParkingLot';
hScene = figure;
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2), refPosesY(:,2), 'LineWidth', 2, 'DisplayName', 'Reference Path');
scatter(parkedPoses(:,1), parkedPoses(:,2), [], 'filled', 'DisplayName', 'Parked Vehicles');
xlim([-60 40])
ylim([10 60])
hScene.Position = [100, 100, 1000, 500]; % Resize figure
legend
hold off

Откройте модель и добавьте припаркованные транспортные средства

modelName = 'GenerateImageDataOfParkingLot';
open_system(modelName);
snapnow;

helperAddParkedVehicles(modelName, parkedPoses);

Настройте датчик автомобиля, оборудованного датчиком и камеры

Настройте автомобиль, оборудованный датчиком, проходящий заданный ссылочный путь при помощи Симуляции 3D Транспортное средство с блоком Ground Following. Подсистема Варианта Камеры содержит две настройки датчиков камеры: монокуляр и стерео. В обеих настройках камера смонтирована на центре крыши транспортного средства. Можно использовать Camera Calibrator или приложение Stereo Camera Calibrator, чтобы оценить внутренние параметры фактической камеры, которую вы хотите симулировать. Этот пример показывает монокулярный рабочий процесс камеры, сначала сопровождаемый рабочим процессом стереофотоаппарата.

% Select monocular camera
useMonoCamera = 1;

% Inspect the monocular camera
open_system([modelName, '/Camera/Monocular']);
snapnow;

% Camera intrinsics
focalLength    = [700, 700];  % specified in units of pixels
principalPoint = [600, 180];  % in pixels [x, y]
imageSize      = [370, 1230]; % in pixels [mrows, ncols]
intrinsics     = cameraIntrinsics(focalLength, principalPoint, imageSize);

Визуализируйте и запишите данные о датчике

Запустите симуляцию, чтобы визуализировать и записать данные о датчике. Используйте блок Video Viewer, чтобы визуализировать изображение выход из датчика камеры. Используйте блок To Workspace, чтобы записать местоположение основной истины и ориентацию датчика камеры.

close(hScene)

if ~ispc
    error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end

% Run simulation
simOut = sim(modelName);
snapnow;

% Extract camera images as an imageDatastore
imds = helperGetCameraImages(simOut);

% Extract ground truth as an array of rigid3d objects
gTruth = helperGetSensorGroundTruth(simOut);

Разработайте монокулярный визуальный алгоритм SLAM Используя записанные данные

Используйте изображения, чтобы оценить монокулярный визуальный алгоритм SLAM. Функциональный helperVisualSLAM реализует монокулярный трубопровод ORB-SLAM:

Инициализация карты: ORB-SLAM запускается путем инициализации карты 3-D точек от двух изображений. Используйте relativeCameraPose чтобы вычислить относительное положение на основе 2D ORB показывают соответствия и triangulate вычислить 3-D точки карты. Две системы координат хранятся в imageviewset возразите как ключевые кадры. 3-D точки карты и их соответствия к ключевым кадрам хранятся в worldpointset объект.
Отслеживание: Если карта инициализируется, для каждого нового изображения, функционального helperTrackLastKeyFrame оценивает положение камеры путем соответствия с функциями в текущей системе координат к функциям в последнем ключевом кадре. Функциональный helperTrackLocalMap совершенствовал предполагаемое положение камеры путем отслеживания локальной карты.
Локальное Отображение: текущая система координат используется, чтобы создать новые 3-D точки карты, если она идентифицирована как ключевой кадр. На данном этапе, bundleAdjustment используется, чтобы минимизировать ошибки перепроекции путем корректировки положения камеры и 3-D точек.
Закрытие цикла: Циклы обнаруживаются для каждого ключевого кадра путем сравнения его против всех предыдущих ключевых кадров с помощью подхода набора признаков. Если закрытие цикла обнаруживается, график положения оптимизирован, чтобы совершенствовать положения камеры всех ключевых кадров с помощью optimizePoseGraph (Navigation Toolbox) функция.

Для получения дополнительной информации реализации алгоритма смотрите Монокулярный Визуальный Одновременный пример Локализации и картографии.

[mapPlot, optimizedPoses, addedFramesIdx] = helperVisualSLAM(imds, intrinsics);

Map initialized with frame 1 and frame 3
Loop edge added between keyframe: 7 and 156

Оцените против основной истины

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

% Plot the camera ground truth trajectory
scaledTrajectory = plotActualTrajectory(mapPlot, gTruth(addedFramesIdx), optimizedPoses);

% Show legend
showLegend(mapPlot);

Можно также вычислить среднеквадратичную ошибку (RMSE) оценок траектории.

helperEstimateTrajectoryError(gTruth(addedFramesIdx), scaledTrajectory);

Absolute RMSE for key frame trajectory (m): 11.8389

Визуальный алгоритм SLAM стерео

В монокулярном визуальном алгоритме SLAM глубина не может быть точно определена с помощью одной камеры. Шкала карты и предполагаемой траектории неизвестна и дрейфует в зависимости от времени. Кроме того, потому что точки карты часто не могут триангулироваться от первой системы координат, загружение системы требует, чтобы несколько представлений произвели первоначальную карту. Используя стереофотоаппарат решает эти задачи и предоставляет более надежное визуальное решение SLAM. Функциональный helperVisualSLAMStereo реализует визуальный трубопровод SLAM стерео. Основное отличие от монокулярного трубопровода на этапе инициализации карты, трубопровод стерео создает 3-D точки карты из пары стереоизображений той же системы координат, вместо того, чтобы создать их из двух изображений различных систем координат. Для получения дополнительной информации реализации алгоритма смотрите Визуальный Одновременный пример Локализации и картографии Стерео.

% Select stereo camera
useMonoCamera = 0;

% Inspect the stereo camera
open_system([modelName, '/Camera/Stereo']);
snapnow;

% Set stereo camera baseline
baseline = 0.5; % In meters

% Run simulation
simOut = sim(modelName);
snapnow;

Извлеките стереоизображения

[imdsLeft, imdsRight] = helperGetCameraImagesStereo(simOut);

% Extract ground truth as an array of rigid3d objects
gTruth = helperGetSensorGroundTruth(simOut);

Запустите визуальный алгоритм SLAM стерео

[mapPlot, optimizedPoses, addedFramesIdx] = helperVisualSLAMStereo(imdsLeft, imdsRight, intrinsics, baseline);

% Plot the camera ground truth trajectory
optimizedTrajectory = plotActualTrajectory(mapPlot, gTruth(addedFramesIdx));

% Show legend
showLegend(mapPlot);

% Calculate the root mean square error (RMSE) of trajectory estimates
helperEstimateTrajectoryError(gTruth(addedFramesIdx), optimizedTrajectory);

Loop edge added between keyframe: 5 and 145
Absolute RMSE for key frame trajectory (m): 0.25164

По сравнению с монокулярным визуальным алгоритмом SLAM визуальный алгоритм SLAM стерео производит более точную оценку траектории камеры.

Закройте модель и фигуры.

close_system(modelName, 0);
close all

Вспомогательные Функции

helperGetCameraImages Выводили камеру

function imds = helperGetCameraImages(simOut)
% Save image data to a temporary folder
dataFolder   = fullfile(tempdir, 'parkingLotImages', filesep);
folderExists = exist(dataFolder, 'dir');
if ~folderExists
    mkdir(dataFolder);
end

files = dir(dataFolder);
if numel(files) < 3
    numFrames = numel(simOut.images.Time);
    for i = 3:numFrames % Ignore the first two frames
        img = squeeze(simOut.images.Data(:,:,:,i));
        imwrite(img, [dataFolder, sprintf('%04d', i-2), '.png'])
    end
end

% Create an imageDatastore object to store all the images
imds = imageDatastore(dataFolder);
end

helperGetCameraImagesStereo Выводили камеру

function [imdsLeft, imdsRight] = helperGetCameraImagesStereo(simOut)
% Save image data to a temporary folder
dataFolderLeft   = fullfile(tempdir, 'parkingLotStereoImages', filesep, 'left', filesep);
dataFolderRight  = fullfile(tempdir, 'parkingLotStereoImages', filesep, 'right', filesep);
folderExists     = exist(dataFolderLeft, 'dir');
if ~folderExists
    mkdir(dataFolderLeft);
    mkdir(dataFolderRight);
end

files = dir(dataFolderLeft);
if numel(files) < 3
    numFrames = numel(simOut.imagesLeft.Time);
    for i = 3:numFrames % Ignore the first two frames
        imgLeft = squeeze(simOut.imagesLeft.Data(:,:,:,i));
        imwrite(imgLeft, [dataFolderLeft, sprintf('%04d', i-2), '.png'])

        imgRight = squeeze(simOut.imagesRight.Data(:,:,:,i));
        imwrite(imgRight, [dataFolderRight, sprintf('%04d', i-2), '.png'])
    end
end

% Use imageDatastore objects to store the stereo images
imdsLeft  = imageDatastore(dataFolderLeft);
imdsRight = imageDatastore(dataFolderRight);
end

helperGetSensorGroundTruth Сохраняют основную истину датчика

function gTruth = helperGetSensorGroundTruth(simOut)
numFrames = numel(simOut.location.Time);
gTruth = repmat(rigid3d, numFrames-2, 1);
for i = 1:numFrames-2 % Ignore the first two frames
    gTruth(i).Translation = squeeze(simOut.location.Data(:, :, i+2));
    % Ignore the roll and the pitch rotations since the ground is flat
    yaw = double(simOut.orientation.Data(:, 3, i+2));
    gTruth(i).Rotation = [cos(yaw), sin(yaw), 0; ...
        -sin(yaw), cos(yaw), 0; ...
        0, 0, 1];
end
end

helperEstimateTrajectoryError Вычисляют ошибку отслеживания

function rmse = helperEstimateTrajectoryError(gTruth, scaledLocations)
gLocations      = vertcat(gTruth.Translation);

rmse = sqrt(mean( sum((scaledLocations - gLocations).^2, 2) ));
disp(['Absolute RMSE for key frame trajectory (m): ', num2str(rmse)]);
end

Документация