ORBSLAM2局部地图初始化和开启线程

简述ORBSLAM2多线程编程
ORBSLAM2系统程序就是典型的多线程编写的。我们的电脑一般都是多线程或是多核的处理器，支持多线程程序运行。ORBSLAM2程序有追踪线程，局部地图构建线程，回环检测线程，显示线程。后续可能会有人构建3D点云线程等。言归正传，讲述局部见图初始化和开启建图线程。局部地图线程开启的时候一直在等待关键帧的到来。
1.局部地图初始化

mpLocalMapper = new LocalMapping(mpMap, mSensor==RGBD);

//变量赋值
LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
	    mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
	    mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
	{
   
	}

2.开启局部地图构建线程
thread（）函数是开启一个线程的功能函数，详细功能需私下查找。

//开启局部地图构建线程
 mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

//功能函数
void LocalMapping::Run()
	{
   

	    mbFinished = false;

	    while(1)
	    {
   
		// Tracking will see that Local Mapping is busy
// 步骤1：设置进程间的访问标志 告诉Tracking线程，LocalMapping线程正在处理新的关键帧，处于繁忙状态
               // LocalMapping线程处理的关键帧都是Tracking线程发过的
               // 在LocalMapping线程还没有处理完关键帧之前Tracking线程最好不要发送太快
		SetAcceptKeyFrames(false);
// 		 bool bool_checknewkeyfraes=CheckNewKeyFrames();
// 		  cout <<"bool_checknewkeyfrae "<<bool_checknewkeyfraes<<endl;   
		// Check if there are keyframes in the queue
	// 等待处理的关键帧列表不为空
		if(CheckNewKeyFrames())
		{
   
		  
		    // BoW conversion and insertion in Map
// 步骤2：计算关键帧特征点的词典单词向量BoW映射，将关键帧插入地图
		    ProcessNewKeyFrame();

		    // Check recent MapPoints
		  // 剔除ProcessNewKeyFrame函数中引入的不合格MapPoints
// 步骤3：对新添加的地图点融合 对于 ProcessNewKeyFrame 和 CreateNewMapPoints 中 最近添加的MapPoints进行检查剔除	    
		  //   MapPointCulling();

		    // Triangulate new MapPoints
		    
// 步骤4： 创建新的地图点 相机运动过程中与相邻关键帧通过三角化恢复出一些新的地图点MapPoints	    
		    CreateNewMapPoints();
		    
		    MapPointCulling();// 从上面 移到下面

	      // 已经处理完队列中的最后的一个关键帧
		    if(!CheckNewKeyFrames())
		    {
   
			// Find more matches in neighbor keyframes and fuse point duplications
// 步骤5：相邻帧地图点融合 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
			SearchInNeighbors();
		    }

		    mbAbortBA = false;

		// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping
		    if(!CheckNewKeyFrames() && !stopRequested())
		    {
   
// 步骤6：局部地图优化 Local BA
			if(mpMap->KeyFramesInMap() > 2)
			    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

// 步骤7： 关键帧融合 检测并剔除当前帧相邻的关键帧中冗余的关键帧 Check redundant local Keyframes
	                // 剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到		
			// Tracking中先把关键帧交给LocalMapping线程
			 // 并且在Tracking中InsertKeyFrame函数的条件比较松，交给LocalMapping线程的关键帧会比较密
                        // 在这里再删除冗余的关键帧
			KeyFrameCulling();
		    }
// 步骤8：将当前帧加入到闭环检测队列中
		    mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
		}
// 步骤9：等待线程空闲 完成一帧关键帧的插入融合工作
		else if(Stop())
		{
   
		    // Safe area to stop
		   cout <<"stop()= "<<Stop()<<endl; //
		    while(isStopped() && !CheckFinish())
		    {
   
			usleep(3000);
		    }
		    if(CheckFinish())//检查 是否完成
			break;
		}
		
               // 检查重置
		ResetIfRequested();

		// Tracking will see that Local Mapping is not busy
// 步骤10：告诉 	Tracking 线程  Local Mapping 线程 空闲 可一处理接收 下一个 关键帧
		SetAcceptKeyFrames(true);

		if(CheckFinish())
		  
		    break;

		usleep(3000);
	    }

	    SetFinish();
	}

3.部分功能函数解析
3.1 ProcessNewKeyFrame()函数

ProcessNewKeyFrame();//处理新关键帧

