ORB_SLAM之localmapping线程

tracking线程只是判断当前帧是否需要加入关键帧，并没有真的加入地图，因为tracking线程的功能是局部定位，而处理地图中的关键帧、地图点如何加入，如何删除的工作是在localmapping线程完成。
认清几个锁与布尔参数
线程的通信与相互影响在ORB比较复杂，需要好好缕清思路。
mbStopRequested,由RequestStop函数设定，主要是在回环线程里，在运行全局优化时，以及检测后，调整Loop时，localMapping需要暂停。
mbStopped：由stop()函数设定。mbNotStop: 由SetNotStop()设定。 这两个真是把人搞得头大。在Tracking线程，如果mapping线程被要求暂停，也就是mpLocalMapper->isStopped() || mpLocalMapper->stopRequested() 为真，则停止往里添加关键帧。如果可以添加关键帧，则Tracking线程添加关键帧时，必须保证LocaMapping不能被终止，则mbNotStop需要为真。
mpLocalMapper->Release(); 此函数在全局优化以及调整Loop后使用，目的是使mbStopped，mbStopRequested都置为false。且清空 mlNewKeyFrames。
mMutexStop锁，主要是在设置以上两个参数时使用，防止同时更改。
mMutexAccept锁，也是防止在设置mbAcceptKeyFrames时保持异步。mMutexNewKFs也是同样的原理。
首先贴出三位博主链接：
localmapping2线程流程图：https://blog.csdn.net/weixin_38358435/article/details/81735842
localmapping2线程流程图：https://blog.csdn.net/u012936940/article/details/82185058
localmapping2代码详解：https://blog.csdn.net/u012936940/article/details/81784080

上图就是运行localmapping的流程图，还有另一位博主整理的：

下面来看看localmapping的文档：
#include “LocalMapping.h”
#include “LoopClosing.h”
#include “ORBmatcher.h”
#include “Optimizer.h”

#include

namespace ORB_SLAM
{
//构造函数
LocalMapping::LocalMapping(Map pMap):
mbResetRequested(false), mpMap(pMap), mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbAcceptKeyFrames(true)
{
}
//设置loopcloser线程
void LocalMapping::SetLoopCloser(LoopClosing pLoopCloser)
{
mpLoopCloser = pLoopCloser;
}
//设置tracking线程
void LocalMapping::SetTracker(Tracking *pTracker)
{
mpTracker=pTracker;
}
//这便是运行的关键部分，整个流程都是这个运行线程
void LocalMapping::Run()
{
//这是ros设置频率的函数，500Hz，设置运行频率，保证while每1/500执行一次
ros::Rate r(500);
while(ros::ok())
{
// Check if there are keyframes in the queue
//检测队列，等待处理的关键帧不为空，即存在需要处理的关键帧
if(CheckNewKeyFrames())
{
// Tracking will see that Local Mapping is busy
//一旦线程接受了一个关键帧，那么此时就将mbAcceptKeyFrames设置为false
SetAcceptKeyFrames(false);
//void LocalMapping::SetAcceptKeyFrames(bool flag)
//{
// boost::mutex::scoped_lock lock(mMutexAccept);
// mbAcceptKeyFrames=flag;
//}
// BoW conversion and insertion in Map
//更新MapPoints与KeyFrame的关联，UpdateConnections（）
ProcessNewKeyFrame();

        // Check recent MapPoints
        //剔除地图中新添加的但质量不好的MapPoints
        //IncreaseedFound/IncreaseVisible<25%
        //观测到该点的关键帧太少
        MapPointCulling();

        // Triangulate new MapPoints
        //运动过程中和共示程度比较高的关键帧通过三角化恢复出一些MapPoints
        CreateNewMapPoints();

        // Find more matches in neighbor keyframes and fuse point duplications
        //检查当前关键帧与相邻帧（两级相邻）重复的MapPoints
        SearchInNeighbors();

        mbAbortBA = false;
        //

        if(!CheckNewKeyFrames() && !stopRequested())
        {
            // Local BA
            //和当前关键帧相连的关键帧及MapPoints做局部BA优化
            Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA);

            // Check redundant local Keyframes
            //其90%以上的MapPoints能被其他共视关键帧（至少三个）观测到的关键帧
            KeyFrameCulling();

            mpMap->SetFlagAfterBA();

            // Tracking will see Local Mapping idle
            if(!CheckNewKeyFrames())
                SetAcceptKeyFrames(true);
        }

        mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
    }

    // Safe area to stop
    if(stopRequested())
    /*
    bool LocalMapping::stopRequested()

{
boost::mutex::scoped_lock lock(mMutexStop);
return mbStopRequested;//暂停请求
}
/
{
Stop();//localmapping设置为停止
/
void LocalMapping::Stop()
{
boost::mutex::scoped_lock lock(mMutexStop);
mbStopped = true;
}
/
ros::Rate r2(1000);
while(isStopped() && ros::ok())
{
r2.sleep();
}
/
bool LocalMapping::isStopped()
{
boost::mutex::scoped_lock lock(mMutexStop);
return mbStopped;
}
*/
SetAcceptKeyFrames(true);//可以接受新的关键帧
}

