DynaSLAM-10 DynaSLAM中双目运行流程（Ⅳ）：DynaSLAM的三大线程解析

目录

1.双目初始化部分

2.三种追踪模式

---

1.双目初始化部分

        进入追踪线程首先判断双目追踪器状态mstate，在刚进入SLAM系统时，

Tracking::Tracking函数默认将其设置为NO_IMAGES_YET，系统经过reset时也会将mstate设置为NO_IMAGES_YET......如果是这种状态，将SLAM系统状态设置为未初始化状态NOT_INITIALIZED。

        mLastProcessedState设置为mstate的值。

        如果SLAM系统没有初始化，则执行双目初始化函数StereoInitialization。

void Tracking::StereoInitialization()
{
    if(mCurrentFrame.N>500)
    {
        // Set Frame pose to the origin
        mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));

        // Create KeyFrame
        KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

        // Insert KeyFrame in the map
        mpMap->AddKeyFrame(pKFini);

        // Create MapPoints and associate to KeyFrame
        for(int i=0; i0)
            {
                cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
                pNewMP->AddObservation(pKFini,i);
                pKFini->AddMapPoint(pNewMP,i);
                pNewMP->ComputeDistinctiveDescriptors();
                pNewMP->UpdateNormalAndDepth();
                mpMap->AddMapPoint(pNewMP);

                mCurrentFrame.mvpMapPoints[i]=pNewMP;
            }
        }

        cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;

        mpLocalMapper->InsertKeyFrame(pKFini);

        mLastFrame = Frame(mCurrentFrame);
        mnLastKeyFrameId=mCurrentFrame.mnId;
        mpLastKeyFrame = pKFini;

        mvpLocalKeyFrames.push_back(pKFini);
        mvpLocalMapPoints=mpMap->GetAllMapPoints();
        mpReferenceKF = pKFini;
        mCurrentFrame.mpReferenceKF = pKFini;

        mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

        mpMap->mvpKeyFrameOrigins.push_back(pKFini);

        mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

        mState=OK;
    }
}

        构造此帧为关键帧并加入mspKeyFrames集合中，这个集合存储着所有的关键帧，同时更新关键帧的最大id mnMaxKFid。

        遍历所有此帧的特征点，如果特征点有深度，则创建地图点存入mvpMapPoints。

        将此关键帧传入局部建图线程的处理队列mlNewKeyFrames中，并将mbAbortBA标志置为true。

        更新有关于Last的信息：

        ①mLastFrame：上一帧，更新为本帧的信息

        ②mnLastKeyFrameId：上一帧的ID，更新为本帧的ID

        ③mpLastKeyFrame：上一关键帧，更新为本帧

        更新局部地图信息：

        ①将本帧加入局部关键帧mvpLocalKeyFrames

        ②将本帧地图点加入局部地图点mvpLocalMapPoints

        更新其他信息：

        ①mpReferenceKF：更新为本帧

        ②mCurrentFrame.mpReferenceKF：更新为本帧

        ③置参考地图点用于绘图显示局部地图点（红色）mpMap->SetReferenceMapPoints

        ④保留最初始的关键帧mvpKeyFrameOrigins。

        ⑤设置当前帧相机的位姿mpMapDrawer->SetCurrentCameraPose

        ⑥将追踪状态mState设置为true。

        最后将跟踪线程的数据拷贝到绘图线程（图像、特征点、地图、跟踪状态）

void FrameDrawer::Update(Tracking *pTracker)
{
    unique_lock lock(mMutex);
    //拷贝跟踪线程的图像
    pTracker->mImGray.copyTo(mIm);
    //拷贝跟踪线程的特征点
    mvCurrentKeys=pTracker->mCurrentFrame.mvKeys;
    N = mvCurrentKeys.size();
    mvbVO = vector(N,false);
    mvbMap = vector(N,false);

    mbOnlyTracking = pTracker->mbOnlyTracking;

    //如果上一帧的时候,追踪器没有进行初始化
    if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED)
    {
        //那么就要获取初始化帧的特征点和匹配信息
        mvIniKeys=pTracker->mInitialFrame.mvKeys;
        mvIniMatches=pTracker->mvIniMatches;
    }
    //如果上一帧是在正常跟踪
    else if(pTracker->mLastProcessedState==Tracking::OK)
    {
        //获取当前帧地图点的信息
        for(int i=0;imCurrentFrame.mvpMapPoints[i];
            if(pMP)
            {
                if(!pTracker->mCurrentFrame.mvbOutlier[i])
                {
                    //该mappoints可以被多帧观测到，则为有效的地图点
                    if(pMP->Observations()>0)
                        mvbMap[i]=true;
                    else
                    //否则表示这个特征点是在当前帧中第一次提取得到的点
                        mvbVO[i]=true;
                }
            }
        }
    }
    //更新追踪线程的跟踪状态
    mState=static_cast(pTracker->mLastProcessedState);
}

}

2.三种追踪模式

        和ORB - SLAM2中完全相同，不做赘述！详见博客：

ORB-SLAM2 --- Tracking::Track 追踪线程解析https://blog.csdn.net/qq_41694024/article/details/128321440        局部建图和回环检测线程也相同，详见博客：

ORB-SLAM2 --- LocalMapping::Run 局部建图线程解析https://blog.csdn.net/qq_41694024/article/details/128500904ORB-SLAM2 --- LoopClosing::Run 回环检测线程解析https://blog.csdn.net/qq_41694024/article/details/128572883