Monocular Visual-Inertial SLAM

Simultaneous Localization and Mapping (SLAM) is a complex process that involves determining the position and orientation of a sensor, such as a camera, in relation to its surroundings. It also entails creating a map of the environment by identifying the 3D locations of various points. This example demonstrates how to effectively perform SLAM by combining images captured by a monocular camera with measurements obtained from an IMU sensor.

The method demonstrated in this example is inspired by ORB-SLAM3 which is a feature-based visual-inertial SLAM algorithm. The IMU and camera fusion is achieved using a factorGraph (Navigation Toolbox), which is a bipartite graph that consists of two types of nodes:

1. Variable Nodes: They represent the unknown random variables in an estimation problem such as the position of a mobile robot or a 3D point.

2. Factor Nodes: They represent a function which quantifies the relationship between the random variables. For example, a camera correspondence factor between 2D image points and 3D map points or an IMU measurement factor between two positions.

Factor graphs are a popular way of designing algorithms for sensor fusion-based SLAM because they provide an efficient and modular solution to SLAM problems and allow the graph to be easily updated if new measurements or constraints become available. Additionally, this type of model provides a flexible approach incorporating different types of sensors and data, including visual, lidar and inertial sensors, which makes it useful for variety of of SLAM applications.

This example illustrates how to construct a monocular visual-inertial SLAM pipeline using a factor graph step by step.

Overview

The visual-inertial SLAM pipeline includes the following steps:

• Initial IMU Bias Estimation (Optional): If your data sequence contains a sufficient number of static frames, you can use them to estimate the IMU bias. This helps you initialize the factor graph later.

• Map Initialization: Start by initializing the map of 3-D points from two video frames. The 3-D points and relative camera pose are computed using triangulation based on 2-D ORB feature correspondences.

• Camera and IMU alignment: Once the map is initialized, generate a set of camera poses using Structure From Motion (SFM). Then use corresponding IMU measurements to align the camera and IMU frames and to find the scale conversion between the IMU and camera odometries.

• Tracking: Once the camera and IMU data are aligned and scaled correctly, initialize the factor graph. Then, for each new key frame, estimate the camera pose by matching features in the current frame to features in the last key frame. Refine the estimated camera pose by tracking the local map. Then, add the camera poses to the factor graph with their corresponding IMU measurements.

• Local Mapping: Use the current frame to create new 3-D map points if it is identified as a key frame. At this stage, the factor graph is optimized, and the camera poses and 3-D points are adjusted using a set of local key frames.

• Loop Closure: Detect loops for each key frame by comparing the key frame against all previous key frames using the bag-of-features approach. Once a loop closure is detected, add a new link to the factor graph to reflect the connection found by the loop closure.

Download the Input Data Sequence

The data used in this demo has been extracted from the Blackbird data set (NYC Subway Winter). It represents a sequence from a UAV flying around in a simulated subway environment. Download the MAT file which contains the images, camera intrinsics and IMU measurements.

uavData = helperDownloadSLAMData();

images     = uavData.images;
intrinsics = uavData.intrinsics;
timeStamps = uavData.timeStamps;

Set up the noise parameters using the factorIMUParameters object. These values are typically provided by the IMU manufacturer.

imuParams = factorIMUParameters(SampleRate=100,GyroscopeNoise=0.1, ...
            GyroscopeBiasNoise=3e-3,AccelerometerNoise=0.3, ...
            AccelerometerBiasNoise=1e-3,ReferenceFrame="ENU");

Initial Bias Estimation

A section of the input data sequence has only static frames. These frames are useful for estimating the initial bias of the IMU sensor, which you can then use as a prior when we construct the factor graph. Accounting for the bias helps to reduce pose estimation errors because it can serve as an initial guess during the optimization. This step is optional, as it depends on the availibity of static frames.

startBiasIdx  = 40; %  Index from which the bias estimation starts
startFrameIdx = 150; % Index from which the SLAM pipline starts
initBias = helperEstimateInitialBias(uavData,startBiasIdx,startFrameIdx,imuParams);

Map Initialization

We start by initializing the map which holds the 3-D world points. This step is crucial and has a significant impact on the accuracy of final SLAM result. Find the initial ORB feature point correspondences using the matchFeatures function between a pair of images. After the correspondences are found, you can use these two geometric transformation models to establish map initialization:

• Homography: If the scene is planar, a homography projective transformation is a better choice to describe feature point correspondences.

• Fundamental Matrix: If the scene is non-planar, a fundamental matrix must be used instead.