    ResetIfRequested();//重置相关变量，供下次使用
    r.sleep();//沉睡1/500秒，供删除数据使用，我是这么理解的
}

}

void LocalMapping::InsertKeyFrame(KeyFrame *pKF)//插入关键帧
{
boost::mutex::scoped_lock lock(mMutexNewKFs);
mlNewKeyFrames.push_back(pKF);
mbAbortBA=true;//BA标志
SetAcceptKeyFrames(false);
}

//检查队列等待处理的关键帧列表是否不为空
bool LocalMapping::CheckNewKeyFrames()
{
boost::mutex::scoped_lock lock(mMutexNewKFs);
return(!mlNewKeyFrames.empty());
}
//处理新的关键帧
void LocalMapping::ProcessNewKeyFrame()
{
//从队列中取得新的关键帧，并在队列中将此关键帧清除
{
boost::mutex::scoped_lock lock(mMutexNewKFs);
mpCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();
}

// Compute Bags of Words structures
//计算该关键帧特征点的BOW向量
mpCurrentKeyFrame->ComputeBoW();

if(mpCurrentKeyFrame->mnId==0)//关键帧ID
    return;

// Associate MapPoints to the new keyframe and update normal and descriptor
//跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定
//在TrackLocalMap函数中将局部地图中的MapPoints与当前帧进行了匹配，但没有与当前帧进行关联
vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
if(mpCurrentKeyFrame->mnId>1) //This operations are already done in the tracking for the first two keyframes
{
    for(size_t i=0; iisBad())
            {
            // 添加观测
                pMP->AddObservation(mpCurrentKeyFrame, i);
                // 获得该点的平均观测方向和观测距离范围
                pMP->UpdateNormalAndDepth();
                 // 加入关键帧后，更新3d点的最佳描述子
                pMP->ComputeDistinctiveDescriptors();
            }
        }
    }
}

//处理第一帧，这与ORB_SLAM2不一样
if(mpCurrentKeyFrame->mnId==1)
{
for(size_t i=0; i {
MapPoint* pMP = vpMapPointMatches[i];
if(pMP)
{
mlpRecentAddedMapPoints.push_back(pMP);
}
}
}

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

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

}

