ORBSLAM3跑tum数据集的轨迹与真实轨迹对齐C++代码

以下是使用ORB-SLAM3对TUM数据集进行轨迹对齐的C++代码示例：

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <cmath>

using namespace std;

// 读取轨迹文件
bool readTrajectory(const string& filename, vector<double>& timestamps, vector<vector<double> >& poses)
{
    ifstream file(filename);
    if (!file.is_open())
    {
        cerr << "Failed to open file: " << filename << endl;
        return false;
    }

    while (!file.eof())
    {
        string line;
        getline(file, line);
        if (!line.empty() && line.at(0) != '#')
        {
            stringstream ss(line);
            double timestamp;
            ss >> timestamp;

            vector<double> pose(7);
            for (int i = 0; i < 7; ++i)
            {
                ss >> pose[i];
            }

            timestamps.push_back(timestamp);
            poses.push_back(pose);
        }
    }

    file.close();
    return true;
}

// 对齐两个轨迹
void alignTrajectories(const vector<double>& timestamps1, const vector<vector<double> >& poses1,
                       const vector<double>& timestamps2, const vector<vector<double> >& poses2,
                       vector<vector<double> >& aligned_poses2)
{
    aligned_poses2.clear();

    // 计算两个轨迹的平均旋转和平移
    double mean_rotation1 = 0.0;
    vector<double> mean_translation1(3, 0.0);
    for (size_t i = 0; i < poses1.size(); ++i)
    {
        double qw = poses1[i][0], qx = poses1[i][1], qy = poses1[i][2], qz = poses1[i][3];
        double tx = poses1[i][4], ty = poses1[i][5], tz = poses1[i][6];

        double roll = atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy));
        double pitch = asin(2.0 * (qw * qy - qx * qz));
        double yaw = atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));

        mean_rotation1 += yaw;
        mean_translation1[0] += tx;
        mean_translation1[1] += ty;
        mean_translation1[2] += tz;
    }
    mean_rotation1 /= poses1.size();
    mean_translation1[0] /= poses1.size();
    mean_translation1[1] /= poses1.size();
    mean_translation1[2] /= poses1.size();

    double mean_rotation2 = 0.0;
    vector<double> mean_translation2(3, 0.0);
    for (size_t i = 0; i < poses2.size(); ++i)
    {
        double qw = poses2[i][0], qx = poses2[i][1], qy = poses2[i][2], qz = poses2[i][3];
        double tx = poses2[i][4], ty = poses2[i][5], tz = poses2[i][6];

        double roll = atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy));
        double pitch = asin(2.0 * (qw * qy - qx * qz));
        double yaw = atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));

        mean_rotation2 += yaw;
        mean_translation2[0] += tx;
        mean_translation2[1] += ty;
        mean_translation2[2] += tz;
    }
    mean_rotation2 /= poses2.size();
    mean_translation2[0] /= poses2.size();
    mean_translation2[1] /= poses2.size();
    mean_translation2[2] /= poses2.size();

    // 对齐轨迹2
    for (size_t i = 0; i < poses2.size(); ++i)
    {
        double qw = poses2[i][0], qx = poses2[i][1], qy = poses2[i][2], qz = poses2[i][3];
        double tx = poses2[i][4], ty = poses2[i][5], tz = poses2[i][6];

        double roll = atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy));
        double pitch = asin(2.0 * (qw * qy - qx * qz));
        double yaw = atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));

        // 将姿态变换到轨迹1的参考系下
        double aligned_yaw = yaw - mean_rotation2 + mean_rotation1;
        double aligned_tx = tx - mean_translation2[0] + mean_translation1[0];
        double aligned_ty = ty - mean_translation2[1] + mean_translation1[1];
        double aligned_tz = tz - mean_translation2[2] + mean_translation1[2];

        double aligned_qw = cos(aligned_yaw / 2.0);
        double aligned_qx = 0.0;
        double aligned_qy = 0.0;
        double aligned_qz = sin(aligned_yaw / 2.0);

        vector<double> aligned_pose(7);
        aligned_pose[0] = aligned_qw;
        aligned_pose[1] = aligned_qx;
        aligned_pose[2] = aligned_qy;
        aligned_pose[3] = aligned_qz;
        aligned_pose[4] = aligned_tx;
        aligned_pose[5] = aligned_ty;
        aligned_pose[6] = aligned_tz;

        aligned_poses2.push_back(aligned_pose);
    }
}

