Визуальная одновременная локализация и сопоставляющий (vSLAM), относится к процессу вычисления положения и ориентации камеры относительно ее среды, одновременно сопоставляя среду. Процесс использует только визуальные входные параметры от камеры. Приложения для vSLAM включают дополненную реальность, робототехнику и автономное управление автомобилем.
В этом примере показано, как обработать данные изображения от монокулярной камеры, чтобы создать карту внутренней среды и оценить траекторию камеры. Пример использует ORB-SLAM [1], который является основанным на функции vSLAM алгоритмом.
Чтобы ускорить расчеты, можно включить параллельные вычисления из theComputer диалогового окна Vision Toolbox Preferences. Чтобы открыть настройки Computer Vision Toolbox™, на вкладке Home, в разделе Environment, нажимают Preferences. Затем выберите Computer Vision Toolbox.
Следующие термины часто используются в этом примере:
Ключевые кадры: подмножество видеокадров, которые содержат сигналы для локализации и отслеживания. Два последовательных ключевых кадра обычно включают достаточное визуальное изменение.
Точки карты: список 3-D точек, которые представляют карту среды, восстановленной от ключевых кадров.
График Covisibility: график, состоящий из ключевого кадра как узлы. Два ключевых кадра соединяются ребром, если они совместно используют общие точки карты. Вес ребра является количеством разделяемых точек карты.
Существенный График: подграф covisibility графика, содержащего только ребра с высоким весом, т.е. более разделяемые точки карты.
База данных распознавания: база данных раньше распознавала, посетили ли место в прошлом. База данных хранит визуальное отображение слова к изображению на основе входного набора признаков. Это используется, чтобы искать изображение, которое визуально похоже на изображение запроса.
Конвейер ORB-SLAM включает:
Инициализация карты: ORB-SLAM запускается путем инициализации карты 3-D точек от двух видеокадров. 3-D точки и относительное положение камеры вычисляются с помощью триангуляции на основе 2D соответствий функции ORB.
Отслеживание: Если карта инициализируется для каждой новой системы координат, положение камеры оценивается путем соответствия с функциями в текущей системе координат к функциям в последнем ключевом кадре. Предполагаемое положение камеры усовершенствовано путем отслеживания локальной карты.
Локальное Отображение: текущая система координат используется, чтобы создать новые 3-D точки карты, если она идентифицирована как ключевой кадр. На данном этапе свяжитесь, корректировка используется, чтобы минимизировать ошибки перепроекции путем корректировки положения камеры и 3-D точек.
Закрытие цикла: Циклы обнаруживаются для каждого ключевого кадра путем сравнения его против всех предыдущих ключевых кадров с помощью подхода набора признаков. Если закрытие цикла обнаруживается, график положения оптимизирован, чтобы совершенствовать положения камеры всех ключевых кадров.
Данные, используемые в этом примере, от Сравнительный тест RGB-D TUM [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 между парой изображений. После того, как соответствия найдены, две модели геометрического преобразования используются, чтобы установить инициализацию карты:
Homography: Если сцена является плоской, homography, проективное преобразование является лучшим выбором описать соответствия характерной точки.
Основная Матрица: Если сцена является неплоской, основная матрица должна использоваться вместо этого.
homography и основная матрица могут быть вычислены с помощью estimateGeometricTransform и 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 [preFeatures, prePoints] = helperDetectAndExtractFeatures(currI); currFrameIdx = currFrameIdx + 1; firstI = currI; % Preserve the first frame isMapInitialized = false; % Map initialization loop while ~isMapInitialized && hasdata(imds) currI = readimage(imds, currFrameIdx); [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI); currFrameIdx = currFrameIdx + 1; % Find putative feature matches indexPairs = matchFeatures(preFeatures, currFeatures, 'Unique', true, ... 'MaxRatio', 0.7, 'MatchThreshold', 70); % 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.7 || numel(size(relOrient))==3 continue end % Triangulate two views to obtain 3-D map points relPose = rigid3d(relOrient, relLoc); [isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(... rigid3d, relPose, inlierPrePoints, inlierCurrPoints, intrinsics); 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 42
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 и helperMapPointSet сохранить эти два ключевых кадра и соответствующие точки карты:
imageviewset хранит ключевые кадры и их атрибуты, такие как дескрипторы ORB, характерные точки и положения камеры и связи между ключевыми кадрами, такими как соответствия характерных точек и относительные положения камеры. Положение камеры хранится как rigid3d объект.
helperMapPointSet хранит точки карты и их атрибуты, такие как 3-D положения, направления представления, представительные дескрипторы ORB и область значений расстояния, на котором может наблюдаться точка карты. Объект точек карты также хранит 3-D в 2D соответствия проекции: которые сопоставляют точки, наблюдаются в ключевом кадре и какие ключевые кадры наблюдают точку карты.
% Create an empty imageviewset object to store key frames vSetKeyFrames = imageviewset; % Create an empty helperMapPointSet object to store 3D map points mapPointSet = helperMapPointSet; % 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] = addMapPoint(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 = addObservation(mapPointSet, newPointIdx, preViewId, indexPairs(:,1), .... preLocations(indexPairs(:,1),:), preScales(indexPairs(:,1))); % Add image points corresponding to the map points in the second key frame mapPointSet = addObservation(mapPointSet, newPointIdx, currViewId, indexPairs(:,2), ... currLocations(indexPairs(:,2),:), currScales(indexPairs(:,2)));
Совершенствуйте начальную реконструкцию с помощью bundleAdjustment, это оптимизирует и положения камеры и точки мира, чтобы минимизировать полные ошибки перепроекции. После улучшения атрибуты точек карты включая 3-D местоположения, направление представления и область значений глубины обновляются и хранятся в mapPointSet. Можно использовать 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 = updateLocation(mapPointSet, refinedPoints); % Update view direction and depth mapPointSet = updateViewAndRange(mapPointSet, vSetKeyFrames.Views, newPointIdx); % 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 точки карты.
Оцените положение камеры с алгоритмом Перспективного n значений с помощью estimateWorldCameraPose.
Учитывая положение камеры, спроектируйте точки карты, наблюдаемые последним ключевым кадром в текущую систему координат, и ищите соответствия функции с помощью helperMatchFeaturesInRadius.
С 3-D к 2D соответствию в текущей системе координат совершенствуйте положение камеры путем выполнения корректировки пакета только для движения с помощью bundleAdjustmentMotion.
Спроектируйте локальные точки карты в текущую систему координат, чтобы искать больше соответствий функции с помощью helperMatchFeaturesInRadius и совершенствуйте положение камеры снова с помощью bundleAdjustmentMotion.
Последний шаг отслеживания должен решить, является ли текущая система координат новым ключевым кадром. Если текущая система координат является ключевым кадром, продолжите к Локальному процессу Отображения. В противном случае начните Отслеживать для следующей системы координат.
% Main loop while ~isLoopClosed && hasdata(imds) currI = readimage(imds, currFrameIdx); [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI); % 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); % 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, vSetKeyFrames, mapPointsIdx, ... featureIdx, currPose, currFeatures, currPoints, intrinsics); % 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 80 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); % Update view direction and depth mapPointSet = updateViewAndRange(mapPointSet, vSetKeyFrames.Views, mapPointsIdx); % Remove outlier map points that are observed in fewer than 3 key frames mapPointSet = helperCullRecentMapPoints(mapPointSet, vSetKeyFrames, newPointIdx); % Create new map points by triangulation [mapPointSet, vSetKeyFrames, newPointIdx] = helperCreateNewMapPoints(mapPointSet, vSetKeyFrames, ... currKeyFrameId, intrinsics); % Local bundle adjustment [mapPointSet, vSetKeyFrames] = helperLocalBundleAdjustment(mapPointSet, vSetKeyFrames, ... currKeyFrameId, intrinsics); % 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. Система координат ключа-кандидата допустима, если она не соединяется с последним ключевым кадром, и три из его соседних ключевых кадров являются кандидатами цикла.
Когда допустимый кандидат цикла будет найден, вычислите относительное положение между кандидатом цикла система координат и текущим ключевым кадром с помощью той же стратегии в качестве той, используемой в процессе Отслеживания. Затем добавьте связь цикла с относительным положением и обновите mapPointSet и vSetKeyFrames.
% Initialize the loop closure database if currKeyFrameId == 3 % Load the bag of features data created offline bofData = load('bagOfFeaturesData.mat'); loopDatabase = invertedImageIndex(bofData.bof); loopCandidates = [1; 2]; % Check loop closure after some key frames have been created elseif currKeyFrameId > 20 % Detect possible loop closure key frame candidates [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ... loopDatabase, currI, loopCandidates); if isDetected % Add loop closure connections [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnections(... mapPointSet, vSetKeyFrames, validLoopCandidates, ... currKeyFrameId, currFeatures, currPoints, intrinsics); end end % If no loop closure is detected, add the image into the database if ~isLoopClosed currds = imageDatastore(imds.Files{currFrameIdx}); addImages(loopDatabase, currds, '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
Loop edge added between keyframe: 3 and 123 Loop edge added between keyframe: 1 and 123 Loop edge added between keyframe: 2 and 123 Loop edge added between keyframe: 4 and 123 Loop edge added between keyframe: 5 and 123 Loop edge added between keyframe: 6 and 123 Loop edge added between keyframe: 7 and 123 Loop edge added between keyframe: 8 and 123 Loop edge added between keyframe: 9 and 123 Loop edge added between keyframe: 10 and 123
Наконец, оптимизация графика положения выполняется по существенному графику в vSetKeyFrames откорректировать дрейф попеременно и перевод. Существенный график создается внутренне путем удаления связей с меньше, чем minNumMatches соответствия в covisibility графике.
% Optimize the poses minNumMatches = 40; vSetKeyFramesOptim = optimizePoses(vSetKeyFrames, minNumMatches, 'Tolerance', 1e-16, 'Verbose', true);
Iteration 1, residual error 0.036293 Iteration 2, residual error 0.036189 Iteration 3, residual error 0.036189 Iteration 4, residual error 0.036189 Iteration 5, residual error 0.036189 Iteration 6, residual error 0.036189 Iteration 7, residual error 0.036189 Solver stopped because change in function value was less than specified function tolerance.
% 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.098218
Это завершает обзор того, как создать карту внутренней среды и оценить траекторию камеры с помощью ORB-SLAM.
Короткие функции помощника включены ниже. Большая функция включена в отдельные файлы.
helperAddLoopConnections добавьте связи между текущим ключевым кадром и допустимым кандидатом цикла.
helperAddNewKeyFrame добавьте ключевые кадры в набор ключевого кадра.
helperCheckLoopClosure обнаружьте кандидатов цикла ключевые кадры путем получения визуально подобных изображений из базы данных.
helperCreateNewMapPoints создайте новые точки карты триангуляцией.
helperFindProjectedPointsInImage проверяйте, ли спроектированные мировые точки в изображении.
helperHammingDistance вычислите расстояние Хемминга между двумя группами бинарных характеристических векторов.
helperLocalBundleAdjustment совершенствуйте положение текущего ключевого кадра и карту окружающей сцены.
helperMapPointSet управляйте данными о карте для визуального SLAM.
helperMatchFeaturesInRadius совпадайте с функциями в радиусе.
helperSelectStrongConnections выберите сильные связи с больше, чем конкретное количество соответствий.
helperSURFFeatureExtractorFunction реализует извлечение признаков SURF, используемое в bagOfFeatures.
helperTrackLastKeyFrame оцените текущее положение камеры путем отслеживания последнего ключевого кадра.
helperTrackLocalMap совершенствуйте текущее положение камеры путем отслеживания локальной карты.
helperVisualizeMatchedFeatures покажите совпадающие функции в системе координат.
helperVisualizeMotionAndStructure покажите точки карты и траекторию камеры.
helperDetectAndExtractFeatures обнаружьте и извлеките и функции ORB из изображения.
function [features, validPoints] = helperDetectAndExtractFeatures(Irgb, varargin) scaleFactor = 1.2; numLevels = 8; numPoints = 1000; % In this example, the images are already undistorted. In a general % workflow, uncomment the following code to undistort the images. % % if nargin > 1 % intrinsics = varargin{1}; % end % Irgb = undistortImage(Irgb, intrinsics); % 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
helperHomographyScore вычислите homography и оцените реконструкцию.
function [H, score, inliersIndex] = helperComputeHomography(matchedPoints1, matchedPoints2) [H, inlierPoints1, inlierPoints2] = estimateGeometricTransform( ... matchedPoints1, matchedPoints2, 'projective', ... 'MaxNumTrials', 1e3, 'MaxDistance', 4, 'Confidence', 90); [~, inliersIndex] = intersect(matchedPoints1.Location, ... inlierPoints1.Location, 'row', 'stable'); 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) [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] = triangulate(matchedPoints1, ... matchedPoints2, camMatrix1, camMatrix2); % Filter points by view direction and reprojection error minReprojError = 1; inlierIdx = xyzPoints(:,3) > 0 & 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 minParallax = 3; % in degrees isValid = all(cosAngle < cosd(minParallax) & cosAngle>0); end
helperIsKeyFrame проверяйте, является ли система координат ключевым кадром.
function isKeyFrame = helperIsKeyFrame(mapPoints, ... refKeyFrameId, lastKeyFrameIndex, currFrameIndex, mapPointsIndices) numPointsRefKeyFrame = numel(getMapPointIndex(mapPoints, refKeyFrameId)); % More than 20 frames have passed from last key frame insertion tooManyNonKeyFrames = currFrameIndex >= lastKeyFrameIndex + 20; % Track less than 80 map points tooFewMapPoints = numel(mapPointsIndices) < 80; % 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 mapPoints = helperCullRecentMapPoints(mapPoints, keyFrames, newPointIdx) for i = 1: numel(newPointIdx) idx = newPointIdx(i); % If a map point is observed in less than 3 key frames, drop it if numel(mapPoints.Observations{idx, 1})< 3 &&... max(mapPoints.Observations{idx, 1}) < keyFrames.Views.ViewId(end) mapPoints = updateValidity(mapPoints, idx, false); end end end
helperEstimateTrajectoryError calculate ошибка отслеживания.
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
[1] Mur-Artal, Рауль, Хосе Мария Мартинес Монтьель и Хуан Д. Тардос. "ORB-SLAM: универсальная и точная монокулярная система SLAM". Транзакции IEEE на Робототехнике 31, № 5, стр 1147-116, 2015.
[2] Штурм, Юрген, Николас Энджелхард, Феликс Эндрес, Вольфрам Бергард и Дэниел Кремерс. "Сравнительный тест для оценки RGB-D системы SLAM". В Продолжениях Международной конференции IEEE/RSJ по вопросам Интеллектуальных Роботов и Систем, стр 573-580, 2012.