前言
做课题时候,需要仔细分析KeyFrameTrajectory.txt,当时直接记录在了CSDN中,这两天有空,简单简单整理一下,分享一下。本文完全是个人理解,如有错误,欢迎指正,提前感谢!
一、KeyFrameTrajectory.txt记录内容分析
KeyFrameTrajectory.txt文件内记录了ORB-SLAM2系统运行开始到结束这段时间内相机的关键帧位姿,理解KeyFrameTrajectory.txt文件内容是进行后续应用的第一步。
txt文件中包括八列,代表含义如下:
#timestamp tx ty tz qx qy qz qw
#timestamp是该关键帧图像的时间戳,t记录了不同方向的位移,q是用四元数方式记录了旋转
#合在一起,每一行就是记录了一帧图像的相机所处的位姿
#第一关键帧是世界坐标系的原点,而txt文件中记录的信息是在此坐标系下的。
每一列都代表着不同的信息,其实和TUM数据集中记录的方式一样。
二、生成代码分析
1.调用与定义
以单目相机TUM运行模式为例,调用代码位于example/mono_tum.cc,行数可能不一定对,因为源代码已经被我改得乱七八糟了。
//mono_tum.cc line123/line53
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
//关键函数是SLAM.SaveKeyFrameTrajectoryTUM
关键函数的定义位置位于include/system.h
//system.h line109/line51
class System
{
public:
void SaveKeyFrameTrajectoryTUM(const string &filename);//就一个输入,文件名,const string形式的
}
2.源代码分析
关于函数的源代码位于src/system.cc
//system.cc line383
void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
//从地图中读取所有的关键帧
vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
//将各关键帧以lId为依据进行从小到大的排序
sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
// 作者注释:变换所有关键帧,以使得第一帧为原点。因此txt文件记录的轨迹信息世界坐标系的原点是第一关键帧
// 也就是初始化后的第一关键帧,之前的时间都在尝试初始化。
// 经过了一次loop closure后,第一个关键帧可能不在原点
//cv::Mat Two = vpKFs[0]->GetPoseInverse();
//在这里添加改写
/*打开txt文件,开始写入信息*/
ofstream f;
f.open(filename.c_str());
f << fixed;//给文件流f设置fixed属性,即禁止使用科学计数法表示浮点数
/*逐关键帧写入*/
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];//某一帧关键帧
// pKF->SetPose(pKF->GetPose()*Two);
if(pKF->isBad())
continue;//坏关键帧跳过
cv::Mat R = pKF->GetRotation().t();//计算旋转矩阵
vector<float> q = Converter::toQuaternion(R);//由旋转矩阵计算出四元数
cv::Mat t = pKF->GetCameraCenter();//计算位移
/*将地图中存储的关键帧信息写入txt文件*/
f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
<< " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;// setprecision设定了小数点后保留位数,时间戳保留六位
}
f.close();//文件打开后一定要关闭,养成好习惯
cout << endl << "trajectory saved!" << endl;
}
以上。
总结
有啥写错的,欢迎评论区指正。然后没啥总结的,散会!!!
版权声明:本文为MFL_xiaotaiye原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。