【ORB_SLAM2关键帧轨迹文件KeyFrameTrajectory.txt及相关代码解读】

---

前言

做课题时候，需要仔细分析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;
}

以上。

---

总结

有啥写错的，欢迎评论区指正。然后没啥总结的，散会！！！