ORB-SLAM3源码阅读笔记（11）-数据流

以双目惯导程序为例，其他程序也大致相同。
此节总结主程序到tracking的数据流。

1.原始数据订阅

  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);&ImageGrabber::GrabImageRight,&igb);

通过调用GrabImu、GrabImageLeft、GrabImageRight将imu、left、right图像分别存储到muGrabber. imuBuf队列、imgLeftBuf（只存储最新帧图像）、imgRightBuf（只存储最新帧图像）中。

2.数据预处理

2.1 void ImageGrabber::SyncWithImu()
主要作用：同步imu和图像
实现步骤：
（1）左右目图像时间戳差大于最大的时间偏移，则删除imgLeftBuf、imgLeftBuf队列中时间较早的一帧图像
（2）如果左目图像时间比最新的IMU数据大，说明IMU数据还没准备好，需要等一下IMU数据

2.2其他作用：
将同步后图像从ros格式转换为opencv格式，存储到imLeft 、imRight 。

imLeft = GetImage(imgLeftBuf.front());
imRight = GetImage(imgRightBuf.front())

将同步后imu数据存储到vimumeas容器

         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));

图像直方图均衡化：均衡化后的图像存储位置不变

      if(mbClahe)
      {
        mClahe->apply(imLeft,imLeft);
        mClahe->apply(imRight,imRight);
      }

图像畸变矫正，存储容器也不变仍在imLeft 、imRight

//此时的映射系数M1l,M2l，M1r,M2r是通过内参、畸变矩阵、立体矫正后的旋转矩阵、校正后内参计算得来的
    if(do_rectify)
      {
        cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR);
      }

3.追踪线程

//输入经过畸变矫正后的图像和imu数据
mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas);

3.1进入追踪线程以后

再次进行了畸变矫正：
这次畸变矫正和之前矫正的区别在于参数文件是否提供立体矫正后的旋转矩阵、校正后内参。

输入：imLeft、imRight
返回：imLeftToFeed、imRightToFeed

    cv::Mat imLeftToFeed, imRightToFeed;
    if(settings_ && settings_->needToRectify()){
        cv::Mat M1l = settings_->M1l();
        cv::Mat M2l = settings_->M2l();
        cv::Mat M1r = settings_->M1r();
        cv::Mat M2r = settings_->M2r();

        cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR);
        cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR);
    }

此时的映射系数M1l,M2l，M1r,M2r也是通过内参、畸变矩阵、立体矫正后的旋转矩阵、校正后内参计算得来的。但是需要注意的是立体矫正后的旋转矩阵、校正后内参则是通过setting.cc中的cv::stereoRectify得来的。
此时感觉存在一个bug

   if(settings_ && settings_->needToRectify()){
        cv::Mat M1l = settings_->M1l();
        cv::Mat M2l = settings_->M2l();
        cv::Mat M1r = settings_->M1r();
        cv::Mat M2r = settings_->M2r();

        cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR);
        cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR);
    }
    //图像尺寸是否需要重置,返回重定义尺寸的灰度图
    else if(settings_ && settings_->needToResize()){
        cv::resize(imLeft,imLeftToFeed,settings_->newImSize());
        cv::resize(imRight,imRightToFeed,settings_->newImSize());
    }

图像矫正和图像尺寸重置，只能2选1，如果两种操作都认为是需要的，进行两种处理，但这时重置尺寸的图像会将覆盖畸变修正的图像。因此修正程序如下：

//使用标定好的参数进行矫正
    cv::Mat imLeftToFeed, imRightToFeed;
    if(settings_ && settings_->needToRectify()){
        cv::Mat M1l = settings_->M1l();
        cv::Mat M2l = settings_->M2l();
        cv::Mat M1r = settings_->M1r();
        cv::Mat M2r = settings_->M2r();

        cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR);
        cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR);

        if(settings_ && settings_->needToResize()){
        cv::resize(imLeftToFeed,imLeftToFeed,settings_->newImSize());
        cv::resize(imRightToFeed,imRightToFeed,settings_->newImSize());   
      }  
    }
    //图像尺寸是否需要重置,返回重定义尺寸的灰度图
    else if(settings_ && settings_->needToResize()){
        cv::resize(imLeft,imLeftToFeed,settings_->newImSize());
        cv::resize(imRight,imRightToFeed,settings_->newImSize());
    }
    else{
        imLeftToFeed = imLeft.clone();
        imRightToFeed = imRight.clone();
    }

计算相机系和世界系的变换矩阵
输入：左目灰度图,右目灰度图,时间戳,调试用文件名
返回：相机系和世界系的变换矩阵

  Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed,imRightToFeed,timestamp,filename);

追踪线程结束后得到的信息：

    // 更新跟踪状态和参数
    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;//记录跟踪状态
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//当前帧的地图点
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//当前帧的去畸变后关键点

    return Tcw;//返回世界坐标系到相机的位姿