plot

Plot 3-D map points and estimated camera trajectory in stereo visual SLAM

Since R2024a

Syntax

plot(vslam)

plot(vslam,Name=Value)

ax = plot(___)

Description

plot(vslam) plots the 3-D map points and estimated camera trajectory from the visual SLAM object vslam.

plot(vslam,Name=Value) specifies options using one or more name-value arguments. For example, MarkerSize=10 sets the diameter of the marker size to 10 points.

ax = plot(___) returns the axes handle for the 3-D mapped points of the plot, using any combination of input arguments from previous syntaxes.

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.

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Example: plot(vslam,MarkerSize=10), sets the diameter of the marker to 10 points.

MarkerSize — Diameter of markers for 3-D map points

6 (default) | positive integer

Diameter of markers for the 3-D map points, specified as a positive integer in points.

MarkerColor — Color of markers for 3-D map points

[0 1 0] (default) | RGB triplet | short or long color name

Color of the markers for the 3-D map points, specified as an RGB triplet or a short or long color name. Each value within an RGB triplet must be in the range [0 1].

ColorSource — Colormap source for the markers

"Y" (default) | "X" | "Z" | "MarkerColor"

Colormap source for markers, specified as "X", "Y", "Z", or "MarkerColor".

CameraSize — Width of camera base

0.1 (default) | positive scalar

Width of the camera base, specified as a positive scalar in the data units of the axes.

CameraColor — Color of camera

[1 0 0] (default) | RGB triplet | short or long color name

Color of the camera, specified as an RGB triplet or a short or long color name. Each value within an RGB triplet must be in the range [0 1].

Parent — Axes for visualization

Axes object | UIAxes object

Axes for visualization, specified as an Axes graphics object or a UIAxes object. Use this name-value argument to visualize the plot in a UI for which you have specified Figure and Axes properties.

Output Arguments

ax — Axes handle

Axes graphics object

Axes handle, returned as an axes graphics object.

Version History

Introduced in R2024a