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

以下是一个简单的C++实现，可以将两个ORBSLAM运动轨迹txt文件对齐输出为一个新的轨迹txt文件。

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

using namespace std;

// 读取轨迹txt文件
vector<vector<double>> readTrajectory(string fileName) {
    vector<vector<double>> trajectory;
    ifstream file(fileName);
    if (!file) {
        cerr << "无法打开文件: " << fileName << endl;
        exit(1);
    }

    string line;
    while (getline(file, line)) {
        istringstream iss(line);
        vector<double> row;
        double val;
        while (iss >> val) {
            row.push_back(val);
        }
        trajectory.push_back(row);
    }
    file.close();
    return trajectory;
}

// 对齐两个轨迹
void alignTrajectories(vector<vector<double>>& trajectory1, vector<vector<double>>& trajectory2) {
    // 取轨迹中的第一个位置作为基准位置
    double x0 = trajectory1[0][1];
    double y0 = trajectory1[0][2];
    double z0 = trajectory1[0][3];

    // 将轨迹1平移到基准位置
    for (auto& row : trajectory1) {
        row[1] -= x0;
        row[2] -= y0;
        row[3] -= z0;
    }

    // 找到轨迹1和轨迹2之间的平移和旋转
    double s = 0, sx = 0, sy = 0, sz = 0;
    for (size_t i = 0; i < trajectory1.size(); i++) {
        sx += trajectory1[i][1] * trajectory2[i][1] + trajectory1[i][2] * trajectory2[i][4] + trajectory1[i][3] * trajectory2[i][7];
        sy += trajectory1[i][1] * trajectory2[i][2] + trajectory1[i][2] * trajectory2[i][5] + trajectory1[i][3] * trajectory2[i][8];
        sz += trajectory1[i][1] * trajectory2[i][3] + trajectory1[i][2] * trajectory2[i][6] + trajectory1[i][3] * trajectory2[i][9];
        s += trajectory1[i][1] * trajectory1[i][1] + trajectory1[i][2] * trajectory1[i][2] + trajectory1[i][3] * trajectory1[i][3];
    }

    double a11 = sx / s;
    double a12 = sy / s;
    double a13 = sz / s;

    double a21 = -sy / s;
    double a22 = a11;
    double a23 = 0;

    double a31 = -sz / s;
    double a32 = 0;
    double a33 = a11;

    // 将轨迹2旋转和平移
    for (auto& row : trajectory2) {
        double x = row[1];
        double y = row[2];
        double z = row[3];
        row[1] = a11 * x + a12 * y + a13 * z;
        row[2] = a21 * x + a22 * y + a23 * z;
        row[3] = a31 * x + a32 * y + a33 * z;
        row[1] += x0;
        row[2] += y0;
        row[3] += z0;
    }
}

// 输出轨迹到文件
void writeTrajectory(vector<vector<double>>& trajectory, string fileName) {
    ofstream file(fileName);
    if (!file) {
        cerr << "无法打开文件: " << fileName << endl;
        exit(1);
    }

    for (auto& row : trajectory) {
        for (auto val : row) {
            file << val << " ";
        }
        file << endl;
    }
    file.close();
}

int main() {
    // 读取轨迹1和轨迹2
    string fileName1 = "trajectory1.txt";
    string fileName2 = "trajectory2.txt";
    vector<vector<double>> trajectory1 = readTrajectory(fileName1);
    vector<vector<double>> trajectory2 = readTrajectory(fileName2);

    // 对齐两个轨迹
    alignTrajectories(trajectory1, trajectory2);

    // 输出对齐后的轨迹
    string outputFileName = "aligned_trajectory.txt";
    writeTrajectory(trajectory2, outputFileName);

    cout << "轨迹已对齐并输出到文件: " << outputFileName << endl;

    return 0;
}

请注意，此实现假定两个轨迹都以相同的时间间隔采样，如果采样间隔不同，则需要进行额外的处理。