В этом примере показано, как разработать визуальный алгоритм Одновременной локализации и картографии (SLAM) с помощью данных изображения, полученных из Нереальной среды симуляции Engine®.
Визуальный SLAM является процессом вычисления положения и ориентации камеры относительно ее среды, одновременно сопоставляя среду. Разработка визуального алгоритма SLAM и оценка его эффективности в различных условиях являются сложной задачей. Одна из самых сложных задач генерирует основную истину датчика камеры, особенно в наружных средах. Использование симуляции позволяет тестировать под множеством сценариев и настроек камеры при обеспечении точной основной истины.
Этот пример демонстрирует использование Нереальной симуляции Engine, чтобы разработать визуальный алгоритм SLAM или для монокуляра или для стереофотоаппарата в сценарии парковки. Для получения дополнительной информации о реализации визуальных конвейеров SLAM, смотрите Монокулярный Визуальный Одновременный пример Локализации и картографии и Визуальный Одновременный пример Локализации и картографии Стерео.
Используйте Симуляцию 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. Подсистема Варианта Камеры содержит две настройки датчиков камеры: монокуляр и стерео. В обеих настройках камера смонтирована на центре крыши транспортного средства. Можно использовать 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 точки карты из пары стереоизображений той же системы координат, вместо того, чтобы создать их из двух изображений различных систем координат. Для получения дополнительной информации реализации алгоритма смотрите Визуальный Одновременный пример Локализации и картографии Стерео.
% 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
bundleAdjustment | imageviewset | optimizePoses | relativeCameraPose | triangulate