ORB_SLAM3 SaveTrajectoryTUM 注释

void System::SaveTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
    if(mSensor==MONOCULAR)
    {
        cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
        return;
    }

    vector<KeyFrame *> vpKFs = mpAtlas->GetAllKeyFrames(); //获取地图集中所有的关键帧


    vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();//第一帧的位姿，也是世界坐标系
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//第一帧的位姿，也是世界坐标系

    // Transform all keyframes so that the first keyframe is at the origin.
    // After a loop closure the first keyframe might not be at the origin.
    cv::Mat Two = vpKFs[0]->GetPoseInverse();

    ofstream f;
    f.open(filename.c_str());
    f << fixed;

    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
    // We need to get first the keyframe pose and then concatenate the relative transformation.
    // Frames not localized (tracking failure) are not saved.

    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
    // which is true when tracking failed (lbL).
     //每一帧都有一个参考关键帧，相对于参考关键帧有一个相对位姿，所以每一帧的位姿是按相对参考关键帧位姿保存的
     //这样保存的目的在于：关键帧位姿在localmapping中是不断调整的，所以认为更加准确，如果直接保存
     //Frame的位姿，那就没那么准确
    list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();//参考关键帧
    list<double>::iterator lT = mpTracker->mlFrameTimes.begin(); //时间戳
    list<bool>::iterator lbL = mpTracker->mlbLost.begin();  //是否跟踪失败


    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),  //相对参考关键帧的位姿
        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
    {
        if(*lbL)
            continue;

        KeyFrame* pKF = *lRit;

        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);

        // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
        while(pKF->isBad())//如果参考关键帧是坏的(可能是检查冗余时被剔除了)，则用其父帧，并记录父帧到原参考关键帧的位姿变换
        {
            Trw = Trw*pKF->mTcp;
            pKF = pKF->GetParent();
        }

        Trw = Trw * pKF->GetPose() * Two;                    //参考关键帧的位姿
        cv::Mat Tcw = (*lit) * Trw;                          //参考关键帧到当前帧的相对位姿*参考关键帧位姿=当前帧位姿
        cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t(); //相机到世界的旋转
        cv::Mat twc = -Rwc * Tcw.rowRange(0, 3).col(3);      //相机到世界的平移
        vector<float> q = Converter::toQuaternion(Rwc);      //以四元数的形式输出

        f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
    }
    f.close();
    // cout << endl << "trajectory saved!" << endl;
}