ORB-SLAM2 Tracking 部分读代码一_mlrelativeframeposes.push_back(mlrelativeframepose

这里没有从构造开始说，这篇当草稿，想到哪里写哪里，刚开始看应该会有写错的，在意的别看，

主要为了自己以后看，看的时候耐心点，一个字一个字的看，这样上下看不舒服，自己看的时候左边代码，右边下面的文字吧，不然看不下去了，用CSDN不会搞，之后再想办法吧。

一、通过恒速模型使用上一帧的地图点来跟踪当前帧

bool Tracking::TrackWithMotionModel()
{
    // 最小距离 < 0.9*次小距离 匹配成功，检查旋转
    ORBmatcher matcher(0.9,true);
 
    // Update last frame pose according to its reference keyframe
    // Create "visual odometry" points
    // Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
    UpdateLastFrame();
 
    // Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿。
    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
    
    // 清空当前帧的地图点
    fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
 
    // Project points seen in previous frame
    // 设置特征匹配过程中的搜索半径
    int th;
    if(mSensor!=System::STEREO)
        th=15;//单目
    else
        th=7;//双目
 
    // Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
 
    // If few matches, uses a wider window search
    // 如果匹配点太少，则扩大搜索半径再来一次
    if(nmatches<20)
    {
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th
    }
 
    // 如果还是不能够获得足够的匹配点,那么就认为跟踪失败
    if(nmatches<20)
        return false;
 
    // Optimize frame pose with all matches
    // Step 4：利用3D-2D投影关系，优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
 
    // Discard outliers
    // Step 5：剔除地图点中外点
    int nmatchesMap = 0;
    for(int i =0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            if(mCurrentFrame.mvbOutlier[i])
            {
                // 如果优化后判断某个地图点是外点，清除它的所有关系
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
 
                mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                // 累加成功匹配到的地图点数目
                nmatchesMap++;
        }
    }    
 
    if(mbOnlyTracking)
    {
        // 纯定位模式下：如果成功追踪的地图点非常少,那么这里的mbVO标志就会置位
        mbVO = nmatchesMap<10;
        return nmatches>20;
    }
 
    // Step 6：匹配超过10个点就认为跟踪成功
    return nmatchesMap>=10;
}

第一步： 更新上一帧的位姿，对于双目或者RGBD还会根据深度值生成临时地图点

第二步： 根据上一帧特征点对应地图点进行投影匹配

第三步： 优化当前帧位姿

第四步： 剔除地图点中外点

如果匹配数大于10,则认为跟踪成功，返回 True

Tracking::TrackWithMotionModel()  方法

1.先创建一个 ORBmatcher 要求最小值要小于0.9*次小值,考虑旋转  ，下面用到的时候再讲

2. 更新上一帧的位姿，如果是双目或者RGBD还会生成临时地图点 调用 UpdateLastFrame(); 现在先开始看这个之后位置对调一下

3.通过恒速模型 将估计的运动TF  mVelocity   × 上一帧的位姿 mLastFrame.mTcw 获取当前估计的位姿并写给  当前帧 mCurrentFrame.SetPose

问：清除 当前帧的地图点 （ 这里还不明白为什么要清楚当前帧的地图点！！！，一会看明白了回来写）

答： 因为在  下面的 SearchByProjection 内会遍历 mCurrentFrame.mvpMapPoints 看候选点是否已经有对应的MapPoints,并且会对 CurrentFrame.mvpMapPoints[bestIdx2]=pMP; 赋值

4. 设置特帧匹配的搜索半径，单目为15个像素，双目为7个像素 ， SearchByProjection 这个方法在ORBmatcher里，这块写到的时候再讲不单独讲  

5.如果匹配的点少于20 个，清楚当前帧的地图点，并扩大一倍的半径再搜索一次。

6.如果匹配点还是少于20个，则跟踪失败

7.优化当前帧的位姿 ，调用  Optimizer::PoseOptimization(&mCurrentFrame);  在Optimizer.cc内，目前不细说，等讲到这个文件的时候再讲，先大概说下思路  构建顶点和边 通过g20进行优化，返回内点的个数，Tracking中的运动跟踪、参考帧跟踪、地图跟踪、重定位都是用这个做的，比较关键。

8.剔除地图点中的外点，遍历所有关键帧，判断是否是地图点，如果是外点，则清除该点，调用以下部分，注意这里就和新增永久Mappoint反的，具体可以看下临时地图点和永久地图点是怎么加的。

MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];

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

mCurrentFrame.mvbOutlier[i]=false;

pMP->mbTrackInView = false;

pMP->mnLastFrameSeen = mCurrentFrame.mnId;

如果不是外点，累积匹配到的地图点自增。

