В этом примере показано, как разработать визуальный алгоритм одновременной локализации и отображения (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. Функция 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 точки карты из пары стереоизображений одной и той же системы координат, вместо создания их из двух изображений разных систем координат. Для деталей реализации алгоритма смотрите пример 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