Визуальная одновременная локализация и картография стерео

Скрипт Open Live Script

Визуальная одновременная локализация и картография (vSLAM), относится к процессу вычисления положения и ориентации камеры относительно ее среды, одновременно сопоставляя среду. Процесс использует только визуальные входные параметры от камеры. Приложения для vSLAM включают дополненную реальность, робототехнику и автономное управление автомобилем.

vSLAM может быть выполнен при помощи только монокулярной камеры. Однако, поскольку глубина не может быть точно вычислена с помощью одной камеры, шкала карты и предполагаемой траектории неизвестна и дрейфует в зависимости от времени. Кроме того, чтобы загрузить систему, несколько представлений требуются, чтобы производить первоначальную карту, когда она не может быть триангулирована от первой системы координат. Используя стереофотоаппарат решает эти задачи и предоставляет более надежное vSLAM решение.

В этом примере показано, как обработать данные изображения от стереофотоаппарата, чтобы создать карту наружной среды и оценить траекторию камеры. Пример использует версию алгоритма ORB-SLAM2 [1], который основан на функции и поддерживает стереофотоаппараты.

Обзор конвейера обработки

Конвейер для стерео vSLAM очень похож на монокуляр vSLAM конвейер в Монокулярном Визуальном Одновременном примере Локализации и картографии. Существенное различие - то, которые в точках карты этапа 3-D Инициализации Карты создаются из пары стереоизображений той же стереопары вместо двух изображений различных систем координат.

  • Инициализация карты: конвейер запускается путем инициализации карты 3-D точек от пары стереоизображений с помощью карты несоизмеримости. Левое изображение хранится как первый ключевой кадр.

  • Отслеживание: Если карта инициализируется для каждой новой стереопары, положение камеры оценивается путем соответствия с функциями в левом изображении к функциям в последнем ключевом кадре. Предполагаемое положение камеры усовершенствовано путем отслеживания локальной карты.

  • Локальное Отображение: Если текущее левое изображение идентифицировано как ключевой кадр, новые 3-D точки карты вычисляются из несоизмеримости стереопары. На данном этапе свяжитесь, корректировка используется, чтобы минимизировать ошибки перепроекции путем корректировки положения камеры и 3-D точек.

  • Закрытие цикла: Циклы обнаруживаются для каждого ключевого кадра путем сравнения его против всех предыдущих ключевых кадров с помощью подхода набора признаков. Если закрытие цикла обнаруживается, график положения оптимизирован, чтобы совершенствовать положения камеры всех ключевых кадров.

Загрузите и исследуйте входную последовательность стереоизображения

Данные, используемые в этом примере, от Долгосрочного Набора данных Локализации и картографии UTIAS, обеспеченного Институтом Университета Торонто Космических Исследований. Можно загрузить данные на временную директорию с помощью веб-браузера или путем выполнения следующего кода:

ftpObj       = ftp('asrl3.utias.utoronto.ca');
tempFolder   = fullfile(tempdir);
dataFolder   = [tempFolder, '2020-vtr-dataset/UTIAS-In-The-Dark/'];
zipFileName  = [dataFolder, 'run_000005.zip'];
folderExists = exist(dataFolder, 'dir');

% Create a folder in a temporary directory to save the downloaded file
if ~folderExists  
    mkdir(dataFolder); 
    disp('Downloading run_000005.zip (818 MB). This download can take a few minutes.') 
    mget(ftpObj,'/2020-vtr-dataset/UTIAS-In-The-Dark/run_000005.zip', tempFolder);

    % Extract contents of the downloaded file
    disp('Extracting run_000005.zip (818 MB) ...') 
    unzip(zipFileName, dataFolder); 
end

Используйте два imageDatastore объекты сохранить стереоизображения.

imgFolderLeft  = [dataFolder,'images/left/'];
imgFolderRight = [dataFolder,'images/right/'];
imdsLeft       = imageDatastore(imgFolderLeft);
imdsRight      = imageDatastore(imgFolderRight);