9.纯定位模式下，如果匹配到的地图点很少，则对 mbVO置位，并且返回 nmatches和20的比较

10. 不在纯定位下，返回匹配到的地图点 nmatchesMap和10的比较

二、更新上一帧位姿，在上一帧生成临时地图点，使用单目时只计算上一帧的世界坐标系位姿，使用双目或Rgbd时选取有生渎职并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性，由恒速跟踪模型调用

void Tracking::UpdateLastFrame()
{
    // Update pose according to reference keyframe
    // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
    // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧
    KeyFrame* pRef = mLastFrame.mpReferenceKF;     // 这个是上一帧数据的关键帧
    // ref_keyframe 到 lastframe的位姿变换 
    cv::Mat Tlr = mlRelativeFramePoses.back();  //所有参考帧的位姿
 
    // 将上一帧的世界坐标系下的位姿计算出来
    // l:last, r:reference, w:world
    // Tlw = Tlr*Trw      T LastWorld = TLastReference*TReferenceWorld
    mLastFrame.SetPose(Tlr*pRef->GetPose()); 
 
    // 如果上一帧为关键帧，或者单目的情况，则退出
    if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
        return;
 
    // Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点
    // 注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除
 
    // Create "visual odometry" MapPoints
    // We sort points according to their measured depth by the stereo/RGB-D sensor
    // Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）
    vector<pair<float,int> > vDepthIdx;
    vDepthIdx.reserve(mLastFrame.N);
 
    for(int i=0; i<mLastFrame.N;i++)
    {
        float z = mLastFrame.mvDepth[i];
        if(z>0)
        {
            // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
            vDepthIdx.push_back(make_pair(z,i));
        }
    }
 
    // 如果上一帧中没有有效深度的点,那么就直接退出
    if(vDepthIdx.empty())
        return;
 
    // 按照深度从小到大排序
    sort(vDepthIdx.begin(),vDepthIdx.end());
 
    // We insert all close points (depth<mThDepth)
    // If less than 100 close points, we insert the 100 closest ones.
    // Step 2.2：从中找出不是地图点的部分  
    int nPoints = 0;    //新添加的临时地图点的个数
    for(size_t j=0; j<vDepthIdx.size();j++)
    {
        int i = vDepthIdx[j].second;   // first是深度  second是特征点的ID
 
        bool bCreateNew = false;
 
        // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点
        MapPoint* pMP = mLastFrame.mvpMapPoints[i];   //mvpMapPoints 为特征点对应的地图点
        if(!pMP)   //为空对应对应该点没有地图点
            bCreateNew = true;
        else if(pMP->Observations()<1)    //这里对应该点被创建为地图点，但是没有被观测到    
        {
            // 地图点被创建后就没有被观测，认为不靠谱，也需要重新创建
            bCreateNew = true;
        }
 
        if(bCreateNew)
        {
            // Step 2.3：需要创建的点，包装为地图点。只是为了提高双目和RGBD的跟踪成功率，并没有添加复杂属性，因为后面会扔掉
            // 反投影到世界坐标系中                这里是恒速模型加的临时地图点，这里没有更新地图点的操作
            cv::Mat x3D = mLastFrame.UnprojectStereo(i);  //将对应索引下的特征点反投影为世界坐标下的 3D点
            MapPoint* pNewMP = new MapPoint(     // 将3D点包装为地图点    ，注意这里是临时地图点，永久地图点需要  做 AddObservation 等操作，具体看CreateNewKeyFrame
                x3D,            // 世界坐标系坐标
                mpMap,          // 跟踪的全局地图
                &mLastFrame,    // 存在这个特征点的帧(上一帧)
                i);             // 特征点id
 
            // 加入上一帧的地图点中
            mLastFrame.mvpMapPoints[i]=pNewMP; 
 
            // 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除
            mlpTemporalPoints.push_back(pNewMP);
            nPoints++;
        }
        else
        {
            // 因为从近到远排序，记录其中不需要创建地图点的个数
            nPoints++;
        }
 
        // Step 2.4：如果地图点质量不好，停止创建地图点
        // 停止新增临时地图点必须同时满足以下条件：
        // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
        // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
        if(vDepthIdx[j].first>mThDepth && nPoints>100)    //如果深度大于阈值（深度越近说明越准确）或者已经加了100个点则退出
            break;
    }
}

该方法的流程为： 

1. 取上一帧的参考关键帧

2. 通过T上一帧参考关键帧× T上一参考关键帧在世界坐标系下位姿 获得T上一帧在世界坐标系下的位姿 ，将其赋值给  mLastFrame.SetPose(Tlr*pRef->GetPose());

3. 如果上一帧为关键帧或者为单目相机则退出

4.否则 遍历上一帧写入 vDepthIdx  (first为深度值，second为对应点的ID)

