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

2023-12-27 13:57:01

目录

一.前言

二.代码

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>)来使用。

文章来源:https://blog.csdn.net/anananajiushiwo/article/details/135238592
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。