// 剔除ProcessNewKeyFrame和CreateNewMapPoints函数中引入的质量不好的MapPoints
//注意删除标准，见上,此部分比较简单，就是要利用三个条件判断是否剔除mappoints，设置badflag
void LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints
list::iterator lit = mlpRecentAddedMapPoints.begin();
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
while(lit!=mlpRecentAddedMapPoints.end())//遍历所有mappoint
{
MapPoint* pMP = lit;
if(pMP->isBad())
{
// 已经是坏点的MapPoints直接从链表中删除
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(pMP->GetFoundRatio()<0.25f )
{
/ 跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例小于25%
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if((nCurrentKFid-pMP->mnFirstKFid)>=2 && pMP->Observations()<=2)
{
// 从该点建立开始，到现在已经过了不少于2个关键帧
// 但是观测到该点的关键帧数却不超过2帧，则该点不合格
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if((nCurrentKFid-pMP->mnFirstKFid)>=3)
// 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
// 因此没有SetBadFlag()，仅从队列中删除，放弃对该点的检测
lit = mlpRecentAddedMapPoints.erase(lit);
else
lit++;
}
}
//产生新的地图点，与tracking线程里不一样，那里直接利用深度信息产生地图点，这里是根据当前帧的邻居帧，进行再次匹配，利用三角化生成地图点，这就增加了slam的可能距离。
//void LocalMapping::CreateNewMapPoints/根据当前关键帧恢复出一些新的地图点，不包括和当前关键帧匹配的局部地图点//(已经在ProcessNewKeyFrame中处理)()
/注意理解与前面两步的先后关系，先处理新关键帧与局部地图点之间的关系，然后对局部地图点进行检查
//最后再通过新关键帧恢复新的地图点
void LocalMapping::CreateNewMapPoints()
{
// Take neighbor keyframes in covisibility graph
// 在当前关键帧的共视关键帧中找到共视程度最高的20帧相邻帧
vector> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(20);

ORBmatcher matcher(0.6,false);//orbmatcher介绍

cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
cv::Mat Rwc1 = Rcw1.t();
cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
cv::Mat Tcw1(3,4,CV_32F);
Rcw1.copyTo(Tcw1.colRange(0,3));
tcw1.copyTo(Tcw1.col(3));
cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();

//内参
const float fx1 = mpCurrentKeyFrame->fx;
const float fy1 = mpCurrentKeyFrame->fy;
const float cx1 = mpCurrentKeyFrame->cx;
const float cy1 = mpCurrentKeyFrame->cy;
const float invfx1 = 1.0f/fx1;
const float invfy1 = 1.0f/fy1;

const float ratioFactor = 1.5f*mpCurrentKeyFrame->GetScaleFactor();//1.5*第一层的尺度因子。描述或许不准确

// Search matches with epipolar restriction and triangulate
//遍历所有的相邻关键帧
for(size_t i=0; iGetCameraCenter();
    cv::Mat vBaseline = Ow2-Ow1;
    const float baseline = cv::norm(vBaseline);
    
       // 邻接关键帧的场景深度中值
    const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
    
        // baseline与景深的比例
    const float ratioBaselineDepth = baseline/medianDepthKF2;

    if(ratioBaselineDepth<0.01)
        continue;

    // Compute Fundamental Matrix
    // 根据两个关键帧的位姿计算它们之间的基本矩阵
    cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);

    // Search matches that fulfil epipolar constraint
    // 通过极线约束限制匹配时的搜索范围，进行特征点匹配
    vector vMatchedKeysUn1;
    vector vMatchedKeysUn2;
    vector > vMatchedIndices;
    matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedKeysUn1,vMatchedKeysUn2,vMatchedIndices);

    cv::Mat Rcw2 = pKF2->GetRotation();
    cv::Mat Rwc2 = Rcw2.t();
    cv::Mat tcw2 = pKF2->GetTranslation();
    cv::Mat Tcw2(3,4,CV_32F);
    Rcw2.copyTo(Tcw2.colRange(0,3));
    tcw2.copyTo(Tcw2.col(3));

    const float fx2 = pKF2->fx;
    const float fy2 = pKF2->fy;
    const float cx2 = pKF2->cx;
    const float cy2 = pKF2->cy;
    const float invfx2 = 1.0f/fx2;
    const float invfy2 = 1.0f/fy2;

    // Triangulate each match
    
    // 对每对匹配通过三角化生成3D点
    for(size_t ikp=0, iendkp=vMatchedKeysUn1.size(); ikp(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0 );
        // 由相机坐标系转到世界坐标系，得到视差角
        cv::Mat ray1 = Rwc1*xn1;
        cv::Mat xn2 = (cv::Mat_(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0 );
        cv::Mat ray2 = Rwc2*xn2;
        const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));

//视差角不合适
if(cosParallaxRays<0 || cosParallaxRays>0.9998)
continue;

        // Linear Triangulation Method
        cv::Mat A(4,4,CV_32F);
        A.row(0) = xn1.at(0)*Tcw1.row(2)-Tcw1.row(0);
        A.row(1) = xn1.at(1)*Tcw1.row(2)-Tcw1.row(1);
        A.row(2) = xn2.at(0)*Tcw2.row(2)-Tcw2.row(0);
        A.row(3) = xn2.at(1)*Tcw2.row(2)-Tcw2.row(1);

        cv::Mat w,u,vt;
        cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);

        cv::Mat x3D = vt.row(3).t();

        if(x3D.at(3)==0)
            continue;

        // Euclidean coordinates
        x3D = x3D.rowRange(0,3)/x3D.at(3);
        cv::Mat x3Dt = x3D.t();

        //Check triangulation in front of cameras
         // 检测深度正负
        float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at(2);
        if(z1<=0)
            continue;

        float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at(2);
        if(z2<=0)
            continue;

        //Check reprojection error in first keyframe
        // 计算3D点在当前关键帧下的重投影误差
        float sigmaSquare1 = mpCurrentKeyFrame->GetSigma2(kp1.octave);
        float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at(0);
        float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at(1);
        float invz1 = 1.0/z1;
        float u1 = fx1*x1*invz1+cx1;
        float v1 = fy1*y1*invz1+cy1;
        float errX1 = u1 - kp1.pt.x;
        float errY1 = v1 - kp1.pt.y;
        if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
            continue;

        //Check reprojection error in second keyframe
        // 计算3D点在另一个关键帧下的重投影误差
        float sigmaSquare2 = pKF2->GetSigma2(kp2.octave);
        float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at(0);
        float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at(1);
        float invz2 = 1.0/z2;
        float u2 = fx2*x2*invz2+cx2;
        float v2 = fy2*y2*invz2+cy2;
        float errX2 = u2 - kp2.pt.x;
        float errY2 = v2 - kp2.pt.y;
        if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
            continue;

        //Check scale consistency
        // 检查尺度连续性 
        cv::Mat normal1 = x3D-Ow1;
        float dist1 = cv::norm(normal1);

        cv::Mat normal2 = x3D-Ow2;
        float dist2 = cv::norm(normal2);

        if(dist1==0 || dist2==0)
            continue;

        float ratioDist = dist1/dist2;
        float ratioOctave = mpCurrentKeyFrame->GetScaleFactor(kp1.octave)/pKF2->GetScaleFactor(kp2.octave);
        if(ratioDist*ratioFactorratioOctave*ratioFactor)
            continue;

ratioDistratioFactor < ratioOctave 或 ratioDist/ratioOctave > ratioFactor表明尺度变化是连续的
// Triangulation is succesfull
// 三角化生成3D点成功，构造成MapPoint
MapPoint pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
// 为该MapPoint添加属性
pMP->AddObservation(pKF2,idx2);
pMP->AddObservation(mpCurrentKeyFrame,idx1);

        mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
        pKF2->AddMapPoint(pMP,idx2);

        pMP->ComputeDistinctiveDescriptors();

        pMP->UpdateNormalAndDepth();

        mpMap->AddMapPoint(pMP);
        // 将新产生的点放入检测队列
        mlpRecentAddedMapPoints.push_back(pMP);
    }
}

}