5.如果上一帧数据的深度都为负（正常看到的都是前方的点），则退出

6.否则 将以深度值为键值从小到大排序，遍历排序后的点（对双目或者RGBD来说一般距离越近的点越准确）, 如果该点不是上一帧中的地图点或者是地图点但是没有被观测过，则创建临时地图点，当创建的临时地图点大于100个或者深度值大于阈值则不再添加临时地图点。

注意该方法添加的是临时地图点，永久地图点需要调用CreateNewKeyFrame中调用的以下方法

pNewMP->AddObservation(pKF,i);

pKF->AddMapPoint(pNewMP,i);

pNewMP->ComputeDistinctiveDescriptors();

pNewMP->UpdateNormalAndDepth();

mpMap->AddMapPoint(pNewMP);

临时地图点会在 CreateNewKeyFrame前删除  ，具体在哪删除，我写到这里的时候还没看到，接着往下看，这篇应该会写到的。

估计是去操作   mLastFrame.mvpMapPoints

三、创建新的关键帧

如果是双目或者RGBD，在创建关键帧的时候会同时创建新的地图点

下面贴代码

void Tracking::CreateNewKeyFrame()
{
    // 如果局部建图线程关闭了,就无法插入关键帧
    if(!mpLocalMapper->SetNotStop(true))
        return;
 
    // Step 1：将当前帧构造成关键帧
    KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
    // Step 2：将当前关键帧设置为当前帧的参考关键帧
    // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
    mpReferenceKF = pKF;
    mCurrentFrame.mpReferenceKF = pKF;
 
    // 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同
    // Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作
    if(mSensor!=System::MONOCULAR)
    {
        // 根据Tcw计算mRcw、mtcw和mRwc、mOw
        mCurrentFrame.UpdatePoseMatrices();
 
        // We sort points by the measured depth by the stereo/RGBD sensor.
        // We create all those MapPoints whose depth < mThDepth.
        // If there are less than 100 close points we create the 100 closest.
        // Step 3.1：得到当前帧有深度值的特征点（不一定是地图点）
        vector<pair<float,int> > vDepthIdx;
        vDepthIdx.reserve(mCurrentFrame.N);
        for(int i=0; i<mCurrentFrame.N; i++)
        {
            float z = mCurrentFrame.mvDepth[i];
            if(z>0)
            {
                // 第一个元素是深度,第二个元素是对应的特征点的id
                vDepthIdx.push_back(make_pair(z,i));
            }
        }
 
        if(!vDepthIdx.empty())
        {
            // Step 3.2：按照深度从小到大排序   从小到大的原因是深度值越近越准
            sort(vDepthIdx.begin(),vDepthIdx.end());
 
            // Step 3.3：从中找出不是地图点的生成临时地图点 
            // 处理的近点的个数
            int nPoints = 0;
            for(size_t j=0; j<vDepthIdx.size();j++)
            {
                int i = vDepthIdx[j].second;    //  i  是点的id
 
                bool bCreateNew = false;
 
                // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(!pMP)    // 这个个点在上一帧没有地图点
                    bCreateNew = true;
                else if(pMP->Observations()<1)    //这个点在上一帧有地图点但是没有被观测到
                {
                    bCreateNew = true;
                    mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);    //地图点置空
                }
 
                // 如果需要就新建地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪
                if(bCreateNew)   //要创建地图点  这里是永久的地图点                     恒速模型里是临时地图点
                {
                    cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);  //当前帧反投影到3D坐标系下
                    MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);   //将点包装成地图点
                    // 这些添加属性的操作是每次创建MapPoint后都要做的  ！！！！！！！！！！！！！！！！  做了下面的才会增加地图点  不然像UpdateLastFrame里加的只是临时地图点
                    pNewMP->AddObservation(pKF,i);
                    pKF->AddMapPoint(pNewMP,i);
                    pNewMP->ComputeDistinctiveDescriptors();
                    pNewMP->UpdateNormalAndDepth();
                    mpMap->AddMapPoint(pNewMP);
 
                    mCurrentFrame.mvpMapPoints[i]=pNewMP;
                    nPoints++;
                }
                else
                {
                    // 因为从近到远排序，记录其中不需要创建地图点的个数
                    nPoints++;
                }
 
                // Step 3.4：停止新建地图点必须同时满足以下条件：
                // 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
                // 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
                if(vDepthIdx[j].first>mThDepth && nPoints>100)
                    break;
            }
        }
    }
 
    // Step 4：插入关键帧
    // 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸
    mpLocalMapper->InsertKeyFrame(pKF);
 
    // 插入好了，允许局部建图停止
    mpLocalMapper->SetNotStop(false);
 
    // 当前帧成为新的关键帧，更新
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKF;
}

