2. ORB-SLAM3_V1 源码阅读笔记 -Examples -ros_stereo_inertial.cc

主要参考：https://zhuanlan.zhihu.com/p/349401824，在此基础上对1.0版本的orbslam3代码解读。
一、ORB-SLAM3的整体流程如下图所示：

相比于ORB-SLAM2，主要的改进在于：

1. 加入了单目+IMU模式和双目+IMU模式。
2. 增加了多地图管理，在跟丢时会新建一个地图，从而提高跟踪的鲁棒性。在回环时也用到了跟丢之前的地图。
3. 改进了回环检测算法，提高了召回率。检测时会用到其他地图的信息，进行地图融合。
4. 增加了鱼眼相机模型，在重定位时使用了与相机模型无关的MLPnP算法计算位姿。

二、程序入口
程序的main函数在Examples文件夹中，分为ROS接口和非ROS接口的。下面以ros_stereo_inertial.cc为例。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 

#include 

#include "../../../include/System.h"
#include "../include/ImuTypes.h"

using namespace std;

/**
 * @brief imu 数据类
 * 
 */
class ImuGrabber
{
public:
  ImuGrabber(){};
  void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);

  queue<sensor_msgs::ImuConstPtr> imuBuf;
  std::mutex mBufMutex;
};

/**
 * @brief 图像数据类
 * 
 */
class ImageGrabber
{
public:
  ImageGrabber(ORB_SLAM3::System *pSLAM, ImuGrabber *pImuGb, const bool bRect, const bool bClahe) : mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe) {}

  void GrabImageLeft(const sensor_msgs::ImageConstPtr &msg);
  void GrabImageRight(const sensor_msgs::ImageConstPtr &msg);
  cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
  void SyncWithImu();

  queue<sensor_msgs::ImageConstPtr> imgLeftBuf, imgRightBuf;
  std::mutex mBufMutexLeft, mBufMutexRight;

  ORB_SLAM3::System *mpSLAM;//系统类
  ImuGrabber *mpImuGb;//imu类

  const bool do_rectify;//是否进行图像畸变 矫正
  cv::Mat M1l, M2l, M1r, M2r;

  const bool mbClahe;//是否进行直方图均衡化
  cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
};

/**
 * @brief  ros接口，双目惯导主函数    rosrun ORB_SLAM3 Stereo_Inertial        Vocabulary/ORBvoc.bin   Examples_old/Stereo-Inertial/my_MYNT.yaml     true   true
 * 
 * @param argc 输入的参数个数
 * @param argv  输入的具体参数，argv[0]:Stereo_Inertial    argv[1]:ORBvoc.txt      argv[2]: my_MYNT.yaml   argv[3]:true
 *
 */
int main(int argc, char **argv)
{
  // Step 1 初始化节点名和话题名
  ros::init(argc, argv, "Stereo_Inertial");
  ros::NodeHandle n("~");
  ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
  bool bEqual = false;
  if (argc < 4 || argc > 5)
  {
    cerr << endl
         << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl;
    ros::shutdown();
    return 1;
  }

  // Step 2 创建SLAM系统类、imu数据类、图像数据类的对象
  // Create SLAM system. It initializes all system threads and gets ready to process frames.
  // 2.1 创建SLAM系统的对象，初始化SLAM系统的所有线程，做好处理图像的准备
  ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::IMU_STEREO, true);
  
  //2.2 创建 imu数据类的对象
  ImuGrabber imugb;

  //2.3创建图像数据类的对象
  //是否进行图像畸变校
  std::string sbRect(argv[3]);
  //是否进行直方图均衡化
  if (argc == 5)
  {
    std::string sbEqual(argv[4]);
    if (sbEqual == "true")
      bEqual = true;
  }
  ImageGrabber igb(&SLAM, &imugb, sbRect == "true", bEqual);//图像数据类的对象

  //读取图像畸变校正参数
  if (igb.do_rectify)
  {
    // Load settings related to stereo calibration
    cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
    if (!fsSettings.isOpened())
    {
      cerr << "ERROR: Wrong path to settings" << endl;
      return -1;
    }

    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    fsSettings["LEFT.K"] >> K_l;
    fsSettings["RIGHT.K"] >> K_r;

    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

    if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
        rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0)
    {
      cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
      return -1;
    }

    cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), cv::Size(cols_l, rows_l), CV_32F, igb.M1l, igb.M2l);
    cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), cv::Size(cols_r, rows_r), CV_32F, igb.M1r, igb.M2r);
  }
  
  // Step 3 接受imu数据、图像数据，通过回调函数进行数据预处理。
  // Maximum delay, 5 seconds
  //最大延迟5s
  ros::Subscriber sub_imu = n.subscribe("/mynteye/imu/data_raw", 1000, &ImuGrabber::GrabImu, &imugb);
  ros::Subscriber sub_img_left = n.subscribe("/mynteye/left/image_raw", 100, &ImageGrabber::GrabImageLeft, &igb);
  ros::Subscriber sub_img_right = n.subscribe("/mynteye/right/image_raw", 100, &ImageGrabber::GrabImageRight, &igb);

 // Step 4 开启同步线程，进行imu数据和图像匹配，追踪
  std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb);

  ros::spin();

  return 0;
}

/**
 * @brief 左相机数据的回调函数，预处理图像数据
 * 
 * @param[in] img_msg ，原始左目图像数据
 * @attention 队列中只有最新一帧图像数据
 */
