使用 MATLAB 实现对 EuRoC 和 TUM 数据集的轨迹评估

使用 MATLAB 实现对 EuRoC 和 TUM 数据集的轨迹评估

# 不会使用EVO评估工具的看过来↓↓↓
在计算机视觉和机器人领域，轨迹评估是一个关键环节，用于衡量 SLAM（Simultaneous Localization and Mapping，同时定位与建图）算法的性能。传统的工具如 EVO（Evaluation of Visual Odometry and SLAM）提供了强大的功能，但有时我们可能需要一个更灵活的解决方案。本文将介绍如何使用 MATLAB 实现对 EuRoC 和 TUM 数据集的轨迹评估，特别是针对 ORB-SLAM3 和 VINS-MONO 等算法。

先展示效果图再讲解代码：

①位置误差和旋转误差可视化：

②位置轨迹可视化（3D/2D）：

③量化误差表：

1. 引言

EuRoC 数据集和 TUM 数据集是 SLAM 算法评估中最常用的基准数据集之一。EuRoC 数据集提供了丰富的视觉和惯性数据，而 TUM 数据集则专注于室内环境的 RGB-D 数据。评估这些数据集时，我们通常需要计算轨迹的绝对位置误差（APE）和绝对旋转误差（ARE），并绘制轨迹对比图和误差曲线。

在本文中，我们将使用 MATLAB 实现一个完整的轨迹评估流程，包括：

1. 数据加载和预处理。
2. 时间戳对齐。
3. 轨迹标准化。
4. 误差计算。
5. 结果可视化。

2. MATLAB 实现

以下是完整的 MATLAB 实现代码，涵盖了从数据加载到结果可视化的所有步骤。

2.1 数据加载和预处理

我们首先定义文件路径和轨迹名称，并加载真实轨迹数据。真实轨迹数据通常存储在 CSV 文件中，包含时间戳和位置信息。