该方法的流程为：

1. 如果关闭了局部建图线程 !mpLocalMapper->SetNotStop(true)   则无法插入关键帧了，直接 return

2. 将当前帧构造成关键帧，只需要调用 KeyFrame(xxxx)的构造就行了

3. 将当前关键帧设置为当前帧的参考关键帧，同时将 mpReferenceKF 和  mCurrentFrame.mpReferenceKF 设置为关键帧 ，在UpdateLocalKeyFrames中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧。  这个函数也在Tracking中下面单独讲

4. 如果不是地图点，创建关键帧的时候同时创建地图点，大体与二说的的 Tracking::UpdateLastFrame 相同，但需要注意这里创建的是永久地图点，调用了以下部分

pNewMP->AddObservation(pKF,i);

pKF->AddMapPoint(pNewMP,i);

pNewMP->ComputeDistinctiveDescriptors();

pNewMP->UpdateNormalAndDepth();

mpMap->AddMapPoint(pNewMP);

mCurrentFrame.mvpMapPoints[i]=pNewMP;

而二 Tracking::UpdateLastFrame 创建的是临时地图点

5. 创建完关键帧后，插入关键帧，将其插入到localmapping中 

mpLocalMapper->InsertKeyFrame(pKF);

6. 插入完成后，允许局部建图停止

7.将当前帧设置为新的关键帧，更新旧的关键帧及其ID号

mnLastKeyFrameId = mCurrentFrame.mnId;

mpLastKeyFrame = pKF;

四、更新局部关键帧

遍历当前帧的地图点，将观测到的这些地图点的关键帧和相邻的关键帧及其父子关键帧做为mvpLocalKeyFrames , 这里我理解为本质图？？？？？ 之后看一下共视图、本质图和 spanning tree,看完回来答疑

先贴代码

void Tracking::UpdateLocalKeyFrames()
{
    // Each map point vote for the keyframes in which it has been observed
    // Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧
    map<KeyFrame*,int> keyframeCounter;
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
            if(!pMP->isBad())
            {
                // 得到观测到该地图点的关键帧和该地图点在关键帧中的索引
                const map<KeyFrame*,size_t> observations = pMP->GetObservations();
                // 由于一个地图点可以被多个关键帧观测到,因此对于每一次观测,都对观测到这个地图点的关键帧进行累计投票
                for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
                    // 这里的操作非常精彩！
                    // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
                    // it->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
                    // 所以最后keyframeCounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点，也就是共视程度
                    keyframeCounter[it->first]++;      
            }
            else
            {
                mCurrentFrame.mvpMapPoints[i]=NULL;
            }
        }
    }
 
    // 没有当前帧没有共视关键帧，返回
    if(keyframeCounter.empty())
        return;
 
    // 存储具有最多观测次数（max）的关键帧
    int max=0;
    KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
 
    // Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有3种类型
    // 先清空局部关键帧
    mvpLocalKeyFrames.clear();
    // 先申请3倍内存，不够后面再加
    mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
 
    // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
    // Step 2.1 类型1：能观测到当前帧地图点的关键帧作为局部关键帧 （将邻居拉拢入伙）（一级共视关键帧） 
    for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
    {
        KeyFrame* pKF = it->first;
 
        // 如果设定为要删除的，跳过
        if(pKF->isBad())
            continue;
        
        // 寻找具有最大观测数目的关键帧
        if(it->second>max)
        {
            max=it->second;
            pKFmax=pKF;
        }
 
        // 添加到局部关键帧的列表里
        mvpLocalKeyFrames.push_back(it->first);
        
        // 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id
        // 表示它已经是当前帧的局部关键帧了，可以防止重复添加局部关键帧
        pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
    }
 
 
    // Include also some not-already-included keyframes that are neighbors to already-included keyframes
    // Step 2.2 遍历一级共视关键帧，寻找更多的局部关键帧 
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        // Limit the number of keyframes
        // 处理的局部关键帧不超过80帧
        if(mvpLocalKeyFrames.size()>80)
            break;
 
        KeyFrame* pKF = *itKF;
 
        // 类型2:一级共视关键帧的共视（前10个）关键帧，称为二级共视关键帧（将邻居的邻居拉拢入伙）
        // 如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧
        const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
        // vNeighs 是按照共视程度从大到小排列
        for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
        {
            KeyFrame* pNeighKF = *itNeighKF;
            if(!pNeighKF->isBad())
            {
                // mnTrackReferenceForFrame防止重复添加局部关键帧
                if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pNeighKF);
                    pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    //? 找到一个就直接跳出for循环？
                    break;
                }
            }
        }
 
        // 类型3:将一级共视关键帧的子关键帧作为局部关键帧（将邻居的孩子们拉拢入伙）
        const set<KeyFrame*> spChilds = pKF->GetChilds();
        for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
        {
            KeyFrame* pChildKF = *sit;
            if(!pChildKF->isBad())
            {
                if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pChildKF);
                    pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    //? 找到一个就直接跳出for循环？
                    break;
                }
            }
        }
 
        // 类型3:将一级共视关键帧的父关键帧（将邻居的父母们拉拢入伙）
        KeyFrame* pParent = pKF->GetParent();
        if(pParent)
        {
            // mnTrackReferenceForFrame防止重复添加局部关键帧
            if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
            {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                //! 感觉是个bug！如果找到父关键帧会直接跳出整个循环
                break;
            }
        }
 
    }
 
    // Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
    if(pKFmax)
    {
        mpReferenceKF = pKFmax;
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
    }
}