void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg)
{
  mBufMutexLeft.lock();
  if (!imgLeftBuf.empty())
    imgLeftBuf.pop();
  imgLeftBuf.push(img_msg);
  mBufMutexLeft.unlock();
}

/**
 * @brief 右相机数据的回调函数，预处理图像数据
 * 
 * @param[in] img_msg ,右目原始图像
 * 
 * @attention 队列中始终只有最新一帧图像
 */
void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg)
{
  mBufMutexRight.lock();
  if (!imgRightBuf.empty())
    imgRightBuf.pop();
  imgRightBuf.push(img_msg);
  mBufMutexRight.unlock();
}

/**
 * @brief ROS格式转成opencv格式
 * 
 * @param[in] img_msg 
 * @return cv::Mat 
 */
cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg)
{
  // Copy the ros image message to cv::Mat.
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);
  }
  catch (cv_bridge::Exception &e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }

  if (cv_ptr->image.type() == 0)
  {
    return cv_ptr->image.clone();
  }
  else
  {
    std::cout << "Error type" << std::endl;
    return cv_ptr->image.clone();
  }
}

/**
 * @brief 图像、imu数据配准，追踪
 * 
 */
void ImageGrabber::SyncWithImu()
{
  const double maxTimeDiff = 0.01;//双目图像的最大时间差
  while (1)
  {
    cv::Mat imLeft, imRight;
    double tImLeft = 0, tImRight = 0;
    //双目图像和imu数据都不为空
    if (!imgLeftBuf.empty() && !imgRightBuf.empty() && !mpImuGb->imuBuf.empty())
    {
      //左目图像时间
      tImLeft = imgLeftBuf.front()->header.stamp.toSec();
      //右目图像时间
      tImRight = imgRightBuf.front()->header.stamp.toSec();
     
      // Step 4.1双目图像配准
      //如果左目图像比右目图像新0.01秒以上，则认为不同步，如果imgRightBuf中不只一帧图像，则
      //丢弃最早的那帧，直到满足时间差条件或者imgRightBuf中只剩一帧
      //(感觉两个图像队列中一直都是只有一帧图像，因为获取图像的回调函数中就只存了最新的那帧)
      this->mBufMutexRight.lock();
      while ((tImLeft - tImRight) > maxTimeDiff && imgRightBuf.size() > 1)
      {
        imgRightBuf.pop();
        tImRight = imgRightBuf.front()->header.stamp.toSec();
      }
      this->mBufMutexRight.unlock();

      this->mBufMutexLeft.lock();
      while ((tImRight - tImLeft) > maxTimeDiff && imgLeftBuf.size() > 1)
      {
        imgLeftBuf.pop();
        tImLeft = imgLeftBuf.front()->header.stamp.toSec();
      }
      this->mBufMutexLeft.unlock();

      //此时两个图像队列中肯定各自只有一帧，如果时间差太大，直接continue
      if ((tImLeft - tImRight) > maxTimeDiff || (tImRight - tImLeft) > maxTimeDiff)
      {
        // std::cout << "big time difference" << std::endl;
        continue;
      }
      
      
      //如果左目图像时间比最新的IMU数据大，说明IMU数据还没准备好，需要等一下IMU数据
      if (tImLeft > mpImuGb->imuBuf.back()->header.stamp.toSec())
        continue;

      this->mBufMutexLeft.lock();
      imLeft = GetImage(imgLeftBuf.front());//获取图像，ROS格式转成opencv格式
      imgLeftBuf.pop();
      this->mBufMutexLeft.unlock();

      this->mBufMutexRight.lock();
      imRight = GetImage(imgRightBuf.front());//获取图像，ROS格式转成opencv格式
      imgRightBuf.pop();
      this->mBufMutexRight.unlock();
      
      // Step 4.2 图像、imu时间对齐
      vector<ORB_SLAM3::IMU::Point> vImuMeas;
      mpImuGb->mBufMutex.lock();
      if (!mpImuGb->imuBuf.empty())
      {
        // Load imu measurements from buffer
        vImuMeas.clear();
        //获取当前左目图像时间戳之前的imu数据
        while (!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec() <= tImLeft)
        {
          double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
          cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
          cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
          vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t));
          mpImuGb->imuBuf.pop();
        }
      }
      mpImuGb->mBufMutex.unlock();
      // Step 4.3直方图均衡化
      if (mbClahe)
      {
        mClahe->apply(imLeft, imLeft);
        mClahe->apply(imRight, imRight);
      }
      // Step 4.4图像矫正
      if (do_rectify)
      {
        cv::remap(imLeft, imLeft, M1l, M2l, cv::INTER_LINEAR);
        cv::remap(imRight, imRight, M1r, M2r, cv::INTER_LINEAR);
      }
      // Step 4.5 追踪
      mpSLAM->TrackStereo(imLeft, imRight, tImLeft, vImuMeas);

      std::chrono::milliseconds tSleep(1);//tsleep：1微妙
      std::this_thread::sleep_for(tSleep);//线程休眠一微秒
    }
  }
}

/**
 * @brief imu回调函数，预处理数据
 * 
 * @param imu_msg imu原始数据
 * @attention 图像存入imuBuf队列
*/
void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
{
  mBufMutex.lock();
  imuBuf.push(imu_msg);
  mBufMutex.unlock();
  return;
}