addFrame

Add pair of color and depth images to stereo visual SLAM object

Since R2024a

Syntax

addFrame(vslam,I1,I2)

addFrame(___,gyro,accel)

addFrame(___,DisparityMap=Value)

Description

addFrame(vslam,I1,I2) adds the pair of RGB or grayscale stereo images, I1 and I2, to the stereo visual SLAM object vslam.

Note

The stereovslam 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 and acceleration measurements for visual-inertial fusion.

addFrame(___,DisparityMap=Value) sets the disparity map for the stereo input images I1 and I2. The disparity map must be a single or double matrix and the same width and height as the input images. If unspecified, the function defaults to calculating frame disparity using semi-global block matching.

Examples

Stereo Visual SLAM Using UTIAS Data Set

Perform stereo visual simultaneous localization and mapping (vSLAM) using the data from the UTIAS Long-Term Localization and Mapping Dataset provided by University of Toronto Institute for Aerospace Studies. You can download the data to a directory using a web browser, or by running this code:

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 and extract its contents.

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);

    disp("Extracting run_000005.zip (818 MB) ...") 
    unzip(zipFileName,dataFolder); 
end

Create two imageDatastore objects to store the stereo images.

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

Specify the intrinsic parameters and the baseline of the stereo camera, and use them to create a stereo visual SLAM object. The focal length, principal point, and image size is in pixels, and the baseline is in meters.

focalLength = [387.777 387.777];  
principalPoint = [257.446 197.718];  
imageSize = [384 512];            
intrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);
baseline = 0.239965; 

vslam = stereovslam(intrinsics,baseline,MaxNumPoints=600, ...
    TrackFeatureRange=[30 120],SkipMaxFrames=5);

Process each pair of stereo images and visualize the camera poses and 3-D map points.

for i = 1:numel(imdsLeft.Files)
    leftImage = readimage(imdsLeft,i);
    rightImage = readimage(imdsRight,i);
    addFrame(vslam,leftImage,rightImage);

    if hasNewKeyFrame(vslam)
        % Query 3-D map points and camera poses
        xyzPoints = mapPoints(vslam);
        [camPoses,viewIds] = poses(vslam);

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

    % Get current status of system
    status = checkStatus(vslam);
    
    % Stop adding frames when tracking is lost
    if status == uint8(0)
        break
    end
end 

Once all the frames have been processed, reset the system.

while ~isDone(vslam)
    plot(vslam)
end

reset(vslam)

Input Arguments

vslam — Stereo visual SLAM object

stereovslam object

Stereo visual SLAM object, specified as a stereovslam object.

I1 — Image 1

RGB image | grayscale image

Image 1, specified as a nonsparse RGB or grayscale image. The stereo visual slam object tracks the pose of camera 1, the left camera, which corresponds to image I1 in the stereo camera system. The input images must be undistorted and rectified. You can undistort and then rectify the images by using the undistortImage and rectifyStereoImages functions, respectively, before adding the images.

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

I2 — Image 2

RGB image | grayscale image

Image 2, specified as a nonsparse RGB or grayscale image. Image 2 corresponds to the right camera in the stereo camera system. The input images must be undistorted and rectified. You can undistort and then rectify the images by using the undistortImage and rectifyStereoImages functions, respectively, before adding the images.

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

gyro — Gyroscope measurement

N-by-3 matrix

Gyroscope measurement, specified as an N-by-3 matrix. Each row of the matrix represents the x, y, and z components of the gyroscope measurement for a single frame, of the form [gx,gy,gz]. N represents the total number of frames. N represents the number of IMU measurements between the previous and current camera frames. The number of gyroscope and accelerometer measurements must be equal.

accel — Acceleration measurement

N-by-3 matrix

Acceleration measurement, specified as an N-by-3 matrix. Each row of the matrix represents the x, y, and z components of the acceleration measurement for a single frame, of the form [ax,ay,az]. N represents the number of IMU measurements between the previous and current camera frames. The number of gyroscope and accelerometer measurements must be equal.

Version History

Introduced in R2024a

R2024b: Set disparity map for stereo vSLAM images

Use the frame disparity by using the DisparityMap name-value argument.