% Inspect the first pair of images
currFrameIdx   = 1;
currILeft      = readimage(imdsLeft, currFrameIdx);
currIRight     = readimage(imdsRight, currFrameIdx);
imshowpair(currILeft, currIRight, 'montage');

Сопоставьте инициализацию

Конвейер ORB-SLAM запускается путем инициализации карты, которая содержит 3-D мировые точки. Этот шаг крайне важен и оказывает значительное влияние на точность итогового результата SLAM. Начальные соответствия характерной точки ORB найдены с помощью matchFeatures между двумя изображениями стереопары. Совпадающие пары должны удовлетворить следующим ограничениям:

  • Горизонтальный сдвиг между двумя соответствующими характерными точками в исправленном изображении стереопары меньше максимальной несоизмеримости. Можно определить аппроксимированное максимальное значение несоизмеримости из анаглифа стерео изображения стереопары. Для получения дополнительной информации смотрите Range of Disparity Выбора.

  • Вертикальный сдвиг между двумя соответствующими характерными точками в исправленном изображении стереопары меньше порога.

  • Шкалы совпадающих функций почти идентичны.

3-D мировые местоположения, соответствующие совпадающим характерным точкам, определяются можно следующим образом:

  • Используйте disparitySGM вычислить карту несоизмеримости для каждой пары стереоизображений при помощи метода полуглобального соответствия (SGM).

  • Используйте reconstructScene вычислить 3-D мировые координаты точки из карты несоизмеримости.

  • Найдите местоположения в карте несоизмеримости, которые соответствуют характерным точкам и их 3-D мировым местоположениям.

% Set random seed for reproducibility
rng(0);

% Load the initial camera pose. The initial camera pose is derived based 
% on the transformation between the camera and the vehicle:
% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/transform_camera_vehicle.tx 
initialPoseData = load('initialPose.mat');
initialPose     = initialPoseData.initialPose;

% Create a stereoParameters object to store the stereo camera parameters.
% The intrinsics for the dataset can be found at the following page:
% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/camera_parameters.txt
focalLength     = [387.777 387.777];     % specified in pixels
principalPoint  = [257.446 197.718];     % specified in pixels [x, y]
baseline        = 0.239965;              % specified in meters
intrinsicMatrix = [focalLength(1), 0, 0; ...
    0, focalLength(2), 0; ...
    principalPoint(1), principalPoint(2), 1];
imageSize       = size(currILeft,[1,2]); % in pixels [mrows, ncols]
cameraParam     = cameraParameters('IntrinsicMatrix', intrinsicMatrix, 'ImageSize', imageSize);
intrinsics      = cameraParam.Intrinsics;
stereoParams    = stereoParameters(cameraParam, cameraParam, eye(3), [-baseline, 0 0]);

% In this example, the images are already undistorted. In a general
% workflow, uncomment the following code to undistort the images.
% currILeft  = undistortImage(currILeft, intrinsics);
% currIRight = undistortImage(currIRight, intrinsics);

% Rectify the stereo images
[currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereoParams, 'OutputView','full');

% Detect and extract ORB features from the rectified stereo images
scaleFactor = 1.2;
numLevels   = 8;
[currFeaturesLeft,  currPointsLeft]   = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels); 
[currFeaturesRight, currPointsRight]  = helperDetectAndExtractFeatures(currIRight, scaleFactor, numLevels);

% Match feature points between the stereo images and get the 3-D world positions 
maxDisparity = 48;  % specified in pixels
[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, ...
    currFeaturesLeft, currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, initialPose, maxDisparity);

Управление данными и визуализация

После того, как карта инициализируется с помощью первой стереопары, можно использовать imageviewset, worldpointset и helperViewDirectionAndDepth сохранить первые ключевые кадры и соответствующие точки карты:

% Create an empty imageviewset object to store key frames
vSetKeyFrames = imageviewset;

% Create an empty worldpointset object to store 3-D map points
mapPointSet   = worldpointset;

% Create a helperViewDirectionAndDepth object to store view direction and depth 
directionAndDepth = helperViewDirectionAndDepth(size(xyzPoints, 1));

