Монокулярная визуальная одновременная локализация и картография
Визуальная одновременная локализация и картография (vSLAM), относится к процессу вычисления положения и ориентации камеры относительно её окружения, одновременно отображая среду. Процесс использует только визуальные входы от камеры. Приложения для vSLAM включают дополненную реальность, робототехнику и автономное управление автомобилем.
Этот пример показывает, как обработать данные изображения с монокулярной камеры, чтобы создать карту внутреннего окружения и оценить траекторию камеры. В примере используется ORB-SLAM [1], который является функциональным алгоритмом vSLAM.
Чтобы ускорить расчеты, можно включить параллельные вычисления из диалогового окна Computer Vision Toolbox Preferences. Чтобы открыть настройки Computer Vision Toolbox™, на вкладке Home, в разделе Environment, нажмите Preferences. Затем выберите Computer Vision Toolbox.
Глоссарий
В этом примере часто используются следующие термины:
Ключевые системы координат: Подмножество видеокадров, которые содержат сигналы для локализации и отслеживания. Две последовательные ключевые системы координаты обычно включают достаточное визуальное изменение.
Map Points: Список 3-D точек, которые представляют карту окружения, восстановленную из ключевых систем координат.
Covisibility Графика: График, состоящее из ключевой системы координат как узлов. Два ключевых кадра соединяются ребро, если они имеют общие точки карты. Вес ребра - это количество общих точек карты.
Essential Graph: подграфик графика ковисибильности, содержащий только ребра с высоким весом, т.е. более общие точки карты.
База данных распознавания: база данных, используемая для распознавания того, посещалось ли место в прошлом. База данных хранит визуальное отображение слова на изображение на основе входного набора признаков. Он используется для поиска изображения, которое визуально похоже на изображение запроса.
Обзор ORB-SLAM
Трубопровод ORB-SLAM включает в себя:
Инициализация карты: ORB-SLAM начинается с инициализации карты 3-D точек из двух видеокадров. Точки 3-D и относительное положение камеры вычисляются с помощью триангуляции, основанной на 2-D особенностях ORB.
Отслеживание: После инициализации карты для каждой новой системы координат положение камеры оценивается путем соответствия характеристик в текущей системе координат функциям в последней ключевой системе координат. Предполагаемое положение камеры уточняется путем отслеживания локальной карты.
Локальное отображение: Текущая система координат используется для создания новых точек карты 3-D, если он идентифицирован как ключевая система координат. На данном этапе регулировка пучка используется, чтобы минимизировать ошибки репроекции путем регулировки положения камеры и точек 3-D.
Закрытие цикла: Циклы обнаруживаются для каждой ключевой системы координат путем сравнения его со всеми предыдущими ключевыми системами координат с помощью подхода набор признаков. После обнаружения замыкания цикла график положения оптимизируется, чтобы уточнить положения камеры всех ключевых систем координат.
Загрузите и исследуйте последовательность входных изображений
Данные, используемые в этом примере, взяты из Тестирование TUM RGB-D [2]. Загрузить данные во временную директорию можно с помощью веб-браузера или запустив следующий код:
baseDownloadURL = 'https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.tgz'; dataFolder = fullfile(tempdir, 'tum_rgbd_dataset', filesep); options = weboptions('Timeout', Inf); tgzFileName = [dataFolder, 'fr3_office.tgz']; folderExists = exist(dataFolder, 'dir'); % Create a folder in a temporary directory to save the downloaded file if ~folderExists mkdir(dataFolder); disp('Downloading fr3_office.tgz (1.38 GB). This download can take a few minutes.') websave(tgzFileName, baseDownloadURL, options); % Extract contents of the downloaded file disp('Extracting fr3_office.tgz (1.38 GB) ...') untar(tgzFileName, dataFolder); end
Создайте imageDatastore объект для просмотра изображений RGB.
imageFolder = [dataFolder,'rgbd_dataset_freiburg3_long_office_household/rgb/']; imds = imageDatastore(imageFolder); % Inspect the first image currFrameIdx = 1; currI = readimage(imds, currFrameIdx); himage = imshow(currI);
Инициализация карты
Трубопровод ORB-SLAM начинается с инициализации карты, которая содержит 3-D мировых точек. Этот шаг имеет решающее значение и оказывает значительное влияние на точность окончательного результата SLAM. Начальные соответствия точек функций ORB найдены с помощью matchFeatures между парой изображений. После того, как соответствия найдены, для установления инициализации карты используются две геометрические модели преобразования:
Гомография: Если сцена плоская, гомографическое проективное преобразование является лучшим выбором для описания соответствий функций.
Фундаментальная матрица: Если сцена не является планарной, вместо этого должна использоваться фундаментальная матрица.
Гомография и фундаментальная матрица могут быть вычислены с помощью estimateGeometricTransform2D и estimateFundamentalMatrix, соответственно. Модель, которая приводит к меньшей ошибке репроекции, выбрана, чтобы оценить относительное вращение и перемещение между двумя системами координат, используя relativeCameraPose. Поскольку изображения получаются с помощью монокулярной камеры, которая не предоставляет информацию о глубине, относительный преобразование может быть восстановлено только до определенного масштабного коэффициента.
Учитывая относительное положение камеры и совпадающие точки функции на двух изображениях, 3-D местоположения совпадающих точек определяются с помощью triangulate функция. Триангулированная точка карты действительна, когда она расположена в передней части обеих камер, когда ее ошибка репроекции низка, и когда параллакс двух видов точки достаточно велик.
% Set random seed for reproducibility rng(0); % Create a cameraIntrinsics object to store the camera intrinsic parameters. % The intrinsics for the dataset can be found at the following page: % https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats % Note that the images in the dataset are already undistorted, hence there % is no need to specify the distortion coefficients. focalLength = [535.4, 539.2]; % in units of pixels principalPoint = [320.1, 247.6]; % in units of pixels imageSize = size(currI,[1 2]); % in units of pixels intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize); % Detect and extract ORB features scaleFactor = 1.2; numLevels = 8; [preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels); currFrameIdx = currFrameIdx + 1; firstI = currI; % Preserve the first frame isMapInitialized = false; % Map initialization loop while ~isMapInitialized && currFrameIdx < numel(imds.Files) currI = readimage(imds, currFrameIdx); [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels); currFrameIdx = currFrameIdx + 1; % Find putative feature matches indexPairs = matchFeatures(preFeatures, currFeatures, 'Unique', true, ... 'MaxRatio', 0.9, 'MatchThreshold', 40); preMatchedPoints = prePoints(indexPairs(:,1),:); currMatchedPoints = currPoints(indexPairs(:,2),:); % If not enough matches are found, check the next frame minMatches = 100; if size(indexPairs, 1) < minMatches continue end preMatchedPoints = prePoints(indexPairs(:,1),:); currMatchedPoints = currPoints(indexPairs(:,2),:); % Compute homography and evaluate reconstruction [tformH, scoreH, inliersIdxH] = helperComputeHomography(preMatchedPoints, currMatchedPoints); % Compute fundamental matrix and evaluate reconstruction [tformF, scoreF, inliersIdxF] = helperComputeFundamentalMatrix(preMatchedPoints, currMatchedPoints); % Select the model based on a heuristic ratio = scoreH/(scoreH + scoreF); ratioThreshold = 0.45; if ratio > ratioThreshold inlierTformIdx = inliersIdxH; tform = tformH; else inlierTformIdx = inliersIdxF; tform = tformF; end % Computes the camera location up to scale. Use half of the % points to reduce computation inlierPrePoints = preMatchedPoints(inlierTformIdx); inlierCurrPoints = currMatchedPoints(inlierTformIdx); [relOrient, relLoc, validFraction] = relativeCameraPose(tform, intrinsics, ... inlierPrePoints(1:2:end), inlierCurrPoints(1:2:end)); % If not enough inliers are found, move to the next frame if validFraction < 0.9 || numel(size(relOrient))==3 continue end % Triangulate two views to obtain 3-D map points relPose = rigid3d(relOrient, relLoc); minParallax = 1; % In degrees [isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(... rigid3d, relPose, inlierPrePoints, inlierCurrPoints, intrinsics, minParallax); if ~isValid continue end % Get the original index of features in the two key frames indexPairs = indexPairs(inlierTformIdx(inlierTriangulationIdx),:); isMapInitialized = true; disp(['Map initialized with frame 1 and frame ', num2str(currFrameIdx-1)]) end % End of map initialization loop
Map initialized with frame 1 and frame 26
if isMapInitialized close(himage.Parent.Parent); % Close the previous figure % Show matched features hfeature = showMatchedFeatures(firstI, currI, prePoints(indexPairs(:,1)), ... currPoints(indexPairs(:, 2)), 'Montage'); else error('Unable to initialize map.') end
Сохраните начальные ключевые системы координат и точки карты
После того, как карта будет инициализирована с помощью двух систем координат, можно использовать imageviewset, worldpointset и helperViewDirectionAndDepth для хранения двух ключевых систем координат и соответствующих точек карты:
imageviewsetсохраняет ключевые системы координат и их атрибуты, такие как дескрипторы ORB, характерные точки и положения камеры, и соединения между ключевыми системами координат, такие как совпадения характерных точек и относительные положения камеры. Он также создает и обновляет график положения. Абсолютные положения камеры и относительные положения камеры на ребрах одометрии сохраняются какrigid3dобъекты. Относительные положения камеры на ребрах замыкания контура сохраняются какaffine3dworldpointsetсохраняет 3-D положения точек карты и 3-D в 2-D проекционных соответствиях: какие точки карты наблюдаются в ключевой системе координат и какие ключевые системы координат наблюдают точку карты.helperViewDirectionAndDepthсохраняет другие атрибуты точек карты, такие как среднее направление представления, репрезентативные дескрипторы ORB и область значений расстояний, на котором можно наблюдать точку карты.
% 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(xyzWorldPoints, 1)); % Add the first key frame. Place the camera associated with the first % key frame at the origin, oriented along the Z-axis preViewId = 1; vSetKeyFrames = addView(vSetKeyFrames, preViewId, rigid3d, 'Points', prePoints,... 'Features', preFeatures.Features); % Add the second key frame currViewId = 2; vSetKeyFrames = addView(vSetKeyFrames, currViewId, relPose, 'Points', currPoints,... 'Features', currFeatures.Features); % Add connection between the first and the second key frame vSetKeyFrames = addConnection(vSetKeyFrames, preViewId, currViewId, relPose, 'Matches', indexPairs); % Add 3-D map points [mapPointSet, newPointIdx] = addWorldPoints(mapPointSet, xyzWorldPoints); % Add observations of the map points preLocations = prePoints.Location; currLocations = currPoints.Location; preScales = prePoints.Scale; currScales = currPoints.Scale; % Add image points corresponding to the map points in the first key frame mapPointSet = addCorrespondences(mapPointSet, preViewId, newPointIdx, indexPairs(:,1)); % Add image points corresponding to the map points in the second key frame mapPointSet = addCorrespondences(mapPointSet, currViewId, newPointIdx, indexPairs(:,2));
Уточнение и визуализация первоначальной реконструкции
Уточнить первоначальную реконструкцию можно используя bundleAdjustment, что оптимизирует как положения камеры, так и мировые точки, чтобы минимизировать общие ошибки репроекции. После уточнения обновляются атрибуты точек карты, включая 3-D местоположения, направление просмотра и область значений глубин. Можно использовать helperVisualizeMotionAndStructure для визуализации точек карты и местоположения камеры.
% Run full bundle adjustment on the first two key frames tracks = findTracks(vSetKeyFrames); cameraPoses = poses(vSetKeyFrames); [refinedPoints, refinedAbsPoses] = bundleAdjustment(xyzWorldPoints, tracks, ... cameraPoses, intrinsics, 'FixedViewIDs', 1, ... 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-7,... 'RelativeTolerance', 1e-15, 'MaxIteration', 50); % Scale the map and the camera pose using the median depth of map points medianDepth = median(vecnorm(refinedPoints.')); refinedPoints = refinedPoints / medianDepth; refinedAbsPoses.AbsolutePose(currViewId).Translation = ... refinedAbsPoses.AbsolutePose(currViewId).Translation / medianDepth; relPose.Translation = relPose.Translation/medianDepth; % Update key frames with the refined poses vSetKeyFrames = updateView(vSetKeyFrames, refinedAbsPoses); vSetKeyFrames = updateConnection(vSetKeyFrames, preViewId, currViewId, relPose); % Update map points with the refined positions mapPointSet = updateWorldPoints(mapPointSet, newPointIdx, refinedPoints); % Update view direction and depth directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, newPointIdx, true); % Visualize matched features in the current frame close(hfeature.Parent.Parent); featurePlot = helperVisualizeMatchedFeatures(currI, currPoints(indexPairs(:,2)));
% Visualize initial map points and camera trajectory mapPlot = helperVisualizeMotionAndStructure(vSetKeyFrames, mapPointSet); % Show legend showLegend(mapPlot);
Отслеживание
Процесс отслеживания выполняется с использованием каждой системы координат и определяет, когда вставлять новую ключевую систему координат. Чтобы упростить этот пример, мы завершим процесс отслеживания, когда будет найдено закрытие цикла.
% ViewId of the current key frame currKeyFrameId = currViewId; % ViewId of the last key frame lastKeyFrameId = currViewId; % ViewId of the reference key frame that has the most co-visible % map points with the current key frame refKeyFrameId = currViewId; % Index of the last key frame in the input image sequence lastKeyFrameIdx = currFrameIdx - 1; % Indices of all the key frames in the input image sequence addedFramesIdx = [1; lastKeyFrameIdx]; isLoopClosed = false;
Каждая система координат обрабатывается следующим образом:
Функции ORB извлекаются для каждой новой системы координат и затем совпадают (используя
matchFeatures), с функциями в последней ключевой системе координат, которые знали соответствующие точки карты 3-D.Оцените положение камеры с помощью алгоритма Perspective-n-Point с помощью
estimateWorldCameraPose.Учитывая положение камеры, проецируйте точки карты, наблюдаемые последней ключевой системой координат, в текущую систему координат и ищите соответствия функций, используя
matchFeaturesInRadius.С 3-D для 2-D соответствия в текущей системе координат уточните положение камеры, выполнив настройку пучка только для движения с помощью
bundleAdjustmentMotion.Проецируйте локальные точки карты в текущую систему координат, чтобы найти больше соответствий функций, используя
matchFeaturesInRadiusи еще раз уточните положение камеры, используяbundleAdjustmentMotion.Последним шагом отслеживания является решение, является ли текущая система координат новой ключевой системой координат. Если текущая система координат является ключевой системой координат, перейдите к процессу локального отображения. В противном случае запустите Tracking для следующей системы координат.
% Main loop while ~isLoopClosed && currFrameIdx < numel(imds.Files) currI = readimage(imds, currFrameIdx); [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels); % Track the last key frame % mapPointsIdx: Indices of the map points observed in the current frame % featureIdx: Indices of the corresponding feature points in the % current frame [currPose, mapPointsIdx, featureIdx] = helperTrackLastKeyFrame(mapPointSet, ... vSetKeyFrames.Views, currFeatures, currPoints, lastKeyFrameId, intrinsics, scaleFactor); % 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 [refKeyFrameId, localKeyFrameIds, currPose, mapPointsIdx, featureIdx] = ... helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, mapPointsIdx, ... featureIdx, currPose, currFeatures, currPoints, intrinsics, scaleFactor, numLevels); % Check if the current frame is a key frame. % A frame is a key frame if both of the following conditions are satisfied: % % 1. At least 20 frames have passed since the last key frame or the % current frame tracks fewer than 100 map points % 2. The map points tracked by the current frame are fewer than 90% of % points tracked by the reference key frame isKeyFrame = helperIsKeyFrame(mapPointSet, refKeyFrameId, lastKeyFrameIdx, ... currFrameIdx, mapPointsIdx); % Visualize matched features updatePlot(featurePlot, currI, currPoints(featureIdx)); 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, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds); % Remove outlier map points that are observed in fewer than 3 key frames [mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet, ... directionAndDepth, mapPointsIdx, newPointIdx); % Create new map points by triangulation minNumMatches = 20; minParallax = 3; [mapPointSet, vSetKeyFrames, newPointIdx] = helperCreateNewMapPoints(mapPointSet, vSetKeyFrames, ... currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax); % Update view direction and depth directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, ... [mapPointsIdx; newPointIdx], true); % Local bundle adjustment [mapPointSet, directionAndDepth, vSetKeyFrames, newPointIdx] = helperLocalBundleAdjustment( ... mapPointSet, directionAndDepth, vSetKeyFrames, ... currKeyFrameId, intrinsics, newPointIdx); % Visualize 3D world points and camera trajectory updatePlot(mapPlot, vSetKeyFrames, mapPointSet);
Закрытие цикла
Шаг замыкания цикла принимает текущую ключевую систему координат, обработанный процессом локального отображения, и пытается обнаружить и закрыть цикл. Обнаружение цикла выполняется с помощью подхода мешков слов. Визуальный словарь, представленный как bagOfFeatures объект создается в автономном режиме с дескрипторами SURF, извлеченными из большого набора изображений в наборе данных путем вызова:
bag = bagOfFeatures(imds,'CustomExtractor', @helperSURFFeatureExtractorFunction);
где imds является imageDatastore объект, хранящий обучающие изображения и helperSURFFeatureExtractorFunction - функция извлечения признаков SURF. Дополнительные сведения см. в разделе Поиск изображений с сумкой визуальных слов.
Процесс закрытия цикла постепенно создает базу данных, представленную как invertedImageIndex объект, который хранит визуальное отображение слова на изображение на основе пакета признаков SURF. Кандидаты цикла идентифицируются путем запросов изображений в базе данных, которые визуально аналогичны текущей ключевой системе координат с помощью evaluateImageRetrieval. Ключевая система координат кандидата действительна, если она не соединена с последним ключом системы координат и трёх ее соседних систем координат ключа являются кандидатами цикла.
Когда найден допустимый кандидат цикла, используйте estimateGeometricTransform3D чтобы вычислить относительное положение между циклом кандидатом системы координат и текущей ключевой системы координат. Относительное положение представляет 3-D преобразование подобия, сохраненное в affine3d объект. Затем добавьте цикла соединение с относительным положением и обновите mapPointSet и vSetKeyFrames.
% Initialize the loop closure database if currKeyFrameId == 3 % Load the bag of features data created offline bofData = load('bagOfFeaturesData.mat'); % Initialize the place recognition database loopCandidates = [1; 2]; loopDatabase = indexImages(subset(imds, loopCandidates), bofData.bof); % Check loop closure after some key frames have been created elseif currKeyFrameId > 20 % Minimum number of feature matches of loop edges loopEdgeNumMatches = 50; % Detect possible loop closure key frame candidates [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ... loopDatabase, currI, loopCandidates, loopEdgeNumMatches); if isDetected % Add loop closure connections [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnections(... mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ... currFeatures, currPoints, loopEdgeNumMatches); end end % If no loop closure is detected, add the image into the database if ~isLoopClosed addImages(loopDatabase, subset(imds, currFrameIdx), 'Verbose', false); loopCandidates= [loopCandidates; currKeyFrameId]; %#ok<AGROW> end % Update IDs and indices lastKeyFrameId = currKeyFrameId; lastKeyFrameIdx = currFrameIdx; addedFramesIdx = [addedFramesIdx; currFrameIdx]; %#ok<AGROW> 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: 6 and 204
Наконец, оптимизация графика положения подобия выполняется над существенным графом в vSetKeyFrames для коррекции дрейфа положений фотоаппарата. Необходимый график создаётся внутренне путем удаления соединений с меньшим количеством minNumMatches соответствует в графике ковисibility. После оптимизации графика положения подобия обновите 3-D местоположения точек карты, используя оптимизированные положения и ассоциированные шкалы.
% Optimize the poses minNumMatches = 30; [vSetKeyFramesOptim, poseScales] = optimizePoses(vSetKeyFrames, minNumMatches, 'Tolerance', 1e-16); % Update map points after optimizing the poses mapPointSet = helperUpdateGlobalMap(mapPointSet, directionAndDepth, ... vSetKeyFrames, vSetKeyFramesOptim, poseScales); updatePlot(mapPlot, vSetKeyFrames, mapPointSet); % Plot the optimized camera trajectory optimizedPoses = poses(vSetKeyFramesOptim); plotOptimizedTrajectory(mapPlot, optimizedPoses) % Update legend showLegend(mapPlot);
Сравнение с основной истиной
Можно сравнить оптимизированную траекторию камеры с основной истиной для оценки точности ORB-SLAM. Загруженные данные содержат groundtruth.txt файл, который хранит основную истину положения камеры каждой системы координат. Данные сохранены в виде MAT-файла. Можно также вычислить корневую среднюю квадратную ошибку (RMSE) оценок траектории.
% Load ground truth gTruthData = load('orbslamGroundTruth.mat'); gTruth = gTruthData.gTruth; % Plot the actual camera trajectory plotActualTrajectory(mapPlot, gTruth(addedFramesIdx), optimizedPoses); % Show legend showLegend(mapPlot);
% Evaluate tracking accuracy
helperEstimateTrajectoryError(gTruth(addedFramesIdx), optimizedPoses);Absolute RMSE for key frame trajectory (m): 0.25809
На этом завершается обзор того, как создать карту комнатного окружения и оценить траекторию камеры с помощью ORB-SLAM.
Вспомогательные функции
Короткие вспомогательные функции включены ниже. Большая функция включена в отдельные файлы.
helperAddLoopConnections добавить соединения между текущим ключевым кадром и допустимым кандидатом цикла.
helperAddNewKeyFrame добавить ключевые системы координат в набор ключевых систем координат.
helperCheckLoopClosure обнаружение циклических ключевых систем координат кандидатов путем извлечения визуально похожих изображений из базы данных.
helperCreateNewMapPoints создать новые точки карты путем триангуляции.
helperLocalBundleAdjustment уточнить положение текущей ключевой системы координат и карту окружающей сцены.
helperSURFFeatureExtractorFunction реализует редукцию данных SURF, используемое в bagOfFeatures.
helperTrackLastKeyFrame оцените текущее положение камеры путем отслеживания последней ключевой системы координат.
helperTrackLocalMap уточнить положение текущей камеры путем отслеживания локальной карты.
helperViewDirectionAndDepth сохраните среднее направление обзора и предсказанную глубину точек карты
helperVisualizeMatchedFeatures отображение совпадающих функций в системе координат.
helperVisualizeMotionAndStructure показать точки карты и траекторию камеры.
helperDetectAndExtractFeatures обнаружить и извлечь из изображения функции и ORB.
function [features, validPoints] = helperDetectAndExtractFeatures(Irgb, ... scaleFactor, numLevels, varargin) numPoints = 1000; % In this example, the images are already undistorted. In a general % workflow, uncomment the following code to undistort the images. % % if nargin > 3 % intrinsics = varargin{1}; % end % Irgb = undistortImage(Irgb, intrinsics); % 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
helperHomographyScore вычислить гомографию и оценить реконструкцию.
function [H, score, inliersIndex] = helperComputeHomography(matchedPoints1, matchedPoints2) [H, inliersLogicalIndex] = estimateGeometricTransform2D( ... matchedPoints1, matchedPoints2, 'projective', ... 'MaxNumTrials', 1e3, 'MaxDistance', 4, 'Confidence', 90); inlierPoints1 = matchedPoints1(inliersLogicalIndex); inlierPoints2 = matchedPoints2(inliersLogicalIndex); inliersIndex = find(inliersLogicalIndex); locations1 = inlierPoints1.Location; locations2 = inlierPoints2.Location; xy1In2 = transformPointsForward(H, locations1); xy2In1 = transformPointsInverse(H, locations2); error1in2 = sum((locations2 - xy1In2).^2, 2); error2in1 = sum((locations1 - xy2In1).^2, 2); outlierThreshold = 6; score = sum(max(outlierThreshold-error1in2, 0)) + ... sum(max(outlierThreshold-error2in1, 0)); end
helperFundamentalMatrixScore вычислить фундаментальную матрицу и оценить реконструкцию.
function [F, score, inliersIndex] = helperComputeFundamentalMatrix(matchedPoints1, matchedPoints2) [F, inliersLogicalIndex] = estimateFundamentalMatrix( ... matchedPoints1, matchedPoints2, 'Method','RANSAC',... 'NumTrials', 1e3, 'DistanceThreshold', 0.01); inlierPoints1 = matchedPoints1(inliersLogicalIndex); inlierPoints2 = matchedPoints2(inliersLogicalIndex); inliersIndex = find(inliersLogicalIndex); locations1 = inlierPoints1.Location; locations2 = inlierPoints2.Location; % Distance from points to epipolar line lineIn1 = epipolarLine(F', locations2); error2in1 = (sum([locations1, ones(size(locations1, 1),1)].* lineIn1, 2)).^2 ... ./ sum(lineIn1(:,1:2).^2, 2); lineIn2 = epipolarLine(F, locations1); error1in2 = (sum([locations2, ones(size(locations2, 1),1)].* lineIn2, 2)).^2 ... ./ sum(lineIn2(:,1:2).^2, 2); outlierThreshold = 4; score = sum(max(outlierThreshold-error1in2, 0)) + ... sum(max(outlierThreshold-error2in1, 0)); end
helperTriangulateTwoFrames триангуляция двух систем координат для инициализации карты.
function [isValid, xyzPoints, inlierIdx] = helperTriangulateTwoFrames(... pose1, pose2, matchedPoints1, matchedPoints2, intrinsics, minParallax) [R1, t1] = cameraPoseToExtrinsics(pose1.Rotation, pose1.Translation); camMatrix1 = cameraMatrix(intrinsics, R1, t1); [R2, t2] = cameraPoseToExtrinsics(pose2.Rotation, pose2.Translation); camMatrix2 = cameraMatrix(intrinsics, R2, t2); [xyzPoints, reprojectionErrors, isInFront] = triangulate(matchedPoints1, ... matchedPoints2, camMatrix1, camMatrix2); % Filter points by view direction and reprojection error minReprojError = 1; inlierIdx = isInFront & reprojectionErrors < minReprojError; xyzPoints = xyzPoints(inlierIdx ,:); % A good two-view with significant parallax ray1 = xyzPoints - pose1.Translation; ray2 = xyzPoints - pose2.Translation; cosAngle = sum(ray1 .* ray2, 2) ./ (vecnorm(ray1, 2, 2) .* vecnorm(ray2, 2, 2)); % Check parallax isValid = all(cosAngle < cosd(minParallax) & cosAngle>0); end
helperIsKeyFrame проверьте, является ли система координат ключевой системой координат.
function isKeyFrame = helperIsKeyFrame(mapPoints, ... refKeyFrameId, lastKeyFrameIndex, currFrameIndex, mapPointsIndices) numPointsRefKeyFrame = numel(findWorldPointsInView(mapPoints, refKeyFrameId)); % More than 20 frames have passed from last key frame insertion tooManyNonKeyFrames = currFrameIndex > lastKeyFrameIndex + 20; % 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 || tooFewMapPoints) && tooFewTrackedPoints; end
helperCullRecentMapPoints cull недавно добавил точки карты.
function [mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx) outlierIdx = setdiff(newPointIdx, mapPointsIdx); if ~isempty(outlierIdx) mapPointSet = removeWorldPoints(mapPointSet, outlierIdx); directionAndDepth = remove(directionAndDepth, outlierIdx); mapPointsIdx = mapPointsIdx - arrayfun(@(x) nnz(x>outlierIdx), mapPointsIdx); end end
helperEstimateTrajectoryError вычислите ошибку отслеживания.
function rmse = helperEstimateTrajectoryError(gTruth, cameraPoses) locations = vertcat(cameraPoses.AbsolutePose.Translation); gLocations = vertcat(gTruth.Translation); scale = median(vecnorm(gLocations, 2, 2))/ median(vecnorm(locations, 2, 2)); scaledLocations = locations * scale; rmse = sqrt(mean( sum((scaledLocations - gLocations).^2, 2) )); disp(['Absolute RMSE for key frame trajectory (m): ', num2str(rmse)]); end
helperUpdateGlobalMap обновление 3-D местоположения точек карты после оптимизации графика положения
function [mapPointSet, directionAndDepth] = helperUpdateGlobalMap(... mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim, poseScales) %helperUpdateGlobalMap update map points after pose graph optimization 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); poseNew = posesNew(majorViewIds).T; poseNew(1:3, 1:3) = poseNew(1:3, 1:3) * poseScales(majorViewIds); tform = posesOld(majorViewIds).T \ poseNew; positionsNew(i, :) = positionsOld(i, :) * tform(1:3,1:3) + tform(4, 1:3); end mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew); end
Ссылка
[1] Мур-Арталь, Рауль, Хосе Мария Мартинес Монтьель, и Хуан Д. Тардос. ORB-SLAM: универсальная и точная монокулярная система SLAM. Транзакции IEEE по робототехнике 31, № 5, стр. 1147-116, 2015.
[2] Штурм, Юрген, Николас Энгельхард, Феликс Эндрес, Вольфрам Бургард и Даниэль Кремерс. «Эталонный тест для оценки систем RGB-D SLAM». В трудах Международной конференции по интеллектуальным роботам и системам IEEE/RSJ, стр. 573-580, 2012.