The homography and the fundamental matrix can be computed using estgeotform2d and estimateFundamentalMatrix, respectively. The model that results in a smaller reprojection error is selected to estimate the relative rotation and translation between the two frames using estrelpose. Because the RGB images are taken by a monocular camera which does not provide the depth information, the relative translation can only be recovered up to a specific scale factor.

Given the relative camera pose and the matched feature points in the two images, the 3-D locations of the matched points are determined using triangulateMultiview function. A triangulated map point is valid when it is in the front of both cameras, when its reprojection error is low, and when the parallax of the two views of the point is sufficiently large.

% Set random seed for reproducibility
rng(0);

% Detect and extract ORB features
scaleFactor = 1.2;
numLevels   = 8;
numPoints   = 5000;
numSkipFrames = 20;
numPointsKeyFrame = 200;

currI = images{1, startFrameIdx};
[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, numPoints, intrinsics); 

currFrameIdx = startFrameIdx + 1;
firstI       = currI; % Preserve the first frame 

isMapInitialized  = false;

% Map initialization loop
while ~isMapInitialized && currFrameIdx < size(images,2)

    currI = images{currFrameIdx};

    [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, numPoints, intrinsics); 
    
    currFrameIdx = currFrameIdx + 1;
    
    % Find putative feature matches
    indexPairs = matchFeatures(preFeatures, currFeatures, Unique=true, MaxRatio=0.5, MatchThreshold=80);

    % 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);
    [relPose, validFraction] = estrelpose(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(relPose)~=1
        continue
    end
    
    % Triangulate two views to obtain 3-D map points
    minParallax = 1; % In degrees
    [isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(...
        rigidtform3d, 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 ', num2str(startFrameIdx),' and frame ', num2str(currFrameIdx-1)])
end % End of map initialization loop

Map initialized with frame 150 and frame 214

if isMapInitialized
    % Show matched features
    hfeature = showMatchedFeatures(firstI, currI, prePoints(indexPairs(:,1)), ...
        currPoints(indexPairs(:, 2)), "Montage");
else
    error('Unable to initialize the map.')
end

Store Initial Key Frames and Map Points

After the map is initialized using two frames, you can use imageviewset and worldpointset to store the two key frames and the corresponding map points:

• imageviewset stores the key frames and their attributes, such as ORB descriptors, feature points, camera poses, and graph connections between the key frames, such as feature points matches and relative camera poses. It also builds and updates a pose graph. The absolute camera poses and relative camera poses of odometry edges are stored as rigidtform3d objects. The relative camera poses of loop-closure edges are stored as affinetform3d objects.

• worldpointset stores 3-D positions of the map points and the 3-D to 2-D projection correspondences, indicating which map points are observed in a key frame and which key frames observe a map point. It also stores other attributes of map points, such as the mean view direction, the representative ORB descriptors, and the range of distance at which the map point can be observed.

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

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

% 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, rigidtform3d, 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));

Initialize Loop Closure Detection Database

Loop detection in this example is performed using the bags-of-words approach. You can create a visual vocabulary represented as a bagOfFeatures object offline using the ORB descriptors extracted from a large set of images in the dataset by calling:

bag = bagOfFeatures(imds,CustomExtractor=@helperORBFeatureExtractorFunction,TreeProperties=[3, 10],StrongestFeatures=1);

where imds is an imageDatastore object storing the training images and helperORBFeatureExtractorFunction is the ORB feature extractor function. See Image Retrieval with Bag of Visual Words for more information.

The loop closure process builds a database, by encode the incoming images into feature vectors compactly representing each view.

% Load the bag of features data created offline
bofData         = load("uavBoF.mat");
encodedImages   = [];

% Add features of the first two key frames to the database
encodedImages = [encodedImages; encode(bofData.bag,firstI,Verbose=false)];
encodedImages = [encodedImages; encode(bofData.bag,currI ,Verbose=false)];

Initialize the Factor Graph and Configure Trajectory and Map Visualization

Refine the initial reconstruction using a factorGraph (Navigation Toolbox), which optimizes both camera poses and world points to minimize the overall reprojection errors.

In MATLAB, working with a factor graph involves managing a set of unique IDs for different parts of the graph, including: poses, 3D points or IMU measurements. By using these IDs, you can add additional constraints can be added between the variable nodes in the factor graph, such as the corresponding 2D image matches for a set of 3D points, or gyroscope measurements between two sets of poses. This is accomplished using factor nodes. You can generate unique IDs using the generateNodeID (Navigation Toolbox) function.

To initialize the factor graph, two pose variable nodes representing the first and second poses are combined with a factorCameraSE3AndPointXYZ (Navigation Toolbox) that represents the links between 2D feature points and 3D map points. At this stage, IMU measurements cannot be added to the graph yet, as the correct alignment and scaling of IMU data require several camera poses to be collected first.

Once the initialization is successful, update the attributes of the map points including 3-D locations, view direction, and depth range. You can use helperVisualizeMotionAndStructure to visualize the map points and the camera locations.

% Initialize the factor graph
%   - viewToNode : array to convert imageViewSet indices to factorGraph pose id
%   - pointToNode: array to convert worldPointSet indices to factorGraph 3d point id
[fGraph, refinedAbsPoses, refinedPoints, viewToNode, pointToNode] = helperInitFactorGraph(vSetKeyFrames, mapPointSet, intrinsics);

% 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 
mapPointSet = updateLimitsAndDirection(mapPointSet, newPointIdx, vSetKeyFrames.Views);

% Update representative view
mapPointSet = updateRepresentativeView(mapPointSet, newPointIdx, vSetKeyFrames.Views);

% Update factor graph with refined poses 
fgRefinedPose = double([refinedAbsPoses.AbsolutePose(1).Translation rotm2quat(refinedAbsPoses.AbsolutePose(1).R)]);
nodeState(fGraph,1,fgRefinedPose);
fgRefinedPose = double([refinedAbsPoses.AbsolutePose(2).Translation rotm2quat(refinedAbsPoses.AbsolutePose(2).R)]);
nodeState(fGraph,2,fgRefinedPose);

% Update factor graph with refined points
ptsIds = nodeIDs(fGraph,NodeType="POINT_XYZ");
nodeState(fGraph,ptsIds,refinedPoints);

% 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       = helperVisualizeMotionAndStructureVI(vSetKeyFrames, mapPointSet);

showPlotLegend(mapPlot);

% ViewId of the current key frame
currKeyFrameId   = currViewId;

% ViewId of the last key frame
lastKeyFrameId   = 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];

