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

目录

一.前言

二.代码

2.1完整代码

2.2cv::remap介绍

一.前言

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

1. 检查输入传感器是否为立体或立体惯性，如果不是则报错并退出。
2. 根据设置，对输入的左右图像进行校正或调整大小，或者直接克隆。
3. 检查并处理模式更改，包括激活或停用本地化模式。
4. 检查并处理重置，包括重置追踪器或重置活动地图。
5. 如果传感器是立体惯性的，那么抓取IMU数据。
6. 抓取立体图像，获取相机姿态。
7. 更新追踪状态和追踪的地图点。

二.代码

2.1完整代码

Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
{
    if(mSensor!=STEREO && mSensor!=IMU_STEREO)
    {
        cerr << "ERROR: you called TrackStereo but input sensor was not set to Stereo nor Stereo-Inertial." << endl;
        exit(-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);
    }
    else if(settings_ && settings_->needToResize()){
        cv::resize(imLeft,imLeftToFeed,settings_->newImSize());
        cv::resize(imRight,imRightToFeed,settings_->newImSize());
    }
    else{
        imLeftToFeed = imLeft.clone();
        imRightToFeed = imRight.clone();
    }

    // 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_STEREO)
        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
            mpTracker->GrabImuData(vImuMeas[i_imu]);

    // std::cout << "start GrabImageStereo" << std::endl;
    Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed,imRightToFeed,timestamp,filename);

    // std::cout << "out grabber" << std::endl;

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

    return Tcw;
}

2.2cv::remap介绍

??? cv::remap是OpenCV库中的一个函数，用于实现图像的重映射。重映射是指从一个图像中的位置获取像素，并将其重新映射到目标图像的指定位置。这个过程可以实现图像的变形、扭曲、反转等操作，通常用于图像数据的增强，以提升深度模型的泛化能力。

??? cv::remap函数的原型如下：

void cv::remap(InputArray src, OutputArray dst, InputArray map1, InputArray map2, int interpolation, int borderMode = BORDER_CONSTANT, const Scalar& borderValue = Scalar());

参数的含义如下：

• src：原始图像。
• dst：目标图像，其大小和map1的大小相同，数据类型和src的数据类型一致。
• map1：表示（x,y）坐标点或者是x坐标，类型为CV_16SC2、CV_32FC1或者CV_32FC2。
• map2：表示y坐标，类型是CV_16UC1、CV_32FC1，当map1是（x,y）坐标时，map2可以为空。
• interpolation：插值算法，用于处理映射后的坐标可能为非整数的情况。
• borderMode：边界模式，默认为BORDER_CONSTANT。
• borderValue：边界值，默认为Scalar()，当边界模式为BORDER_CONSTANT时使用。

??? 重映射的一个关键特点是，映射函数可以是动态变化的，而且往往是由复杂算法实时计算得到的。这使得重映射特别适用于视频图像的重建等场景，其中像素的坐标变换需要根据实时数据进行计算。