Orb-slam2 data sharing and transmission between different threads

1. Data transfer between threads

System.cc Entry function System::System in
Initialize trace thread

   //Initialize the Tracking thread
   //(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracking(this,	// When the trace is lost , For system reset 
    					mpVocabulary,		// Dictionaries 
    					mpFrameDrawer, 	// Frame renderer 
    					mpMapDrawer,		// Mapmaker 
                        mpMap, 			// Map 
                        mpKeyFrameDatabase,// Keyframe map 
                        strSettingsFile, 	// Set file path 
                        mSensor);	// Sensor type iomanip

Initialize the local drawing thread

//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap,  //  Specify make iomanip
                                 mSensor==MONOCULAR);	    //  This is to judge whether the sensor is monocular 

Initialize loopback detection thread

//Initialize the Loop Closing thread and launchiomanip
mpLoopCloser = new LoopClosing(mpMap, 		//  Map 
                            mpKeyFrameDatabase,//  Key frame database 
                            mpVocabulary, 	   // ORB Dictionaries 
                            mSensor!=MONOCULAR);			//  Whether the current sensor is monocular 

At the end of the entry function , take Started trace 、 The handles of local mapping and loopback detection are passed to each other , To realize Tracking and LocalMapping And data transmission between loopback detection threads .

//Set pointers between threads
// Set the pointer between processes 
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);

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

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

ORB-SLAM2 Through this way To realize the real-time update of data transmission between threads , Without the help of IPC Data transfer . This way of data transmission , Still need to consider Thread race The problem of , need Lock the data accessed by multiple threads .

2. Tracking Threads

Update the data path of other threads ： Private member variables （ Pointer operation , all Tracking Thread data update , What is actually updated is the data of the other two threads ）

//Other Thread Pointers
/// Local mapper handle 
LocalMapping* mpLocalMapper;
/// Loop detector handle 
LoopClosing* mpLoopClosing;

Get function ： Private member functions

// Set up the local builder 
void Tracking::SetLocalMapper(LocalMapping *pLocalMapper)
{
    
    mpLocalMapper=pLocalMapper;
}

// Set loop detector 
void Tracking::SetLoopClosing(LoopClosing *pLoopClosing)
{
    
    mpLoopClosing=pLoopClosing;
}

Tracking The obtained key frame information is updated through the above two member variables LoaclMapping and LoopClosing Thread data information . In turn, the execution of the other two threads , It can also update the data of the tracking thread , And then affect the tracking process .

Data entry of tracking thread and local mapping thread ：

// Perform the operation of inserting keys , In fact, it is also waiting in the list 
mpLocalMapper->InsertKeyFrame(pKF);

The trace thread creates keyframes void Tracking::CreateNewKeyFrame() When , It is necessary to judge and control the state of the local drawing thread , Functions and variables in the local mapping thread will also be used .

// If you can't keep the local builder on , You can't insert keys smoothly 
if(!mpLocalMapper->SetNotStop(true))
	return;

...
    
// Perform the operation of inserting keys , In fact, it is also waiting in the list 
mpLocalMapper->InsertKeyFrame(pKF);

// Then now allow the local builder to stop 
mpLocalMapper->SetNotStop(false);

Before the trace thread creates a keyframe , You need to decide whether you need a new key frame bool Tracking::NeedNewKeyFrame()

...

// If Local Mapping is freezed by a Loop Closure do not insert keyframes
//  If the local map is used by closed-loop detection , No keyframes are inserted 
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
	return false;
...
    
// Local Mapping accept keyframes?
// step 4： Query whether the local map Explorer is busy , That is, whether new keyframes can be accepted at present 
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
...
    
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
//  When bLocalMappingIdle Returns when a new keyframe cannot be received , You need to control the local drawing thread , So that it can receive new keyframes 
mpLocalMapper->InterruptBA();
if(mSensor!=System::MONOCULAR)
{
    
    //  You can't block too many keyframes in the queue 
    // tracking Insert keyframes, not directly , And insert it into mlNewKeyFrames in ,
    //  then localmapper One by one pop Come out and insert into mspKeyFrames
    if(mpLocalMapper->KeyframesInQueue()<3)
        // There are not many keyframes in the queue , You can insert 
        return true;
    else
        // Too many key frames buffered in the queue , Temporarily unable to insert 
        return false;
}
else
    // For monocular case , You can't insert keys directly 
    //?  Why is the monocular case handled differently here ?
    return false;