% Add the first key frame
currKeyFrameId = 1;
vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose, 'Points', currPointsLeft,...
    'Features', currFeaturesLeft.Features);

% Add 3-D map points
[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);

% Add observations of the map points
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, stereoMapPointsIdx, matchedPairs(:, 1));

% Visualize matched features in the first key frame
featurePlot = helperVisualizeMatchedFeaturesStereo(currILeft, currIRight, currPointsLeft, ...
    currPointsRight, matchedPairs);

% Visualize initial map points and camera trajectory
mapPlot     = helperVisualizeMotionAndStructureStereo(vSetKeyFrames, mapPointSet);

% Show legend
showLegend(mapPlot);

Отслеживание

Процесс отслеживания выполняется с помощью каждой пары и определяет, когда вставить новый ключевой кадр. 

% ViewId of the last key frame
lastKeyFrameId    = currKeyFrameId;

% ViewId of the reference key frame that has the most co-visible 
% map points with the current key frame
refKeyFrameId     = currKeyFrameId;

% Index of the last key frame in the input image sequence
lastKeyFrameIdx   = currFrameIdx; 

% Indices of all the key frames in the input image sequence
addedFramesIdx    = lastKeyFrameIdx;

currFrameIdx      = 2;
isLoopClosed      = false;

Каждый кадр обрабатывается можно следующим образом:

  1. Функции ORB извлечены для каждой новой стереопары изображений и затем соответствующие (использование matchFeatures), с функциями в последнем ключевом кадре, которые знали соответствующие 3-D точки карты.

  2. Оцените положение камеры с алгоритмом Перспективного n значений с помощью estimateWorldCameraPose.

  3. Учитывая положение камеры, спроектируйте точки карты, наблюдаемые последним ключевым кадром в текущую систему координат, и ищите соответствия функции с помощью matchFeaturesInRadius.

  4. С 3-D к 2D соответствиям в текущей системе координат совершенствуйте положение камеры путем выполнения корректировки пакета только для движения с помощью bundleAdjustmentMotion.

  5. Спроектируйте локальные точки карты в текущую систему координат, чтобы искать больше соответствий функции с помощью matchFeaturesInRadius и совершенствуйте положение камеры снова с помощью bundleAdjustmentMotion.

  6. Последний шаг отслеживания должен решить, должна ли текущая система координат быть новым ключевым кадром. Система координат является ключевым кадром, если обоим из следующих условий удовлетворяют:

  • По крайней мере 5 систем координат передали начиная с последнего ключевого кадра или текущей системы координат отслеживает меньше чем 100 точек карты.

  • Точки карты, прослеженные текущей системой координат, составляют меньше чем 90% точек, прослеженных ссылочным ключевым кадром.

Если текущая система координат должна стать ключевым кадром, продолжиться к Локальному процессу Отображения. В противном случае начните Отслеживать для следующей системы координат.