//功能函数
 /**
 * @brief 处理列表中的关键帧
 * 
 * - 计算Bow，加速三角化新的MapPoints
 * - 关联当前关键帧至MapPoints，并更新MapPoints的平均观测方向和观测距离范围
 * - 插入关键帧，更新Covisibility图和Essential图
 * @see VI-A keyframe insertion
 */
	void LocalMapping::ProcessNewKeyFrame()
	{
   	  
// 步骤1：从缓冲队列中取出一帧待处理的关键帧
             // Tracking线程向LocalMapping中插入关键帧存在该队列中
	    {
   
		unique_lock<mutex> lock(mMutexNewKFs);
		// 从列表中获得一个等待被插入的关键帧
		mpCurrentKeyFrame = mlNewKeyFrames.front();
		// 出队
		mlNewKeyFrames.pop_front();
	    }

	    // Compute Bags of Words structures
// 步骤2：计算该关键帧特征点的Bow映射关系    
	    //  根据词典 计算当前关键帧Bow，便于后面三角化恢复新地图点    
	    // 帧描述子 用字典单词线性表示的 向量
	    mpCurrentKeyFrame->ComputeBoW();

	    // Associate MapPoints to the new keyframe and update normal and descriptor
	    // 当前关键帧  TrackLocalMap中跟踪局部地图 匹配上的 地图点
// 步骤3：跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定
	      // 在TrackLocalMap函数中将局部地图中的MapPoints与当前帧进行了匹配，
	      // 但没有对这些匹配上的MapPoints与当前帧进行关联    
	    const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
	    for(size_t i=0; i<vpMapPointMatches.size(); i++)
	    {
   
	      // 每一个与当前关键帧匹配好的地图点
		MapPoint* pMP = vpMapPointMatches[i];
		//地图点存在
		if(pMP)
		{
   
		    if(!pMP->isBad())
		    {
   
		       // 为当前帧在tracking过重跟踪到的MapPoints更新属性
		      //下视野内
			if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
			{
   
			  // 地图点添加关键帧
			    pMP->AddObservation(mpCurrentKeyFrame, i);
			    // 地图点更新 平均观测方向 和 观测距离深度
			    pMP->UpdateNormalAndDepth();
			    // 加入关键帧后，更新地图点的最佳描述子
			    pMP->ComputeDistinctiveDescriptors();
			}
			else // 双目追踪时插入的点 可能不在 帧上this can only happen for new stereo points inserted by the Tracking
			{
   
			   // 将双目或RGBD跟踪过程中新插入的MapPoints放入mlpRecentAddedMapPoints，等待检查
                           // CreateNewMapPoints函数中通过三角化也会生成MapPoints
                           // 这些MapPoints都会经过MapPointCulling函数的检验
			    mlpRecentAddedMapPoints.push_back(pMP);
			    // 候选待检查地图点存放在mlpRecentAddedMapPoints
			}
		    }
		}
	    }    

	    // Update links in the Covisibility Graph
// 步骤4：更新关键帧间的连接关系，Covisibility图和Essential图(tree)
	    mpCurrentKeyFrame->UpdateConnections();

	    // Insert Keyframe in Map
// 步骤5：将该关键帧插入到地图中
	    mpMap->AddKeyFrame(mpCurrentKeyFrame);
	}

3.2 CreateNewMapPoints()函数

CreateNewMapPoints()；//构造新地图点

//功能函数
/**
 * @brief 相机运动过程中和共视程度比较高的关键帧通过三角化恢复出一些MapPoints
 *  根据当前关键帧恢复出一些新的地图点，不包括和当前关键帧匹配的局部地图点（已经在ProcessNewKeyFrame中处理）
 *  先处理新关键帧与局部地图点之间的关系，然后对局部地图点进行检查，
 *  最后再通过新关键帧恢复 新的局部地图点：CreateNewMapPoints()
 * 
 * 步骤1：在当前关键帧的 共视关键帧 中找到 共视程度 最高的前nn帧 相邻帧vpNeighKFs
 * 步骤2：遍历和当前关键帧 相邻的 每一个关键帧vpNeighKFs	
 * 步骤3：判断相机运动的基线在（两针间的相机相对坐标）是不是足够长
 * 步骤4：根据两个关键帧的位姿计算它们之间的基本矩阵 F =  inv(K1 转置) * t12 叉乘 R12 * inv(K2)
 * 步骤5：通过帧间词典向量加速匹配，极线约束限制匹配时的搜索范围，进行特征点匹配	
 * 步骤6：对每对匹配点 2d-2d 通过三角化生成3D点,和 Triangulate函数差不多	
 *  步骤6.1：取出匹配特征点
 *  步骤6.2：利用匹配点反投影得到视差角   用来决定使用三角化恢复(视差角较大) 还是 直接2-d点反投影(视差角较小)
 *  步骤6.3：对于双目，利用双目基线 深度 得到视差角
 *  步骤6.4：视差角较大时 使用 三角化恢复3D点
 *  步骤6.4：对于双目 视差角较小时 二维点 利用深度值 反投影 成 三维点    单目的话直接跳过
 *  步骤6.5：检测生成的3D点是否在相机前方
 *  步骤6.6：计算3D点在当前关键帧下的重投影误差  误差较大跳过
 *  步骤6.7：计算3D点在 邻接关键帧 下的重投影误差 误差较大跳过
 *  步骤6.9：三角化生成3D点成功，构造成地图点 MapPoint
 *  步骤6.9：为该MapPoint添加属性 
 *  步骤6.10：将新产生的点放入检测队列 mlpRecentAddedMapPoints  交给 MapPointCulling() 检查生成的点是否合适
 * @see  
 */	
	void LocalMapping::CreateNewMapPoints()
	{
   
	    // Retrieve neighbor keyframes in covisibility graph
	    int nn = 10;// 双目/深度 共视帧 数量
	    if(mbMonocular)
		nn=20;//单目
		
// 步骤1：在当前关键帧的 共视关键帧