Визуальная одновременная локализация и картирование (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 между двумя изображениями стереопары. Совпадающие пары должны удовлетворять следующим ограничениям:
Смещение по горизонтали между двумя соответствующими характерными точками в выпрямленном изображении стереопары меньше максимального различия. Можно определить приблизительное максимальное значение рассогласования по стереоанаглифу изображения стереопары. Дополнительные сведения см. в разделе Выбор диапазона различий.
Вертикальный сдвиг между двумя соответствующими характерными точками в выпрямленном изображении стереопары меньше порогового значения.
Масштабы совпадающих элементов почти идентичны.
Местоположения 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);
Процесс отслеживания выполняется с использованием каждой пары и определяет, когда вставлять новый ключевой кадр.
% 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.
Оценка позы камеры с помощью алгоритма Perspective-n-Point estimateWorldCameraPose.
С учетом позы камеры спроецируйте точки карты, наблюдаемые последним ключевым кадром, в текущий кадр и выполните поиск соответствий элементов с помощью matchFeaturesInRadius.
В 3-D к 2-D соответствиям в текущем кадре уточните позу камеры, выполнив настройку пучка только для движения с помощью bundleAdjustmentMotion.
Проецирование точек локальной карты в текущий кадр для поиска дополнительных соответствий элементов с помощью matchFeaturesInRadius и снова уточнить позу камеры с помощью bundleAdjustmentMotion.
Последним шагом отслеживания является решение о том, должен ли текущий кадр быть новым ключевым кадром. Кадр является ключевым кадром, если выполняются оба следующих условия:
С момента последнего ключевого кадра прошло по меньшей мере 5 кадров или текущий кадр отслеживает менее 100 точек карты.
Точки карты, отслеживаемые текущим кадром, составляют менее 90% точек, отслеживаемых опорным ключевым кадром.
Если текущий кадр должен стать ключевым кадром, перейдите к процессу локального сопоставления. В противном случае запустите Отслеживание для следующего кадра.
% Main loop while 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 = 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 = 10;
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);Этап замыкания цикла принимает текущий ключевой кадр, обработанный процессом локального отображения, и пытается обнаружить и закрыть цикл. Обнаружение петли осуществляется с использованием подхода мешков слов. Визуальный словарь, представленный как bagOfFeatures создается автономно с дескрипторами SURF, извлеченными из большого набора изображений в наборе данных путем вызова:
bag = bagOfFeatures(imds,'CustomExtractor', @helperSURFFeatureExtractorFunction);
где imds является imageDatastore объект, хранящий обучающие изображения и helperSURFFeatureExtractorFunction - функция извлечения элемента SURF. Дополнительные сведения см. в разделе Извлечение изображения с помощью пакета визуальных слов.
Процесс закрытия цикла постепенно создает базу данных, представляемую как invertedImageIndex объект, в котором хранится визуальное преобразование слова в изображение на основе пакета элементов SURF. Кандидаты цикла идентифицируются путем запроса изображений в базе данных, которые визуально аналогичны текущему ключевому кадру с помощью evaluateImageRetrieval. Ключевой кадр-кандидат является действительным, если он не подключен к последнему ключевому кадру, и три его соседних ключевых кадра являются циклическими кандидатами.
Когда найден действительный кандидат замыкания контура, вычислите относительную позу между кадром кандидата замыкания контура и текущим ключевым кадром, используя estimateGeometricTransform3D. Затем добавьте соединение цикла с относительной позой и обновите mapPointSet и vSetKeyFrames.
% Initialize the loop closure database if currKeyFrameId == 2 % Load the bag of features data created offline bofData = load('bagOfFeaturesUTIAS.mat'); % Initialize the place recognition database loopCandidates = [1; 2]; loopDatabase = indexImages(subset(imdsLeft, loopCandidates), bofData.bof); % Check loop closure after some key frames have been created elseif currKeyFrameId > 50 % Minimum number of feature matches of loop edges loopEdgeNumMatches = 40; % Detect possible loop closure key frame candidates [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ... loopDatabase, currILeft, loopCandidates, loopEdgeNumMatches); isTooCloseView = currKeyFrameId - max(validLoopCandidates) < 20; if isDetected && ~isTooCloseView % Add loop closure connections [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo(... mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ... currFeaturesLeft, currPointsLeft, loopEdgeNumMatches); end end % If no loop closure is detected, add the image into the database if ~isLoopClosed addImages(loopDatabase, subset(imdsLeft, currFrameIdx), 'Verbose', false); loopCandidates= [loopCandidates; currKeyFrameId]; end % Update IDs and indices lastKeyFrameId = currKeyFrameId; lastKeyFrameIdx = currFrameIdx; addedFramesIdx = [addedFramesIdx; currFrameIdx]; currFrameIdx = currFrameIdx + 1; end % End of main loop
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: 2 and 268
Наконец, примените оптимизацию графика позы над существенным графиком в vSetKeyFrames для исправления дрейфа. Основной график создается внутри, удаляя соединения с менее чем minNumMatches соответствует на графике ковисабильности. После оптимизации графика позы обновите 3-D расположения точек карты с помощью оптимизированной позы.
% Optimize the poses minNumMatches = 10; 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 («Сопоставить панель инструментов») из «Сопоставить панель инструментов». В этом примере можно просто загрузить преобразованные данные 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 = rgb2gray(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 10 frames have passed from last key frame insertion tooManyNonKeyFrames = currFrameIndex >= lastKeyFrameIndex + 10; % Track less than 100 map points tooFewMapPoints = numel(mapPointsIndices) < max(100, 0.25 * numPointsRefKeyFrame); % 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] Мур-Артал, Рауль и Хуан Д. Тардос. «ORB-SLAM2: Система SLAM с открытым исходным кодом для монокулярных, стерео и RGB-D камер». Транзакции IEEE по робототехнике 33, № 5 (2017): 1255-1262.