% Main loop
while currFrameIdx < numel(imdsLeft.Files)

    currILeft  = readimage(imdsLeft, currFrameIdx);
    currIRight = readimage(imdsRight, currFrameIdx);
    [currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereoParams, 'OutputView','full');

    [currFeaturesLeft, currPointsLeft]    = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels);
    [currFeaturesRight, currPointsRight]  = helperDetectAndExtractFeatures(currIRight, scaleFactor, numLevels);

    % Track the last key frame
    % trackedMapPointsIdx:  Indices of the map points observed in the current left frame 
    % trackedFeatureIdx:    Indices of the corresponding feature points in the current left frame
    [currPose, trackedMapPointsIdx, trackedFeatureIdx] = helperTrackLastKeyFrame(mapPointSet, ...
        vSetKeyFrames.Views, currFeaturesLeft, currPointsLeft, lastKeyFrameId, intrinsics, scaleFactor);
    
    if isempty(currPose) || numel(trackedMapPointsIdx) < 30
        currFrameIdx = currFrameIdx + 1;
        continue
    end
    
    % Track the local map
    % refKeyFrameId:      ViewId of the reference key frame that has the most 
    %                     co-visible map points with the current frame
    % localKeyFrameIds:   ViewId of the connected key frames of the current frame
    if currKeyFrameId == 1
        refKeyFrameId    = 1;
        localKeyFrameIds = 1;
    else
        [refKeyFrameId, localKeyFrameIds, currPose, trackedMapPointsIdx, trackedFeatureIdx] = ...
            helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, trackedMapPointsIdx, ...
            trackedFeatureIdx, currPose, currFeaturesLeft, currPointsLeft, intrinsics, scaleFactor, numLevels);
    end
    
    % Match feature points between the stereo images and get the 3-D world positions
    [xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, currFeaturesLeft, ...
        currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, currPose, maxDisparity);
    
    [untrackedFeatureIdx, ia] = setdiff(matchedPairs(:, 1), trackedFeatureIdx);
    xyzPoints = xyzPoints(ia, :);
    
    % Check if the current frame is a key frame
    isKeyFrame = helperIsKeyFrame(mapPointSet, refKeyFrameId, lastKeyFrameIdx, ...
        currFrameIdx, trackedMapPointsIdx);

    % Visualize matched features in the stereo image
    updatePlot(featurePlot, currILeft, currIRight, currPointsLeft, currPointsRight, trackedFeatureIdx, matchedPairs);
    
    if ~isKeyFrame
        currFrameIdx = currFrameIdx + 1;
        continue
    end
    
    % Update current key frame ID
    currKeyFrameId  = currKeyFrameId + 1;

Локальное отображение

Локальное отображение выполняется для каждого ключевого кадра. Когда новый ключевой кадр будет определен, добавьте его в ключевые кадры и обновите атрибуты точек карты, наблюдаемых новым ключевым кадром. Гарантировать тот mapPointSet содержит как можно меньше выбросов, допустимая точка карты должна наблюдаться по крайней мере в 3 ключевых кадрах.