先声明几个术语

1：能观测到当前帧地图点的关键帧，也称一级共视关键帧

2：一级共视关键帧的共视关键帧，称为二级共视关键帧

3：一级共视关键帧的子关键帧、父关键帧

该方法的流程为：

1. 遍历当前帧的所有地图点，如果点是好点，则获取能够看到该地图点的所有关键帧，每个关键站看到过一次地图点，则计数+1, 记关键帧看到过多少当前帧的地图点的个数为共视程度（用 map 类型的 keyframeCounter 来存），该关键帧为一个共视关键帧。

2.如果当前帧没有共视关键帧，则返回。

3. 定义一个关键帧类用来存储共视程度最高的关键帧，清除局部关键帧，申请更多内存存局部关键帧。

4.遍历keyframeCounter 获取共视程度最高的关键帧，并且将所有好点的关键帧都存到局部关键帧里（坏点不要），存储局部关键帧的ID号，防止重复添加 （ 这里就是三CreateNewKeyFrame()里提到的更新局部关键帧） ,这一步也就是添加一级共视关键帧。

5.遍历局部关键帧（也就是上面说的一级共视关键帧），如果局部关键帧的个数超过80帧则退出， 取10个与前关键帧的连接的关键帧（这些关键帧按共视程度从大到小排序），将不是坏点并且非重复的关键帧添加到局部关键帧。这一步也就是添加二级共视关键帧。

6.将一级共视关键帧的父关键帧、子关键帧添加到局部关键帧中，不过这里有点奇怪，找到一个子关键帧就会退出所有子关键帧的遍历，有可能是作者想控制数量以及子关键帧的遍历有可能也是按共视程度从大到小排的。 然后父关键帧这里应该是有问题，如果找到一个父关键帧会退出第5步所有局部关键帧的遍历，这里应该是个bug ，现在不敢改，等整个代码看完确定了再改。

五、判断当前帧是否要插入关键帧

先贴代码