IMU and Camera Alignment and Scaling

While using a monocular camera, pose estimates are obtained at an unknown scale, and differ from the metric measurements collected by an IMU. To address this issue, accelerometer measurements are used to estimate a scale factor which can scale the input camera poses to match the metric scale of the IMU measurements. In addition to that, you must align the reference frames of the input poses with the local navigation reference frame of the IMU to remove the constant gravity acceleration that is captured by the accelerometer can be removed. The estimateGravityRotationAndPoseScale function estimates both the scale and gravity rotation which you can use to improve the accuracy of the sensor fusion.

The frames used to obtain this alignment and scaling should be collected while keeping in mind a set of criteria:

• Map initialization should be as accurate as possible, because it affects the camera poses greatly. Extracting a large number of features may be necessary to obtain great accuracy.

• Camera poses should contain both static and moving frames, with the moving frames containing a strong acceleration in the gravity axis direction.

• For efficiency, a sliding window should be used to store the poses. The window size should be kept relatively small to reduce the amount of scale drift in the poses.

Once the estimation has converged, update the poses on the factor graph by rotating and scaling them correctly. Also update the factor graph by setting the bias prior using the previously estimated value. Use the factorIMU (Navigation Toolbox) to link the corresponding IMU measurements to the previously stored poses. Finally, optimize the factor graph to adjust the poses and 3D points according to the newly added constrains. Use factorGraphSolverOptions (Navigation Toolbox) to set up the optimization parameters for the factor graph.

% Main loop
isLastFrameKeyFrame = true;
isIMUInitialized    = false;
isLoopClosed        = false;

keyTimeStamps   = [timeStamps.imageTimeStamps(startFrameIdx); timeStamps.imageTimeStamps(currFrameIdx-1)];
keyFrameToFrame = [startFrameIdx; currFrameIdx-1];  % Array to convert keyframe ids to frame id
biasToNode      = []; % Array to convert imageViewSet indices to factorGraph bias id
velToNode       = []; % Array to convert imageViewSet indices to factorGraph velocity id

loopCtr = 0; % Loop closure rolling window counter
scale   = 0; % Placeholder for IMU-Camera scale