// 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
void LocalMapping::SearchInNeighbors()
{
// Retrieve neighbor keyframes
// 获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧
// 找到当前帧一级相邻与二级相邻关键帧
vector vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(20);
vector vpTargetKFs;
for(vector::iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
continue;
vpTargetKFs.push_back(pKFi);// 一级相邻帧
pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;

    // Extend to some second neighbors
    vector vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
    for(vector::iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
    {
        KeyFrame* pKFi2 = *vit2;
        if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
            continue;
        vpTargetKFs.push_back(pKFi2);// 存入二级相邻帧
    }
}


// Search matches by projection from current KF in target KFs
ORBmatcher matcher(0.6);
// 将当前帧的MapPoints分别与一级二级相邻帧进行融合
vector vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(vector::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
{
    KeyFrame* pKFi = *vit;
    matcher.Fuse(pKFi,vpMapPointMatches);
}

// Search matches by projection from target KFs in current KF
// 用于存储一级邻接和二级邻接关键帧所有MapPoints的集合
vector vpFuseCandidates;
vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());

// 将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合
// 遍历每一个一级邻接和二级邻接关键帧
for(vector::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
{
KeyFrame* pKFi = vitKF;
vector> vpMapPointsKFi = pKFi->GetMapPointMatches();
for(vector::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
{
MapPoint* pMP = *vitMP;
if(!pMP)
continue;
// 判断MapPoints是否为坏点，或者是否已经加进集合vpFuseCandidates
if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
continue;
// 加入集合，并标记已经加入
pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
vpFuseCandidates.push_back(pMP);
}
}

matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);


// Update points
// 更新当前帧MapPoints的描述子，深度，观测主方向等属性
vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(size_t i=0, iend=vpMapPointMatches.size(); iisBad())
        {
          // 在所有找到pMP的关键帧中，获得最佳的描述子
            pMP->ComputeDistinctiveDescriptors();
            pMP->UpdateNormalAndDepth();
        }
    }
}

// Update connections in covisibility graph
// 步更新当前帧的MapPoints后更新与其它帧的连接关系
// 更新covisibility图
mpCurrentKeyFrame->UpdateConnections();

}

cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
{
cv::Mat R1w = pKF1->GetRotation();
cv::Mat t1w = pKF1->GetTranslation();
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();

cv::Mat R12 = R1w*R2w.t();
cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;

cv::Mat t12x = SkewSymmetricMatrix(t12);

cv::Mat K1 = pKF1->GetCalibrationMatrix();
cv::Mat K2 = pKF2->GetCalibrationMatrix();


return K1.t().inv()*t12x*R12*K2.inv();

}

void LocalMapping::RequestStop()
{
boost::mutex::scoped_lock lock(mMutexStop);
mbStopRequested = true;
boost::mutex::scoped_lock lock2(mMutexNewKFs);
mbAbortBA = true;
}

void LocalMapping::Stop()
{
boost::mutex::scoped_lock lock(mMutexStop);
mbStopped = true;
}

bool LocalMapping::isStopped()
{
boost::mutex::scoped_lock lock(mMutexStop);
return mbStopped;
}

bool LocalMapping::stopRequested()
{
boost::mutex::scoped_lock lock(mMutexStop);
return mbStopRequested;
}

void LocalMapping::Release()
{
boost::mutex::scoped_lock lock(mMutexStop);
mbStopped = false;
mbStopRequested = false;
for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
delete *lit;
mlNewKeyFrames.clear();
}

bool LocalMapping::AcceptKeyFrames()
{
boost::mutex::scoped_lock lock(mMutexAccept);
return mbAcceptKeyFrames;
}

void LocalMapping::SetAcceptKeyFrames(bool flag)
{
boost::mutex::scoped_lock lock(mMutexAccept);
mbAcceptKeyFrames=flag;
}

void LocalMapping::InterruptBA()
{
mbAbortBA = true;
}
// 关键帧剔除
void LocalMapping::KeyFrameCulling()
{
// Check redundant keyframes (only local keyframes)
// A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
// in at least other 3 keyframes (in the same or finer scale)
// 提取当前帧的共视关键帧
vector vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();

for(vector::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
{
    KeyFrame* pKF = *vit;
    if(pKF->mnId==0)
        continue;
        // 提取每个共视关键帧的MapPoints
    vector vpMapPoints = pKF->GetMapPointMatches();

    int nRedundantObservations=0;
    int nMPs=0;
    // 遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
    for(size_t i=0, iend=vpMapPoints.size(); iisBad())
            {
                nMPs++;
                if(pMP->Observations()>3)
                {
                    int scaleLevel = pKF->GetKeyPointUn(i).octave;
                    map observations = pMP->GetObservations();
                     // 判断该MapPoint是否同时被三个关键帧观测到
                    int nObs=0;
                    for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
                    {
                        KeyFrame* pKFi = mit->first;
                        if(pKFi==pKF)
                            continue;
                        int scaleLeveli = pKFi->GetKeyPointUn(mit->second).octave;
                        // 要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
                        if(scaleLeveli<=scaleLevel+1)
                        {
                            nObs++;
                            // 已经找到三个同尺度的关键帧可以观测到该MapPoint，不用继续找了
                            if(nObs>=3)
                                break;
                        }
                    }
                    // 该MapPoint至少被三个关键帧观测到
                    if(nObs>=3)
                    {
                        nRedundantObservations++;
                    }
                }
            }
        }
    }

// 该局部关键帧90%以上的MapPoints能被其它至少三个关键帧观测到，则认为是冗余关键帧
if(nRedundantObservations>0.9*nMPs)
pKF->SetBadFlag();
}
}

cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
{
return (cv::Mat_(3,3) << 0, -v.at(2), v.at(1),
v.at(2), 0,-v.at(0),
-v.at(1), v.at(0), 0);
}

void LocalMapping::RequestReset()
{
{
boost::mutex::scoped_lock lock(mMutexReset);
mbResetRequested = true;
}

ros::Rate r(500);
while(ros::ok())
{
    {
    boost::mutex::scoped_lock lock2(mMutexReset);
    if(!mbResetRequested)
        break;
    }
    r.sleep();
}

}

void LocalMapping::ResetIfRequested()
{
boost::mutex::scoped_lock lock(mMutexReset);
if(mbResetRequested)
{
mlNewKeyFrames.clear();
mlpRecentAddedMapPoints.clear();
mbResetRequested=false;
}
}

} //namespace ORB_SLAM