bool Tracking::NeedNewKeyFrame()
{
    // Step 1：纯VO模式下不插入关键帧
    if(mbOnlyTracking)
        return false;
 
    // If Local Mapping is freezed by a Loop Closure do not insert keyframes
    // Step 2：如果局部地图线程被闭环检测使用，则不插入关键帧
    if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;
    // 获取当前地图中的关键帧数目
    const int nKFs = mpMap->KeyFramesInMap();
 
    // Do not insert keyframes if not enough frames have passed from last relocalisation
    // mCurrentFrame.mnId是当前帧的ID
    // mnLastRelocFrameId是最近一次重定位帧的ID
    // mMaxFrames等于图像输入的帧率
    //  Step 3：如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧
    if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs>mMaxFrames)                                     
        return false;
 
    // Tracked MapPoints in the reference keyframe
    // Step 4：得到参考关键帧跟踪到的地图点数量
    // UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 
 
    // 地图点的最小观测次数
    int nMinObs = 3;
    if(nKFs<=2)
        nMinObs=2;
    // 参考关键帧地图点中观测的数目>= nMinObs的地图点数目
    int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs); //  mpReferenceKF 当前帧的参考关键帧
 
    // Local Mapping accept keyframes?
    // Step 5：查询局部地图线程是否繁忙，当前能否接受新的关键帧
    bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
 
    // Check how many "close" points are being tracked and how many could be potentially created.
    // Step 6：对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧
     int nNonTrackedClose = 0;  //双目或RGB-D中没有跟踪到的近点
    int nTrackedClose= 0;       //双目或RGB-D中成功跟踪的近点（三维点）
    if(mSensor!=System::MONOCULAR)
    {
        for(int i =0; i<mCurrentFrame.N; i++)
        {
            // 深度值在有效范围内
            if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
            {
                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                    nTrackedClose++;
                else
                    nNonTrackedClose++;
            }
        }
    }
 
    // 双目或RGBD情况下：跟踪到的地图点中近点太少 同时 没有跟踪到的三维点太多，可以插入关键帧了
    // 单目时，为false
    bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
 
    // Step 7：决策是否需要插入关键帧
    // Thresholds
    // Step 7.1：设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧
    float thRefRatio = 0.75f;
 
    // 关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低
    if(nKFs<2)
        thRefRatio = 0.4f;
 
    //单目情况下插入关键帧的频率很高    
    if(mSensor==System::MONOCULAR)
        thRefRatio = 0.9f;
    // 下面的 c1a c1b c1c只要满足一个   c2必须满足
    // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
    // Step 7.2：很长时间没有插入关键帧，可以插入
    const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
 
    // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
    // Step 7.3：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入
    const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
 
    // Condition 1c: tracking is weak
    // Step 7.4：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose
    const bool c1c =  mSensor!=System::MONOCULAR &&             //只考虑在双目，RGB-D的情况
                    (mnMatchesInliers<nRefMatches*0.25 ||       //当前帧和地图点匹配的数目非常少
                      bNeedToInsertClose) ;                     //需要插入
 
    // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
    // Step 7.5：和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少
    const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
 
    if((c1a||c1b||c1c)&&c2)
    {
        // If the mapping accepts keyframes, insert keyframe.
        // Otherwise send a signal to interrupt BA
        // Step 7.6：local mapping空闲时可以直接插入，不空闲的时候要根据情况插入
        if(bLocalMappingIdle)
        {
            //可以插入关键帧
            return true;
        }
        else
        {
            mpLocalMapper->InterruptBA();   //关掉BA
            if(mSensor!=System::MONOCULAR)
            {
                // 队列里不能阻塞太多关键帧
                // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
                // 然后localmapper再逐个pop出来插入到mspKeyFrames
                if(mpLocalMapper->KeyframesInQueue()<3)
                    //队列中的关键帧数目不是很多,可以插入
                    return true;
                else
                    //队列中缓冲的关键帧数目太多,暂时不能插入
                    return false;
            }
            else
                //对于单目情况,就直接无法插入关键帧了
                //? 为什么这里对单目情况的处理不一样?
                //回答：可能是单目关键帧相对比较密集
                return false;
        }
    }
    else
        //不满足上面的条件,自然不能插入关键帧
        return false;
}

该方法的流程为：

1.判断是否为纯VO模式，是的话 return  不插入关键帧

2.判断局部地图线程是否被使用，是则 return 不插入关键帧

3. 如果 当前帧的ID和上一次插入关键帧的ID较为接近或者关键帧的数目已经大于最大限制了， return 不插入关键帧

4. 设置地图点的最小观测次数

5.获取参考帧的地图点的观测次数大于最小观测次数的特征点的个数

6.查询局部建图线程是否繁忙

7.如果不是单目相机，如果点的深度的在给定阈值中则为近点cnt自增，不在阈确定    取消 值范围内则不是近点cnt自增。

8.判断近点的个数和远点的个数，获取一个允许插入变量bNeedToInsertClose。

9. 配置单目、双目、RGBD插入关键帧的阈值

10. 如果（长时间没有插入关键帧）或（在插入关键帧的最小间隔外并且局部地图线程处于空闲状态）或（双目、RGBD下匹配的点少于0.25倍参考关键帧的个数）并且（匹配的点数小于插入关键帧的阈值乘参考关键帧的个数或bNeedToInsertClose 为 true)并且跟踪到的内点个数大于15个，则允许插入关键帧。

这里非常重要，在部分其他框架中，只考虑了时间、运动，但是没有概率到车会出来在一个地方左右不停的摆动的情况，导致增加无效的输入（。。。。。。如 Cartographer，后面想想这块怎么解吧）。

六、估计运动、跟踪局部地图部分 Track()

先贴代码