function testerror4()
    % 文件路径和轨迹名称
    trajectory_names = {'Mono', 'Kf-Mono', 'Monoi', 'Kf-Monoi', ...
                        'Stereo', 'Kf-Stereo', 'Stereoi', 'Kf-Stereoi'};
    file_list = {'f_dataset-MH01_mono.txt', 'kf_dataset-MH01_mono.txt', ...
                 'f_dataset-MH01_monoi.txt', 'kf_dataset-MH01_monoi.txt', ...
                 'f_dataset-MH01_stereo.txt', 'kf_dataset-MH01_stereo.txt', ...
                 'f_dataset-MH01_stereoi.txt', 'kf_dataset-MH01_stereoi.txt'};
    groundtruth_file = 'groundtruth.csv';
    
    % 读取真实轨迹并进行初始化处理
    opts = detectImportOptions(groundtruth_file);
    opts.VariableNamingRule = 'preserve';  % 保留原始列标题
    groundtruth = readtable(groundtruth_file, opts);
    
    % 提取时间戳和位置数据
    gt_timestamps = table2array(groundtruth(:, 1));  % 假设时间戳在第1列
    gt_positions = table2array(groundtruth(:, 2:4));  % 假设位置数据在第2-4列
    
    % 提取四元数数据（如果存在）
    if size(groundtruth, 2) >= 8
        gt_rotations = table2array(groundtruth(:, 5:8));  % 假设四元数数据在第5-8列
    else
        warning('Ground truth data does not contain quaternion information. Using default quaternions.');
        gt_rotations = repmat([1, 0, 0, 0], size(gt_positions, 1), 1);  % 默认单位四元数
    end
    
    % 标准化真实轨迹
    gt_positions = normalize_trajectory(gt_positions);
    
    % 绕 z 轴旋转 45°
    angle = -81; % 旋转角度
    R = [cosd(angle) -sind(angle) 0;
         sind(angle) cosd(angle) 0;
         0 0 1];  % 旋转矩阵
    
    % 对真实轨迹进行旋转
    gt_positions = (R * gt_positions')';  % 转置操作确保矩阵维度匹配
end

2.2 时间戳对齐

为了确保真实轨迹和估计轨迹的时间戳一致，我们需要对齐时间戳。这里我们使用线性插值来对齐轨迹。

function [aligned_positions, aligned_rotations] = align_with_gt(positions, rotations, timestamps, gt_timestamps)
    % 检查时间戳范围是否匹配
    if timestamps(1) > gt_timestamps(end) || timestamps(end) < gt_timestamps(1)
        warning('Timestamp ranges do not overlap. Alignment may fail.');
    end

    % 插值对齐
    aligned_positions = interp1(timestamps, positions, gt_timestamps, 'linear', 'extrap');
    aligned_rotations = interp1(timestamps, rotations, gt_timestamps, 'linear', 'extrap');
end

2.3 误差计算

我们计算位置误差和旋转误差，并评估多种误差指标（如 RMSE、MAE 和最大误差）。

function [position_error, rotation_error] = compute_errors(positions, rotations, gt_positions, gt_rotations)
    % 确保 positions 和 groundtruth 的行数相同
    n_positions = size(positions, 1);
    n_groundtruth = size(gt_positions, 1);
    
    % 如果行数不一致，裁剪长的，或者填充短的（这里我们选择裁剪）
    min_length = min(n_positions, n_groundtruth);
    positions = positions(1:min_length, :);
    gt_positions = gt_positions(1:min_length, :);
    rotations = rotations(1:min_length, :);
    gt_rotations = gt_rotations(1:min_length, :);
    
    % 位置误差
    position_error = sqrt(sum((positions - gt_positions).^2, 2));
    
    % 姿态误差：计算旋转误差，使用四元数
    rotation_error = zeros(size(rotations, 1), 1);
    for i = 1:size(rotations, 1)
        q_pred = rotations(i, :);  % 预测的旋转四元数
        q_gt = gt_rotations(i, :);  % 真实轨迹的旋转四元数
        rotation_error(i) = quaternion_angle_difference(q_pred, q_gt);
    end
end

2.4 结果可视化

我们绘制三维轨迹对比图和误差曲线，以便直观地比较不同轨迹的性能。

function plot_3d_trajectory(all_positions, gt_positions, trajectory_names)
    figure;
    hold on;
    colors = lines(length(all_positions));
    
    % 绘制真实轨迹
    plot3(gt_positions(:,1), gt_positions(:,2), gt_positions(:,3), 'k-', 'LineWidth', 2);
    
    % 绘制估计轨迹
    for i = 1:length(all_positions)
        positions = all_positions{i};
        plot3(positions(:,1), positions(:,2), positions(:,3), '-', 'Color', colors(i, :), 'LineWidth', 1.5);
    end
    
    legend(['Ground Truth', trajectory_names]);
    xlabel('X');
    ylabel('Y');
    zlabel('Z');
    title('3D Trajectories Comparison');
    grid on;
    axis equal;
    hold off;
end

2.5 导出对齐后的数据

对齐后的数据可以保存到指定文件夹中，方便后续分析。

function export_aligned_data(timestamps, positions, rotations, filename)
    aligned_data = [timestamps, positions, rotations];
    output_dir = 'F:\桌面\毕业论文\代码\对齐后'; % 指定保存目录
    output_filename = fullfile(output_dir, [filename(1:end-4), '_aligned.txt']);
    
    % 检查是否存在 NaN 值
    if any(isnan(aligned_data(:)))
        warning('NaN values detected in aligned data. Saving file with NaN values.');
    end
    
    writematrix(aligned_data, output_filename);
    fprintf('Aligned data saved to: %s\n', output_filename);
end

3. 测试与结果

运行上述代码后，你将得到以下结果：

1. 对齐后的轨迹数据：保存到指定文件夹中。
2. 三维轨迹对比图：直观展示真实轨迹和估计轨迹的差异。
3. 误差曲线图：展示位置误差和旋转误差的变化。
4. 误差评估指标：包括 RMSE、MAE 和最大误差等。

4. 总结

本文介绍了一个完整的 MATLAB 实现，用于评估 EuRoC 和 TUM 数据集上的 SLAM 算法性能。通过时间戳对齐、轨迹标准化、误差计算和结果可视化，我们可以全面评估 ORB-SLAM3 和 VINS-MONO 等算法的性能。这种方法不仅灵活，而且可以轻松扩展到其他数据集和算法。

希望本文对你有所帮助！如果你有任何问题或建议，请随时留言交流