ORB-SLAM2 --- Tracking::TrackLocalMap函数

目录

1.函数作用

2.函数步骤 

3.code 

4.函数解释 

ORB-SLAM2 --- Tracking::UpdateLocalKeyFrames函数https://blog.csdn.net/qq_41694024/article/details/128307627

1.函数作用

        在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿。
        前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化。

2.函数步骤 

@brief 用局部地图进行跟踪，进一步优化位姿
1. 更新局部地图，包括局部关键帧和关键点
2. 对局部MapPoints进行投影匹配
3. 根据匹配对估计当前帧的姿态
4. 根据姿态剔除误匹配
@return true if success
Step 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints 
Step 2：在局部地图中查找与当前帧匹配的MapPoints， 其实也就是对局部地图点进行跟踪
Step 3：更新局部所有MapPoints后对位姿再次优化
Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
Step 5：决定是否跟踪成功

3.code 

   

    

     

      

     
     

      

       bool Tracking::TrackLocalMap()
      
     
    

     

      

     
     

      

       {
      
     
    

     

      

     
     

      
    
       // We have an estimation of the camera pose and some map points tracked in the frame.
      
     
    

     

      

     
     

      
    
       // We retrieve the local map and try to find matches to points in the local map.
      
     
    

     

      

     
     

      
 
      
     
    

     

      

     
     

      
    
       // Update Local KeyFrames and Local Points
      
     
    

     

      

     
     

      
    
       // Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
      
     
    

     

      

     
     

      
    
       UpdateLocalMap();
      
     
    

     

      

     
     

      
 
      
     
    

     

      

     
     

      
    
       // Step 2：筛选局部地图中新增的在视野范围内的地图点，投影到当前帧搜索匹配，得到更多的匹配关系
      
     
    

     

      

     
     

      
    
       SearchLocalPoints();
      
     
    

     

      

     
     

      
 
      
     
    

     

      

     
     

      
    
       // Optimize Pose
      
     
    

     

      

     
     

      
    
       // 在这个函数之前，在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿优化，
      
     
    

     

      

     
     

      
    
       // Step 3：前面新增了更多的匹配关系，BA优化得到更准确的位姿
      
     
    

     

      

     
     

      

           Optimizer::
       PoseOptimization(&mCurrentFrame);
      
     
    

     

      

     
     

      

           mnMatchesInliers = 
       0;
      
     
    

     

      

     
     

      
 
      
     
    

     

      

     
     

      
    
       // Update MapPoints Statistics
      
     
    

     

      

     
     

      
    
       // Step 4：更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目
      
     
    

     

      

     
     

      
    
       for(
       int i=
       0; i<mCurrentFrame.N; i++)
      
     
    

     

      

     
     

      

           {
      
     
    

     

      

     
     

      
        
       if(mCurrentFrame.mvpMapPoints[i])
      
     
    

     

      

     
     

      

               {
      
     
    

     

      

     
     

      
            
       // 由于当前帧的地图点可以被当前帧观测到，其被观测统计量加1
      
     
    

     

      

     
     

      
            
       if(!mCurrentFrame.mvbOutlier[i])
      
     
    

     

      

     
     

      

                   {
      
     
    

     

      

     
     

      
                
       // 找到该点的帧数mnFound 加 1
      
     
    

     

      

     
     

      

                       mCurrentFrame.mvpMapPoints[i]->
       IncreaseFound();
      
     
    

     

      

     
     

      
                
       //查看当前是否是在纯定位过程
      
     
    

     

      

     
     

      
                
       if(!mbOnlyTracking)
      
     
    

     

      

     
     

      

                       {
      
     
    

     

      

     
     

      
                    
       // 如果该地图点被相机观测数目nObs大于0，匹配内点计数+1
      
     
    

     

      

     
     

      
                    
       // nObs： 被观测到的相机数目，单目+1，双目或RGB-D则+2
      
     
    

     

      

     
     

      
                    
       if(mCurrentFrame.mvpMapPoints[i]->
       Observations()>
       0)
      
     
    

     

      

     
     

      

                               mnMatchesInliers++;
      
     
    

     

      

     
     

      

                       }
      
     
    

     

      

     
     

      
                
       else
      
     
    

     

      

     
     

      
                    
       // 记录当前帧跟踪到的地图点数目，用于统计跟踪效果
      
     
    

     

      

     
     

      

                           mnMatchesInliers++;
      
     
    

     

      

     
     

      

                   }
      
     
    

     

      

     
     

      
            
       // 如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点
      
     
    

     

      

     
     

      
            
       // ?单目就不管吗
      
     
    

     

      

     
     

      
            
       else 
       if(mSensor==System::STEREO)  
      
     
    

     

      

     
     

      

                       mCurrentFrame.mvpMapPoints[i] = 
       static_cast<MapPoint*>(
       NULL);
      
     
    

     

      

     
     

      
 
      
     
    

     

      

     
     

      

               }
      
     
    

     

      

     
     

      

           }
      
     
    

     

      

     
     

      
 
      
     
    

     

      

     
     

      
    
       // Decide if the tracking was succesful
      
     
    

     

      

     
     

      
    
       // More restrictive if there was a relocalization recently
      
     
    

     

      

     
     

      
    
       // Step 5：根据跟踪匹配数目及重定位情况决定是否跟踪成功
      
     
    

     

      

     
     

      
    
       // 如果最近刚刚发生了重定位,那么至少成功匹配50个点才认为是成功跟踪
      
     
    

     

      

     
     

      
    
       if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<
       50)
      
     
    

     

      

     
     

      
        
       return 
       false;
      
     
    

     

      

     
     

      
 
      
     
    

     

      

     
     

      
    
       //如果是正常的状态话只要跟踪的地图点大于30个就认为成功了
      
     
    

     

      

     
     

      
    
       if(mnMatchesInliers<
       30)
      
     
    

     

      

     
     

      
        
       return 
       false;
      
     
    

     

      

     
     

      
    
       else
      
     
    

     

      

     
     

      
        
       return 
       true;
      
     
    

     

      

     
     

      

       }
      
     
   
  

   
  

4.函数解释 

        首先更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints：

ORB-SLAM2 --- Tracking::UpdateLocalKeyFrames函数https://blog.csdn.net/qq_41694024/article/details/128307627

ORB-SLAM2 --- Tracking::UpdateLocalPoints函数https://blog.csdn.net/qq_41694024/article/details/128321282ORB-SLAM2 --- Tracking::SearchLocalPoints函数解析https://blog.csdn.net/qq_41694024/article/details/128334584        根据先前得到的更多的匹配关系，优化当前帧的位姿。

        更新当前帧的地图点被观测程度，并统计跟踪局部地图后匹配数目。

        最后判断匹配的地图点是否足够。