软件工程与实践第三篇

ORB-SLAM3代码分析（二）

2021SC@SDUSC
提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

主要是System.cc里一些主要函数的分析

---

一、System.cc代码分析

1.System.cc中的主要函数接口

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
//追踪单目相机数据或单目+IMU模式
cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename);
//双目或双目+IMU模式
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, string filename)
//追踪深度相机数据
void System::SaveTrajectoryTUM(const string &filename)
//以TUM格式(tx,ty,tz,qx,qy,qz,qw)保存所有成功定位的帧的位姿
void SaveKeyFrameTrajectoryTUM(const string &filename);
//以TUM格式保存所有关键帧的位姿
void SaveTrajectoryEuRoC(const string &filename);
//以EuRoC格式保存所有帧的位姿
void SaveKeyFrameTrajectoryEuRoC(const string &filename);
//以EuRoC格式保存所有关键帧的位姿
void SaveTrajectoryKITTI(const string &filename);
//以KITTI格式保存所有帧的位姿

· 我们可以看到前两个函数都是有关追踪相机与IMU的数据，这涉及到ORB-SLAM3中将IMU与相机数据融合获取位姿数据。IMU 虽然可以测得角速度和加速度，但这些量都存在明显的漂移，使得积分 两次得到的位姿数据非常不可靠。但在短时间快速移动中，IMU可以弥补相机出现运动模糊，或者两帧之间重叠区域太少以至于无法进行特征匹配的问题。
2. 跟踪线程相关函数

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
{
     //检查传感器类型是否为单目或单目IMU
    if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl;
        exit(-1);
    }

    // Check mode change
    //这一部分主要是对局部地图线程进行操作
    //mbActivateLocalizationMode表示是否停止局部地图线程
    //mbDeactivateLocalizationMode表示是否清空局部地图
    {
        //独占锁，不会发生混乱，没有死锁或者在临界区
        unique_lock<mutex> lock(mMutexMode);
        //mbActivateLocalizationMode为true则为纯定位模式，局部建图线程会关闭
        if(mbActivateLocalizationMode)
        {
            //LocalMapping线程停止运行
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            //等待知道LocalMapping线程真正停止运行
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }
            //告诉Tracking线程，现在只进行跟踪，不建图
            mpTracker->InformOnlyTracking(true);
            //设置为false，避免重复进行以上操作
            mbActivateLocalizationMode = false;
        }
        //关闭纯定位模式，重新打开局部建图线程
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

    // Check reset检查是否进行重置
    {
        unique_lock<mutex> lock(mMutexReset);
        if(mbReset)
        {
            mpTracker->Reset();
            mbReset = false;
            mbResetActiveMap = false;
        }
        else if(mbResetActiveMap)
        {
            cout << "SYSTEM-> Reseting active map in monocular case" << endl;
            mpTracker->ResetActiveMap();
            mbResetActiveMap = false;
        }
    }
    // 如果是单目+imu模式，把IMU数据存储到mlQueueImuData中
    if (mSensor == System::IMU_MONOCULAR)
        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
            mpTracker->GrabImuData(vImuMeas[i_imu]);
    // 开始跟踪，返回相机位姿
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp,filename);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;//记录跟踪状态
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//当前帧的地图点
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//当前帧的去畸变后关键点
    //返回世界坐标系到相机的位姿
    return Tcw;
}

3. 保存轨迹相关函数

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();//获取地图集中所有的关键帧
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//按ID排序
    // 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;
    
    //每一帧都有一个参考关键帧，相对于参考关键帧有一个相对位姿，所以每一帧的位姿是按相对参考关键帧位姿保存的
    //这样保存的目的在于：关键帧位姿在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<< "trajectory saved!" << endl;
}

未完待续