• ORB-SLAM2从理论到代码实现（九）：LocalMapping程序详解

    1. 流程图

    LocalMapping线程负责对新加入的KeyFrames和MapPoints筛选融合，剔除冗余的KeyFrames和MapPoints，维护稳定的KeyFrame集合，传给后续的LoopClosing线程。
    主要的功能点在：

    处理新的关键帧ProcessNewKeyFrame()

    剔除不合格地图点MapPointCulling()

    三角化恢复新地图点CreateNewMapPoints()

    融合当前帧与相邻帧重复的地图点SearchInNeighbors()

    局部BA优化LocalBundleAdjustment()

    剔除冗余关键帧KeyFrameCulling()

     

    2. 入口函数LocalMapping::Run()

    入口函数是LocalMapping.cc中的Run()函数，Run()函数也相当于LocalMapping的主函数。这个线程在系统运行起来的时候处于休眠或者等待状态。当有新的关键帧加入的时候线程就将自己设置为繁忙状态（告诉Tracking线程我很忙，暂时不接受新的关键帧）并立刻处理新的关键帧；当处理完一个关键帧后，就会将自己设置为空闲状态（告诉Tracking线程，我可以接受新的关键帧了）并进入睡眠状态3毫秒。当代码如下：

    1. void LocalMapping::Run()
    2. {
    3.  
    4.     mbFinished = false;
    5.  
    6.     while(1)
    7.     {
    8.         // Tracking will see that Local Mapping is busy
    9.         // 告诉Tracking，LocalMapping正处于繁忙状态，
    10.         // LocalMapping线程处理的关键帧都是Tracking线程发过的
    11.         // 在LocalMapping线程还没有处理完关键帧之前Tracking线程最好不要发送太快
    12.         SetAcceptKeyFrames(false);
    13.  
    14.         // Check if there are keyframes in the queue
    15.         // 等待处理的关键帧列表不为空
    16.         if(CheckNewKeyFrames())
    17.         {
    18.             // BoW conversion and insertion in Map
    19.             ProcessNewKeyFrame();
    20.  
    21.             // Check recent MapPoints
    22.             // 剔除ProcessNewKeyFrame函数中引入的不合格MapPoints
    23.             MapPointCulling();
    24.  
    25.             // Triangulate new MapPoints
    26.             // 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints
    27.             CreateNewMapPoints();
    28.  
    29.             // 已经处理完队列中的最后的一个关键帧
    30.             if(!CheckNewKeyFrames())
    31.             {
    32.                 // Find more matches in neighbor keyframes and fuse point duplications
    33.                 // 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
    34.                 SearchInNeighbors();
    35.             }
    36.  
    37.             mbAbortBA = false;
    38.  
    39.             if(!CheckNewKeyFrames() && !stopRequested())
    40.             {
    41.                 // Local BA
    42.                 if(mpMap->KeyFramesInMap()>2)
    43.                     Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
    44.  
    45.                 // Check redundant local Keyframes
    46.                 // 检测并剔除当前帧相邻的关键帧中冗余的关键帧
    47.                 // 剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到
    48.                 // trick!
    49.                 // Tracking中先把关键帧交给LocalMapping线程
    50.                 // 并且在Tracking中InsertKeyFrame函数的条件比较松，交给LocalMapping线程的关键帧会比较密
    51.                 // 在这里再删除冗余的关键帧
    52.                 KeyFrameCulling();
    53.             }
    54.  
    55.             // 将当前帧加入到闭环检测队列中
    56.             mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
    57.         }
    58.         else if(Stop())
    59.         {
    60.             // Safe area to stop
    61.             while(isStopped() && !CheckFinish())
    62.             {
    63.                 usleep(3000);
    64.             }
    65.             if(CheckFinish())
    66.                 break;
    67.         }
    68.  
    69.         ResetIfRequested();
    70.  
    71.         // Tracking will see that Local Mapping is busy
    72.         SetAcceptKeyFrames(true);
    73.  
    74.         if(CheckFinish())
    75.             break;
    76.  
    77.         usleep(3000);
    78.     }
    79.  
    80.     SetFinish();
    81. }

    3. void LocalMapping::SetAcceptKeyFrames()

    告诉Tracking线程，LocalMapping是否处于繁忙状态。如果处于繁忙状态则不要再添加新的关键帧了，否则可以添加关键帧。

    1. void LocalMapping::SetAcceptKeyFrames(bool flag)
    2. {
    3.     unique_lock lock(mMutexAccept);
    4.     mbAcceptKeyFrames=flag;
    5. }

    4. void LocalMapping::ProcessNewKeyFrame()

    作用是，从队列取出第一个关键帧（该关键帧时队列中按时间上最旧的关键帧），计算该关键帧的Bow特征，更新关键帧观测到的地图点的信息，并且将该关键帧新生成的地图点添加进mlpRecentAddedMapPoints，等待后续检测。然后将该关键帧插入地图。

    1. void LocalMapping::ProcessNewKeyFrame()
    2. {
    3.     {
    4.         unique_lock lock(mMutexNewKFs);
    5.         mpCurrentKeyFrame = mlNewKeyFrames.front();
    6.         mlNewKeyFrames.pop_front();
    7.     }
    8.  
    9.     // Compute Bags of Words structures
    10.     mpCurrentKeyFrame->ComputeBoW();
    11.  
    12.     // Associate MapPoints to the new keyframe and update normal and descriptor
    13.     const vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    14.  
    15.     for(size_t i=0; isize(); i++)
    16.     {
    17.         MapPoint* pMP = vpMapPointMatches[i];
    18.         if(pMP)
    19.         {
    20.             if(!pMP->isBad())
    21.             {
    22.                 // 如果该地图点没有记录该帧，则添加上这个记录。
    23.                 if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
    24.                 {
    25.                     // i记录了地图点在关键帧中的索引
    26.                     pMP->AddObservation(mpCurrentKeyFrame, i);
    27.                     // 因为地图点增加了新的观测，而法向量是所有观测到该点的关键帧都求一个法徽号向量之后求均，所以需要更新             
    28.                     pMP->UpdateNormalAndDepth();
    29.                     // 从所有关键帧的观测点中选择一个作为该点的描述子
    30.                     pMP->ComputeDistinctiveDescriptors();
    31.                 }
    32.                 else // this can only happen for new stereo points inserted by the Tracking
    33.                 {
    34.                     mlpRecentAddedMapPoints.push_back(pMP);
    35.                 }
    36.             }
    37.         }
    38.     }    
    39.  
    40.     // Update links in the Covisibility Graph
    41.     mpCurrentKeyFrame->UpdateConnections();
    42.  
    43.     // Insert Keyframe in Map
    44.     mpMap->AddKeyFrame(mpCurrentKeyFrame);
    45. }

    5. void LocalMapping::MapPointCulling()

    主要作用是筛选mlpRecentAddedMapPoints里的点，对于不好的点标记为bad。

    已经是坏点的MapPoints直接从检查链表中删除；
    跟踪到该MapPoint的Frame中被判定为内点的比例须大于25%，注意不一定是关键帧。
    从该点建立开始，到现在已经过了不小于2个关键帧，但是观测到该点的关键帧数却不超过cnThObs帧，那么该点检验不合格。
    从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点，因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测mlpRecentAddedMapPoints剩下的点，需要继续经过以后的检测。

    1. void LocalMapping::MapPointCulling()
    2. {
    3.     // Check Recent Added MapPoints
    4.     list::iterator lit = mlpRecentAddedMapPoints.begin();
    5.     const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
    6.  
    7.     int nThObs;
    8.     if(mbMonocular)
    9.         nThObs = 2;
    10.     else
    11.         nThObs = 3;
    12.     const int cnThObs = nThObs;
    13.  
    14.     while(lit!=mlpRecentAddedMapPoints.end())
    15.     {
    16.         MapPoint* pMP = *lit;
    17.         if(pMP->isBad())
    18.         {
    19.             lit = mlpRecentAddedMapPoints.erase(lit);
    20.         }
    21.         else if(pMP->GetFoundRatio()<0.25f )
    22.         {
    23.             pMP->SetBadFlag();
    24.             lit = mlpRecentAddedMapPoints.erase(lit);
    25.         }
    26.         else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
    27.         {
    28.             pMP->SetBadFlag();
    29.             lit = mlpRecentAddedMapPoints.erase(lit);
    30.         }
    31.         else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
    32.             lit = mlpRecentAddedMapPoints.erase(lit);
    33.         else
    34.             lit++;
    35.     }
    36. }
    37.

    5. void LocalMapping::CreateNewMapPoints()

    利用三角化新建一些地图点

    在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
    遍历相邻关键帧vpNeighKFs，得到基线向量vBaseline = Ow2-Ow1
    判断相机运动的基线是不是足够长，邻接关键帧的场景深度中值medianDepthKF2，baseline与景深的比例，如果特别远(比例特别小)，那么不考虑当前邻接的关键帧，不生成3D点
    根据两个关键帧的位姿计算它们之间的基本矩阵F
    通过极线约束限制匹配时的搜索范围，对满足对极约束的特征点进行特征点匹配
    对每对匹配通过三角化生成3D点,和Triangulate函数差不多
    接着分别检查新得到的点在两个平面上的重投影误差，如果大于一定的值，直接抛弃该点。
    检查尺度连续性
    如果满足对极约束则建立当前帧的地图点及其属性（a.观测到该MapPoint的关键帧 b.该MapPoint的描述子 c.该MapPoint的平均观测方向和深度范围）
    将地图点加入关键帧，加入全局map

    1. void LocalMapping::CreateNewMapPoints()
    2. {
    3.   // Retrieve neighbor keyframes in covisibility graph
    4.   int nn = 10;
    5.   if(mbMonocular)
    6.     nn=20;
    7.   //在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
    8.   const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
    9.  
    10.   ORBmatcher matcher(0.6,false);
    11.  
    12.   cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
    13.   cv::Mat Rwc1 = Rcw1.t();
    14.   cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
    15.   cv::Mat Tcw1(3,4,CV_32F);
    16.   Rcw1.copyTo(Tcw1.colRange(0,3));
    17.   tcw1.copyTo(Tcw1.col(3));
    18.   cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
    19.  
    20.   const float &fx1 = mpCurrentKeyFrame->fx;
    21.   const float &fy1 = mpCurrentKeyFrame->fy;
    22.   const float &cx1 = mpCurrentKeyFrame->cx;
    23.   const float &cy1 = mpCurrentKeyFrame->cy;
    24.   const float &invfx1 = mpCurrentKeyFrame->invfx;
    25.   const float &invfy1 = mpCurrentKeyFrame->invfy;
    26.  
    27.   const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
    28.  
    29.   int nnew=0;
    30.  
    31.   // Search matches with epipolar restriction and triangulate
    32.   for(size_t i=0; isize(); i++)
    33.   {
    34.     // 基于实时性考虑，如果已经处理了与新关键帧最邻近的一个关键帧，但又来了更新的关键帧，则停止立即返回，去处理更新的关键帧。
    35.     if(i>0 && CheckNewKeyFrames())
    36.       return;
    37.  
    38.     KeyFrame* pKF2 = vpNeighKFs[i];
    39.  
    40.     // Check first that baseline is not too short
    41.     cv::Mat Ow2 = pKF2->GetCameraCenter();
    42.     cv::Mat vBaseline = Ow2-Ow1;
    43.     const float baseline = cv::norm(vBaseline);
    44.  
    45.     if(!mbMonocular)
    46.     {
    47.       if(baselinemb)
    48.         continue;
    49.     }
    50.     else
    51.     {
    52.       const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
    53.       const float ratioBaselineDepth = baseline/medianDepthKF2;
    54.  
    55.       if(ratioBaselineDepth<0.01)
    56.         continue;
    57.     }
    58.  
    59.     // Compute Fundamental Matrix
    60.     //根据两个关键帧的位姿计算它们之间的基本矩阵F
    61.     cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
    62.  
    63.     // Search matches that fullfil epipolar constraint
    64.     vectorsize_t,size_t> > vMatchedIndices;
    65.     matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);
    66.  
    67.     cv::Mat Rcw2 = pKF2->GetRotation();
    68.     cv::Mat Rwc2 = Rcw2.t();
    69.     cv::Mat tcw2 = pKF2->GetTranslation();
    70.     cv::Mat Tcw2(3,4,CV_32F);
    71.     Rcw2.copyTo(Tcw2.colRange(0,3));
    72.     tcw2.copyTo(Tcw2.col(3));
    73.  
    74.     const float &fx2 = pKF2->fx;
    75.     const float &fy2 = pKF2->fy;
    76.     const float &cx2 = pKF2->cx;
    77.     const float &cy2 = pKF2->cy;
    78.     const float &invfx2 = pKF2->invfx;
    79.     const float &invfy2 = pKF2->invfy;
    80.  
    81.     // Triangulate each match
    82.     const int nmatches = vMatchedIndices.size();
    83.     for(int ikp=0; ikp
    84.     {
    85.       const int &idx1 = vMatchedIndices[ikp].first;
    86.       const int &idx2 = vMatchedIndices[ikp].second;
    87.  
    88.       const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
    89.       const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
    90.       bool bStereo1 = kp1_ur>=0; // 在上一帧中被双目观测到
    91.  
    92.       const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
    93.       const float kp2_ur = pKF2->mvuRight[idx2];
    94.       bool bStereo2 = kp2_ur>=0; // 在当前帧中被双目观测到
    95.  
    96.       // Check parallax between rays
    97.       cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
    98.       cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);
    99.  
    100.       cv::Mat ray1 = Rwc1*xn1;
    101.       cv::Mat ray2 = Rwc2*xn2;
    102.       const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
    103.  
    104.       float cosParallaxStereo = cosParallaxRays+1;
    105.       float cosParallaxStereo1 = cosParallaxStereo;
    106.       float cosParallaxStereo2 = cosParallaxStereo;
    107.  
    108.       if(bStereo1)
    109.         cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
    110.       else if(bStereo2)
    111.         cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
    112.  
    113.       cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
    114.  
    115.       cv::Mat x3D;
    116.       if(cosParallaxRays0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
    117.       {
    118.         // Linear Triangulation Method
    119.         cv::Mat A(4,4,CV_32F);
    120.         A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
    121.         A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
    122.         A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
    123.         A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
    124.  
    125.         cv::Mat w,u,vt;
    126.         cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    127.  
    128.         x3D = vt.row(3).t();
    129.  
    130.         if(x3D.at<float>(3)==0)
    131.           continue;
    132.  
    133.         // Euclidean coordinates
    134.         x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
    135.  
    136.       }
    137.       else if(bStereo1 && cosParallaxStereo1
    138.       {
    139.         x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);
    140.       }
    141.       else if(bStereo2 && cosParallaxStereo2
    142.       {
    143.         x3D = pKF2->UnprojectStereo(idx2);
    144.       }
    145.       else
    146.         continue; //No stereo and very low parallax
    147.  
    148.       cv::Mat x3Dt = x3D.t();
    149.  
    150.       //Check triangulation in front of cameras
    151.       float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
    152.       if(z1<=0)
    153.         continue;
    154.  
    155.       float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
    156.       if(z2<=0)
    157.         continue;
    158.  
    159.       //Check reprojection error in first keyframe
    160.       const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
    161.       const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
    162.       const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
    163.       const float invz1 = 1.0/z1;
    164.  
    165.       if(!bStereo1)
    166.       {
    167.         float u1 = fx1*x1*invz1+cx1;
    168.         float v1 = fy1*y1*invz1+cy1;
    169.         float errX1 = u1 - kp1.pt.x;
    170.         float errY1 = v1 - kp1.pt.y;
    171.         if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
    172.           continue;
    173.       }
    174.       else
    175.       {
    176.         float u1 = fx1*x1*invz1+cx1;
    177.         float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;
    178.         float v1 = fy1*y1*invz1+cy1;
    179.         float errX1 = u1 - kp1.pt.x;
    180.         float errY1 = v1 - kp1.pt.y;
    181.         float errX1_r = u1_r - kp1_ur;
    182.         if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
    183.           continue;
    184.       }
    185.  
    186.       //Check reprojection error in second keyframe
    187.       const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
    188.       const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
    189.       const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
    190.       const float invz2 = 1.0/z2;
    191.       if(!bStereo2)
    192.       {
    193.         float u2 = fx2*x2*invz2+cx2;
    194.         float v2 = fy2*y2*invz2+cy2;
    195.         float errX2 = u2 - kp2.pt.x;
    196.         float errY2 = v2 - kp2.pt.y;
    197.         if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
    198.           continue;
    199.       }
    200.       else
    201.       {
    202.         float u2 = fx2*x2*invz2+cx2;
    203.         float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;
    204.         float v2 = fy2*y2*invz2+cy2;
    205.         float errX2 = u2 - kp2.pt.x;
    206.         float errY2 = v2 - kp2.pt.y;
    207.         float errX2_r = u2_r - kp2_ur;
    208.         if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
    209.           continue;
    210.       }
    211.  
    212.       //Check scale consistency
    213.       cv::Mat normal1 = x3D-Ow1;
    214.       float dist1 = cv::norm(normal1);
    215.  
    216.       cv::Mat normal2 = x3D-Ow2;
    217.       float dist2 = cv::norm(normal2);
    218.  
    219.       if(dist1==0 || dist2==0)
    220.         continue;
    221.  
    222.       const float ratioDist = dist2/dist1;
    223.       const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];
    224.  
    225.       /*if(fabs(ratioDist-ratioOctave)>ratioFactor)
    226.           continue;*/
    227.       if(ratioDist*ratioFactorratioOctave*ratioFactor)
    228.         continue;
    229.  
    230.       // Triangulation is succesfull
    231.       MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
    232.  
    233.       pMP->AddObservation(mpCurrentKeyFrame,idx1);
    234.       pMP->AddObservation(pKF2,idx2);
    235.  
    236.       mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
    237.       pKF2->AddMapPoint(pMP,idx2);
    238.  
    239.       pMP->ComputeDistinctiveDescriptors();
    240.  
    241.       pMP->UpdateNormalAndDepth();
    242.  
    243.       mpMap->AddMapPoint(pMP);
    244.       mlpRecentAddedMapPoints.push_back(pMP);
    245.  
    246.       nnew++;
    247.     }
    248.   }
    249. }

    6. void LocalMapping::SearchInNeighbors().

    检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints，更新当前关键帧的连接关系。

    获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧，找到当前帧一级相邻与二级相邻关键帧
    将当前帧的MapPoints分别与一级二级相邻帧(的MapPoints)进行融合
    matcher.Fuse(pKFi,vpMapPointMatches);
    投影当前帧的MapPoints到相邻关键帧pKFi中，并判断是否有重复的MapPoints
    如果MapPoint能匹配关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并（选择观测数多的）
    如果MapPoint能匹配关键帧的特征点，并且该点没有对应的MapPoint，那么为该点添加MapPoint
    将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合
    更新当前帧MapPoints的描述子，深度，观测主方向等属性
    在这里插入代码片
    更新当前帧的MapPoints后更新与其它帧的连接关系

    1. void LocalMapping::SearchInNeighbors()
    2. {
    3.     // Retrieve neighbor keyframes
    4.     int nn = 10;
    5.     if(mbMonocular)
    6.         nn=20;
    7.     const vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
    8.     vector vpTargetKFs;
    9.     for(vector::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
    10.     {
    11.         KeyFrame* pKFi = *vit;
    12.         if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
    13.             continue;
    14.         vpTargetKFs.push_back(pKFi);
    15.         pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;
    16.  
    17.         // Extend to some second neighbors
    18.         const vector vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
    19.         for(vector::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
    20.         {
    21.             KeyFrame* pKFi2 = *vit2;
    22.             if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
    23.                 continue;
    24.             vpTargetKFs.push_back(pKFi2);
    25.         }
    26.     }
    27.  
    28.  
    29.     // Search matches by projection from current KF in target KFs
    30.     ORBmatcher matcher;
    31.     vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    32.     for(vector::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
    33.     {
    34.         KeyFrame* pKFi = *vit;
    35.  
    36.         matcher.Fuse(pKFi,vpMapPointMatches);
    37.     }
    38.  
    39.     // Search matches by projection from target KFs in current KF
    40.     vector vpFuseCandidates;
    41.     vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());
    42.  
    43.     for(vector::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
    44.     {
    45.         KeyFrame* pKFi = *vitKF;
    46.  
    47.         vector vpMapPointsKFi = pKFi->GetMapPointMatches();
    48.  
    49.         for(vector::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
    50.         {
    51.             MapPoint* pMP = *vitMP;
    52.             if(!pMP)
    53.                 continue;
    54.             if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
    55.                 continue;
    56.             pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
    57.             vpFuseCandidates.push_back(pMP);
    58.         }
    59.     }
    60.  
    61.     matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
    62.  
    63.  
    64.     // Update points
    65.     vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    66.     for(size_t i=0, iend=vpMapPointMatches.size(); i
    67.     {
    68.         MapPoint* pMP=vpMapPointMatches[i];
    69.         if(pMP)
    70.         {
    71.             if(!pMP->isBad())
    72.             {
    73.                 pMP->ComputeDistinctiveDescriptors();
    74.                 pMP->UpdateNormalAndDepth();
    75.             }
    76.         }
    77.     }
    78.  
    79.     // Update connections in covisibility graph
    80.     mpCurrentKeyFrame->UpdateConnections();
    81. }
    82.

    7. void Optimizer::LocalBundleAdjustment()

    优化的顶点是包括局部地图帧的位姿，概念lLocalKeyFrames，指的是当前关键帧和其相连接的关键帧组成的集合。还包括这些关键帧可以观测到的所有地图点，地图点的位置也会优化。还有一些帧，这些帧能够观测到这些地图点，但却不是局部地图里，这些帧的位姿也作为顶点添加进图中，但是却固定不动，不会被优化。

    1. void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
    2. {    
    3.     // Local KeyFrames: First Breath Search from Current Keyframe
    4.     list lLocalKeyFrames;
    5.  
    6.     lLocalKeyFrames.push_back(pKF);
    7.     pKF->mnBALocalForKF = pKF->mnId;
    8.  
    9.     const vector vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
    10.     for(int i=0, iend=vNeighKFs.size(); i
    11.     {
    12.         KeyFrame* pKFi = vNeighKFs[i];
    13.         pKFi->mnBALocalForKF = pKF->mnId;
    14.         if(!pKFi->isBad())
    15.             lLocalKeyFrames.push_back(pKFi);
    16.     }
    17.  
    18.     // Local MapPoints seen in Local KeyFrames
    19.     list lLocalMapPoints;
    20.     for(list::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    21.     {
    22.         vector vpMPs = (*lit)->GetMapPointMatches();
    23.         for(vector::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
    24.         {
    25.             MapPoint* pMP = *vit;
    26.             if(pMP)
    27.                 if(!pMP->isBad())
    28.                     if(pMP->mnBALocalForKF!=pKF->mnId)
    29.                     {
    30.                         lLocalMapPoints.push_back(pMP);
    31.                         pMP->mnBALocalForKF=pKF->mnId;
    32.                     }
    33.         }
    34.     }
    35.  
    36.     // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
    37.     list lFixedCameras;
    38.     for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    39.     {
    40.         mapsize_t> observations = (*lit)->GetObservations();
    41.         for(mapsize_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    42.         {
    43.             KeyFrame* pKFi = mit->first;
    44.  
    45.             if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
    46.             {                
    47.                 pKFi->mnBAFixedForKF=pKF->mnId;
    48.                 if(!pKFi->isBad())
    49.                     lFixedCameras.push_back(pKFi);
    50.             }
    51.         }
    52.     }
    53.  
    54.     // Setup optimizer
    55.     g2o::SparseOptimizer optimizer;
    56.     g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    57.  
    58.     linearSolver = new g2o::LinearSolverEigen();
    59.  
    60.     g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    61.  
    62.     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    63.     optimizer.setAlgorithm(solver);
    64.  
    65.     if(pbStopFlag)
    66.         optimizer.setForceStopFlag(pbStopFlag);
    67.  
    68.     unsigned long maxKFid = 0;
    69.  
    70.     // Set Local KeyFrame vertices
    71.     for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    72.     {
    73.         KeyFrame* pKFi = *lit;
    74.         g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    75.         vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
    76.         vSE3->setId(pKFi->mnId);
    77.         vSE3->setFixed(pKFi->mnId==0);
    78.         optimizer.addVertex(vSE3);
    79.         if(pKFi->mnId>maxKFid)
    80.             maxKFid=pKFi->mnId;
    81.     }
    82.  
    83.     // Set Fixed KeyFrame vertices
    84.     for(list::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
    85.     {
    86.         KeyFrame* pKFi = *lit;
    87.         g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    88.         vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
    89.         vSE3->setId(pKFi->mnId);
    90.         vSE3->setFixed(true);
    91.         optimizer.addVertex(vSE3);
    92.         if(pKFi->mnId>maxKFid)
    93.             maxKFid=pKFi->mnId;
    94.     }
    95.  
    96.     // Set MapPoint vertices
    97.     const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
    98.  
    99.     vector vpEdgesMono;
    100.     vpEdgesMono.reserve(nExpectedSize);
    101.  
    102.     vector vpEdgeKFMono;
    103.     vpEdgeKFMono.reserve(nExpectedSize);
    104.  
    105.     vector vpMapPointEdgeMono;
    106.     vpMapPointEdgeMono.reserve(nExpectedSize);
    107.  
    108.     vector vpEdgesStereo;
    109.     vpEdgesStereo.reserve(nExpectedSize);
    110.  
    111.     vector vpEdgeKFStereo;
    112.     vpEdgeKFStereo.reserve(nExpectedSize);
    113.  
    114.     vector vpMapPointEdgeStereo;
    115.     vpMapPointEdgeStereo.reserve(nExpectedSize);
    116.  
    117.     const float thHuberMono = sqrt(5.991);
    118.     const float thHuberStereo = sqrt(7.815);
    119.  
    120.     for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    121.     {
    122.         MapPoint* pMP = *lit;
    123.         g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
    124.         vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
    125.         int id = pMP->mnId+maxKFid+1;
    126.         vPoint->setId(id);
    127.         vPoint->setMarginalized(true);
    128.         optimizer.addVertex(vPoint);
    129.  
    130.         const mapsize_t> observations = pMP->GetObservations();
    131.  
    132.         //Set edges
    133.         for(mapsize_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    134.         {
    135.             KeyFrame* pKFi = mit->first;
    136.  
    137.             if(!pKFi->isBad())
    138.             {                
    139.                 const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
    140.  
    141.                 // Monocular observation
    142.                 if(pKFi->mvuRight[mit->second]<0)
    143.                 {
    144.                     Eigen::Matrix<double,2,1> obs;
    145.                     obs << kpUn.pt.x, kpUn.pt.y;
    146.  
    147.                     g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
    148.  
    149.                     e->setVertex(0, dynamic_cast(optimizer.vertex(id)));
    150.                     e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId)));
    151.                     e->setMeasurement(obs);
    152.                     const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
    153.                     e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
    154.  
    155.                     g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
    156.                     e->setRobustKernel(rk);
    157.                     rk->setDelta(thHuberMono);
    158.  
    159.                     e->fx = pKFi->fx;
    160.                     e->fy = pKFi->fy;
    161.                     e->cx = pKFi->cx;
    162.                     e->cy = pKFi->cy;
    163.  
    164.                     optimizer.addEdge(e);
    165.                     vpEdgesMono.push_back(e);
    166.                     vpEdgeKFMono.push_back(pKFi);
    167.                     vpMapPointEdgeMono.push_back(pMP);
    168.                 }
    169.                 else // Stereo observation
    170.                 {
    171.                     Eigen::Matrix<double,3,1> obs;
    172.                     const float kp_ur = pKFi->mvuRight[mit->second];
    173.                     obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
    174.  
    175.                     g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
    176.  
    177.                     e->setVertex(0, dynamic_cast(optimizer.vertex(id)));
    178.                     e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId)));
    179.                     e->setMeasurement(obs);
    180.                     const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
    181.                     Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
    182.                     e->setInformation(Info);
    183.  
    184.                     g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
    185.                     e->setRobustKernel(rk);
    186.                     rk->setDelta(thHuberStereo);
    187.  
    188.                     e->fx = pKFi->fx;
    189.                     e->fy = pKFi->fy;
    190.                     e->cx = pKFi->cx;
    191.                     e->cy = pKFi->cy;
    192.                     e->bf = pKFi->mbf;
    193.  
    194.                     optimizer.addEdge(e);
    195.                     vpEdgesStereo.push_back(e);
    196.                     vpEdgeKFStereo.push_back(pKFi);
    197.                     vpMapPointEdgeStereo.push_back(pMP);
    198.                 }
    199.             }
    200.         }
    201.     }
    202.  
    203.     if(pbStopFlag)
    204.         if(*pbStopFlag)
    205.             return;
    206.  
    207.     optimizer.initializeOptimization();
    208.     optimizer.optimize(5);
    209.  
    210.     bool bDoMore= true;
    211.  
    212.     if(pbStopFlag)
    213.         if(*pbStopFlag)
    214.             bDoMore = false;
    215.  
    216.     if(bDoMore)
    217.     {
    218.  
    219.     // Check inlier observations
    220.     for(size_t i=0, iend=vpEdgesMono.size(); i
    221.     {
    222.         g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    223.         MapPoint* pMP = vpMapPointEdgeMono[i];
    224.  
    225.         if(pMP->isBad())
    226.             continue;
    227.  
    228.         if(e->chi2()>5.991 || !e->isDepthPositive())
    229.         {
    230.             e->setLevel(1);
    231.         }
    232.  
    233.         e->setRobustKernel(0);
    234.     }
    235.  
    236.     for(size_t i=0, iend=vpEdgesStereo.size(); i
    237.     {
    238.         g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
    239.         MapPoint* pMP = vpMapPointEdgeStereo[i];
    240.  
    241.         if(pMP->isBad())
    242.             continue;
    243.  
    244.         if(e->chi2()>7.815 || !e->isDepthPositive())
    245.         {
    246.             e->setLevel(1);
    247.         }
    248.  
    249.         e->setRobustKernel(0);
    250.     }
    251.  
    252.     // Optimize again without the outliers
    253.  
    254.     optimizer.initializeOptimization(0);
    255.     optimizer.optimize(10);
    256.  
    257.     }
    258.  
    259.     vector > vToErase;
    260.     vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
    261.  
    262.     // Check inlier observations       
    263.     for(size_t i=0, iend=vpEdgesMono.size(); i
    264.     {
    265.         g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    266.         MapPoint* pMP = vpMapPointEdgeMono[i];
    267.  
    268.         if(pMP->isBad())
    269.             continue;
    270.  
    271.         if(e->chi2()>5.991 || !e->isDepthPositive())
    272.         {
    273.             KeyFrame* pKFi = vpEdgeKFMono[i];
    274.             vToErase.push_back(make_pair(pKFi,pMP));
    275.         }
    276.     }
    277.  
    278.     for(size_t i=0, iend=vpEdgesStereo.size(); i
    279.     {
    280.         g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
    281.         MapPoint* pMP = vpMapPointEdgeStereo[i];
    282.  
    283.         if(pMP->isBad())
    284.             continue;
    285.  
    286.         if(e->chi2()>7.815 || !e->isDepthPositive())
    287.         {
    288.             KeyFrame* pKFi = vpEdgeKFStereo[i];
    289.             vToErase.push_back(make_pair(pKFi,pMP));
    290.         }
    291.     }
    292.  
    293.     // Get Map Mutex
    294.     unique_lock lock(pMap->mMutexMapUpdate);
    295.  
    296.     if(!vToErase.empty())
    297.     {
    298.         for(size_t i=0;isize();i++)
    299.         {
    300.             KeyFrame* pKFi = vToErase[i].first;
    301.             MapPoint* pMPi = vToErase[i].second;
    302.             pKFi->EraseMapPointMatch(pMPi);
    303.             pMPi->EraseObservation(pKFi);
    304.         }
    305.     }
    306.  
    307.     // Recover optimized data
    308.  
    309.     //Keyframes
    310.     for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    311.     {
    312.         KeyFrame* pKF = *lit;
    313.         g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKF->mnId));
    314.         g2o::SE3Quat SE3quat = vSE3->estimate();
    315.         pKF->SetPose(Converter::toCvMat(SE3quat));
    316.     }
    317.  
    318.     //Points
    319.     for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    320.     {
    321.         MapPoint* pMP = *lit;
    322.         g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+maxKFid+1));
    323.         pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
    324.         pMP->UpdateNormalAndDepth();
    325.     }
    326. }

    8. void LocalMapping::KeyFrameCulling()


    在Covisibility Graph，也就是局部地图中的关键帧，一个关键帧的90%以上的MapPoints能被其他关键帧（至少3个，这里的其他关键帧不特指当前的局部地图关键帧）观测到，则认为该关键帧为冗余关键帧。

    根据Covisibility Graph提取当前帧的共视关键帧
    对所有的局部关键帧进行遍历，提取每个共视关键帧的MapPoints
    遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
    该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧

    1. void LocalMapping::KeyFrameCulling()
    2. {
    3.     // Check redundant keyframes (only local keyframes)
    4.     // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
    5.     // in at least other 3 keyframes (in the same or finer scale)
    6.     // We only consider close stereo points
    7.     // 检查冗余关键帧。一个关键帧的地图点中90%的地图点可以被至少其他3个相同或者更小等级（这里的等级是图像金字塔的层数）关键帧看到，则认为这个关键帧是冗余的。
    8.     vector vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
    9.  
    10.     for(vector::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
    11.     {
    12.         KeyFrame* pKF = *vit;
    13.         if(pKF->mnId==0)
    14.             continue;
    15.         const vector vpMapPoints = pKF->GetMapPointMatches();
    16.  
    17.         int nObs = 3;
    18.         const int thObs=nObs;
    19.         int nRedundantObservations=0;
    20.         int nMPs=0;
    21.         for(size_t i=0, iend=vpMapPoints.size(); i
    22.         {
    23.             MapPoint* pMP = vpMapPoints[i];
    24.             if(pMP)
    25.             {
    26.                 if(!pMP->isBad())
    27.                 {
    28.                     if(!mbMonocular)
    29.                     {
    30.                         if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
    31.                             continue;
    32.                     }
    33.                     //MapPoint计数
    34.                     nMPs++;
    35.                     //该pMP是否可以被大于3个的关键帧看到
    36.                     if(pMP->Observations()>thObs)
    37.                     {
    38.                         const int &scaleLevel = pKF->mvKeysUn[i].octave;
    39.                         const mapsize_t> observations = pMP->GetObservations();
    40.                         int nObs=0;
    41.                         for(mapsize_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    42.                         {
    43.                             KeyFrame* pKFi = mit->first;
    44.                             if(pKFi==pKF)
    45.                                 continue;
    46.                             // 获取关键点在金字塔图像中所处的层数
    47.                             const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
    48.                             const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
    49.                             //pKFi的关键点所处的层数<=scaleLevel+1
    50.                             //为什么要用层数来判断呢？还没有想明白
    51.                             if(scaleLeveli<=scaleLevel+1)
    52.                             {
    53.                                 //共视关键帧计数
    54.                                 nObs++;
    55.                                 if(nObs>=thObs)
    56.                                     break;
    57.                             }
    58.                         }
    59.                         //共视关键帧大于等于3判断
    60.                         if(nObs>=thObs)
    61.                         {
    62.                             nRedundantObservations++;
    63.                         }
    64.                     }
    65.                 }
    66.             }
    67.         }  
    68.  
    69.         if(nRedundantObservations>0.9*nMPs)
    70.             //关键帧设置为bag，即被认为是冗余关键帧
    71.             pKF->SetBadFlag();
    72.     }
    73. }
    74.

    参考文献

    主要内容来自下文，重写了一说说明，添加了一些注释

    读懂ORBSLAM2——LocalMapping线程_令狐少侠、的博客-CSDN博客

  • 相关阅读:
    vue-router的基本用法
    Camera2 OpenCamera流程
    【欧拉函数】CF1731E
    基于springboot实现校园在线拍卖系统项目【项目源码】计算机毕业设计
    教程更新 | RK3568驱动指南第六篇-平台总线
    HTML5期末考核大作业，电影网站——橙色国外电影 web期末作业设计网页
    python基于django学生成绩管理系统o8mkp
    numpy 和 tensorflow 中的各种乘法（点乘和矩阵乘）
    朝阳药品数据分析案例
    Mybatis-Plus 条件构造器Wrapper
  • 原文地址：https://blog.csdn.net/xhtchina/article/details/126678378