В этом примере показано, как разработать визуальный алгоритм одновременной локализации и отображения (SLAM) с использованием данных изображения, полученных из среды моделирования Unreal Engine ®.
Визуальный SLAM - это процесс вычисления положения и ориентации камеры относительно ее окружения с одновременным отображением среды. Разработка визуального алгоритма SLAM и оценка его производительности в различных условиях является сложной задачей. Одна из самых серьезных проблем заключается в том, чтобы получить достоверную информацию о датчике камеры, особенно в наружной среде. Использование моделирования позволяет проводить тестирование в различных сценариях и конфигурациях камер, обеспечивая при этом точную достоверность данных на земле.
Этот пример демонстрирует использование моделирования Unreal Engine для разработки визуального алгоритма SLAM для монокулярной или стереокамеры в сценарии парковки. Дополнительные сведения о реализации визуальных SLAM-конвейеров см. в примере «Одновременная монокулярная визуальная локализация и отображение» и в примере «Одновременная стерео визуальная локализация и отображение».
Для настройки среды моделирования используется блок «Моделирование» 3D «Конфигурация сцены». Выберите встроенную сцену «Большая стоянка», которая содержит несколько припаркованных транспортных средств. Визуальный алгоритм SLAM сопоставляет элементы между последовательными изображениями. Для увеличения количества возможных совпадений элементов можно использовать подсистему «Припаркованные транспортные средства» для добавления к сцене дополнительных припаркованных транспортных средств. Для указания стояночных поз транспортных средств используйте helperAddParkedVehicle функция. При выборе более естественной сцены наличие дополнительных транспортных средств не требуется. Естественные сцены обычно имеют достаточно текстуры и разновидности признаков, подходящих для согласования признаков.
Для интерактивного выбора последовательности стоянок можно следовать примеру «Выбор ППМ для моделирования нереального двигателя» (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);
Настройте транспортное средство эго, проходящее указанный справочный путь при помощи Моделирования 3D Транспортное средство с Землей После блока. Подсистема варианта камеры содержит две конфигурации датчиков камеры: монокулярную и стереосистему. В обеих конфигурациях камера установлена на центре крыши транспортного средства. Можно использовать приложение «Калибратор камеры» или «Калибратор стереокамеры» для оценки характеристик фактической камеры, которую требуется смоделировать. В этом примере показан рабочий процесс монокулярной камеры, за которым следует рабочий процесс стереокамеры.
% 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 для визуализации изображения, выводимого с датчика камеры. Блок «В рабочее пространство» используется для записи местоположения и ориентации датчика камеры.
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. Функция helperVisualSLAM реализует монокулярный трубопровод ORB-SLAM:
Инициализация карты: ORB-SLAM начинается с инициализации карты точек 3-D с двух изображений. Использовать relativeCameraPosetriangulateimageviewsetworldpointset объект.
Отслеживание: После инициализации карты для каждого нового изображения функция helperTrackLastKeyFrame оценивает положение камеры путем сопоставления функций в текущем кадре с функциями в последнем ключевом кадре. Функция helperTrackLocalMap корректирует предполагаемую позу камеры путем отслеживания локальной карты.
Локальное сопоставление: текущий кадр используется для создания новых точек карты 3-D, если он идентифицирован как ключевой кадр. На данном этапе bundleAdjustment
Замыкание петли: Петли обнаруживаются для каждого ключевого кадра, сравнивая его со всеми предыдущими ключевыми кадрами с использованием подхода мешка функций. После обнаружения замыкания контура график позы оптимизируется для уточнения позиций камеры всех ключевых кадров с помощью 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. Функция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; % 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
helperGetCamureImages Получение выходных данных камеры
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
helperGetCamureStereo Получение выходных данных камеры
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
helperTrajectiveError Вычислить ошибку отслеживания
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