Стерео-визуальная одновременная локализация и сопоставление
Визуальная одновременная локализация и картография (vSLAM), относится к процессу вычисления положения и ориентации камеры относительно её окружения, одновременно отображая среду. Процесс использует только визуальные входы от камеры. Приложения для vSLAM включают дополненную реальность, робототехнику и автономное управление автомобилем.
vSLAM можно выполнить, используя всего лишь монокулярную камеру. Однако, поскольку глубина не может быть точно вычислена с помощью одной камеры, шкала карты и предполагаемая траектория неизвестны и дрейфуют с течением времени. В сложение для загрузки системы требуется несколько представлений для создания начальной карты, поскольку она не может быть триангулирована с первой системы координат. Использование стереофотоаппарата решает эти проблемы и обеспечивает более надежное решение vSLAM.
В этом примере показано, как обработать данные изображения со стереофотоаппарата, чтобы создать карту наружного окружения и оценить траекторию камеры. В примере используется версия алгоритма ORB-SLAM2 [1], которая основана на функциях и поддерживает стереофотоаппараты.
Обзор технологического трубопровода
Трубопровод для стерео vSLAM очень похож на монокулярный трубопровод vSLAM в примере Monocular Visual Simultaneous Localization and Mapping. Существенное различие - то, которые в точках карты стадии 3-D Инициализации Карты созданы из пары изображений стерео той же пары стерео вместо двух изображений различных систем координат.
Инициализация карты: Конвейер начинается с инициализации карты 3-D точек из пары стерео- изображений с помощью карты расхождения. Левое изображение сохранено как первая система координат.
Отслеживать: Как только карта инициализирована, для каждого нового стерео пары, положение камеры оценивается путем соответствия признаков в левом изображении функциям в последней ключевой системе координат. Предполагаемое положение камеры уточняется путем отслеживания локальной карты.
Локальное отображение: Если текущее левое изображение идентифицировано как ключевая система координат, новые точки карты 3-D вычисляются из несоответствия стерео пары. На данном этапе регулировка пучка используется, чтобы минимизировать ошибки репроекции путем регулировки положения камеры и точек 3-D.
Закрытие цикла: Циклы обнаруживаются для каждой ключевой системы координат путем сравнения его со всеми предыдущими ключевыми системами координат с помощью подхода набор признаков. После обнаружения замыкания цикла график положения оптимизируется, чтобы уточнить положения камеры всех ключевых систем координат.
Загрузите и исследуйте Вход Stereo Изображения Sequence
Данные, используемые в этом примере, взяты из долгосрочного набора данных локализации и картографии 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вычислить карту несоответствия для каждой пары стерео- изображения с помощью метода semi-global matching (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.Проецируйте локальные точки карты в текущую систему координат, чтобы найти больше соответствий функций, используя
matchFeaturesInRadiusbundleAdjustmentMotion.Последним шагом отслеживания является решение, должен ли текущая система координат быть новой ключевой системой координат. Система координат является ключевой системой координат, если оба из следующих условий удовлетворены:
По меньшей мере 5 системы координат прошли с момента последней ключевой системы координат или текущая система координат отслеживает менее 100 точек карты.
Точки карты, отслеживаемые текущими системами координат, составляют менее 90% точек, отслеживаемых ссылкой ключевыми системами координат.
Если текущая система координат должен стать ключевой системой координат, перейдите к процессу локального отображения. В противном случае запустите Tracking для следующей системы координат.
% 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 соответствует в графике ковисibility. После оптимизации графика положения обновите 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 (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 = 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 cull недавно добавил точки карты.
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.