ORB-SLAM2不同线程间的数据共享与传递

1. 线程间数据传递

System.cc入口函数System::System中
初始化跟踪线程

   //Initialize the Tracking thread
   //(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracking(this,	//当跟踪丢失后，用于系统复位
    					mpVocabulary,		//字典
    					mpFrameDrawer, 	//帧绘制器
    					mpMapDrawer,		//地图绘制器
                        mpMap, 			//地图
                        mpKeyFrameDatabase,//关键帧地图
                        strSettingsFile, 	//设置文件路径
                        mSensor);	//传感器类型iomanip

初始化局部建图线程

//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap,  // 指定使iomanip
                                 mSensor==MONOCULAR);	    // 这是判断传感器是否是单目

初始化回环检测线程

//Initialize the Loop Closing thread and launchiomanip
mpLoopCloser = new LoopClosing(mpMap, 		// 地图
                            mpKeyFrameDatabase,// 关键帧数据库
                            mpVocabulary, 	   // ORB字典
                            mSensor!=MONOCULAR);			// 当前的传感器是否是单目

在入口函数的最后，将启动的跟踪、局部建图和回环检测的句柄相互传递，进而实现Tracking和LocalMapping以及回环检测线程之间的数据传递。

//Set pointers between threads
//设置进程间的指针
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);

mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);

mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);

ORB-SLAM2是通过这种方式实现线程间数据传递实时更新的，而没有借助IPC进行数据传递。这种数据传递方式，仍然需要考虑线程竞争的问题，需要对多个线程访问的数据加锁。

2. Tracking线程

更新其他线程的数据途径：私有成员变量（指针操作，所有Tracking线程数据更新，实际更新的是其他两个线程的数据）

//Other Thread Pointers
///局部建图器句柄
LocalMapping* mpLocalMapper;
///回环检测器句柄
LoopClosing* mpLoopClosing;

获取函数：私有成员函数

//设置局部建图器
void Tracking::SetLocalMapper(LocalMapping *pLocalMapper)
{
    mpLocalMapper=pLocalMapper;
}

//设置回环检测器
void Tracking::SetLoopClosing(LoopClosing *pLoopClosing)
{
    mpLoopClosing=pLoopClosing;
}

Tracking获取的关键帧信息通过上述两个成员变量更新LoaclMapping和LoopClosing线程的数据信息。反过来另外两个线程的执行情况，同样能够更新跟踪线程的数据，进而影响跟踪的过程。

跟踪线程与局部建图线程的数据入口：

//执行插入关键帧的操作,其实也是在列表中等待
mpLocalMapper->InsertKeyFrame(pKF);

跟踪线程创建关键帧void Tracking::CreateNewKeyFrame()的时候，需要对局部建图线程进行状态判断以及状态控制，也会用到局部建图线程中的函数和变量。

//如果不能保持局部建图器开启的状态,就无法顺利插入关键帧
if(!mpLocalMapper->SetNotStop(true))
	return;

...
    
//执行插入关键帧的操作,其实也是在列表中等待
mpLocalMapper->InsertKeyFrame(pKF);

//然后现在允许局部建图器停止了
mpLocalMapper->SetNotStop(false);

跟踪线程创建关键帧之前，需要判断是否需要新的关键帧bool Tracking::NeedNewKeyFrame()

...

// If Local Mapping is freezed by a Loop Closure do not insert keyframes
// 如果局部地图被闭环检测使用，则不插入关键帧
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
	return false;
...
    
// Local Mapping accept keyframes?
// step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
...
    
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
// 当bLocalMappingIdle返回无法接收新的关键帧时，需要对局部建图线程进行控制，使得其可以接收新的关键帧
mpLocalMapper->InterruptBA();
if(mSensor!=System::MONOCULAR)
{
    // 队列里不能阻塞太多关键帧
    // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
    // 然后localmapper再逐个pop出来插入到mspKeyFrames
    if(mpLocalMapper->KeyframesInQueue()<3)
        //队列中的关键帧数目不是很多,可以插入
        return true;
    else
        //队列中缓冲的关键帧数目太多,暂时不能插入
        return false;
}
else
    //对于单目情况,就直接无法插入关键帧了
    //? 为什么这里对单目情况的处理不一样?
    return false;

