利用ICP把自己跑出的数据与开源数据集对齐并显示（一.时间戳对齐）

　我们采集的数据集时间戳通常与给定的groundtruth的时间戳不对齐，下面以ＥuRoC数据集data.csv和我们采集的CameraTrajectory.txt为例介绍两者之间的对齐。

一.data.csv转ground.txt

    //改写成txt文件
    ifstream ifile("data.csv");
    string line,s;
    vector<vector<double>> timestamp;
    while(getline(ifile,line))
    {
        stringstream record(line);
        record>>s;
        if(s[0]=='#')//跳过注释
            continue;
        vector<double> time;
        double ti;
        string str;
        for(auto &s:line)//把每一行的逗号都替换成空格
            if(s==',')
                s=' ';
        stringstream rd(line);
        for(auto i=0;i<8;i++)
        {
            rd>>ti;
            if(i==0)
            {
                setprecision(8);//使得时间戳与自己数据时间戳格式一致
                ti=ti/(10e8);
            }
            time.push_back(ti);
        }
        timestamp.push_back(time);
        record.clear();
    }
    ifile.close();
    ofstream ofile("../ground.txt");
    for(auto time:timestamp) {
        for (auto ti:time)
            ofile <<fixed<< ti << " ";
        ofile<<endl;
    }
    ofile.close();

二.对齐ground.txt与CameraTrajectory.txt

    ifstream fileGround("../ground.txt");
    ifstream fileEstimated("../CameraTrajectory.txt");
    string lineNew;
    vector<vector<double>> vTraGround,vTraEstimated,vTraGroundAligned,vTraEstimatedAligned;
    while(getline(fileGround,lineNew))//读取ground并保存成矩阵
    {
        stringstream record(lineNew);
        vector<double> vTmpGround;
        double tmp;
        for(auto i=0;i<8;i++)
        {
            record>>tmp;
            vTmpGround.push_back(tmp) ;
        }
        vTraGround.push_back(vTmpGround);
        record.clear();
    }
    while(getline(fileEstimated,lineNew))//读取CaneraTrajectory并保存成矩阵
    {
        stringstream record(lineNew);
        vector<double> vTmpEstimated;
        double tmp;
        for(auto i=0;i<8;i++)
        {
            record>>tmp;
            vTmpEstimated.push_back(tmp);
        }
        vTraEstimated.push_back(vTmpEstimated);
        record.clear();
    }
    auto it = vTraGround.begin();
    for(auto tmpEst:vTraEstimated)//对齐时间戳
    {
        double timeStampEst = tmpEst[0];
        for (; it != vTraGround.end(); it++)
        {
            double timeStampGrd = (*it)[0];
            if(abs(timeStampGrd-timeStampEst)<=0.0004)//允许一定的容差
            {
                vTraGroundAligned.push_back(*it);
                vTraEstimatedAligned.push_back(tmpEst);
            }
            else if(timeStampGrd < timeStampEst)
                continue;
            else if(timeStampGrd > timeStampEst)
                break;
        }
    }
    ofstream ofileGrd("../GroundAligned.txt");//写入对齐后的ground数据
    for(auto vTra:vTraGroundAligned) {
        for (auto ele:vTra)
            ofileGrd << fixed << ele << " ";
        ofileGrd << endl;
    }
    ofstream ofileEst("../EstimatedAligned.txt");//写入对齐后的CameraTrajectory数据
    for(auto vTra:vTraEstimatedAligned) {
        for (auto ele:vTra)
            ofileEst << fixed << ele << " ";
        ofileEst << endl;
    }
    return 0;