【ORB_SLAM2源码解读】分析ORB_SLAM2 RGBD 第0帧是怎么计算位置姿态的

ORB_SLAM2 RGBD 第零帧分析

1. 判断传感器类型，不满足结束结束整个程序

SLAM.TrackRGBD(imRGB,imD,tframe);

if(mSensor!=RGBD)
{
    cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
    exit(-1);
}   

break、continue、return、exit() 返回值的区别和作用（状态机、图灵机、结构体、枚举）

2. 模式切换，这部分代码在默认情况根本不会被执行，只有当界面按钮选中 取消 后才会执行，所以我们暂时不分析这段代码

// Check mode change 这一部分主要是对局部地图线程进行操作
// mbActivateLocalizationMode为true会关闭局部地图线程,在显示窗口通过按键赋值 pangolin::Var menuLocalizationMode("menu.Localization Mode",false,true);
{
    // 独占锁
    // Change mode flags
    // std::mutex mMutexMode; bool mbActivateLocalizationMode; bool mbDeactivateLocalizationMode;
    unique_lock<mutex> lock(mMutexMode);
    if(mbActivateLocalizationMode)// 纯定位模式 关闭local mapping线程 只执行tracking线程
    {
        std::cout << "LocalMapping::Stop() " << std::endl;
        // System::System() 系统构造函数会创建 mpLocalMapper
        // LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
        // mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
        //         mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
        // {
        // }

        // void LocalMapping::RequestStop()
        // {
        //     unique_lock lock(mMutexStop);
        //     mbStopRequested = true;
        //     unique_lock lock2(mMutexNewKFs);
        //     mbAbortBA = true;
        // }

        // bool LocalMapping::isStopped()
        // {
        //     unique_lock lock(mMutexStop);
        //     return mbStopped;
        // }

        mpLocalMapper->RequestStop();//关闭局部地图的线程

        // Wait until Local Mapping has effectively stopped
        while(!mpLocalMapper->isStopped())// 局部地图线程是否已经关闭
        {
            usleep(1000);
        }

        mpTracker->InformOnlyTracking(true);//只定位
        mbActivateLocalizationMode = false;// 复位标记防止重复执行
    }
    if(mbDeactivateLocalizationMode)// 关闭纯定位模式 执行 local mapping线程 和tracking线程
    {
        std::cout << "LocalMapping::RequestStop() " << std::endl;
        mpTracker->InformOnlyTracking(false);// 也是状态切换的变量 局部地图线程开始工作
        mpLocalMapper->Release();// 关键帧从局部地图中删除
        mbDeactivateLocalizationMode = false;// 复位标记防止重复执行
    }
}

3. 跟踪线程重，这部分代码在默认情况根本不会被执行，只有当界面按钮选中 取消 后才会执行，所以我们暂时不分析这段代码

// Check reset
{
    unique_lock<mutex> lock(mMutexReset);// mbReset加锁
    if(mbReset)
    {
        std::cout << "if(mbReset) " << mbReset << std::endl;
        // if(mbReset) 1
        // System Reseting
        // Reseting Local Mapper... done
        // Reseting Loop Closing... done
        // Reseting Database... done

        mpTracker->Reset();// mpViwer可视化视图停止更新, mpLocalMapper局部地图停止, mpLoopClosing闭环检测停止, mpMap和mpKeyFrameDB清空
        mbReset = false;
    }
}

4. 计算当前帧的位置姿态

cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);

5. 创建普通帧

mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

6. 初始化

Track();
if(mState==NOT_INITIALIZED)
void Tracking::StereoInitialization()

7. 判断当前帧的位置姿态是否为空记录一些状态

if(!mCurrentFrame.mTcw.empty())
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);

8. 回到系统System::TrackRGBD记录一些值并返回位置姿态

unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;

9. 计算两帧之间系统需要休眠的时间

std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
Tcw = SLAM.TrackRGBD(imRGB,imD,tframe);
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

vTimesTrack[ni]=ttrack;

// Wait to load the next frame
double T=0;
if(ni<nImages-1)
    T = vTimestamps[ni+1]-tframe;
else if(ni>0)
    T = tframe-vTimestamps[ni-1];

// 两帧图像之间的时间差 和系统跟踪消耗的时间 共同决定系统需要休眠的时间
if(ttrack<T)
    usleep((T-ttrack)*1e6);

参考文章

从另一个角度看ORB-SLAM3——第0帧
从另一个角度看ORB-SLAM3——第1帧及单目初始化
ORB-SLAM3 单目惯导 Track续
IMU预积分原理及代码实现
需要强调的是，IMU原始观测是离散信号，只反应了某一时间点的运动特性。为了能使观测数据能够尽可能反应真实世界的连续运动过程，VINS-Mono使用了中值积分法，即通过对两个相邻时间点的IMU测量值求平均来等效这一时间段的运动特性。这里将该值称为中值观测。