Новые точки карты создаются путем триангулирования характерных точек ORB в текущем ключевом кадре и его связанных ключевых кадрах. Для каждой несопоставленной характерной точки в текущем ключевом кадре ищите соответствие с другими несопоставленными точками в связанных ключевых кадрах с помощью matchFeatures. Локальная корректировка пакета совершенствовала положение текущего ключевого кадра, положения связанных ключевых кадров и все точки карты, наблюдаемые в этих ключевых кадрах.

    % Add the new key frame    
    [mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame(mapPointSet, vSetKeyFrames, ...
        currPose, currFeaturesLeft, currPointsLeft, trackedMapPointsIdx, trackedFeatureIdx, localKeyFrameIds);
        
    % Remove outlier map points that are observed in fewer than 3 key frames
    if currKeyFrameId == 2
        triangulatedMapPointsIdx = [];
    end
    
    [mapPointSet, directionAndDepth, trackedMapPointsIdx] = ...
        helperCullRecentMapPoints(mapPointSet, directionAndDepth, trackedMapPointsIdx, triangulatedMapPointsIdx, ...
        stereoMapPointsIdx);
    
    % Add new map points computed from disparity 
    [mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);
    mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, stereoMapPointsIdx, ...
        untrackedFeatureIdx);
    
    % Create new map points by triangulation
    minNumMatches = 10;
    minParallax   = 0.35;
    [mapPointSet, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = helperCreateNewMapPointsStereo( ...
        mapPointSet, vSetKeyFrames, currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax, ...
        untrackedFeatureIdx, stereoMapPointsIdx);
    
    % Update view direction and depth
    directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, ...
        [trackedMapPointsIdx; triangulatedMapPointsIdx; stereoMapPointsIdx], true);
    
    % Local bundle adjustment
    [mapPointSet, directionAndDepth, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = ...
        helperLocalBundleAdjustmentStereo(mapPointSet, directionAndDepth, vSetKeyFrames, ...
        currKeyFrameId, intrinsics, triangulatedMapPointsIdx, stereoMapPointsIdx); 
    
    % Visualize 3-D world points and camera trajectory
    updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

Закрытие цикла

Шаг закрытия цикла берет текущий ключевой кадр, обработанный локальным процессом отображения, и пытается обнаружить и замкнуть круг. Обнаружение цикла выполняется с помощью подхода сумок слов. Визуальный словарь, представленный как bagOfFeatures объект создается оффлайн с дескрипторами SURF, извлеченными из большого набора изображений в наборе данных путем вызова:

bag = bagOfFeatures(imds,'CustomExtractor', @helperSURFFeatureExtractorFunction);

где imds imageDatastore объект, хранящий учебные изображения и helperSURFFeatureExtractorFunction функция экстрактора функции SURF. Смотрите Извлечение Изображений с Мешком Визуальных Слов для получения дополнительной информации.

Процесс закрытия цикла инкрементно создает базу данных, представленную как invertedImageIndex объект, который хранит визуальное отображение слова к изображению на основе мешка функций SURF. Кандидаты цикла идентифицированы путем запроса изображений в базе данных, которые визуально похожи на текущий ключевой кадр с помощью evaluateImageRetrieval. Система координат ключа-кандидата допустима, если она не соединяется с последним ключевым кадром, и три из его соседних ключевых кадров являются кандидатами цикла.

Когда допустимый кандидат закрытия цикла будет найден, вычислите относительное положение между кандидатом закрытия цикла система координат и текущим ключевым кадром с помощью estimateGeometricTransform3D. Затем добавьте связь цикла с относительным положением и обновите mapPointSet и vSetKeyFrames.

     % Initialize the loop closure database
    if currKeyFrameId == 2
        % Load the bag of features data created offline
        bofData         = load('bagOfFeaturesUTIAS.mat');
        
        % Initialize the place recognition database
        loopCandidates  = [1; 2];
        loopDatabase    = indexImages(subset(imdsLeft, loopCandidates), bofData.bof);
       
    % Check loop closure after some key frames have been created    
    elseif currKeyFrameId > 50
        
        % Minimum number of feature matches of loop edges
        loopEdgeNumMatches = 40;
        
        % Detect possible loop closure key frame candidates
        [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ...
            loopDatabase, currILeft, loopCandidates, loopEdgeNumMatches);
        
        isTooCloseView = currKeyFrameId - max(validLoopCandidates) < 20;
        if isDetected && ~isTooCloseView
            % Add loop closure connections
            [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo(...
                mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ...
                currFeaturesLeft, currPointsLeft, loopEdgeNumMatches);
        end
    end
    
    % If no loop closure is detected, add the image into the database
    if ~isLoopClosed
        addImages(loopDatabase, subset(imdsLeft, currFrameIdx), 'Verbose', false);
        loopCandidates= [loopCandidates; currKeyFrameId];  
    end
    
    % Update IDs and indices
    lastKeyFrameId  = currKeyFrameId;
    lastKeyFrameIdx = currFrameIdx;
    addedFramesIdx  = [addedFramesIdx; currFrameIdx]; 
    currFrameIdx    = currFrameIdx + 1;
end % End of main loop
Creating an inverted image index using Bag-Of-Features.
-------------------------------------------------------

Encoding images using Bag-Of-Features.
--------------------------------------

* Encoding 2 images...done.
Finished creating the image index.

Loop edge added between keyframe: 2 and 268

Наконец, примените оптимизацию графика положения по существенному графику в vSetKeyFrames откорректировать дрейф. Существенный график создается внутренне путем удаления связей с меньше, чем minNumMatches соответствия в covisibility графике. После оптимизации графика положения обновите 3-D местоположения точек карты с помощью оптимизированных положений.

% Optimize the poses
minNumMatches = 10;
vSetKeyFramesOptim = optimizePoses(vSetKeyFrames, minNumMatches, 'Tolerance', 1e-16);

% Update map points after optimizing the poses
mapPointSet = helperUpdateGlobalMap(mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim);

updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

% Plot the optimized camera trajectory
optimizedPoses  = poses(vSetKeyFramesOptim);
plotOptimizedTrajectory(mapPlot, optimizedPoses)

% Update legend
showLegend(mapPlot);

Сравните с основной истиной

Можно сравнить оптимизированную траекторию камеры с основной истиной, чтобы оценить точность решения. Загруженные данные содержат gps.txt файл, который хранит местоположение GPS для каждой системы координат. Можно преобразовать местоположение GPS от географического до локальных Декартовых координат с помощью latlon2local (Automated Driving Toolbox) от Automated Driving Toolbox или geodetic2enu (Mapping Toolbox) от Mapping Toolbox. В этом примере можно просто загрузить конвертированные данные о GPS из M-файла.

% Load GPS data
gpsData     = load('gpsLocation.mat');
gpsLocation = gpsData.gpsLocation;

% Transform GPS locations to the reference coordinate system
gTruth = helperTransformGPSLocations(gpsLocation, optimizedPoses);

% Plot the GPS locations
plotActualTrajectory(mapPlot, gTruth(addedFramesIdx, :));

% Show legend
showLegend(mapPlot);

Вспомогательные Функции

Короткие функции помощника описаны ниже. Большая функция включена в отдельные файлы.

helperDetectAndExtractFeatures обнаружьте и извлеките и функции ORB из изображения.

function [features, validPoints] = helperDetectAndExtractFeatures(Irgb, scaleFactor, numLevels)
 
numPoints = 800;

% Detect ORB features
Igray  = rgb2gray(Irgb);

points = detectORBFeatures(Igray, 'ScaleFactor', scaleFactor, 'NumLevels', numLevels);

% Select a subset of features, uniformly distributed throughout the image
points = selectUniform(points, numPoints, size(Igray, 1:2));

% Extract features
[features, validPoints] = extractFeatures(Igray, points);
end

helperReconstructFromStereo восстановите сцену из стереоизображения с помощью карты несоизмеримости

function [xyzPoints, indexPairs] = helperReconstructFromStereo(I1, I2, ...
    features1, features2, points1, points2, stereoParams, currPose, maxDisparity)

indexPairs     = helperFindValidFeaturePairs(features1, features2, points1, points2, maxDisparity);

% Compute disparity for all pixels in the left image. In practice, it is more 
% common to compute disparity just for the matched feature points.
disparityMap   = disparitySGM(rgb2gray(I1), rgb2gray(I2), 'DisparityRange', [0, maxDisparity]);
xyzPointsAll   = reconstructScene(disparityMap, stereoParams);

% Find the corresponding world point of the matched feature points 
locations      = floor(points1.Location(indexPairs(:, 1), [2 1]));
xyzPoints      = [];
isPointFound   = false(size(points1));

for i = 1:size(locations, 1)
    point3d = squeeze(xyzPointsAll(locations(i,1), locations(i, 2), :))';
    isPointValid   = all(~isnan(point3d)) && all(isfinite(point3d)) &&  point3d(3) > 0;
    isDepthInRange = point3d(3) < 200*norm(stereoParams.TranslationOfCamera2);
    if isPointValid  && isDepthInRange
        xyzPoints       = [xyzPoints; point3d]; %#ok<*AGROW> 
        isPointFound(i) = true;
    end
end
indexPairs = indexPairs(isPointFound, :);
xyzPoints  = xyzPoints * currPose.Rotation + currPose.Translation;
end

helperFindValidFeaturePairs совпадайте с функциями между парой стереоизображений

function indexPairs = helperFindValidFeaturePairs(features1, features2, points1, points2, maxDisparity)
indexPairs  = matchFeatures(features1, features2,...
    'Unique', true, 'MaxRatio', 1, 'MatchThreshold', 40);

matchedPoints1 = points1.Location(indexPairs(:,1), :);
matchedPoints2 = points2.Location(indexPairs(:,2), :);
scales1        = points1.Scale(indexPairs(:,1), :);
scales2        = points2.Scale(indexPairs(:,2), :);

dist2EpipolarLine = abs(matchedPoints2(:, 2) - matchedPoints1(:, 2));
shiftDist = matchedPoints1(:, 1) - matchedPoints2(:, 1);

isCloseToEpipolarline = dist2EpipolarLine < 2*scales2;
isDisparityValid      = shiftDist > 0 & shiftDist < maxDisparity;
isScaleIdentical      = scales1 == scales2;
indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :);
end

helperIsKeyFrame проверяйте, является ли система координат ключевым кадром.

function isKeyFrame = helperIsKeyFrame(mapPoints, ...
    refKeyFrameId, lastKeyFrameIndex, currFrameIndex, mapPointsIndices)

numPointsRefKeyFrame = numel(findWorldPointsInView(mapPoints, refKeyFrameId));

% More than 10 frames have passed from last key frame insertion
tooManyNonKeyFrames = currFrameIndex >= lastKeyFrameIndex + 10;

% Track less than 100 map points
tooFewMapPoints     = numel(mapPointsIndices) < max(100, 0.25 * numPointsRefKeyFrame);

% Tracked map points are fewer than 90% of points tracked by
% the reference key frame
tooFewTrackedPoints = numel(mapPointsIndices) < 0.9 * numPointsRefKeyFrame;

isKeyFrame = (tooManyNonKeyFrames && tooFewTrackedPoints) || tooFewMapPoints;
end

helperCullRecentMapPoints отходы недавно добавленные точки карты.

function [mapPointSet, directionAndDepth, mapPointsIdx] = ...
    helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx, stereoMapPointsIndices)