// 计算轨迹误差
void computeTrajectoryError(const vector<vector<double> >& poses1, const vector<vector<double> >& poses2,
                            double& rmse_translation, double& rmse_rotation)
{
    rmse_translation = 0.0;
    rmse_rotation = 0.0;

    for (size_t i = 0; i < poses1.size(); ++i)
    {
        double qw1 = poses1[i][0], qx1 = poses1[i][1], qy1 = poses1[i][2], qz1 = poses1[i][3];
        double tx1 = poses1[i][4], ty1 = poses1[i][5], tz1 = poses1[i][6];

        double qw2 = poses2[i][0], qx2 = poses2[i][1], qy2 = poses2[i][2], qz2 = poses2[i][3];
        double tx2 = poses2[i][4], ty2 = poses2[i][5], tz2 = poses2[i][6];

        // 计算平移误差
        double dx = tx1 - tx2;
        double dy = ty1 - ty2;
        double dz = tz1 - tz2;
        double translation_error = sqrt(dx * dx + dy * dy + dz * dz);
        rmse_translation += translation_error * translation_error;

        // 计算旋转误差
        double dot_product = qw1 * qw2 + qx1 * qx2 + qy1 * qy2 + qz1 * qz2;
        double rotation_error = acos(2.0 * dot_product * dot_product - 1.0);
        rmse_rotation += rotation_error * rotation_error;
    }

    rmse_translation = sqrt(rmse_translation / poses1.size());
    rmse_rotation = sqrt(rmse_rotation / poses1.size());
}

int main(int argc, char** argv)
{
    if (argc < 3)
    {
        cerr << "Usage: " << argv[0] << " trajectory_file1 trajectory_file2" << endl;
        return 1;
    }

    // 读取轨迹文件
    string filename1(argv[1]);
    vector<double> timestamps1;
    vector<vector<double> > poses1;
    if (!readTrajectory(filename1, timestamps1, poses1))
    {
        return 1;
    }

    string filename2(argv[2]);
    vector<double> timestamps2;
    vector<vector<double> > poses2;
    if (!readTrajectory(filename2, timestamps2, poses2))
    {
        return 1;
    }

    // 对齐轨迹2
    vector<vector<double> > aligned_poses2;
    alignTrajectories(timestamps1, poses1, timestamps2, poses2, aligned_poses2);

    // 计算轨迹误差
    double rmse_translation, rmse_rotation;
    computeTrajectoryError(poses1, poses2, rmse_translation, rmse_rotation);
    cout << "Before alignment:" << endl;
    cout << "RMSE translation: " << rmse_translation << " m" << endl;
    cout << "RMSE rotation: " << rmse_rotation << " rad" << endl;

    computeTrajectoryError(poses1, aligned_poses2, rmse_translation, rmse_rotation);
    cout << "After alignment:" << endl;
    cout << "RMSE translation: " << rmse_translation << " m" << endl;
    cout << "RMSE rotation: " << rmse_rotation << " rad" << endl;

    return 0;
}

该代码读取两个轨迹文件（TUM数据集格式），并计算它们之间的平移和旋转误差。然后，它使用平均旋转和平移对第二个轨迹进行对齐，并重新计算误差。注意，该代码使用欧拉角（yaw-pitch-roll）来表示姿态，而不是四元数，因为这更容易理解和调试。如果您需要更高的精度，请改用四元数。