void Tracking::Track()
{
    // track包含两部分：估计运动、跟踪局部地图
    
    // mState为tracking的状态，包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
    // 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
    if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }
 
    // mLastProcessedState 存储了Tracking最新的状态，用于FrameDrawer中的绘制
    mLastProcessedState=mState;
 
    // Get Map Mutex -> Map cannot be changed
    // 地图更新时加锁。保证地图不会发生变化
    // 疑问:这样子会不会影响地图的实时更新?
    // 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
 
    // Step 1：地图初始化
    if(mState==NOT_INITIALIZED)
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            //双目RGBD相机的初始化共用一个函数
            StereoInitialization();
        else
            //单目初始化
            MonocularInitialization();
 
        //更新帧绘制器中存储的最新状态
        mpFrameDrawer->Update(this);
 
        //这个状态量在上面的初始化函数中被更新
        if(mState!=OK)
            return;
    }
    else
    {
        // System is initialized. Track Frame.
        // bOK为临时变量，用于表示每个函数是否执行成功
        bool bOK;
 
        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
        // mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式
        // tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking
        if(!mbOnlyTracking)
        {
            // Local Mapping is activated. This is the normal behaviour, unless
            // you explicitly activate the "only tracking" mode.
 
            // Step 2：跟踪进入正常SLAM模式，有地图更新
            // 是否正常跟踪
            if(mState==OK)
            {
                // Local Mapping might have changed some MapPoints tracked in last frame
                // Step 2.1 检查并更新上一帧被替换的MapPoints
                // 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查
                CheckReplacedInLastFrame();
 
                // Step 2.2 运动模型是空的或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪
                // 第一个条件,如果运动模型为空,说明是刚初始化开始，或者已经跟丢了
                // 第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面，我们将重定位帧来恢复位姿
                // mnLastRelocFrameId 上一次重定位的那一帧
                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
                {
                    // 用最近的关键帧来跟踪当前的普通帧
                    // 通过BoW的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点都对应3D点重投影误差即可得到位姿
                    bOK = TrackReferenceKeyFrame();
                }
                else
                {
                    // 用最近的普通帧来跟踪当前的普通帧
                    // 根据恒速模型设定当前帧的初始位姿
                    // 通过投影的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点所对应3D点的投影误差即可得到位姿
                    bOK = TrackWithMotionModel();
                    if(!bOK)
                        //根据恒速模型失败了，只能根据参考关键帧来跟踪
                        bOK = TrackReferenceKeyFrame();
                }
            }
            else
            {
                // 如果跟踪状态不成功,那么就只能重定位了
                // BOW搜索，EPnP求解位姿
                bOK = Relocalization();
            }
        }
        else        
        {
            // Localization Mode: Local Mapping is deactivated
            // Step 2：只进行跟踪tracking，局部地图不工作
            if(mState==LOST)
            {
                // Step 2.1 如果跟丢了，只能重定位
                bOK = Relocalization();
            }
            else    
            {
                // mbVO是mbOnlyTracking为true时的才有的一个变量
                // mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常 (注意有点反直觉)
                // mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏
                if(!mbVO)
                {
                    // Step 2.2 如果跟踪正常，使用恒速模型 或 参考关键帧跟踪
                    // In last frame we tracked enough MapPoints in the map
                    if(!mVelocity.empty())
                    {
                        bOK = TrackWithMotionModel();
                        // ? 为了和前面模式统一，这个地方是不是应该加上
                        // if(!bOK)
                        //    bOK = TrackReferenceKeyFrame();
                    }
                    else
                    {
                        // 如果恒速模型不被满足,那么就只能够通过参考关键帧来定位
                        bOK = TrackReferenceKeyFrame();
                    }
                }
                else
                {
                    // In last frame we tracked mainly "visual odometry" points.
                    // We compute two camera poses, one from motion model and one doing relocalization.
                    // If relocalization is sucessfull we choose that solution, otherwise we retain
                    // the "visual odometry" solution.
 
                    // mbVO为true，表明此帧匹配了很少（小于10）的地图点，要跪的节奏，既做跟踪又做重定位
 
                    //MM=Motion Model,通过运动模型进行跟踪的结果
                    bool bOKMM = false;
                    //通过重定位方法来跟踪的结果
                    bool bOKReloc = false;
                    
                    //运动模型中构造的地图点
                    vector<MapPoint*> vpMPsMM;
                    //在追踪运动模型后发现的外点
                    vector<bool> vbOutMM;
                    //运动模型得到的位姿
                    cv::Mat TcwMM;
 
                    // Step 2.3 当运动模型有效的时候,根据运动模型计算位姿
                    if(!mVelocity.empty())
                    {
                        bOKMM = TrackWithMotionModel();
 
                        // 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量
                        vpMPsMM = mCurrentFrame.mvpMapPoints;
                        vbOutMM = mCurrentFrame.mvbOutlier;
                        TcwMM = mCurrentFrame.mTcw.clone();
                    }
 
                    // Step 2.4 使用重定位的方法来得到当前帧的位姿
                    bOKReloc = Relocalization();
 
                    // Step 2.5 根据前面的恒速模型、重定位结果来更新状态
                    if(bOKMM && !bOKReloc)
                    {
                        // 恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果
                        mCurrentFrame.SetPose(TcwMM);
                        mCurrentFrame.mvpMapPoints = vpMPsMM;
                        mCurrentFrame.mvbOutlier = vbOutMM;
 
                        //? 疑似bug！这段代码是不是重复增加了观测次数？后面 TrackLocalMap 函数中会有这些操作
                        // 如果当前帧匹配的3D点很少，增加当前可视地图点的被观测次数
                        if(mbVO)
                        {
                            // 更新当前帧的地图点被观测次数
                            for(int i =0; i<mCurrentFrame.N; i++)
                            {
                                //如果这个特征点形成了地图点,并且也不是外点的时候
                                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                                {
                                    //增加能观测到该地图点的帧数
                                    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                                }
                            }
                        }
                    }
                    else if(bOKReloc)
                    {
                        // 只要重定位成功整个跟踪过程正常进行（重定位与跟踪，更相信重定位）
                        mbVO = false;
                    }
                    //有一个成功我们就认为执行成功了
                    bOK = bOKReloc || bOKMM;
                }
            }
        }
 
        // 将最新的关键帧作为当前帧的参考关键帧
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
 
        // If we have an initial estimation of the camera pose and matching. Track the local map.
        // Step 3：在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
        // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
        if(!mbOnlyTracking)
        {
            if(bOK)
                bOK = TrackLocalMap();
        }
        else
        {
            // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
            // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
            // the camera we will use the local map again.
 
            // 重定位成功
            if(bOK && !mbVO)
                bOK = TrackLocalMap();
        }
 
        //根据上面的操作来判断是否追踪成功
        if(bOK)
            mState = OK;
        else
            mState=LOST;
 
        // Step 4：更新显示线程中的图像、特征点、地图点等信息
        mpFrameDrawer->Update(this);
 
        // If tracking were good, check if we insert a keyframe
        //只有在成功追踪时才考虑生成关键帧的问题
        if(bOK)
        {
            // Update motion model
            // Step 5：跟踪成功，更新恒速运动模型
            if(!mLastFrame.mTcw.empty())
            {
                // 更新恒速运动模型 TrackWithMotionModel 中的mVelocity
                cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
                mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
                mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
                // mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc
                mVelocity = mCurrentFrame.mTcw*LastTwc; 
            }
            else
                //否则速度为空
                mVelocity = cv::Mat();
 
            //更新显示中的位姿
            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
 
            // Clean VO matches
            // Step 6：清除观测不到的地图点   
            for(int i=0; i<mCurrentFrame.N; i++)
            {
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(pMP)
                    if(pMP->Observations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                    }
            }
 
            // Delete temporal MapPoints
            // Step 7：清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
            // 步骤6中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
            // 临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
            for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }
 
            // 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
            // 不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露
            mlpTemporalPoints.clear();
 
            // Check if we need to insert a new keyframe
            // Step 8：检测并插入关键帧，对于双目或RGB-D会产生新的地图点
            if(NeedNewKeyFrame())
                CreateNewKeyFrame();
 
            // We allow points with high innovation (considererd outliers by the Huber Function)
            // pass to the new keyframe, so that bundle adjustment will finally decide
            // if they are outliers or not. We don't want next frame to estimate its position
            // with those points so we discard them in the frame.
            // 作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中，让后续的BA来审判他们是不是真正的外点
            // 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉
 
            //  Step 9 删除那些在bundle adjustment中检测为outlier的地图点
            for(int i=0; i<mCurrentFrame.N;i++)
            {
                // 这里第一个条件还要执行判断是因为, 前面的操作中可能删除了其中的地图点
                if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
                    mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
            }
        }
 
        // Reset if the camera get lost soon after initialization
        // Step 10 如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset
        if(mState==LOST)
        {
            //如果地图中的关键帧信息过少的话,直接重新进行初始化了
            if(mpMap->KeyFramesInMap()<=5)
            {
                cout << "Track lost soon after initialisation, reseting..." << endl;
                mpSystem->Reset();
                return;
            }
        }
 
        //确保已经设置了参考关键帧
        if(!mCurrentFrame.mpReferenceKF)
            mCurrentFrame.mpReferenceKF = mpReferenceKF;
 
        // 保存上一帧的数据,当前帧变上一帧
        mLastFrame = Frame(mCurrentFrame);
    }
 
    // Store frame pose information to retrieve the complete camera trajectory afterwards.
    // Step 11：记录位姿信息，用于最后保存所有的轨迹
    if(!mCurrentFrame.mTcw.empty())
    {
        // 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1
        cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
        //保存各种状态
        mlRelativeFramePoses.push_back(Tcr);
        mlpReferences.push_back(mpReferenceKF);
        mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
        mlbLost.push_back(mState==LOST);
    }
    else
    {
        // This can happen if tracking is lost
        // 如果跟踪失败，则相对位姿使用上一次值
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState==LOST);
    }
 
}// Tracking 

该方法的流程为：  这里还是画流程图吧，写不清楚了

流程图看不清之后传个这个文件吧 Track()方法的流程图.dia