poses

Absolute camera poses of stereo key frames

Since R2024a

Syntax

camPoses = poses(vslam)

[camPoses,keyFrameIDs] = poses(vslam)

Description

camPoses = poses(vslam) returns the absolute primary camera poses camPoses of the key frames from the visual simultaneous localization and mapping (vSLAM) object vslam.

[camPoses,keyFrameIDs] = poses(vslam) returns the IDs keyFrameIDs of the key frames corresponding to the primary camera poses. The IDs represent the order in which the image frames have been added to the vslam object by the addFrame object function.

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.

Output Arguments

camPoses — Absolute primary camera poses of key frames

M-element array of rigidtform3d objects

Absolute primary camera poses of the key frames, returned as an M-element array of rigidtform3d objects.

keyFrameIDs — Key frame IDs for absolute primary camera poses

M-element array of integers

Key frame IDs for the absolute primary camera poses, returned as an M-element array of integers. Each element specifies the key frame ID for the camera pose in the corresponding element of camPoses. The IDs represent the order in which the image frames have been added to vslam by the addFrame object function.

Version History

Introduced in R2024a