ORBSLAM输出的轨迹与数据集真实轨迹对齐并输出txt文件C++代码

以下是一个简单的示例代码，将ORBSLAM的轨迹输出到一个txt文件中，并与真实轨迹对齐。假设你已经有了真实轨迹的数据集和ORBSLAM的轨迹数据集，其中每个数据点包含了一个时间戳、位置和方向信息。

#include <iostream>
#include <fstream>
#include <vector>

struct Pose {
    double timestamp;
    double x, y, z;
    double qx, qy, qz, qw;
};

int main(int argc, char** argv) {
    // 读取真实轨迹数据
    std::vector<Pose> ground_truth_poses;
    std::ifstream ground_truth_file("ground_truth.txt");
    double timestamp, x, y, z, qx, qy, qz, qw;
    while (ground_truth_file >> timestamp >> x >> y >> z >> qx >> qy >> qz >> qw) {
        Pose pose;
        pose.timestamp = timestamp;
        pose.x = x;
        pose.y = y;
        pose.z = z;
        pose.qx = qx;
        pose.qy = qy;
        pose.qz = qz;
        pose.qw = qw;
        ground_truth_poses.push_back(pose);
    }
    ground_truth_file.close();

    // 读取ORBSLAM的轨迹数据
    std::vector<Pose> orbslam_poses;
    std::ifstream orbslam_file("orbslam_trajectory.txt");
    while (orbslam_file >> timestamp >> x >> y >> z >> qx >> qy >> qz >> qw) {
        Pose pose;
        pose.timestamp = timestamp;
        pose.x = x;
        pose.y = y;
        pose.z = z;
        pose.qx = qx;
        pose.qy = qy;
        pose.qz = qz;
        pose.qw = qw;
        orbslam_poses.push_back(pose);
    }
    orbslam_file.close();

    // 对齐ORBSLAM的轨迹和真实轨迹
    std::ofstream aligned_file("aligned_trajectory.txt");
    int i = 0, j = 0;
    while (i < ground_truth_poses.size() && j < orbslam_poses.size()) {
        double gt_timestamp = ground_truth_poses[i].timestamp;
        double orbslam_timestamp = orbslam_poses[j].timestamp;
        if (orbslam_timestamp < gt_timestamp) {
            j++;
        } else if (orbslam_timestamp > gt_timestamp) {
            i++;
        } else {
            // 找到了时间戳匹配的点，将ORBSLAM的位置和方向信息与真实轨迹的时间戳一起输出
            Pose aligned_pose = orbslam_poses[j];
            aligned_file << aligned_pose.timestamp << " "
                         << ground_truth_poses[i].x << " "
                         << ground_truth_poses[i].y << " "
                         << ground_truth_poses[i].z << " "
                         << aligned_pose.qx << " "
                         << aligned_pose.qy << " "
                         << aligned_pose.qz << " "
                         << aligned_pose.qw << std::endl;
            i++;
            j++;
        }
    }
    aligned_file.close();

    return 0;
}

这个示例代码中，我们首先读取真实轨迹和ORBSLAM的轨迹数据，然后对齐这两个轨迹。对齐的方法是，我们先按照时间戳排序，然后在真实轨迹和ORBSLAM轨迹中同时扫描，并将时间戳匹配的点输出到一个新的文件中。输出的格式为：时间戳（来自ORBSLAM），位置（来自真实轨迹），方向（来自ORBSLAM）。

你可以根据需要对这个示例代码进行修改，以适应你自己的数据集和需求。