DynaSLAM-9 DynaSLAM中双目运行流程（Ⅲ）：图像处理

目录

1.遍历每一张图片进入追踪线程估计相机位姿

2.System::TrackStereo

3.Tracking::GrabImageStereo 

4.Frame::Frame 

---

1.遍历每一张图片进入追踪线程估计相机位姿

    for(int ni=0; niGetSegmentation(imLeft, string(argv[4])+"/imLeft",
                    vstrImageLeft[ni].replace(vstrImageLeft[ni].begin(), vstrImageLeft[ni].end() - 10,""));
            maskRightRCNN = MaskNet->GetSegmentation(imRight, string(argv[4])+"/imRight",
                    vstrImageRight[ni].replace(vstrImageRight[ni].begin(), vstrImageRight[ni].end() - 10,""));
            cv::Mat maskLeftRCNNdil = maskLeftRCNN.clone();
            cv::dilate(maskLeftRCNN, maskLeftRCNNdil, kernel);
            maskLeft = maskLeft - maskLeftRCNNdil;
            cv::Mat maskRightRCNNdil = maskRightRCNN.clone();
            cv::dilate(maskRightRCNN, maskRightRCNNdil, kernel);
            maskRight = maskRight - maskRightRCNNdil;
        }

        下方代码中，执行MaskNet->GetSegmentation()函数，得到每一帧图片对应的分割结果。分割结果（变量maskRCNN）中，1表示先验的动态物体，0表示其他部分。注意，这里还对MaskRCNN分割的结果多进行了一次Dilate处理，也就是说要扣去的人的范围比神经网络的结果再大一点。
        关于Dilate它的本质是取邻域的最大值（是和像素值有关的）。

        关于replace方法的解释：这里用的是其中一种重载算法，效果是用str替换从指定迭代器位置的字符串：

string& replace (const_iterator i1, const_iterator i2, const char* s); 

#include
#include
using namespace std;
int main()
{
	string str = "he is@ a@ good boy";
	char * str1 = "12345";
	str=str.replace(str.begin(),str.begin()+6,str1);   //用str替换从指定迭代器位置的字符串
	cout<

        结果如下：

        if (argc == 5){
            cv::Mat maskLeftRCNN, maskRightRCNN;
            maskLeftRCNN = MaskNet->GetSegmentation(imLeft, string(argv[4])+"/imLeft",
                    vstrImageLeft[ni].replace(vstrImageLeft[ni].begin(), vstrImageLeft[ni].end() - 10,""));
            maskRightRCNN = MaskNet->GetSegmentation(imRight, string(argv[4])+"/imRight",
                    vstrImageRight[ni].replace(vstrImageRight[ni].begin(), vstrImageRight[ni].end() - 10,""));
            cv::Mat maskLeftRCNNdil = maskLeftRCNN.clone();
            cv::dilate(maskLeftRCNN, maskLeftRCNNdil, kernel);
            maskLeft = maskLeft - maskLeftRCNNdil;

            cv::Mat maskRightRCNNdil = maskRightRCNN.clone();
            cv::dilate(maskRightRCNN, maskRightRCNNdil, kernel);
            maskRight = maskRight - maskRightRCNNdil;
        }

        对双目的两个摄像头的图像利用Mask R-CNN网络进行语义分割分割出潜在的可能运动的物体，再对其进行Dilate操作也就是说要扣去的人的范围比神经网络的结果再大一点。在图像中1表示先验的动态物体，0表示其他部分。

        maskLeft是全一的矩阵，maskLeft - maskLeftRCNNdil得到的是像素值范围也是0和1，但1表示图片中要保留的部分（静态部分）。就是下图所示：

        接着，生成的mask就通过SLAM.TrackStereo函数进入了SLAM系统。

SLAM.TrackStereo(imLeft, imRight, maskLeft, maskRight, tframe);

        输入的参数有原来的图像、Mask过的图像、时间戳。

2.System::TrackStereo

cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat &maskLeft, const cv::Mat &maskRight,const double ×tamp)
{
    if(mSensor!=STEREO)
    {
        cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
        exit(-1);
    }   

    // Check mode change
    {
        unique_lock 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 lock(mMutexReset);
    if(mbReset)
    {
        mpTracker->Reset();
        mbReset = false;
    }
    }

    cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight, maskLeft, maskRight, timestamp);

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

        主要是设置一些状态为进入追踪线程做准备。

        如果是仅定位不建图模式mbActivateLocalizationMode = true，让局部建图线程中止：

void LocalMapping::RequestStop()
{
    unique_lock lock(mMutexStop);
    mbStopRequested = true;
    unique_lock lock2(mMutexNewKFs);
    mbAbortBA = true;
}

        置标志mbOnlyTracking = true，表示仅仅进行追踪线程（仅定位）不做局部建图和回环检测的工作。再将mbActivateLocalizationMode 置为false（下一帧可能工作状态不一样）。

        如果是mbDeactivateLocalizationMode = true，表示关闭定位模式，清空追踪线程向局部建图线程传送的关键帧容器mlNewKeyFrames。