The tracking thread only needs to be reset , Use the reset function in the loop detection thread .

// 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 Threads

Update the data of other threads ： Private member variables

//  Loop back detection thread handle 
LoopClosing* mpLoopCloser;
//  Trace thread handle 
Tracking* mpTracker;

Function to get handle ： Private member functions

//  Set loopback detection thread handle 
void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)
{
    
    mpLoopCloser = pLoopCloser;
}

//  Set the trace thread handle 
void LocalMapping::SetTracker(Tracking *pTracker)
{
    
    mpTracker=pTracker;
}

LocalMapping Threads need to use other threads to operate in relatively few places .

LocalMapping There is no need to call Tracking Thread function or data operation . It has only one data entry :

// Tracking Thread to LocalMapping Key frames are inserted into the queue first 
std::list<KeyFrame*> mlNewKeyFrames; ///<  List of keys waiting to be processed 

When the tracking thread detects that the current frame is a key frame , And put this key frame into the list of keys to be processed .

LocalMapping and LoopCloser The interaction of is just data export , Insert the key frame after detection into the queue list of loopback detection .

//  Add the current frame to the closed-loop detection queue 
//  Note that it does not rule out that the keyframes inserted at the beginning are wide , But later it was set as bad The situation of , This needs attention 
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

LoopCloser Threads

Update other thread data ： Private member variables

///  Trace thread handle 
Tracking* mpTracker;
///  Local mapping thread handle 
LocalMapping *mpLocalMapper;

Function to get handle ： Private member functions

//  Set the trace thread handle 
void LoopClosing::SetTracker(Tracking *pTracker)
{
    
    mpTracker=pTracker;
}
//  Set the handle of the local drawing thread 
void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper)
{
    
    mpLocalMapper=pLocalMapper;
}

LoopCloser Thread did not call Tracking Thread related information .
LoopCloser The thread from LoaclMapping The thread gets the data

///  A line ,  The key frames involved in loop detection are stored  ( Of course, these keyframes may also be set to bad, In this way, although the key frame is still stored here, it is no longer substantially involved in the loop detection process )
std::list<KeyFrame*> mlpLoopKeyFrameQueue;

LoaclMapping Threads utilize functions , Update the above member variable values ：

//  Add a key frame to the loop detection process , Called by the local drawing thread 
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    
    unique_lock<mutex> lock(mMutexLoopQueue);
    // NOTICE  Here I 0 Key frames cannot participate in loop detection , Because the first 0 The key frame defines the world coordinate system of the whole map 
    if(pKF->mnId!=0)
        mlpLoopKeyFrameQueue.push_back(pKF);
}

LoopCloser The thread is in loopback correction and global BA when , You need to pause the local drawing thread , Therefore, we need to use LoaclMapping Some functions of local mapping for judgment and thread control .

Loop correction ：

// STEP 0： Request local map stop , Prevent local map threads InsertKeyFrame Function to insert a new key frame 
mpLocalMapper->RequestStop();
...

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

    //  Execute the whole picture BA
mpThreadGBA = new 
    thread(&LoopClosing::RunGlobalBundleAdjustment,this,
                                         mpCurrentKF->mnId);

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

overall situation BA

...
Optimizer::GlobalBundleAdjustemnt(mpMap,     //  Map point object 
                        10,    //  The number of iterations 
                        &mbStopGBA,//  External control  GBA  Stop sign 
                        nLoopKF, //  Of the current keyframe that forms the closed loop id
                        false); //  Do not use robust kernel function 
...
    
// 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));
}
...
    
//  Release , bring LocalMapping The thread starts working again 
mpLocalMapper->Release();