我们采集的数据集时间戳通常与给定的groundtruth的时间戳不对齐,下面以EuRoC数据集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;