手把手带你死磕ORBSLAM3源代码(十二）System.cc Sophus::SE3f System::TrackRGBD类代码分析

目录

一.前言

二.代码

2.1完整代码

2.2 usleep介绍

一.前言

这部分代码主要做了以下几件事情：

1. 检查输入传感器是否为RGBD或IMU_RGBD，如果不是则报错并退出。
2. 根据设置，对输入的图像和深度图进行缩放，或者直接克隆。
3. 检查并处理模式更改，包括激活或停用本地化模式。
4. 检查并处理重置，包括重置追踪器或重置活动地图。
5. 如果传感器是IMU_RGBD，那么抓取IMU数据。
6. 抓取RGB-D图像，获取相机姿态。
7. 更新追踪状态和追踪的地图点。

二.代码

2.1完整代码

Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
{
    if(mSensor!=RGBD  && mSensor!=IMU_RGBD)
    {
        cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
        exit(-1);
    }

    cv::Mat imToFeed = im.clone();
    cv::Mat imDepthToFeed = depthmap.clone();
    if(settings_ && settings_->needToResize()){
        cv::Mat resizedIm;
        cv::resize(im,resizedIm,settings_->newImSize());
        imToFeed = resizedIm;

        cv::resize(depthmap,imDepthToFeed,settings_->newImSize());
    }

    // Check mode change
    {
        unique_lock<mutex> lock(mMutexMode);
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }

            mpTracker->InformOnlyTracking(true);
            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)
        {
            mpTracker->ResetActiveMap();
            mbResetActiveMap = false;
        }
    }

    if (mSensor == System::IMU_RGBD)
        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
            mpTracker->GrabImuData(vImuMeas[i_imu]);

    Sophus::SE3f Tcw = mpTracker->GrabImageRGBD(imToFeed,imDepthToFeed,timestamp,filename);

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

2.2 usleep介绍

??? usleep 不是一个特定于 C++ 的函数，而是一个在 Unix-like 系统（如 Linux）中常见的系统调用，用于使调用线程暂停指定的微秒数。它的原型通常在 <unistd.h> 头文件中定义。

??? 函数原型如下：

int usleep(useconds_t usec);

??? 其中，useconds_t 是一个整数类型，表示微秒数。

??? 使用 usleep 可以让程序暂停一段时间。例如，以下的 C++ 代码会使程序暂停 500,000 微秒（即 0.5 秒）：

#include <unistd.h>  
  
int main() {  
    usleep(500000);  // Pause for 500,000 microseconds (0.5 seconds)  
    return 0;  
}

几点要注意：

1. 移植性：虽然 usleep 在很多 Unix-like 系统上都可用，但它并不是 POSIX 标准的一部分，因此在某些系统上可能不可用。对于需要更高移植性的应用，可以考虑使用 nanosleep。
2. 精度：usleep 的精度可能受到系统计时器分辨率的限制，因此实际暂停时间可能与请求的时间略有不同。
3. 已弃用：在某些系统上，usleep 可能已被标记为已弃用，推荐使用 nanosleep 代替。nanosleep 提供更高的精度，并且可以处理信号中断的情况。
4. C++ 标准库：C++ 标准库并没有直接提供 usleep。如果要在 C++ 中使用它，通常需要通过包含相应的系统头文件（如 <unistd.h>）来使用。