DynaSLAM-12 DynaSLAM中RGB-D运行流程（Ⅱ）：图像处理与低成本追踪函数LightTrack

目录

1.调用GetSegmentation方法得到先验动态物体的掩码

2. System::TrackRGBD

 3.低成本追踪Tracking::LightTrack

---

1.调用GetSegmentation方法得到先验动态物体的掩码

        上篇博客我们说到了初始化了Mask R-CNN网络，这里我们利用GetSegmentation方法对图像进行处理。

        cv::Mat mask = cv::Mat::ones(480,640,CV_8U);
        if (argc == 6 || argc == 7)
        {
            cv::Mat maskRCNN;
            maskRCNN = MaskNet->GetSegmentation(imRGB,string(argv[5]),vstrImageFilenamesRGB[ni].replace(0,4,""));
            cv::Mat maskRCNNdil = maskRCNN.clone();
            cv::dilate(maskRCNN,maskRCNNdil, kernel);
            mask = mask - maskRCNNdil;
        }

        一、这里首先解释一下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<

        二、得到的maskRCNN：利用Mask R-CNN网络得到的

        打开mask.png，由于0，1的向素值很小，原mask显示一团黑。对某一张mask的所有像素*255，结果显示，mask是把先验动态物体所在区域的像素值设置为1。

         三、接下来获取mask，并对mask进行膨胀，存储到dila里，然后用全1矩阵maskRCNNdil减去dila，得到与dila像素值反转的0，1图像。

2. System::TrackRGBD

        输入的参数：

        @imRGB：RGB图像

        @imD：深度图像

        @mask：动态掩码信息

        @tframe：时间戳

        输入到Tracking::GrabImageRGBD的参数还是这些。

        先将图片转化为灰度图，然后构造Frame，构造Frame与构造双目例程的流程相同。

 3.低成本追踪Tracking::LightTrack

void Tracking::LightTrack()
{
    // Get Map Mutex -> Map cannot be changed
	// 进程锁，保证地图不被改变。
    unique_lock lock(mpMap->mMutexMapUpdate);
    bool useMotionModel = true; //set true

    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
    {
        cout << "Light Tracking not working because Tracking is not initialized..." << endl;
        return;
    }
    else
    {
        // 系统已经完成系统初始化，下面进行追踪
        bool bOK;
        {
            // Localization Mode:
            if(mState==LOST)
            {
				//如果跟丢了，使用重定位(这里进行了函数重载)
				// if (update == 0) mnLastRelocFrameId = mCurrentFrame.mnId; 在这里不更改mnLastRelocFrameId属性信息即最后一个重定位帧信息
                bOK = Relocalization(1);
            }
            else
            {	
				//mbVO是对于定位模式才有的变量
				//mbVO=true表示当前帧跟地图点的匹配点少于10个。（不好）
            	//mbVO=false表示当前帧跟地图点的匹配点较多，跟踪能正常工作。
                if(!mbVO)
                {
                    // 使用运动模型追踪
                    if(!mVelocity.empty() && useMotionModel)
                    {
                        bool _bOK = false;
                        bOK = LightTrackWithMotionModel(_bOK);// TODO: check out!!!
                    }
                    else
                    {
                        bOK = TrackReferenceKeyFrame();
                    }
                }

				// mbVO=false 或者是 正常模式SLAM模式
                else
                {
                    // In last frame we tracked mainly "visual odometry" points.
					// 在最新一帧中，我们主要跟踪 "视觉测距 "点。

                    // We compute two camera poses, one from motion model and one doing relocalization.
					// 我们计算两个摄像机的姿势，一个来自运动模型，一个做重新定位。
                    // If relocalization is sucessfull we choose that solution, otherwise we retain
					//如果重新定位成功，我们选择该解决方案，否则我们保留
                    // the "visual odometry" solution.

					// 通过运动模型进行跟踪的结果
                    bool bOKMM = false;
					// 通过重定位模型进行跟踪的结果
                    bool bOKReloc = false;

					// 运动模型中构造的地图点
                    vector vpMPsMM;

					// 在跟踪运动模型中发现的外点
                    vector vbOutMM;

					// 运动模型得到的位姿
                    cv::Mat TcwMM;

                    bool lightTracking = false;
                    bool bVO = false;
					//当运动模型有效时，使用运动模型计算位姿，lightTracking = true
                    if(!mVelocity.empty() && useMotionModel)
                    {
                        lightTracking = true;
                        bOKMM = LightTrackWithMotionModel(bVO); // TODO: check out!!
                        vpMPsMM = mCurrentFrame.mvpMapPoints;
                        vbOutMM = mCurrentFrame.mvbOutlier;
                        TcwMM = mCurrentFrame.mTcw.clone();
                    }
                    
					bOKReloc = Relocalization(1);

					// 运动模型追踪成功  重定位追踪不成功
                    if(bOKMM && !bOKReloc)
                    {
						// 用运动模型的得到位姿和地图点设置设置本帧的位姿
                        mCurrentFrame.SetPose(TcwMM);
                        mCurrentFrame.mvpMapPoints = vpMPsMM;
                        mCurrentFrame.mvbOutlier = vbOutMM;

						// 使用运动模型追踪且满足一定条件 或 使用重定位追踪且追踪效果不好
                        if((lightTracking && bVO) || (!lightTracking && mbVO))
                        {
                            for(int i =0; iIncreaseFound();
                                }
                            }
                        }
                    }

                    bOK = bOKReloc || bOKMM;
                }
            }
        }

        mCurrentFrame.mpReferenceKF = mpReferenceKF;

		// 追踪失败
        if(!bOK)
        {
			// 在地图中的关键帧数目小于5
            if(mpMap->KeyFramesInMap()<=5)
            {
                cout << "Light Tracking not working..." << endl;
                return;
            }
        }

        if(!mCurrentFrame.mpReferenceKF)
            mCurrentFrame.mpReferenceKF = mpReferenceKF;

    }
}

        注释我已经标注的很清晰了，需要解释以下几个问题：

        ①有关于Relocalization重定位追踪的参数：

if (update == 0) mnLastRelocFrameId = mCurrentFrame.mnId;

        和ORB - SLAM2中就有一个区别，即传入参数，如果传入参数为0，则不更改mnLastRelocFrameId （最后一个进行过重定位的帧）变量。

        ②有关于LightTrackWithMotionModel函数：

        mlpTemporalPoints变量是在LightTrackWithMotionModel中调用UpdateLastFrame函数中产生的，由于是根据上一帧来进行追踪，于是我们要上一帧的地图点尽可能多于是我们生成了地图点放在了mlpTemporalPoints中。

        但是LightTrack不是真正的track，它不应该更改原有系统的信息，只是一种粗略的估计。因此将mlpTemporalPoints存储进容器lpTemporalPointsBU中待函数结束在恢复原系统状态。相同地，lastFrameBU存储mLastFrame的信息。

        若两帧匹配的地图点经外点判断后nmatchesMap<10，置状态变量bVO为false。

        若两帧匹配的地图点nmatches>20（未经外点判断），向调用函数返回true。

        ③其余追踪方法与ORB - SLAM2相同，请参阅博客：ORB-SLAM2 --- Tracking::Track 追踪线程解析https://blog.csdn.net/qq_41694024/article/details/128321440