Визуальная одновременная локализация и картография стерео
Визуальная одновременная локализация и картография (vSLAM), относится к процессу вычисления положения и ориентации камеры относительно ее среды, одновременно сопоставляя среду. Процесс использует только визуальные входные параметры от камеры. Приложения для vSLAM включают дополненную реальность, робототехнику и автономное управление автомобилем.
vSLAM может быть выполнен при помощи только монокулярной камеры. Однако, поскольку глубина не может быть точно вычислена с помощью одной камеры, шкала карты и предполагаемой траектории неизвестна и дрейфует в зависимости от времени. Кроме того, чтобы загрузить систему, несколько представлений требуются, чтобы производить первоначальную карту, когда она не может быть триангулирована от первой системы координат. Используя стереофотоаппарат решает эти задачи и предоставляет более надежное vSLAM решение.
В этом примере показано, как обработать данные изображения от стереофотоаппарата, чтобы создать карту наружной среды и оценить траекторию камеры. Пример использует версию алгоритма ORB-SLAM2 [1], который основан на функции и поддерживает стереофотоаппараты.
Обзор конвейера обработки
Трубопровод для стерео vSLAM очень похож на монокуляр vSLAM трубопровод в Монокулярном Визуальном Одновременном примере Локализации и картографии. Существенное различие - то, которые в точках карты этапа 3-D Инициализации Карты создаются из пары стереоизображений той же стереопары вместо двух изображений различных систем координат.
Инициализация карты: трубопровод запускается путем инициализации карты 3-D точек от пары стереоизображений с помощью карты несоизмеримости. Левое изображение хранится как первый ключевой кадр.
Отслеживание: Если карта инициализируется для каждой новой стереопары, положение камеры оценивается путем соответствия с функциями в левом изображении к функциям в последнем ключевом кадре. Предполагаемое положение камеры усовершенствовано путем отслеживания локальной карты.
Локальное Отображение: Если текущее левое изображение идентифицировано как ключевой кадр, новые 3-D точки карты вычисляются из несоизмеримости стереопары. На данном этапе свяжитесь, корректировка используется, чтобы минимизировать ошибки перепроекции путем корректировки положения камеры и 3-D точек.
Закрытие цикла: Циклы обнаруживаются для каждого ключевого кадра путем сравнения его против всех предыдущих ключевых кадров с помощью подхода набора признаков. Если закрытие цикла обнаруживается, график положения оптимизирован, чтобы совершенствовать положения камеры всех ключевых кадров.
Загрузите и исследуйте входную последовательность стереоизображения
Данные, используемые в этом примере, от Долгосрочного Набора данных Локализации и картографии UTIAS, обеспеченного Институтом Университета Торонто Космических Исследований. Можно загрузить данные на временную директорию с помощью веб-браузера или путем выполнения следующего кода:
ftpObj = ftp('asrl3.utias.utoronto.ca'); tempFolder = fullfile(tempdir); dataFolder = [tempFolder, '2020-vtr-dataset/UTIAS-In-The-Dark/']; zipFileName = [dataFolder, 'run_000005.zip']; folderExists = exist(dataFolder, 'dir'); % Create a folder in a temporary directory to save the downloaded file if ~folderExists mkdir(dataFolder); disp('Downloading run_000005.zip (818 MB). This download can take a few minutes.') mget(ftpObj,'/2020-vtr-dataset/UTIAS-In-The-Dark/run_000005.zip', tempFolder); % Extract contents of the downloaded file disp('Extracting run_000005.zip (818 MB) ...') unzip(zipFileName, dataFolder); end
Используйте два imageDatastore объекты сохранить стереоизображения.
imgFolderLeft = [dataFolder,'images/left/']; imgFolderRight = [dataFolder,'images/right/']; imdsLeft = imageDatastore(imgFolderLeft); imdsRight = imageDatastore(imgFolderRight); % Inspect the first pair of images currFrameIdx = 1; currILeft = readimage(imdsLeft, currFrameIdx); currIRight = readimage(imdsRight, currFrameIdx); imshowpair(currILeft, currIRight, 'montage');
Сопоставьте инициализацию
Трубопровод ORB-SLAM запускается путем инициализации карты, которая содержит 3-D мировые точки. Этот шаг крайне важен и оказывает значительное влияние на точность итогового результата SLAM. Начальные соответствия характерной точки ORB найдены с помощью matchFeatures между двумя изображениями стереопары. Совпадающие пары должны удовлетворить следующим ограничениям:
Горизонтальный сдвиг между двумя соответствующими характерными точками в исправленном изображении стереопары меньше максимальной несоизмеримости. Можно определить аппроксимированное максимальное значение несоизмеримости из анаглифа стерео изображения стереопары. Для получения дополнительной информации смотрите Range of Disparity Выбора.
Вертикальный сдвиг между двумя соответствующими характерными точками в исправленном изображении стереопары меньше порога.
Шкалы совпадающих функций почти идентичны.
3-D мировые местоположения, соответствующие совпадающим характерным точкам, определяются можно следующим образом:
Используйте
disparitySGMвычислить карту несоизмеримости для каждой пары стереоизображений при помощи метода полуглобального соответствия (SGM).Используйте
reconstructSceneвычислить 3-D мировые координаты точки из карты несоизмеримости.Найдите местоположения в карте несоизмеримости, которые соответствуют характерным точкам и их 3-D мировым местоположениям.
% Set random seed for reproducibility rng(0); % Load the initial camera pose. The initial camera pose is derived based % on the transformation between the camera and the vehicle: % http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/transform_camera_vehicle.tx initialPoseData = load('initialPose.mat'); initialPose = initialPoseData.initialPose; % Create a stereoParameters object to store the stereo camera parameters. % The intrinsics for the dataset can be found at the following page: % http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/camera_parameters.txt focalLength = [387.777 387.777]; % specified in pixels principalPoint = [257.446 197.718]; % specified in pixels [x, y] baseline = 0.239965; % specified in meters intrinsicMatrix = [focalLength(1), 0, 0; ... 0, focalLength(2), 0; ... principalPoint(1), principalPoint(2), 1]; imageSize = size(currILeft,[1,2]); % in pixels [mrows, ncols] cameraParam = cameraParameters('IntrinsicMatrix', intrinsicMatrix, 'ImageSize', imageSize); intrinsics = cameraParam.Intrinsics; stereoParams = stereoParameters(cameraParam, cameraParam, eye(3), [-baseline, 0 0]); % In this example, the images are already undistorted. In a general % workflow, uncomment the following code to undistort the images. % currILeft = undistortImage(currILeft, intrinsics); % currIRight = undistortImage(currIRight, intrinsics); % Rectify the stereo images [currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereoParams, 'OutputView','full'); % Detect and extract ORB features from the rectified stereo images scaleFactor = 1.2; numLevels = 8; [currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels); [currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures(currIRight, scaleFactor, numLevels); % Match feature points between the stereo images and get the 3-D world positions maxDisparity = 48; % specified in pixels [xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, ... currFeaturesLeft, currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, initialPose, maxDisparity);
Управление данными и визуализация
После того, как карта инициализируется с помощью первой стереопары, можно использовать imageviewset, worldpointset и helperViewDirectionAndDepth сохранить первые ключевые кадры и соответствующие точки карты:
% Create an empty imageviewset object to store key frames vSetKeyFrames = imageviewset; % Create an empty worldpointset object to store 3-D map points mapPointSet = worldpointset; % Create a helperViewDirectionAndDepth object to store view direction and depth directionAndDepth = helperViewDirectionAndDepth(size(xyzPoints, 1)); % Add the first key frame currKeyFrameId = 1; vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose, 'Points', currPointsLeft,... 'Features', currFeaturesLeft.Features); % Add 3-D map points [mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints); % Add observations of the map points mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, stereoMapPointsIdx, matchedPairs(:, 1)); % Visualize matched features in the first key frame featurePlot = helperVisualizeMatchedFeaturesStereo(currILeft, currIRight, currPointsLeft, ... currPointsRight, matchedPairs); % Visualize initial map points and camera trajectory mapPlot = helperVisualizeMotionAndStructureStereo(vSetKeyFrames, mapPointSet); % Show legend showLegend(mapPlot);
Инициализируйте базу данных распознавания места
Обнаружение цикла выполняется с помощью подхода сумок слов. Визуальный словарь, представленный как bagOfFeatures объект создается оффлайн с дескрипторами ORB, извлеченными из большого набора изображений в наборе данных путем вызова:
bag = bagOfFeatures(imds,'CustomExtractor', @helperORBFeatureExtractorFunction, 'TreeProperties', [2, 20], 'StrongestFeatures', 1);
где imds imageDatastore объект, хранящий учебные изображения и helperORBFeatureExtractorFunction функция экстрактора функции ORB. Смотрите Извлечение Изображений с Мешком Визуальных Слов для получения дополнительной информации.
Процесс закрытия цикла инкрементно создает базу данных, представленную как invertedImageIndex объект, который хранит визуальное отображение слова к изображению на основе мешка функций ORB.
% Load the bag of features data created offline bofData = load('bagOfFeaturesUTIAS.mat'); % Initialize the place recognition database loopDatabase = invertedImageIndex(bofData.bof,"SaveFeatureLocations", false); % Add features of the first key frame to the database addImageFeatures(loopDatabase, currFeaturesLeft, currKeyFrameId);
Отслеживание
Процесс отслеживания выполняется с помощью каждой пары и определяет, когда вставить новый ключевой кадр.
% ViewId of the last key frame lastKeyFrameId = currKeyFrameId; % ViewId of the reference key frame that has the most co-visible % map points with the current key frame refKeyFrameId = currKeyFrameId; % Index of the last key frame in the input image sequence lastKeyFrameIdx = currFrameIdx; % Indices of all the key frames in the input image sequence addedFramesIdx = lastKeyFrameIdx; currFrameIdx = 2; isLoopClosed = false;
Каждый кадр обрабатывается можно следующим образом:
Функции ORB извлечены для каждой новой стереопары изображений и затем соответствующие (использование
matchFeatures), с функциями в последнем ключевом кадре, которые знали соответствующие 3-D точки карты.Оцените положение камеры с алгоритмом Перспективного n значений с помощью
estimateWorldCameraPose.Учитывая положение камеры, спроектируйте точки карты, наблюдаемые последним ключевым кадром в текущую систему координат, и ищите соответствия функции с помощью
matchFeaturesInRadius.С 3-D к 2D соответствиям в текущей системе координат совершенствуйте положение камеры путем выполнения корректировки пакета только для движения с помощью
bundleAdjustmentMotion.Спроектируйте локальные точки карты в текущую систему координат, чтобы искать больше соответствий функции с помощью
matchFeaturesInRadiusbundleAdjustmentMotion.Последний шаг отслеживания должен решить, должна ли текущая система координат быть новым ключевым кадром. Система координат является ключевым кадром, если обоим из следующих условий удовлетворяют:
По крайней мере 5 систем координат передали начиная с последнего ключевого кадра или текущей системы координат отслеживает меньше чем 100 точек карты.
Точки карты, прослеженные текущей системой координат, составляют меньше чем 90% точек, прослеженных ссылочным ключевым кадром.
Если текущая система координат должна стать ключевым кадром, продолжиться к Локальному процессу Отображения. В противном случае начните Отслеживать для следующей системы координат.
% Main loop while ~isLoopClosed && currFrameIdx <= numel(imdsLeft.Files) currILeft = readimage(imdsLeft, currFrameIdx); currIRight = readimage(imdsRight, currFrameIdx); [currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereoParams, 'OutputView','full'); [currFeaturesLeft, currPointsLeft] = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels); [currFeaturesRight, currPointsRight] = helperDetectAndExtractFeatures(currIRight, scaleFactor, numLevels); % Track the last key frame % trackedMapPointsIdx: Indices of the map points observed in the current left frame % trackedFeatureIdx: Indices of the corresponding feature points in the current left frame [currPose, trackedMapPointsIdx, trackedFeatureIdx] = helperTrackLastKeyFrame(mapPointSet, ... vSetKeyFrames.Views, currFeaturesLeft, currPointsLeft, lastKeyFrameId, intrinsics, scaleFactor); if isempty(currPose) || numel(trackedMapPointsIdx) < 30 currFrameIdx = currFrameIdx + 1; continue end % Track the local map % refKeyFrameId: ViewId of the reference key frame that has the most % co-visible map points with the current frame % localKeyFrameIds: ViewId of the connected key frames of the current frame if currKeyFrameId == 1 refKeyFrameId = 1; localKeyFrameIds = 1; else [refKeyFrameId, localKeyFrameIds, currPose, trackedMapPointsIdx, trackedFeatureIdx] = ... helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, trackedMapPointsIdx, ... trackedFeatureIdx, currPose, currFeaturesLeft, currPointsLeft, intrinsics, scaleFactor, numLevels); end % Match feature points between the stereo images and get the 3-D world positions [xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, currFeaturesLeft, ... currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, currPose, maxDisparity); [untrackedFeatureIdx, ia] = setdiff(matchedPairs(:, 1), trackedFeatureIdx); xyzPoints = xyzPoints(ia, :); % Check if the current frame is a key frame isKeyFrame = helperIsKeyFrame(mapPointSet, refKeyFrameId, lastKeyFrameIdx, ... currFrameIdx, trackedMapPointsIdx); % Visualize matched features in the stereo image updatePlot(featurePlot, currILeft, currIRight, currPointsLeft, currPointsRight, trackedFeatureIdx, matchedPairs); if ~isKeyFrame && currFrameIdx < numel(imdsLeft.Files) currFrameIdx = currFrameIdx + 1; continue end % Update current key frame ID currKeyFrameId = currKeyFrameId + 1;
Локальное отображение
Локальное отображение выполняется для каждого ключевого кадра. Когда новый ключевой кадр будет определен, добавьте его в ключевые кадры и обновите атрибуты точек карты, наблюдаемых новым ключевым кадром. Гарантировать тот mapPointSet содержит как можно меньше выбросов, допустимая точка карты должна наблюдаться по крайней мере в 3 ключевых кадрах.
Новые точки карты создаются путем триангулирования характерных точек ORB в текущем ключевом кадре и его связанных ключевых кадрах. Для каждой несопоставленной характерной точки в текущем ключевом кадре ищите соответствие с другими несопоставленными точками в связанных ключевых кадрах с помощью matchFeatures. Локальная корректировка пакета совершенствовала положение текущего ключевого кадра, положения связанных ключевых кадров и все точки карты, наблюдаемые в этих ключевых кадрах.
% Add the new key frame
[mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame(mapPointSet, vSetKeyFrames, ...
currPose, currFeaturesLeft, currPointsLeft, trackedMapPointsIdx, trackedFeatureIdx, localKeyFrameIds);
% Remove outlier map points that are observed in fewer than 3 key frames
if currKeyFrameId == 2
triangulatedMapPointsIdx = [];
end
[mapPointSet, directionAndDepth, trackedMapPointsIdx] = ...
helperCullRecentMapPoints(mapPointSet, directionAndDepth, trackedMapPointsIdx, triangulatedMapPointsIdx, ...
stereoMapPointsIdx);
% Add new map points computed from disparity
[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, stereoMapPointsIdx, ...
untrackedFeatureIdx);
% Create new map points by triangulation
minNumMatches = 20;
minParallax = 0.35;
[mapPointSet, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = helperCreateNewMapPointsStereo( ...
mapPointSet, vSetKeyFrames, currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax, ...
untrackedFeatureIdx, stereoMapPointsIdx);
% Update view direction and depth
directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, ...
[trackedMapPointsIdx; triangulatedMapPointsIdx; stereoMapPointsIdx], true);
% Local bundle adjustment
[mapPointSet, directionAndDepth, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = ...
helperLocalBundleAdjustmentStereo(mapPointSet, directionAndDepth, vSetKeyFrames, ...
currKeyFrameId, intrinsics, triangulatedMapPointsIdx, stereoMapPointsIdx);
% Visualize 3-D world points and camera trajectory
updatePlot(mapPlot, vSetKeyFrames, mapPointSet);Закрытие цикла
Шаг закрытия цикла берет текущий ключевой кадр, обработанный локальным процессом отображения, и пытается обнаружить и замкнуть круг. Кандидаты цикла идентифицированы путем запроса изображений в базе данных, которые визуально похожи на текущий ключевой кадр с помощью evaluateImageRetrieval. Система координат ключа-кандидата допустима, если она не соединяется с последним ключевым кадром, и три из его соседних ключевых кадров являются кандидатами цикла.
Когда допустимый кандидат закрытия цикла будет найден, вычислите относительное положение между кандидатом закрытия цикла система координат и текущим ключевым кадром с помощью estimateGeometricTransform3D. Затем добавьте связь цикла с относительным положением и обновите mapPointSet и vSetKeyFrames.
% Check loop closure after some key frames have been created if currKeyFrameId > 50 % Minimum number of feature matches of loop edges loopEdgeNumMatches = 45; % Detect possible loop closure key frame candidates [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ... loopDatabase, currILeft, loopEdgeNumMatches); isTooCloseView = currKeyFrameId - max(validLoopCandidates) < 100; if isDetected && ~isTooCloseView % Add loop closure connections [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo(... mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ... currFeaturesLeft, loopEdgeNumMatches); end end % If no loop closure is detected, add current features into the database if ~isLoopClosed addImageFeatures(loopDatabase, currFeaturesLeft, currKeyFrameId); end % Update IDs and indices lastKeyFrameId = currKeyFrameId; lastKeyFrameIdx = currFrameIdx; addedFramesIdx = [addedFramesIdx; currFrameIdx]; currFrameIdx = currFrameIdx + 1; end % End of main loop
Loop edge added between keyframe: 1 and 282 Loop edge added between keyframe: 4 and 282 Loop edge added between keyframe: 2 and 282
Наконец, примените оптимизацию графика положения по существенному графику в vSetKeyFrames откорректировать дрейф. Существенный график создается внутренне путем удаления связей с меньше, чем minNumMatches соответствия в covisibility графике. После оптимизации графика положения обновите 3-D местоположения точек карты с помощью оптимизированных положений.
% Optimize the poses minNumMatches = 30; vSetKeyFramesOptim = optimizePoses(vSetKeyFrames, minNumMatches, 'Tolerance', 1e-16); % Update map points after optimizing the poses mapPointSet = helperUpdateGlobalMap(mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim); updatePlot(mapPlot, vSetKeyFrames, mapPointSet); % Plot the optimized camera trajectory optimizedPoses = poses(vSetKeyFramesOptim); plotOptimizedTrajectory(mapPlot, optimizedPoses) % Update legend showLegend(mapPlot);
Сравните с основной истиной
Можно сравнить оптимизированную траекторию камеры с основной истиной, чтобы оценить точность решения. Загруженные данные содержат gps.txt файл, который хранит местоположение GPS для каждой системы координат. Можно преобразовать местоположение GPS от географического до локальных Декартовых координат с помощью latlon2local (Automated Driving Toolbox) от Automated Driving Toolbox или geodetic2enu (Mapping Toolbox) от Mapping Toolbox. В этом примере можно просто загрузить конвертированные данные о GPS из M-файла.
% Load GPS data gpsData = load('gpsLocation.mat'); gpsLocation = gpsData.gpsLocation; % Transform GPS locations to the reference coordinate system gTruth = helperTransformGPSLocations(gpsLocation, optimizedPoses); % Plot the GPS locations plotActualTrajectory(mapPlot, gTruth(addedFramesIdx, :)); % Show legend showLegend(mapPlot);
Вспомогательные Функции
Короткие функции помощника описаны ниже. Большая функция включена в отдельные файлы.
helperDetectAndExtractFeatures обнаружьте и извлеките и функции ORB из изображения.
function [features, validPoints] = helperDetectAndExtractFeatures(Irgb, scaleFactor, numLevels) numPoints = 800; % Detect ORB features Igray = im2gray(Irgb); points = detectORBFeatures(Igray, 'ScaleFactor', scaleFactor, 'NumLevels', numLevels); % Select a subset of features, uniformly distributed throughout the image points = selectUniform(points, numPoints, size(Igray, 1:2)); % Extract features [features, validPoints] = extractFeatures(Igray, points); end
helperReconstructFromStereo восстановите сцену из стереоизображения с помощью карты несоизмеримости
function [xyzPoints, indexPairs] = helperReconstructFromStereo(I1, I2, ... features1, features2, points1, points2, stereoParams, currPose, maxDisparity) indexPairs = helperFindValidFeaturePairs(features1, features2, points1, points2, maxDisparity); % Compute disparity for all pixels in the left image. In practice, it is more % common to compute disparity just for the matched feature points. disparityMap = disparitySGM(rgb2gray(I1), rgb2gray(I2), 'DisparityRange', [0, maxDisparity]); xyzPointsAll = reconstructScene(disparityMap, stereoParams); % Find the corresponding world point of the matched feature points locations = floor(points1.Location(indexPairs(:, 1), [2 1])); xyzPoints = []; isPointFound = false(size(points1)); for i = 1:size(locations, 1) point3d = squeeze(xyzPointsAll(locations(i,1), locations(i, 2), :))'; isPointValid = all(~isnan(point3d)) && all(isfinite(point3d)) && point3d(3) > 0; isDepthInRange = point3d(3) < 200*norm(stereoParams.TranslationOfCamera2); if isPointValid && isDepthInRange xyzPoints = [xyzPoints; point3d]; %#ok<*AGROW> isPointFound(i) = true; end end indexPairs = indexPairs(isPointFound, :); xyzPoints = xyzPoints * currPose.Rotation + currPose.Translation; end
helperFindValidFeaturePairs совпадайте с функциями между парой стереоизображений
function indexPairs = helperFindValidFeaturePairs(features1, features2, points1, points2, maxDisparity) indexPairs = matchFeatures(features1, features2,... 'Unique', true, 'MaxRatio', 1, 'MatchThreshold', 40); matchedPoints1 = points1.Location(indexPairs(:,1), :); matchedPoints2 = points2.Location(indexPairs(:,2), :); scales1 = points1.Scale(indexPairs(:,1), :); scales2 = points2.Scale(indexPairs(:,2), :); dist2EpipolarLine = abs(matchedPoints2(:, 2) - matchedPoints1(:, 2)); shiftDist = matchedPoints1(:, 1) - matchedPoints2(:, 1); isCloseToEpipolarline = dist2EpipolarLine < 2*scales2; isDisparityValid = shiftDist > 0 & shiftDist < maxDisparity; isScaleIdentical = scales1 == scales2; indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :); end
helperIsKeyFrame проверяйте, является ли система координат ключевым кадром.
function isKeyFrame = helperIsKeyFrame(mapPoints, ... refKeyFrameId, lastKeyFrameIndex, currFrameIndex, mapPointsIndices) numPointsRefKeyFrame = numel(findWorldPointsInView(mapPoints, refKeyFrameId)); % More than 5 frames have passed from last key frame insertion tooManyNonKeyFrames = currFrameIndex >= lastKeyFrameIndex + 5; % Track less than 100 map points tooFewMapPoints = numel(mapPointsIndices) < 100; % Tracked map points are fewer than 90% of points tracked by % the reference key frame tooFewTrackedPoints = numel(mapPointsIndices) < 0.9 * numPointsRefKeyFrame; isKeyFrame = (tooManyNonKeyFrames && tooFewTrackedPoints) || tooFewMapPoints; end
helperCullRecentMapPoints отходы недавно добавленные точки карты.
function [mapPointSet, directionAndDepth, mapPointsIdx] = ... helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx, stereoMapPointsIndices) outlierIdx = setdiff([newPointIdx; stereoMapPointsIndices], mapPointsIdx); if ~isempty(outlierIdx) mapPointSet = removeWorldPoints(mapPointSet, outlierIdx); directionAndDepth = remove(directionAndDepth, outlierIdx); mapPointsIdx = mapPointsIdx - arrayfun(@(x) nnz(x>outlierIdx), mapPointsIdx); end end
helperUpdateGlobalMap обновите 3-D местоположения точек карты после оптимизации графика положения
function [mapPointSet, directionAndDepth] = helperUpdateGlobalMap(... mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim) posesOld = vSetKeyFrames.Views.AbsolutePose; posesNew = vSetKeyFramesOptim.Views.AbsolutePose; positionsOld = mapPointSet.WorldPoints; positionsNew = positionsOld; indices = 1:mapPointSet.Count; % Update world location of each map point based on the new absolute pose of % the corresponding major view for i = 1: mapPointSet.Count majorViewIds = directionAndDepth.MajorViewId(i); tform = posesOld(majorViewIds).T \ posesNew(majorViewIds).T ; positionsNew(i, :) = positionsOld(i, :) * tform(1:3,1:3) + tform(4, 1:3); end mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew); end
helperTransformGPSLocations преобразуйте местоположения GPS к системе координат привязки
function gTruth = helperTransformGPSLocations(gpsLocations, optimizedPoses) initialYawGPS = atan( (gpsLocations(100, 2) - gpsLocations(1, 2)) / ... (gpsLocations(100, 1) - gpsLocations(1, 1))); initialYawSLAM = atan((optimizedPoses.AbsolutePose(50).Translation(2) - ... optimizedPoses.AbsolutePose(1).Translation(2)) / ... (optimizedPoses.AbsolutePose(59).Translation(1) - ... optimizedPoses.AbsolutePose(1).Translation(1))); relYaw = initialYawGPS - initialYawSLAM; relTranslation = optimizedPoses.AbsolutePose(1).Translation; initialTform = rotationVectorToMatrix([0 0 relYaw]); for i = 1:size(gpsLocations, 1) gTruth(i, :) = initialTform * gpsLocations(i, :)' + relTranslation'; end end
Ссылки
[1] Mur-Artal, Рауль и Хуан Д. Тардос. "ORB-SLAM2: система SLAM с открытым исходным кодом для монокуляра, стерео и камер RGB-D". Транзакции IEEE на Робототехнике 33, № 5 (2017): 1255-1262.