outlierIdx = setdiff([newPointIdx; stereoMapPointsIndices], mapPointsIdx);

if ~isempty(outlierIdx)
    mapPointSet   = removeWorldPoints(mapPointSet, outlierIdx);
    directionAndDepth = remove(directionAndDepth, outlierIdx);
    mapPointsIdx  = mapPointsIdx - arrayfun(@(x) nnz(x>outlierIdx), mapPointsIdx);
end

end

helperUpdateGlobalMap обновите 3-D местоположения точек карты после оптимизации графика положения

function [mapPointSet, directionAndDepth] = helperUpdateGlobalMap(...
    mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim)

posesOld     = vSetKeyFrames.Views.AbsolutePose;
posesNew     = vSetKeyFramesOptim.Views.AbsolutePose;
positionsOld = mapPointSet.WorldPoints;
positionsNew = positionsOld;
indices = 1:mapPointSet.Count;

% Update world location of each map point based on the new absolute pose of 
% the corresponding major view
for i = 1: mapPointSet.Count
    majorViewIds = directionAndDepth.MajorViewId(i);
    tform = posesOld(majorViewIds).T \ posesNew(majorViewIds).T ;
    positionsNew(i, :) = positionsOld(i, :) * tform(1:3,1:3) + tform(4, 1:3);
