给入两个ORBSLAM运动轨迹txt，使轨迹对齐输出轨迹txt文件代码C++

以下是一个简单的C++代码示例，可以将两个ORBSLAM运动轨迹文件对齐，并将结果输出到新的轨迹文件中。代码中使用了Eigen库来进行矩阵计算。

#include <iostream>
#include <fstream>
#include <Eigen/Dense>

using namespace Eigen;

// 定义一个结构体来存储轨迹中的每个位姿信息
struct Pose
{
    double timestamp;
    Vector3d position;
    Quaterniond orientation;
};

// 从文件中读取轨迹数据
bool readTrajectory(const std::string& filename, std::vector<Pose>& trajectory)
{
    std::ifstream file(filename);
    if (!file.is_open())
    {
        std::cerr << "Error: Cannot open file " << filename << std::endl;
        return false;
    }

    while (!file.eof())
    {
        Pose pose;
        file >> pose.timestamp >> pose.position.x() >> pose.position.y() >> pose.position.z() >> pose.orientation.w() >> pose.orientation.x() >> pose.orientation.y() >> pose.orientation.z();
        if (file.good()) {
            trajectory.push_back(pose);
        }
    }

    file.close();
    return true;
}

// 将对齐后的轨迹数据写入文件
bool writeTrajectory(const std::string& filename, const std::vector<Pose>& trajectory)
{
    std::ofstream file(filename);
    if (!file.is_open())
    {
        std::cerr << "Error: Cannot open file " << filename << std::endl;
        return false;
    }

    for (const auto& pose : trajectory)
    {
        file << pose.timestamp << " " << pose.position.x() << " " << pose.position.y() << " " << pose.position.z() << " " << pose.orientation.w() << " " << pose.orientation.x() << " " << pose.orientation.y() << " " << pose.orientation.z() << std::endl;
    }

    file.close();
    return true;
}

int main(int argc, char** argv)
{
    if (argc != 4)
    {
        std::cerr << "Usage: align_trajectory <trajectory1.txt> <trajectory2.txt> <output.txt>" << std::endl;
        return -1;
    }

    // 读取两个轨迹文件
    std::vector<Pose> trajectory1, trajectory2;
    if (!readTrajectory(argv[1], trajectory1) || !readTrajectory(argv[2], trajectory2))
    {
        return -1;
    }

    // 计算两个轨迹的平移和旋转矩阵
    Matrix3d R;
    Vector3d t;
    int N = std::min(trajectory1.size(), trajectory2.size());
    MatrixXd P1(3, N), P2(3, N);
    for (int i = 0; i < N; i++)
    {
        P1.col(i) = trajectory1[i].position;
        P2.col(i) = trajectory2[i].position;
    }
    Vector3d centroid1 = P1.rowwise().mean();
    Vector3d centroid2 = P2.rowwise().mean();
    P1.colwise() -= centroid1;
    P2.colwise() -= centroid2;
    Matrix3d H = P1 * P2.transpose();
    JacobiSVD<MatrixXd> svd(H, ComputeFullU | ComputeFullV);
    Matrix3d U = svd.matrixU();
    Matrix3d V = svd.matrixV();
    R = V * U.transpose();
    if (R.determinant() < 0) {
        R.col(2) *= -1;
    }
    t = centroid2 - R * centroid1;

    // 对齐第二个轨迹并输出
    std::vector<Pose> aligned_trajectory;
    for (const auto& pose : trajectory2)
    {
        Pose aligned_pose;
        aligned_pose.timestamp = pose.timestamp;
        aligned_pose.position = R * pose.position + t;
        aligned_pose.orientation = R * pose.orientation;
        aligned_trajectory.push_back(aligned_pose);
    }
    if (!writeTrajectory(argv[3], aligned_trajectory))
    {
        return -1;
    }

    std::cout << "Done!" << std::endl;
    return 0;
}

使用方法：将以上代码保存为`align_trajectory.cpp`，并使用以下命令进行编译：

g++ align_trajectory.cpp -o align_trajectory -std=c++11 -I /path/to/eigen

其中，`/path/to/eigen`是Eigen库的路径，需要根据实际情况进行修改。编译成功后，可以使用以下命令运行程序：

./align_trajectory trajectory1.txt trajectory2.txt output.txt

其中，`trajectory1.txt`和`trajectory2.txt`分别是要对齐的两个轨迹文件，`output.txt`是输出的对齐后的轨迹文件。