Разрабатывайте алгоритм Visual SLAM, используя Unreal Симуляция

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

Открыть скрипт

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

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

Этот пример демонстрирует использование симуляции Unreal Engine для разработки визуального алгоритма SLAM для монокулярной или стереофотоаппарата в сценарии парковки. Для получения дополнительной информации о реализации визуальных трубопроводов SLAM смотрите пример Monocular Visual Simultaneous Localization and Mapping и пример Stereo Visual Simultaneous Localization and Mapping.

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

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

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

% 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);

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

Настройте ego vehicle, движущийся по заданному исходному пути, с помощью блока Simulation 3D Vehicle with Ground Following. Camera Variant Subsystem содержит два строений датчиков камеры: монокулярную и стерео. В обоих строениях камера установлена на центре крыши транспортного средства. Можно использовать приложение 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

% Open video viewer to examine camera images
open_system([modelName, '/Video Viewer']);

% 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 вычислить относительное положение на основе 2-D 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
Creating an inverted image index using Bag-Of-Features.
-------------------------------------------------------

Encoding images using Bag-Of-Features.
--------------------------------------

* Encoding 2 images...done.
Finished creating the image index.
Loop edge added between keyframe: 12 and 232

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

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

% 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): 2.267

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

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

% 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;

% Extract stereo images
[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);

Creating an inverted image index using Bag-Of-Features.
-------------------------------------------------------

Encoding images using Bag-Of-Features.
--------------------------------------

* Encoding 2 images...done.
Finished creating the image index.
Loop edge added between keyframe: 5 and 141
Absolute RMSE for key frame trajectory (m): 0.45941

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

Закройте модель и рисунки.

close_system([modelName, '/Video Viewer']);
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

Документация по Computer Vision Toolbox

Поддержка

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