end
mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew);
end

helperTransformGPSLocations преобразуйте местоположения GPS к системе координат привязки 

function gTruth = helperTransformGPSLocations(gpsLocations, optimizedPoses)

initialYawGPS  = atan( (gpsLocations(100, 2) - gpsLocations(1, 2)) / ...
    (gpsLocations(100, 1) - gpsLocations(1, 1)));
initialYawSLAM = atan((optimizedPoses.AbsolutePose(50).Translation(2) - ...
    optimizedPoses.AbsolutePose(1).Translation(2)) / ...
    (optimizedPoses.AbsolutePose(59).Translation(1) - ...
    optimizedPoses.AbsolutePose(1).Translation(1)));

relYaw = initialYawGPS - initialYawSLAM;
relTranslation = optimizedPoses.AbsolutePose(1).Translation;

initialTform = rotationVectorToMatrix([0 0 relYaw]);
for i = 1:size(gpsLocations, 1)
    gTruth(i, :) =  initialTform * gpsLocations(i, :)' + relTranslation';
end
end

Ссылки

[1] Mur-Artal, Рауль и Хуан Д. Тардос. "ORB-SLAM2: система SLAM с открытым исходным кодом для монокуляра, стерео и камер RGB-D". Транзакции IEEE на Робототехнике 33, № 5 (2017): 1255-1262.

Документация Computer Vision Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте