Визуальная одновременная локализация и картирование (vSLAM), относится к процессу вычисления положения и ориентации камеры по отношению к окружению при одновременном картировании среды. Процесс использует только визуальные входные данные с камеры. Приложения для vSLAM включают дополненную реальность, робототехнику и автономное вождение.
В этом примере показано, как обрабатывать данные изображения с монокулярной камеры для построения карты среды внутри помещения и оценки траектории камеры. В примере используется алгоритм ORB-SLAM [1], который является алгоритмом vSLAM на основе функций.
Для ускорения вычислений можно включить параллельные вычисления в диалоговом окне «Настройки панели инструментов компьютерного зрения». Чтобы открыть настройки Toolbox™ компьютерного зрения, на вкладке Главная в разделе Среда щелкните Настройки. Затем выберите Инструмент компьютерного зрения (Computer Vision Toolbox).
В этом примере часто используются следующие термины:
Ключевые кадры: подмножество видеокадров, содержащих сигналы для локализации и отслеживания. Два последовательных ключевых кадра обычно включают достаточное визуальное изменение.
Точки карты: список точек 3-D, которые представляют карту среды, реконструированную из ключевых кадров.
Covisibility Graph: Граф, состоящий из ключевого кадра в качестве узлов. Два ключевых кадра соединяются ребром, если они имеют общие точки карты. Вес кромки - это количество общих точек карты.
Существенный граф: подграф графа ковисабильности, содержащий только рёбра с большим весом, т.е. более общие точки карты.
База данных распознавания: база данных, используемая для определения того, посещалось ли место в прошлом. В базе данных сохраняется визуальное сопоставление «слово-изображение» на основе входного пакета элементов. Используется для поиска изображения, визуально похожего на изображение запроса.
Трубопровод ORB-SLAM включает в себя:
Инициализация карты: ORB-SLAM начинается с инициализации карты точек 3-D с двух видеокадров. 3D пункты и относительная поза камеры вычислены, используя триангуляцию на основе 2-х корреспонденций особенности ШАРА.
Отслеживание: Как только карта инициализирована, для каждого нового кадра поза камеры оценивается путем сопоставления функций в текущем кадре с функциями в последнем ключевом кадре. Предполагаемая поза камеры уточняется путем отслеживания локальной карты.
Локальное сопоставление: текущий кадр используется для создания новых точек карты 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. Поскольку изображения RGB сделаны монокулярной камерой, которая не предоставляет информацию о глубине, относительное преобразование может быть восстановлено только до определенного масштабного коэффициента.
Учитывая относительную позу камеры и совпадающие характерные точки на двух изображениях, 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 объекты. Относительные положения камеры краев замыкания петли сохраняются как affine3d объекты.
worldpointset сохраняет 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.
Последним шагом отслеживания является решение о том, является ли текущий кадр новым ключевым кадром. Если текущий кадр является ключевым кадром, перейдите к процессу локального сопоставления. В противном случае запустите Отслеживание для следующего кадра.
% 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 соответствует на графике ковисабильности. После оптимизации позы подобия обновите 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 отбраковать недавно добавленные точки карты.
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] Штурм, Юрген, Николас Энгельгард, Феликс Эндрес, Вольфрам Бургард и Даниэль Кремерс. «Эталонный тест для оценки систем SLAM RGB-D». В трудах Международной конференции IEEE/RSJ по интеллектуальным роботам и системам, стр. 573-580, 2012.