欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

重读ORB_SLAM之LocalMapping线程难点

程序员文章站 2024-03-25 23:47:10
...

1. 认清几个锁与布尔参数

线程的通信与相互影响在ORB比较复杂,需要好好缕清思路。
1.1 mbStopRequested,由RequestStop函数设定,主要是在回环线程里,在运行全局优化时,以及检测后,调整Loop时,localMapping需要暂停。
1.2 mbStopped:由stop()函数设定。mbNotStop: 由SetNotStop()设定。 这两个真是把人搞得头大。在Tracking线程,如果mapping线程被要求暂停,也就是mpLocalMapper->isStopped() || mpLocalMapper->stopRequested() 为真,则停止往里添加关键帧。如果可以添加关键帧,则Tracking线程添加关键帧时,必须保证LocaMapping不能被终止,则mbNotStop需要为真。

   if(!mpLocalMapper->SetNotStop(true))
        return;

这个判断要为真。

1.3 mpLocalMapper->Release(); 此函数在全局优化以及调整Loop后使用,目的是使mbStopped,mbStopRequested都置为false。且清空 mlNewKeyFrames。
1.4 mMutexStop锁,主要是在设置以上两个参数时使用,防止同时更改。
1.5 mMutexAccept锁,也是防止在设置mbAcceptKeyFrames时保持异步。mMutexNewKFs也是同样的原理。

2 整体流程

void LocalMapping::Run()
{

    mbFinished = false;

    while(1)
    {
        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(false);

        // Check if there are keyframes in the queue
        if(CheckNewKeyFrames())
        {
            // BoW conversion and insertion in Map
            /* 1. 首先计算BOW
               2. 对于Tracking 线程匹配到的mappoint做一些处理,比如为mappoint增加观测,更新描述子
               3. 更新当前帧与其邻居帧的关系
               4. 最后把关键帧加入到map里 */

            ProcessNewKeyFrame();

            // Check recent MapPoints
            /*
                这里主要处理刚加入的地图点。刚加入的地图点一般由上一次CreateNewMapPoints产生。在这里做删减。但是这里不知道mlpRecentAddedMapPoints有什么作用。好像哪里都没有用到,而且list里erase这些mappoint指针并不会对map里的mappoint指针有影响。
            */
            MapPointCulling();

            // Triangulate new MapPoints
            /*
              这里生成新的地图点,与Tracking线程里不太一样,那里是直接利用深度信息产生地图点。这里是根据当前帧的邻居帧,进行再次匹配,利用三角化生成地图点。这样就增大了SLAM的可视距离
            */
            CreateNewMapPoints();

            if(!CheckNewKeyFrames())
            {
                // Find more matches in neighbor keyframes and fuse point duplications
                SearchInNeighbors();
            }

            mbAbortBA = false;

            if(!CheckNewKeyFrames() && !stopRequested())
            {
                // Local BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

                // Check redundant local Keyframes
                /*
                如果当前局部地图里有这样的关键帧:其所看到的90%的地图点,也至少被其他三个关键帧看到,则应删除此关键帧。
                */
                KeyFrameCulling();
            }

            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        else if(Stop())
        {
            // Safe area to stop
            while(isStopped() && !CheckFinish())
            {
                usleep(3000);
            }
            if(CheckFinish())
                break;
        }

        ResetIfRequested();

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(true);

        if(CheckFinish())
            break;

        usleep(3000);
    }

    SetFinish();
}