Main Content

compareTrajectories

Compare estimated trajectory against ground truth

Since R2024b

collapse all in page

Syntax

metrics = compareTrajectories(estimatedPoses,groundTruthPose)
metrics = compareTrajectories(___,Name=Value)

Description

metrics = compareTrajectories(estimatedPoses,groundTruthPose) returns metrics for the accuracy of an estimated trajectory, estimatedPoses, against a ground truth trajectory groundTruthPose, to assess the performance of an odometry or a Simultaneous Localization and Mapping (SLAM) system.

The function aligns the estimated trajectory to the ground truth trajectory by performing a least-square fitting between the translation components of poses in the two trajectories.

example

metrics = compareTrajectories(___,Name=Value) specifies options using one or more name-value arguments in addition to the previous syntaxes. For example, AlignmentType="none" sets the transformation type to "none" when aligning the estimated trajectory to the ground truth trajectory.

Examples

collapse all

This example uses:

Open Live Script

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

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 12 objects of type line, text, patch, scatter. This object represents Camera trajectory.

Plot intermediate results and wait until all images are processed.

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

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 12 objects of type line, text, patch, scatter. This object represents Camera trajectory.

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.093645

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

Figure contains an axes object. The axes object with title Absolute Translation Error, xlabel X, ylabel Y contains 2 objects of type patch, line. These objects represent Estimated Trajectory, Ground Truth Trajectory.

Input Arguments

collapse all

Estimated poses, specified as an M-element array of rigidtform3d objects. The estimated poses can be obtained from an odometry or a SLAM system.

Ground truth poses, specified as an M-element array of rigidtform3d objects. The ground truth poses are obtained from an external ground truth system.

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: compareTrajectories(estimatedPoses,groundTruthPose,AlignmentType="none") sets the transformation type to "none" when aligning the estimated trajectory to the ground truth trajectory.

Transformation type to align the estimated trajectory to the ground truth trajectory, specified as "rigid", "similarity", or "none". To disable trajectory alignment, set AlignmentType to "none".

Fixed distance to compute relative trajectory error for each pose within the estimated trajectory, specified as a scalar in world units.

Output Arguments

collapse all

Error metrics object, returned as an trajectoryErrorMetrics object. The object stores error metrics including the absolute trajectory error and the relative trajectory error.

References

[1] Sturm, Jürgen, Nikolas Engelhard, Felix Endres, Wolfram Burgard, and Daniel Cremers. 2012. "A Benchmark for the Evaluation of RGB-D SLAM Systems." In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 573-580.

[2] Zhang, Zichao, and Davide Scaramuzza. 2018. "A Tutorial on Quantitative Trajectory Evaluation for Visual (-Inertial) Odometry." In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 7244-7251.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2024b

See Also

Objects