addFrame

Add image frame to visual SLAM object

Since R2023b

Syntax

addFrame(vslam,I)

addFrame(___,gyro,accel)

Description

addFrame(vslam,I) adds a grayscale or RGB image I, to the visual SLAM object vslam.

Note

The monovslam object runs on multiple threads internally, which can delay the processing of an image frame added by using the addFrame function. Additionally, the object running on multiple threads means the current frame the object is processing can be different than the recently added frame.

addFrame(___,gyro,accel) adds IMU gyroscope gyro, and accelerometer accel, measurements to the visual SLAM object.

Examples

Monocular Visual SLAM Using TUM RGB-D Data Set

Perform monocular visual simultaneous localization and mapping (vSLAM) using the data from the TUM RGB-D Benchmark. You can download the data to a temporary directory using a web browser or by running this code:

baseDownloadURL = "https://cvg.cit.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

Create an imageDatastore object to store all the RGB images.

imageFolder = dataFolder+"rgbd_dataset_freiburg3_long_office_household/rgb/";
imds = imageDatastore(imageFolder);

Specify your camera intrinsic parameters, and use them to create a monocular visual SLAM object.

intrinsics = cameraIntrinsics([535.4 539.2],[320.1 247.6],[480 640]);
vslam = monovslam(intrinsics,TrackFeatureRange=[30,120]);

Process each image frame, and visualize the camera poses and 3-D map points. Note that the monovslam object runs several algorithm parts on separate threads, which can introduce a latency in processing of an image frame added by using the addFrame function.

for i = 1:numel(imds.Files)
    addFrame(vslam,readimage(imds,i))

    if hasNewKeyFrame(vslam)
        % Display 3-D map points and camera trajectory
        plot(vslam);
    end

    % Get current status of system
    status = checkStatus(vslam);
end 

Plot intermediate results and wait until all images are processed.

while ~isDone(vslam)
    if hasNewKeyFrame(vslam)
        plot(vslam);
    end
end

After all the images are processed, you can collect the final 3-D map points and camera poses for further analysis.

xyzPoints = mapPoints(vslam);
[camPoses,addedFramesIdx] = poses(vslam);

% Reset the system
reset(vslam)

Compare the estimated camera trajectory with the ground truth to evaluate the accuracy.

% Load ground truth
gTruthData = load("orbslamGroundTruth.mat");
gTruth     = gTruthData.gTruth;

% Evaluate tracking accuracy
mtrics = compareTrajectories(camPoses, gTruth(addedFramesIdx), AlignmentType="similarity");
disp(['Absolute RMSE for key frame location (m): ', num2str(mtrics.AbsoluteRMSE(2))]);

Absolute RMSE for key frame location (m): 0.20043

% Plot the absolute translation error at each key frame
figure
ax = plot(mtrics, "absolute-translation");
view(ax, [2.70 -49.20]); 

Input Arguments

vslam — Visual SLAM object

monovslam object

Visual SLAM object, specified as a monovslam object.

I — Image

grayscale image | RGB image

Image, specified as a grayscale or RGB image.

Data Types: single | double | int16 | uint8 | uint16 | logical

gyro — Gyroscope IMU measurement

N-by-3 matrices

Gyroscope IMU measurement, specified by N-by-3 matrices. N specifies the number of IMU measurements between the previous and current camera frames. Each row of the matrix specifies a gyroscope measurement of the form [gx, gy, gz].

accel — Acceleration IMU measurement

N-by-3 matrices

Acceleration IMU measurement, specified by N-by-3 matrices. N specifies the number of IMU measurements between the previous and current camera frames. Each row of the matrix specifies an acceleration measurement of the form [ax, ay, az].

Version History

Introduced in R2023b

R2024b: Input arguments added for IMU measurements

Specify IMU measurements, gyro and acceleration, for visual-inertial fusion.