% Factor graph optimization parameters
fgso = factorGraphSolverOptions;
fgso.MaxIterations           = 20;
fgso.VerbosityLevel          = 0;
fgso.FunctionTolerance       = 1e-5;
fgso.GradientTolerance       = 1e-5;
fgso.StepTolerance           = 1e-5;
fgso.TrustRegionStrategyType = 0;

while currFrameIdx < size(images,2)

    if ~isIMUInitialized && currKeyFrameId>50
        camPoses = poses(vSetKeyFrames);

        % Estimate the gravity rotation and pose scale that helps in transforming input camera poses to the 
        % local navigation reference frame of IMU
        [gDir,scale] = helperAlignIMUCamera(camPoses,uavData,imuParams,...
            keyFrameToFrame,currKeyFrameId);

        if scale>0.3

            % Compare the scaled poses to the ground truth
            alignementPlot = helperDrawScaledandRotatedTraj(uavData,camPoses,scale,startFrameIdx,keyFrameToFrame);

            % Scale estimation was successful
            isIMUInitialized = true;
    
            xyzPoints1 = mapPointSet.WorldPoints;
            xyzPoints2 = nodeState(fGraph,nodeIDs(fGraph,NodeType="POINT_XYZ"));
    
            % Transform and scale the input camera poses and XYZ points using the estimated gravity direction and pose scale.
            [updatedCamPoses,updatedXYZPoints1,updatedXYZPoints2] = helperTransformToIMU(...
                camPoses,xyzPoints1,xyzPoints2,gDir,scale); %two sets of points because we can't remove nodes from graphs yet
    
            % Update mapPointSet and vSetKeyFrames
            vSetKeyFrames = updateView(vSetKeyFrames, updatedCamPoses);
            mapPointSet = updateWorldPoints(mapPointSet, (1:mapPointSet.Count), updatedXYZPoints1);
    
            % Update the poses and 3D points values in the factor graph
            for i=1:height(updatedCamPoses)
                pose = updatedCamPoses.AbsolutePose(i); 
                nodeState(fGraph, viewToNode(i), double([pose.Translation rotm2quat(pose.R)]));
            end
            nodeState(fGraph,nodeIDs(fGraph,NodeType="POINT_XYZ"),updatedXYZPoints2);
    
            % Update the factor graph by linking the IMU mesurements
            veloId = generateNodeID(fGraph,length(viewToNode));
            biasId = generateNodeID(fGraph,length(viewToNode));
    
            velToNode  = [velToNode;veloId'];
            biasToNode = [biasToNode;biasId'];
    
            initVelPrior  = factorVelocity3Prior(velToNode(1));
            initBiasPrior = factorIMUBiasPrior(biasToNode(1));
    
            addFactor(fGraph,initVelPrior);
            addFactor(fGraph,initBiasPrior);
    
            nodeState(fGraph, biasToNode(1), initBias);
    
            for i=2:length(viewToNode)
                imuMeasurements = helperExtractIMUMeasurements(uavData, keyFrameToFrame(i-1), keyFrameToFrame(i));
                imuId = [viewToNode(i-1),velToNode(i-1),biasToNode(i-1), ...
                        viewToNode(i)  ,velToNode(i)  ,biasToNode(i)];
                fIMU = factorIMU(imuId,imuMeasurements.gyro,imuMeasurements.accel,imuParams,SensorTransform=uavData.camToIMUTransform);
                addFactor(fGraph,fIMU);
            end
    
            optimize(fGraph,fgso);
            
        end
    end

Tracking

To determine when to insert a new key frame, perform the tracking process for each frame using these steps:

1. Extract ORB features for each new frame and then matche (using matchFeatures), with features in the last key frame that have known corresponding 3-D map points.

2. Estimate the camera pose with the Perspective-n-Point algorithm using estworldpose.

3. Using the camera pose, project the map points observed by the last key frame into the current frame and search for feature correspondences using matchFeaturesInRadius.

4. With 3-D to 2-D correspondence in the current frame, refine the camera pose by performing a motion-only bundle adjustment using factorGraph. Construct a factor graph using the steps in the previous section and then fix the 3D point nodes before proceeding to the optimization step.

5. Project the local map points into the current frame to search for more feature correspondences using matchFeaturesInRadius and refine the camera pose again using motion-only bundle adjustment.

6. Determine if the current frame is a new key frame. If the current frame is a key frame, continue to the Local Mapping process. Otherwise, start Tracking for the next frame.

If tracking is lost because not enough number of feature points could be matched, try inserting new key frames more frequently.

    % Extract ORB features from the current image
    currI = images{1, currFrameIdx};
    [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, ...
        numPoints, intrinsics);

    % Track the last key frame
    [currPose, mapPointsIdx, featureIdx] = helperTrackLastKeyFrameVI(mapPointSet, ...
        vSetKeyFrames.Views, currFeatures, currPoints, lastKeyFrameId, intrinsics, scaleFactor);

    % Track the local map and check if the current frame is a key frame.
    [localKeyFrameIds, currPose, mapPointsIdx, featureIdx, isKeyFrame] = ...
        helperTrackLocalMapVI(mapPointSet, vSetKeyFrames, mapPointsIdx, ...
        featureIdx, currPose, currFeatures, currPoints, intrinsics, scaleFactor, numLevels, ...
        isLastFrameKeyFrame, lastKeyFrameIdx, currFrameIdx, numSkipFrames, numPointsKeyFrame);

    % Visualize matched features
    updatePlot(featurePlot, currI, currPoints(featureIdx));
    
    if ~isKeyFrame
        isLastFrameKeyFrame = false;
        currFrameIdx        = currFrameIdx + 1;
        continue
    else
        isLastFrameKeyFrame = true;
        keyFrameToFrame = [keyFrameToFrame; currFrameIdx];
        encodedImages   = [encodedImages; encode(bofData.bag,currI,Verbose=false)];
        keyTimeStamps   = [keyTimeStamps; timeStamps.imageTimeStamps(currFrameIdx)];
        loopCtr         = loopCtr+1;
    end


    % Update current key frame ID
    currKeyFrameId  = currKeyFrameId + 1;

    % Extract IMU measurements between two KFs
    imuMeasurements = helperExtractIMUMeasurements(uavData, keyFrameToFrame(end-1), keyFrameToFrame(end));
    
    % Add the new key frame and IMU data
    [fGraph, viewToNode, velToNode, biasToNode, mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame(...
        fGraph, viewToNode, pointToNode, velToNode, biasToNode, mapPointSet, vSetKeyFrames, imuMeasurements, ...
        currPose, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds, intrinsics, ...
        uavData.camToIMUTransform, imuParams, isIMUInitialized);
    
    % Remove outlier map points that are observed in fewer than 3 key frames
    [mapPointSet,pointToNode] = helperCullRecentMapPoints(mapPointSet, pointToNode, mapPointsIdx, newPointIdx);
    
    % Create new map points by triangulation
    minNumMatches = 10;
    minParallax   = 3;
    [fGraph, pointToNode, mapPointSet, vSetKeyFrames, newPointIdx] = helperCreateNewMapPointsVI(...
        fGraph, pointToNode, viewToNode, mapPointSet, vSetKeyFrames, ...
        currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax);

Loop Closure

Loop candidates are recognized by comparing feature descriptors, which represent encoded images using a pre-trained bag of words. The detected loop candidate is validated when it is located near a previous pose, and the distance between the feature descriptors associated to both poses is smaller than a set threshold.

When a valid loop candidate is found, use estgeotform3d to compute the relative pose between the loop candidate frame and the current key frame. Then add the loop connection with the relative pose to the factor graph.

    loopClosureCheck = 350; % Before this frame, loop closure does not occur. 
    % Check loop closure after some key frames have been created  
    if currKeyFrameId > loopClosureCheck && loopCtr>10

        % Minimum number of feature matches of loop edges
        loopEdgeNumMatches = 100;

        % Detect possible loop closure key frame candidates
        potentialLoopCandidates = helperFindPotentialLC(vSetKeyFrames);

        if ~isempty(potentialLoopCandidates)
            [isDetected, validLoopCandidates] = helperCheckLoopClosureVI(encodedImages, potentialLoopCandidates);
            if isDetected 
                % Add loop closure connections
                [fGraph, isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsVI(fGraph, viewToNode, pointToNode,...
                    mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ...
                    currFeatures, currPoints, loopEdgeNumMatches, intrinsics);
                if isLoopClosed
                    loopCtr=0;
                end
            end
        end

    end

Local Mapping

Local mapping is performed for every key frame. When a new key frame is determined, add it and its corresponding IMU measurement to the factor graph and update to the mapPointSet and vSetKeyFrames objects. To ensure that mapPointSet contains as few outliers as possible, a valid map point must be observed in at least 3 key frames.

Create new map points by triangulating ORB feature points in the current key frame and its connected key frames. For each unmatched feature point in the current key frame, search for a match with other unmatched points in the connected key frames using matchFeatures. The local optimization of the factor graph refines the pose of the current key frame, the poses of connected key frames, and all the map points observed in these key frames.

    % Local bundle adjustment
    [refinedViews, dist] = connectedViews(vSetKeyFrames, currKeyFrameId, MaxDistance=2);
    refinedKeyFrameIds = refinedViews.ViewId;
    fixedViewIds = refinedKeyFrameIds(dist==2);
    fixedViewIds = fixedViewIds(1:min(10, numel(fixedViewIds)));

    refinedKeyFrameIds = [refinedKeyFrameIds; currKeyFrameId];

    [fGraph, solInfo] = helperLocalFactorGraphOptimization(fGraph, viewToNode, refinedKeyFrameIds, fixedViewIds);

    allOptNodes=solInfo.OptimizedNodeIDs;
    optPointNodes=setdiff(allOptNodes,viewToNode(refinedKeyFrameIds));
    [~,mapPointIdx]=intersect(pointToNode,optPointNodes,'stable');

    % Update world points
    mapPointSet = updateWorldPoints(mapPointSet,mapPointIdx,nodeState(fGraph,pointToNode(mapPointIdx)));

    % Update view poses
    for i=1:length(refinedKeyFrameIds)
        viewId = refinedKeyFrameIds(i);
        fgPose = nodeState(fGraph,viewToNode(viewId));
        absPose = rigidtform3d(quat2rotm(fgPose(4:7)),fgPose(1:3));
        vSetKeyFrames = updateView(vSetKeyFrames,viewId,absPose);
    end

    % Update view direction and depth
    mapPointSet = updateLimitsAndDirection(mapPointSet, mapPointIdx, vSetKeyFrames.Views);
    
    % Update representative view
    mapPointSet = updateRepresentativeView(mapPointSet, mapPointIdx, vSetKeyFrames.Views);

    % Visualize 3D world points and camera trajectory
    updatePlot(mapPlot, vSetKeyFrames, mapPointSet);
    
    % 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: 49 and 361
Loop edge added between keyframe: 112 and 426
Loop edge updated between keyframe: 127 and 443
Loop edge added between keyframe: 143 and 462

Global Optimization

Once all the frames have been processed, we proceed to optimize the factor graph one last time by including all the keyframes, 3D points and IMU measurements.

% Optimize the poses
vSetKeyFramesOptim = vSetKeyFrames;
fGraphOptim = fGraph;

optimize(fGraphOptim,fgso);

% Update map points after optimizing the poses
for i=1:vSetKeyFrames.NumViews
    fgPose = nodeState(fGraphOptim,viewToNode(i));
    absPose = rigidtform3d(quat2rotm(fgPose(4:7)),fgPose(1:3));
    vSetKeyFramesOptim = updateView(vSetKeyFramesOptim,i,absPose);
end

mapPointSet = helperUpdateGlobalMap(mapPointSet,vSetKeyFrames,vSetKeyFramesOptim);

updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

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

% Update legend
showPlotLegend(mapPlot);

% Load ground truth 
gTruth     = uavData.gTruth;

% Plot the actual camera trajectory 
plotActualTrajectory(mapPlot, uavData, keyFrameToFrame, gDir);

% Show legend
showPlotLegend(mapPlot);

Comparison Against the Ground Truth

The downloaded data contains the gTruth array, which you can use to compare the obtained trajectory with the ground truth to qualitatively evaluate the accuracy of the SLAM pipeline. You can also calculate the root-mean-square-error (RMSE) and absolute-mean-error (AME) of trajectory estimates.

[rmse, ame] = helperGetAccuracy(poses(vSetKeyFramesOptim),uavData,keyTimeStamps,gDir);

    "AME for trajectory (m): "    "0.36203"

    "RMSE for trajectory (m): "    "0.26287"

In this example you learned how to utilize the factor graph to combine pose estimates from a camera with IMU measurements to obtain more accurate results.

Supporting Functions

Helper functions related to factor graph processing are included below.

Other functions are included in separate files. For more information on these functions, please refer to the Monocular Visual Simultaneous Localization and Mapping example.

helperAddLoopConnectionsVI adds connections between the current keyframe and the valid loop candidate.

helperCheckLoopClosureVI detects key frames that are loop candidates by retrieving visually similar images from the database.

helperCreateNewMapPointsVI creates new map points by triangulation.

helperTrackLastKeyFrameVI estimates the current camera pose by tracking the last key frame.

helperTrackLocalMapVI refines the current camera pose by tracking the local map.

helperDetectAndExtractFeatures detects and extracts and ORB features from the image.

helperComputeHomography computes homography and evaluates reconstruction.

helperComputeFundamentalMatrix computes fundamental matrix and evaluates reconstruction.

helperTriangulateTwoFrames triangulates two frames to initialize the map.

helperCullRecentMapPoints culls recently added map points.

helperUpdateGlobalMap updates 3-D locations of map points after pose graph optimization

helperAlignIMUCamera estimates the gravity rotation and pose scale to transform the input poses to the local navigation reference frame of IMU using IMU measurements and factor graph optimization.

helperTransformToIMU transforms and scales input poses and XYZ 3-D points according to specified gravity direction and pose scale.

helperLocalFactorGraphOptimization optimizes the factor graph using a set of local key frames.

helperInitFactorGraph initializes the factor graph using the results obtained from SfM.

function [fGraph, refinedAbsPoses, refinedPoints, viewToNode, pointToNode]  = helperInitFactorGraph(vSetKeyFrames, mapPointSet, intrinsics)

    K = intrinsics.K;
    camInfo = ((K(1,1)/1.5)^2)*eye(2);

    refinedKeyFrameIds = vSetKeyFrames.Views.ViewId';
    
    graphIds = [];
    graphMes = [];
    graphPts = [];
    
    fGraph = factorGraph;
    
    % Generate the necessary IDs for poses and 3D points
    zeroId      = generateNodeID(fGraph,1);
    viewToNode  = generateNodeID(fGraph,vSetKeyFrames.NumViews)';
    pointToNode = generateNodeID(fGraph,mapPointSet.Count)';
    
    mapPointIdx = [];
    
    % Extract the data from mapPointSet to initialize the factor graph
    for i=1:length(refinedKeyFrameIds)
      viewId=refinedKeyFrameIds(i);
      [pointIndices,featureIndices]=findWorldPointsInView(mapPointSet,viewId);
    
      pointIds = pointToNode(pointIndices);
      poseIds  = ones(length(pointIds),1)*viewToNode(viewId);
      graphIds = [graphIds; [poseIds pointIds]];
    
      ptsInView   = vSetKeyFrames.Views.Points{viewId,1}.Location;
      graphMes = [graphMes; ptsInView(featureIndices,:)];
    
      mapPointIdx = [mapPointIdx; pointIndices];
      graphPts = [graphPts; mapPointSet.WorldPoints(pointIndices,:)];
    end
    
    [mapPointIdx, uniqueIndex, ~] = unique(mapPointIdx);
    graphPts = graphPts(uniqueIndex,:);
    
    % Set up a pose prior using the inital position and fix it
    initPos = double([vSetKeyFrames.Views.AbsolutePose(1,1).Translation rotm2quat(vSetKeyFrames.Views.AbsolutePose(1,1).R)]);
    initPosePrior = factorPoseSE3Prior(viewToNode(1),Measurement=initPos);
    addFactor(fGraph,initPosePrior);
    fixNode(fGraph,viewToNode(1));
    
    % Set up a factorCameraSE3AndPointXYZ using the matches extracted above
    fCam=factorCameraSE3AndPointXYZ(graphIds, K, Measurement=graphMes, ...
    Information=camInfo);
    addFactor(fGraph,fCam);
    nodeState(fGraph,pointToNode(mapPointIdx),double(graphPts));
    
    % Set up a pose node using the current position
    pose=vSetKeyFrames.Views.AbsolutePose(2,1);
    pose=double([pose.Translation rotm2quat(pose.R)]);
    nodeState(fGraph,viewToNode(2),pose);
    
    % Fix a few 3D points for more stability
    ptsIDS = nodeIDs(fGraph,NodeType="POINT_XYZ");
    idx = randsample(height(ptsIDS'),int32(height(ptsIDS')*0.05));
    fixNode(fGraph,ptsIDS(idx));
    
    optimize(fGraph,factorGraphSolverOptions);
    
    fixNode(fGraph,ptsIDS(idx),false);
    
    % Extract the results of the factor graph optimization and re-package them
    poseIDs  = nodeIDs(fGraph,NodeType="POSE_SE3");
    fgposopt = nodeState(fGraph,poseIDs);
    
    initPose    = rigidtform3d(quat2rotm(fgposopt(1,4:7)),fgposopt(1,1:3));
    refinedPose = rigidtform3d(quat2rotm(fgposopt(2,4:7)),fgposopt(2,1:3));
    
    ViewId=[1;2];
    AbsolutePose=[initPose;refinedPose];
    
    refinedAbsPoses=table(ViewId,AbsolutePose);
    
    ptsIDS = nodeIDs(fGraph,NodeType="POINT_XYZ");
    refinedPoints = nodeState(fGraph,ptsIDS);
end

helperAddNewKeyFrame adds key frame data to the factor graph.

function [fGraph, viewToNode, velToNode, biasToNode, mapPoints, vSetKeyFrames] = helperAddNewKeyFrame(...
    fGraph, viewToNode, pointToNode, velToNode, biasToNode, mapPoints, vSetKeyFrames, imuMeasurements, ...
    cameraPose, currFeatures, currPoints, mapPointsIndices, featureIndices, keyFramesIndices, ...
    intrinsics, camToIMUTransform, imuParams, initIMU)

    N = length(mapPointsIndices);
    K = intrinsics.K;
    camInfo = ((K(1,1)/1.5)^2)*eye(2);
    
    viewId = vSetKeyFrames.Views.ViewId(end)+1;
    
    vSetKeyFrames = addView(vSetKeyFrames, viewId, cameraPose,...
        'Features', currFeatures.Features, ...
        'Points', currPoints);
    
    viewsAbsPoses = vSetKeyFrames.Views.AbsolutePose;
    
    % Generate the pose ID
    poseId = generateNodeID(fGraph,1);
    viewToNode = [viewToNode;poseId];
    
    for i = 1:numel(keyFramesIndices)
        localKeyFrameId = keyFramesIndices(i);
        [index3d, index2d] = findWorldPointsInView(mapPoints, localKeyFrameId);
        [~, index1, index2] = intersect(index3d, mapPointsIndices, 'stable');
        
        prePose   = viewsAbsPoses(localKeyFrameId);
        relPose = rigidtform3d(prePose.R' * cameraPose.R, ...
            (cameraPose.Translation-prePose.Translation)*prePose.R);
        
        if numel(index1) > 5
            vSetKeyFrames = addConnection(vSetKeyFrames, localKeyFrameId, viewId, relPose, ...
                Matches=[index2d(index1),featureIndices(index2)]);
    
            % Add a 3d pose factor to link both frames in the local map
            fgRelPose=double([relPose.Translation rotm2quat(relPose.R)]);
            fPose=factorTwoPoseSE3([viewToNode(localKeyFrameId) viewToNode(viewId)],Measurement=fgRelPose);
            addFactor(fGraph,fPose);
        end
    end
    
    mapPoints = addCorrespondences(mapPoints, viewId, mapPointsIndices, ...
        featureIndices);
    
    % Generate the IDs to be used in the factorCameraSE3AndPointXYZ
    ptsIds  = pointToNode(mapPointsIndices);
    poseIds = ones(N,1)*poseId;
    
    ptsInView = vSetKeyFrames.Views.Points{viewId,1}.Location;
    mesPts = ptsInView(featureIndices,:);
    
    % Add camera factor to the factor graph
    fCam = factorCameraSE3AndPointXYZ([poseIds ptsIds], K, Measurement=mesPts, ...
        Information=camInfo);
    addFactor(fGraph,fCam);
    
    % Add pose node to the factor graph
    pose = double([cameraPose.Translation rotm2quat(cameraPose.R)]);
    nodeState(fGraph,poseId,pose);
    
    % If the camera poses have been scaled properly, then add the IMU
    % factor to the factor graph
    if initIMU
        velId = generateNodeID(fGraph,1);
        velToNode=[velToNode;velId];
    
        biasId = generateNodeID(fGraph,1);
        biasToNode=[biasToNode;biasId];
    
        imuId = [viewToNode(end-1),velToNode(end-1),biasToNode(end-1), ...
                 viewToNode(end)  ,velToNode(end)  ,biasToNode(end)];
        fIMU = factorIMU(imuId,imuMeasurements.gyro,imuMeasurements.accel,imuParams,SensorTransform=camToIMUTransform);
        addFactor(fGraph,fIMU);
    end
end

References

[1] Dellaert, Frank, and Michael Kaess. Factor Graphs for Robot Perception. Now Publishers, 2017.

[2] Campos, Carlos, et al. “ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM.” IEEE Transactions on Robotics, vol. 37, no. 6, Dec. 2021, pp. 1874–90. DOI.org (Crossref), https://doi.org/10.1109/TRO.2021.3075644.