跟踪线程仅在跟踪线程需要复位的时候，用到回环检测线程中的复位功能。

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

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

LocalMapping线程

更新其他线程的数据：私有成员变量

// 回环检测线程句柄
LoopClosing* mpLoopCloser;
// 追踪线程句柄
Tracking* mpTracker;

获取句柄的函数：私有成员函数

// 设置回环检测线程句柄
void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)
{
    mpLoopCloser = pLoopCloser;
}

// 设置追踪线程句柄
void LocalMapping::SetTracker(Tracking *pTracker)
{
    mpTracker=pTracker;
}

LocalMapping线程需要用到其余线程进行操作地方相对较少。

LocalMapping实际并不需要调用Tracking线程的函数或者数据进行操作。其只有一个数据入口:

// Tracking线程向LocalMapping中插入关键帧是先插入到该队列中
std::list<KeyFrame*> mlNewKeyFrames; ///< 等待处理的关键帧列表

在跟踪线程检测到当前帧是关键帧时，并将此关键帧放入待处理的关键帧列表中。

LocalMapping和LoopCloser的交互仅仅是数据出口，即将检测之后的关键帧插入到回环检测的队列列表中。

// 将当前帧加入到闭环检测队列中
// 注意这里不排除一开始的时候插入的关键帧是阔以的,但是在后来被设置成为了bad的情况,这个需要注意
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

LoopCloser线程

跟新其他线程数据：私有成员变量

/// 追踪线程句柄
Tracking* mpTracker;
/// 局部建图线程句柄
LocalMapping *mpLocalMapper;

获取句柄的函数：私有成员函数

// 设置追踪线程句柄
void LoopClosing::SetTracker(Tracking *pTracker)
{
    mpTracker=pTracker;
}
// 设置局部建图线程的句柄
void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper)
{
    mpLocalMapper=pLocalMapper;
}

LoopCloser线程未调用Tracking线程相关的信息。
LoopCloser线程从LoaclMapping线程获取得到数据

/// 一个队列, 其中存储了参与到回环检测的关键帧 (当然这些关键帧也有可能因为各种原因被设置成为bad,这样虽然这个关键帧还是存储在这里但是实际上已经不再实质性地参与到回环检测的过程中去了)
std::list<KeyFrame*> mlpLoopKeyFrameQueue;

LoaclMapping线程利用函数，更新上述成员变量值：

// 将某个关键帧加入到回环检测的过程中,由局部建图线程调用
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    // NOTICE 这里第0个关键帧不能够参与到回环检测的过程中,因为第0关键帧定义了整个地图的世界坐标系
    if(pKF->mnId!=0)
        mlpLoopKeyFrameQueue.push_back(pKF);
}

LoopCloser线程在回环纠正和进行全局BA时，需要暂停局部建图线程，因此这两函数中需要用到LoaclMapping局部建图的一些函数进行判断以及线程控制。

回环纠正：

// STEP 0：请求局部地图停止，防止局部地图线程中InsertKeyFrame函数插入新的关键帧
mpLocalMapper->RequestStop();
...

// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{          				                  					std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
...

    // 执行全局BA
mpThreadGBA = new 
    thread(&LoopClosing::RunGlobalBundleAdjustment,this,
                                         mpCurrentKF->mnId);

// Loop closed. Release Local Mapping.
mpLocalMapper->Release();

全局BA

...
Optimizer::GlobalBundleAdjustemnt(mpMap,     // 地图点对象
                        10,    // 迭代次数
                        &mbStopGBA,// 外界控制 GBA 停止的标志
                        nLoopKF, // 形成了闭环的当前关键帧的id
                        false); // 不使用鲁棒核函数
...
    
// Update all MapPoints and KeyFrames
// Local Mapping was active during BA, that means that there might be new keyframes
// not included in the Global BA and they are not consistent with the updated map.
// We need to propagate the correction through the spanning tree
...

mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped() && 
      !mpLocalMapper->isFinished())
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
...
    
// 释放,使得LocalMapping线程重新开始工作
mpLocalMapper->Release();