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

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

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

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

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

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

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

Можно последовать Выбрать примеру Waypoints for Unreal Engine Simulation, чтобы в интерактивном режиме выбрать последовательность парковки местоположений. Можно использовать тот же подход, чтобы выбрать последовательность 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. Смонтируйте камеру на центре крыши транспортного средства при помощи блока Simulation 3D Camera и задайте его внутренние параметры. Можно использовать Camera Calibrator (Computer Vision Toolbox) приложение, чтобы оценить внутренние параметры камеры фактической камеры, которую вы хотите симулировать.

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

close_system([modelName, '/Video Viewer']);

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

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

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

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

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

Map initialized with frame 1 and frame 3
Loop edge added between keyframe: 5 and 173
Iteration 1, residual error 0.329001
Iteration 2, residual error 0.322270
Iteration 3, residual error 0.322259
Iteration 4, residual error 0.322259
Solver stopped because change in function value was less than specified function tolerance.

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

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

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

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

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

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

function gTruth = helperGetSensorGroundTruth(simOut)
numFrames = numel(simOut.images.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

Смотрите также

bundleAdjustment (Computer Vision Toolbox) | imageviewset (Computer Vision Toolbox) | optimizePoses (Computer Vision Toolbox) | relativeCameraPose (Computer Vision Toolbox) | triangulate (Computer Vision Toolbox)

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