void LocalMapping::Release()
{
    unique_lock lock(mMutexStop);
    unique_lock lock2(mMutexFinish);
    if(mbFinished)
        return;
    mbStopped = false;
    mbStopRequested = false;
    for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
        delete *lit;
    mlNewKeyFrames.clear();

    cout << "Local Mapping RELEASE" << endl;
}

        如果reset = true，表示SLAM系统被重置，将之前绘画板清空，结束绘图线程，清除有关SLAM系统的一切信息，准备重新开始。

void Tracking::Reset()
{

    cout << "System Reseting" << endl;
    if(mpViewer)
    {
        mpViewer->RequestStop();
        while(!mpViewer->isStopped())
            usleep(3000);
    }

    // Reset Local Mapping
    cout << "Reseting Local Mapper...";
    mpLocalMapper->RequestReset();
    cout << " done" << endl;

    // Reset Loop Closing
    cout << "Reseting Loop Closing...";
    mpLoopClosing->RequestReset();
    cout << " done" << endl;

    // Clear BoW Database
    cout << "Reseting Database...";
    mpKeyFrameDB->clear();
    cout << " done" << endl;

    // Clear Map (this erase MapPoints and KeyFrames)
    mpMap->clear();

    KeyFrame::nNextId = 0;
    Frame::nNextId = 0;
    mState = NO_IMAGES_YET;

    if(mpInitializer)
    {
        delete mpInitializer;
        mpInitializer = static_cast(NULL);
    }

    mlRelativeFramePoses.clear();
    mlpReferences.clear();
    mlFrameTimes.clear();
    mlbLost.clear();

    if(mpViewer)
        mpViewer->Release();
}

        如果是正常进入SLAM系统的（reset = false），我们通过GrabImageStereo函数获得当前帧相机相对于双目初始化器的第一帧的相对相机位姿坐标Tcw。

3.Tracking::GrabImageStereo 

cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const cv::Mat &maskLeft, const cv::Mat &maskRight,const double ×tamp)
{
    mImGray = imRectLeft;
    cv::Mat imGrayRight = imRectRight;
    cv::Mat imMaskLeft = maskLeft;
    cv::Mat imMaskRight = maskRight;

    if(mImGray.channels()==3)
    {
        if(mbRGB)
        {
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
        }
        else
        {
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
        }
    }
    else if(mImGray.channels()==4)
    {
        if(mbRGB)
        {
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
        }
        else
        {
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
        }
    }

    cv::Mat _mImGray = mImGray.clone();
    mImGray = mImGray*0;
    _mImGray.copyTo(mImGray,imMaskLeft);

    cv::Mat _imGrayRight = imGrayRight.clone();
    imGrayRight = imGrayRight*0;
    _imGrayRight.copyTo(imGrayRight,imMaskRight);

    mCurrentFrame = Frame(mImGray,imGrayRight,imMaskLeft,imMaskRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    Track();

    return mCurrentFrame.mTcw.clone();
}

        首先将左目右目转换为灰度图存放进mImGray、imGrayRight中。

        image.copyTo(imageROI，mask)。 作用是把mask和imageROI重叠以后把mask中像素值为0（black）的点对应的image中的点变为透明，而保留其他点。

        在这里把原灰度图与传入的mask像素值为0的部分（就是先验动态物体的部分）变为透明，保留其他点，结果存储到mImGray和imGrayRight中。

        完成操作后，构造当前帧。

mCurrentFrame = Frame(mImGray,imGrayRight,imMaskLeft,imMaskRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

        @mImGray：左目灰度图，这里的灰度图先验动态物体的部分的像素是透明的。

        @imGrayRight：右目灰度图，这里的灰度图先验动态物体的部分的像素是透明的。

        @imMaskLeft：左目Mask图，这里先验的物体为白色如下图

        @imMaskRight：右目Mask图，这里先验的物体为白色如下图

        @timestamp：时间戳

        @mpORBextractorLeft：左目ORB提取器

        @mpORBextractorRight：右目ORB提取器

        @mpORBVocabulary：ORB词典的句柄

        @mK：相机的内参矩阵

        @mDistCoef：相机的去畸变参数

        @mbf：相机的基线长度 * 相机的焦距

        @mThDepth：用于区分远点和近点的阈值。近点认为可信度比较高；远点则要求在两个关键帧中得到匹配。

4.Frame::Frame 

Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const cv::Mat &maskLeft, const cv::Mat &maskRight,const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    :mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
     mpReferenceKF(static_cast(NULL))
{
    // Frame ID
    mnId=nNextId++;

    // Scale Level Info
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

    // ORB extraction
    thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
    thread threadRight(&Frame::ExtractORB,this,1,imRight);
    threadLeft.join();
    threadRight.join();

    // Delete those ORB points that fall in Mask borders (Included by Berta)
    cv::Mat MaskLeft_dil = maskLeft.clone();
    cv::Mat MaskRight_dil = maskRight.clone();
    int dilation_size = 15;
    cv::Mat kernel = getStructuringElement(cv::MORPH_ELLIPSE,
                                        cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ),
                                        cv::Point( dilation_size, dilation_size ) );
    cv::erode(maskLeft, MaskLeft_dil, kernel);
    cv::erode(maskRight, MaskRight_dil, kernel);

    if(mvKeys.empty())
        return;

    std::vector _mvKeys;
    cv::Mat _mDescriptors;

    for (size_t i(0); i < mvKeys.size(); ++i)
    {
        int val = (int)MaskLeft_dil.at(mvKeys[i].pt.y,mvKeys[i].pt.x);
        if (val == 1)
        {
            _mvKeys.push_back(mvKeys[i]);
            _mDescriptors.push_back(mDescriptors.row(i));
        }
    }

    mvKeys = _mvKeys;
    mDescriptors = _mDescriptors;

    std::vector _mvKeysRight;
    cv::Mat _mDescriptorsRight;

    for (size_t i(0); i < mvKeysRight.size(); ++i)
    {
        int val = (int)MaskRight_dil.at(mvKeysRight[i].pt.y,mvKeysRight[i].pt.x);
        if (val == 1)
        {
            _mvKeysRight.push_back(mvKeysRight[i]);
            _mDescriptorsRight.push_back(mDescriptorsRight.row(i));
        }
    }

    mvKeysRight = _mvKeysRight;
    mDescriptorsRight = _mDescriptorsRight;


    N = mvKeys.size();

    if(mvKeys.empty())
        return;

    UndistortKeyPoints();

    ComputeStereoMatches();

    mvpMapPoints = vector(N,static_cast(NULL));    
    mvbOutlier = vector(N,false);


    // This is done only for the first Frame (or after a change in the calibration)
    if(mbInitialComputations)
    {
        ComputeImageBounds(imLeft);

        mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
        mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);

        fx = K.at(0,0);
        fy = K.at(1,1);
        cx = K.at(0,2);
        cy = K.at(1,2);
        invfx = 1.0f/fx;
        invfy = 1.0f/fy;

        mbInitialComputations=false;
    }

    mb = mbf/fx;

    AssignFeaturesToGrid();
}

        提取ORB特征点的部分与ORBSLAM2部分相同，至于具体为什么传进来_mask参数我也是很理解....没用到为什么要传呢？？？：ORB-SLAM2 ---- ORBextractor::operator()仿函数https://blog.csdn.net/qq_41694024/article/details/126310734

        DynaSLAM的新增内容是：删除了Mask边界上的特征点：

    // Delete those ORB points that fall in Mask borders (Included by Berta)
    cv::Mat MaskLeft_dil = maskLeft.clone();
    cv::Mat MaskRight_dil = maskRight.clone();
    int dilation_size = 15;
    cv::Mat kernel = getStructuringElement(cv::MORPH_ELLIPSE,
                                        cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ),
                                        cv::Point( dilation_size, dilation_size ) );
    cv::erode(maskLeft, MaskLeft_dil, kernel);
    cv::erode(maskRight, MaskRight_dil, kernel);

        实际上是将Mask部分又膨胀了一圈：

        然后是根据膨胀后的mask_dila重新计算特征点和描述子并赋值（下面只列举了左图，其实右图一样）。

    std::vector _mvKeys;
    cv::Mat _mDescriptors;

    for (size_t i(0); i < mvKeys.size(); ++i)
    {
        int val = (int)MaskLeft_dil.at(mvKeys[i].pt.y,mvKeys[i].pt.x);
        if (val == 1)
        {
            _mvKeys.push_back(mvKeys[i]);
            _mDescriptors.push_back(mDescriptors.row(i));
        }
    }

    mvKeys = _mvKeys;
    mDescriptors = _mDescriptors;

    std::vector _mvKeysRight;
    cv::Mat _mDescriptorsRight;

    for (size_t i(0); i < mvKeysRight.size(); ++i)
    {
        int val = (int)MaskRight_dil.at(mvKeysRight[i].pt.y,mvKeysRight[i].pt.x);
        if (val == 1)
        {
            _mvKeysRight.push_back(mvKeysRight[i]);
            _mDescriptorsRight.push_back(mDescriptorsRight.row(i));
        }
    }

    mvKeysRight = _mvKeysRight;
    mDescriptorsRight = _mDescriptorsRight;

        由于我们之前利用膨胀卷积扩大了Mask的区域，因此我们遍历左图/右图的每一个在ORBextractor::operator()仿函数中提取好的特征点，若特征点的中心坐标的灰度值为1，即没有被mask掉，我们保存特征点和其对应的描述子在临时变量_mvKeys、_mDescriptors中，待处理完毕后，将最终的特征点（去除动态特征点的剩余特征点）放入容器mvKeys 、mDescriptors 中。

        将特征点利用去畸变参数去畸变、匹配左右目的匹配特征点，这里和ORB - SLAM2中代码相同，不做赘述。

       计算去畸变边界、将特征点分配到网格中，这里和ORB - SLAM2中代码相同，不做赘述。

        随后进入追踪线程。