lsd：constraintSearchThreadLoop线程

constraintSearchThreadLoop()函数中完成一致性搜索分为了三个部分：

while(keepRunning)
{
//（1）在keyframegroup中选出一帧关键帧进行一致性搜索
    if(newKeyFrames.size() == 0)  
    { ......}
    else// （2）新关键帧的一致性搜索
    { ......}

//（3）接收终端指令，对keyframegroup中所有关键帧进行一次一致性搜索
    if(doFullReConstraintTrack)
    {.......}
}

为了理解方便我们从第(2)部分开始看起,核心是：

//找出新关键帧的一致性，注意： forceParent=true,useFABMAP=true, closeCandidatesTH=1.0
findConstraintsForNewKeyFrames(newKF, true, true, 1.0);

我们来看下findConstraintsForNewKeyFrames()函数内部关于NewKeyFrame起作用的代码实现，不针对于New Key Frame的就先跳过：

  // 处理第一帧关键帧：直接加入keyFrameGraph，返回
  if(!newKeyFrame->hasTrackingParent())
  { ...... }
  ....
  // 先保存已有的pose，下次用.
newKeyFrame->lastConstraintTrackedCamToWorld = newKeyFrame->getScaledCamToWorld();

// ====get all potential candidates and their initial relative pose. ========
 // findCandidates函数中先搜索欧式距离接近的,再使用fabMap全局搜索
 //欧氏距离判断函数findEuclideanOverlapFrames:通过相机对世界坐标系旋转角度的差异以及相机之间的平移
std::vector<KFConstraintStruct*, Eigen::aligned_allocator<KFConstraintStruct*> > constraints;
  Frame* fabMapResult = 0;
  std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>,
    Eigen::aligned_allocator< Frame* > > candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH);

在上面这段代码里最重要的是：

//使用fabmap搜索，并把结果放在fabMapResult中，比较阈值是closeCandidatesTH。
candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH);

现在跳到这个函数里看看是怎么实现的，主要分为2部分:

//**************第1部分：欧氏手段搜索潜在一致性。*********
// Add all candidates that are similar in an euclidean sense.
  std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > potentialReferenceFrames =
      findEuclideanOverlapFrames(keyframe, closenessTH * 15/(KFDistWeight*KFDistWeight), 1.0 - 0.25 * closenessTH, true);

  //存储通过欧氏距离和视角比较，搜索获得的结果
  for(unsigned int i=0;i<potentialReferenceFrames.size();i++)
    results.insert(potentialReferenceFrames[i].ref);

//*********************第2部分：通过FABMAP进行搜索并添加结果到results********************
  int appearanceBased = 0;
  fabMapResult_out = 0;
  if(includeFABMAP)
  {
    // Add Appearance-based Candidate, and all it's neighbours.
    fabMapResult_out = findAppearanceBasedCandidate(keyframe);
    if(fabMapResult_out != nullptr)
    {
      results.insert(fabMapResult_out);
      results.insert(fabMapResult_out->neighbors.begin(), fabMapResult_out->neighbors.end());
      appearanceBased = 1 + fabMapResult_out->neighbors.size();
    }
  }

继续看欧氏搜索的findEuclideanOverlapFrames()的实现：

 // basically the maximal angle-difference in viewing direction is angleTH*(average FoV).
  // e.g. if the FoV is 130°, then it is angleTH*130°. average FoV ： 0.5f*(fowX + fowY)。
  float cosAngleTH = cosf(angleTH*0.5f*(fowX + fowY));

  Eigen::Vector3d pos = frame->getScaledCamToWorld().translation();
  // 视角：cam坐标系z轴在world坐标系中的方向（旋转矩阵的第3列只作用在z轴上，试着取(0, 0, 1)^T乘一下就出来了）
  Eigen::Vector3d viewingDir = frame->getScaledCamToWorld().rotationMatrix().rightCols<1>();

  std::vector<TrackableKFStruct, Eigen::aligned_allocator<TrackableKFStruct> > potentialReferenceFrames;

  float distFacReciprocal = 1;
  if(checkBothScales)
    // 将newkf的meanIdepth恢复到world坐标系下的尺度
    distFacReciprocal = frame->meanIdepth/frame->getScaledCamToWorld().scale();

  // for each frame, calculate the rough score, consisting of pose, scale and angle overlap.
  graph->keyframesAllMutex.lock_shared();
  for(unsigned int i=0;i<graph->keyframesAll.size();i++)
  {
    Eigen::Vector3d otherPos = graph->keyframesAll[i]->getScaledCamToWorld().translation();

    // get distance between the frames, scaled to fit the potential reference frame.
    // 将reference关键帧的meanIdepth恢复到world坐标系下的尺度
    float distFac = graph->keyframesAll[i]->meanIdepth/graph->keyframesAll[i]->getScaledCamToWorld().scale(); 
    // dist尺度统一到逆深度小的坐标系下
    if(checkBothScales && distFacReciprocal < distFac) distFac = distFacReciprocal;
    // dist尺度统一到distFac所在坐标系下
    Eigen::Vector3d dist = (pos - otherPos) * distFac;

    float dNorm2 = dist.dot(dist);
    // 距离不能差太大，否则不具有一致性
    if(dNorm2 > distanceTH) continue;

    Eigen::Vector3d otherViewingDir = graph->keyframesAll[i]->getScaledCamToWorld().rotationMatrix().rightCols<1>();
    //z轴方向的夹角
    float dirDotProd = otherViewingDir.dot(viewingDir);
    // 视角角度不能差太大，否则不具有一致性
    if(dirDotProd < cosAngleTH) continue;

    // 压入潜在参考帧队列
    potentialReferenceFrames.push_back(TrackableKFStruct());
    potentialReferenceFrames.back().ref = graph->keyframesAll[i];
    potentialReferenceFrames.back().refToFrame = se3FromSim3(graph->keyframesAll[i]->getScaledCamToWorld().inverse() * frame->getScaledCamToWorld()).inverse();
    potentialReferenceFrames.back().dist = dNorm2;
    potentialReferenceFrames.back().angle = dirDotProd;
  }
  graph->keyframesAllMutex.unlock_shared();

  return potentialReferenceFrames;

关于“第2部分：通过FABMAP进行搜索并添加结果到results”,因为我对fabmap不太熟悉，暂时先不讲了。

完成一致性搜索后，现在我们回到findConstraintsForNewKeyFrames函数中，处理搜索到的一致性：局部和大尺度闭环

//从一致性搜索的返回结果里获取newKeyFrame与keyFrameGraph中已有关键帧之间的初始位姿关系
 std::map< Frame*, Sim3, std::less<Frame*>, Eigen::aligned_allocator<std::pair<Frame*, Sim3> > > candidateToFrame_initialEstimateMap;
 ......
  for (Frame* candidate : candidates)
  {
    Sim3 candidateToFrame_initialEstimate = newKeyFrame->getScaledCamToWorld().inverse() * candidate->getScaledCamToWorld();
    candidateToFrame_initialEstimateMap[candidate] = candidateToFrame_initialEstimate;
  }

  std::unordered_map<Frame*, int> distancesToNewKeyFrame;
  if(newKeyFrame->hasTrackingParent())
    // 通过parent对邻近关系做一个排序
    keyFrameGraph->calculateGraphDistancesToFrame(newKeyFrame->getTrackingParent(), &distancesToNewKeyFrame);
  poseConsistencyMutex.unlock_shared();

  // =============== distinguish between close and "far" candidates in Graph =================
  // Do a first check on trackability of close candidates.
  std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>, Eigen::aligned_allocator< Frame* > > closeCandidates;
  std::vector<Frame*, Eigen::aligned_allocator<Frame*> > farCandidates;
  Frame* parent = newKeyFrame->hasTrackingParent() ? newKeyFrame->getTrackingParent() : 0;

  int closeFailed = 0;
  int closeInconsistent = 0;

  SO3 disturbance = SO3::exp(Sophus::Vector3d(0.05,0,0));

  // 对于每一对Constraint,用相互tracking来确保位姿变换是基本固定的，避免后续处理出现虚假的闭环reciprocal tracking check
  //局部的具有约束的一致性检测
  for (Frame* candidate : candidates)
  {
    if (candidate->id() == newKeyFrame->id())
      continue;
    if(!candidate->pose->isInGraph)
      continue;
      //排除parent,没有闭环效应
    if(newKeyFrame->hasTrackingParent() && candidate == newKeyFrame->getTrackingParent())
      continue;
      //排除初始
    if(candidate->idxInKeyframes < INITIALIZATION_PHASE_COUNT)
      continue;

    SE3 c2f_init = se3FromSim3(candidateToFrame_initialEstimateMap[candidate].inverse()).inverse();
    c2f_init.so3() = c2f_init.so3() * disturbance;
    //tracking求两帧之间的关系
    SE3 c2f = constraintSE3Tracker->trackFrameOnPermaref(candidate, newKeyFrame, c2f_init);
    if(!constraintSE3Tracker->trackingWasGood) {closeFailed++; continue;}

    SE3 f2c_init = se3FromSim3(candidateToFrame_initialEstimateMap[candidate]).inverse();
    f2c_init.so3() = disturbance * f2c_init.so3();
    SE3 f2c = constraintSE3Tracker->trackFrameOnPermaref(newKeyFrame, candidate, f2c_init);
    if(!constraintSE3Tracker->trackingWasGood) {closeFailed++; continue;}

    // 两个互逆的矩阵相乘应该是单位阵，6自由度的se(3)=0
    if((f2c.so3() * c2f.so3()).log().norm() >= 0.09) {closeInconsistent++; continue;}

    closeCandidates.insert(candidate);
  }

  // 找出fabmap检测到的大尺度的闭环一致性
  int farFailed = 0;
  int farInconsistent = 0;
  for (Frame* candidate : candidates)
  {
    if (candidate->id() == newKeyFrame->id())
      continue;
    if(!candidate->pose->isInGraph)
      continue;
    if(newKeyFrame->hasTrackingParent() && candidate == newKeyFrame->getTrackingParent())
      continue;
    if(candidate->idxInKeyframes < INITIALIZATION_PHASE_COUNT)
      continue;

    //大尺度的闭环一致性，必须添加。
    if(candidate == fabMapResult)
    {
      farCandidates.push_back(candidate);
      continue;
    }

    //相邻的太近了，舍去
    if(distancesToNewKeyFrame.at(candidate) < 4)
      continue;

    farCandidates.push_back(candidate);
  }

为了实时性考虑，两种类型的一致性不能太多，多的话就删除呗：

 // =============== limit number of close candidates ===============
  // while too many, remove the one with the highest connectivity.
  while((int)closeCandidates.size() > maxLoopClosureCandidates)
  {
    Frame* worst = 0;
    int worstNeighbours = 0;
    for(Frame* f : closeCandidates)
    {
      int neightboursInCandidates = 0;
      for(Frame* n : f->neighbors)
    if(closeCandidates.find(n) != closeCandidates.end())
      neightboursInCandidates++;

      if(neightboursInCandidates > worstNeighbours || worst == 0)
      {
    worst = f;
    worstNeighbours = neightboursInCandidates;
      }
    }

    closeCandidates.erase(worst);
  }

  // =============== limit number of far candidates ===============
  // delete randomly
  int maxNumFarCandidates = (maxLoopClosureCandidates +1) / 2;
  if(maxNumFarCandidates < 5) maxNumFarCandidates = 5;
  while((int)farCandidates.size() > maxNumFarCandidates)
  {
    int toDelete = rand() % farCandidates.size();
    if(farCandidates[toDelete] != fabMapResult)
    {
      farCandidates[toDelete] = farCandidates.back();
      farCandidates.pop_back();
    }
  }

为了避免错误的闭环检测，需要做个reciprocal tracking check，对应论文中：

代码里有分3中情况：

 //第一种情况：closeCandidates
  for (Frame* candidate : closeCandidates)
  {
    KFConstraintStruct* e1=0;
    KFConstraintStruct* e2=0;

    testConstraint(
            candidate, e1, e2,
            candidateToFrame_initialEstimateMap[candidate],
            loopclosureStrictness);


    if(e1 != 0)
    {
      //check通过，添加到constraints
      constraints.push_back(e1);
      constraints.push_back(e2);

      // delete from far candidates if it's in there.
      for(unsigned int k=0;k<farCandidates.size();k++)
      {
        if(farCandidates[k] == candidate)
        {
          farCandidates[k] = farCandidates.back();
          farCandidates.pop_back();
        }
      }
    }
  }

  //第2种情况：farCandidates
  for (Frame* candidate : farCandidates)
  {
   ..........
  }

  //第3种情况：parent
  if(parent != 0 && forceParent)
  {
   ..........
  }

重点看下函数testConstraint()：

candidateTrackingReference->importFrame(candidate);

  Sim3 FtoC = candidateToFrame_initialEstimate.inverse(), CtoF = candidateToFrame_initialEstimate;
  Matrix7x7 FtoCInfo, CtoFInfo;

  //用金字塔3、4层求位姿变换
  float err_level3 = tryTrackSim3(
          newKFTrackingReference, candidateTrackingReference,   // A = frame; b = candidate
          SIM3TRACKING_MAX_LEVEL-1, 3,
          USESSE,
          FtoC, CtoF);

  // 检测返回的err_level3，太大就不符合呗
  if(err_level3 > 3000*strictness)
  {
    e1_out = e2_out = 0;
    //把这次失败的检测记录下来，下次不在进行相关检测了！！！
    newKFTrackingReference->keyframe->trackingFailed.insert(std::pair<Frame*,Sim3>(candidate, candidateToFrame_initialEstimate));
    return;
  }

  //用金字塔2层求位姿变换,进行一致性检测,同上
  .......
  //用金字塔2层求位姿变换,进行一致性检测，同上
  .......

 //4,3,2,1层都检测没问题了，在给e1_out、e2_out添加robustKernel后，返回
  const float kernelDelta = 5 * sqrt(6000*loopclosureStrictness);
  e1_out->robustKernel = new g2o::RobustKernelHuber();
  e1_out->robustKernel->setDelta(kernelDelta);
  e2_out->robustKernel = new g2o::RobustKernelHuber();
  e2_out->robustKernel->setDelta(kernelDelta);

看下上面代码中的tryTrackSim3()函数，你知道e1、e2是什么，都包含那些数据：

//用trackFrameSim3求BtoA的位姿变换，并记录结果（ A = frame; b = candidate）
  BtoA = constraintTracker->trackFrameSim3(
          A,
          B->keyframe,
          BtoA,
          lvlStart,lvlEnd);
  Matrix7x7 BtoAInfo = constraintTracker->lastSim3Hessian;
  float BtoA_meanResidual = constraintTracker->lastResidual;
  float BtoA_meanDResidual = constraintTracker->lastDepthResidual;
  float BtoA_meanPResidual = constraintTracker->lastPhotometricResidual;
  float BtoA_usage = constraintTracker->pointUsage;

  if (constraintTracker->diverged ||
      BtoA.scale() > 1 / Sophus::SophusConstants<sophusType>::epsilon() ||
      BtoA.scale() < Sophus::SophusConstants<sophusType>::epsilon() ||
      BtoAInfo(0,0) == 0 ||
      BtoAInfo(6,6) == 0)
  {
    return 1e20;
  }

//用trackFrameSim3求AtoB的位姿变换，并记录结果（ A = frame; b = candidate）
  AtoB = constraintTracker->trackFrameSim3(
          B,
          A->keyframe,
          AtoB,
          lvlStart,lvlEnd);
  Matrix7x7 AtoBInfo = constraintTracker->lastSim3Hessian;
  float AtoB_meanResidual = constraintTracker->lastResidual;
  float AtoB_meanDResidual = constraintTracker->lastDepthResidual;
  float AtoB_meanPResidual = constraintTracker->lastPhotometricResidual;
  float AtoB_usage = constraintTracker->pointUsage;


  if (constraintTracker->diverged ||
      AtoB.scale() > 1 / Sophus::SophusConstants<sophusType>::epsilon() ||
      AtoB.scale() < Sophus::SophusConstants<sophusType>::epsilon() ||
      AtoBInfo(0,0) == 0 ||
      AtoBInfo(6,6) == 0)
  {
      return 1e20;
  }

  //****************************公式(20)的代码实现**********************************
  // Propagate uncertainty (with d(a * b) / d(b) = Adj_a) and calculate Mahalanobis norm
  Matrix7x7 datimesb_db = AtoB.cast<float>().Adj();
  // 信息矩阵加权
  Matrix7x7 diffHesse = (AtoBInfo.inverse() + datimesb_db * BtoAInfo.inverse() * datimesb_db.transpose()).inverse();
  Vector7 diff = (AtoB * BtoA).log().cast<float>();

  // 一致性残差, 会给到g2o中.
  float reciprocalConsistency = (diffHesse * diff).dot(diff);

  //trackingsim3结果记录下来
  if(e1 != 0 && e2 != 0)
  {
    e1->firstFrame = A->keyframe;
    e1->secondFrame = B->keyframe;
    e1->secondToFirst = BtoA;
    e1->information = BtoAInfo.cast<double>();
    e1->meanResidual = BtoA_meanResidual;
    e1->meanResidualD = BtoA_meanDResidual;
    e1->meanResidualP = BtoA_meanPResidual;
    e1->usage = BtoA_usage;

    e2->firstFrame = B->keyframe;
    e2->secondFrame = A->keyframe;
    e2->secondToFirst = AtoB;
    e2->information = AtoBInfo.cast<double>();
    e2->meanResidual = AtoB_meanResidual;
    e2->meanResidualD = AtoB_meanDResidual;
    e2->meanResidualP = AtoB_meanPResidual;
    e2->usage = AtoB_usage;

    e1->reciprocalConsistency = e2->reciprocalConsistency = reciprocalConsistency;
  }

  return reciprocalConsistency;

通过testConstraint()对一致性的检测，计算出了加入g2o的参数，为optimizationIteration线程做了数据准备。好了现在回到findConstraintsForNewKeyFrames函数，第2种情况：farCandidates和第一种情况：closeCandidates类似，不再重复了。现在看下第三种情况：parent必须加入constraints里，为了这个目标也是煞费苦心啊。

//第3种情况：parent
if(parent != 0 && forceParent)
  {
    KFConstraintStruct* e1=0;
    KFConstraintStruct* e2=0;
    //这里阈值竟然给了100，上面可都是1.5啊，所以几乎一定会通过检测
    testConstraint(
            parent, e1, e2,
            candidateToFrame_initialEstimateMap[parent],
            100);
    if(enablePrintDebugInfo && printConstraintSearchInfo)
      printf(" PARENT (0)\n");

    if(e1 != 0)
    {
      constraints.push_back(e1);
      constraints.push_back(e2);
    }
    else //如果还是不好，那只能给一个先验值了，反正必须把parent加入一致性里。
    {
      float downweightFac = 5;
      const float kernelDelta = 5 * sqrt(6000*loopclosureStrictness) / downweightFac;
      printf("warning: reciprocal tracking on new frame failed badly, added odometry edge (Hacky).\n");

      poseConsistencyMutex.lock_shared();
      constraints.push_back(new KFConstraintStruct());
      constraints.back()->firstFrame = newKeyFrame;
      constraints.back()->secondFrame = newKeyFrame->getTrackingParent();
      constraints.back()->secondToFirst = constraints.back()->firstFrame->getScaledCamToWorld().inverse() * constraints.back()->secondFrame->getScaledCamToWorld();
      constraints.back()->information  <<
              0.8098,-0.1507,-0.0557, 0.1211, 0.7657, 0.0120, 0,
              -0.1507, 2.1724,-0.1103,-1.9279,-0.1182, 0.1943, 0,
              -0.0557,-0.1103, 0.2643,-0.0021,-0.0657,-0.0028, 0.0304,
            0.1211,-1.9279,-0.0021, 2.3110, 0.1039,-0.0934, 0.0005,
            0.7657,-0.1182,-0.0657, 0.1039, 1.0545, 0.0743,-0.0028,
            0.0120, 0.1943,-0.0028,-0.0934, 0.0743, 0.4511, 0,
              0,0, 0.0304, 0.0005,-0.0028, 0, 0.0228;
      constraints.back()->information *= (1e9/(downweightFac*downweightFac));
      constraints.back()->robustKernel = new g2o::RobustKernelHuber();
      constraints.back()->robustKernel->setDelta(kernelDelta);
      constraints.back()->meanResidual = 10;
      constraints.back()->meanResidualD = 10;
      constraints.back()->meanResidualP = 10;
      constraints.back()->usage = 0;

      poseConsistencyMutex.unlock_shared();
    }
  }

总算把一致性检测完了，下面就是收拾收拾了：

  //终于可以把新关键帧加入keyFrameGraph了
  keyFrameGraph->addKeyFrame(newKeyFrame);
  //把一致性加入 keyFrameGraph
  for(unsigned int i=0;i<constraints.size();i++)
      keyFrameGraph->insertConstraint(constraints[i]);

  newConstraintAdded = true;
  newConstraintCreatedSignal.notify_all();

到这里，新关键帧的一致性搜索就完事了，就等着优化线程处理了。

---

现在回到constraintSearchThreadLoop()函数中，来看下第1部分，有人会问为什么会有第1部分呢？个人以为新关键帧只能与时间在它前面的关键帧建立一致性是不妥的，也应该与它后面的关键帧建立一致性。而且，优化后帧的位姿是改变的，有再次优化的必要。这种循环优化能提高精度。这一部分与第2部分差不多：

if(newKeyFrames.size() == 0) // 已有关键帧重新进行一致性搜索
    {
      lock.unlock();
      keyFrameGraph->keyframesForRetrackMutex.lock();
      bool doneSomething = false;
      if(keyFrameGraph->keyframesForRetrack.size() > 10)
      {
        //随机选关键帧
    std::deque< Frame* >::iterator toReTrack = keyFrameGraph->keyframesForRetrack.begin()  + (rand() % (keyFrameGraph->keyframesForRetrack.size()/3));
    Frame* toReTrackFrame = *toReTrack;

    keyFrameGraph->keyframesForRetrack.erase(toReTrack);
    keyFrameGraph->keyframesForRetrack.push_back(toReTrackFrame);

    keyFrameGraph->keyframesForRetrackMutex.unlock();

        //注意参数变了：forceParent=false,useFABMAP=false, closeCandidatesTH=2.0
    int found = findConstraintsForNewKeyFrames(toReTrackFrame, false, false, 2.0);
    if(found == 0)
      failedToRetrack++;
    else
      failedToRetrack=0;

    if(failedToRetrack < (int)keyFrameGraph->keyframesForRetrack.size() - 5)
      doneSomething = true;
      }
      else
    keyFrameGraph->keyframesForRetrackMutex.unlock();

      lock.lock();

      if(!doneSomething)
      {
    if(enablePrintDebugInfo && printConstraintSearchInfo)
      printf("nothing to re-track... waiting.\n");
    newKeyFrameCreatedSignal.timed_wait(lock,boost::posix_time::milliseconds(500));
      }
    }

再次进入findConstraintsForNewKeyFrames函数，只是说下不同点：

  //re-track增量李代数差别太小，就不需要搜索了。
  if(!forceParent && (newKeyFrame->lastConstraintTrackedCamToWorld * 
    newKeyFrame->getScaledCamToWorld().inverse()).log().norm() < 0.01)
    return 0;
   ...........
    // erase the ones that are already neighbours.已经建立的一致性就别搜一次了。
  for(std::unordered_set<Frame*>::iterator c = candidates.begin(); c != candidates.end();)
  {
    if(newKeyFrame->neighbors.find(*c) != newKeyFrame->neighbors.end())
    {
      c = candidates.erase(c);
    }
    else
      ++c;
  }
  ........
  // erase the ones that we tried already before (close) 通过位姿来确定neighbour，与错误太近就不要了
  for(std::unordered_set<Frame*>::iterator c = closeCandidates.begin(); c != closeCandidates.end();)
  {
    if(newKeyFrame->trackingFailed.find(*c) == newKeyFrame->trackingFailed.end())
    {
      ++c;
      continue;
    }

    auto range = newKeyFrame->trackingFailed.equal_range(*c);

    bool skip = false;
    Sim3 f2c = candidateToFrame_initialEstimateMap[*c].inverse();
    for (auto it = range.first; it != range.second; ++it)
    {
      if((f2c * it->second).log().norm() < 0.1)
      {
    skip=true;
    break;
      }
    }

    if(skip)
    {
      c = closeCandidates.erase(c);
    }
    else
      ++c;
  }

  // erase the ones that are already neighbours (far)通过位姿来确定neighbour，与错误太近，或位姿变换太小就不要了
  for(unsigned int i=0;i<farCandidates.size();i++)
  {
    if(newKeyFrame->trackingFailed.find(farCandidates[i]) == newKeyFrame->trackingFailed.end())
      continue;

    auto range = newKeyFrame->trackingFailed.equal_range(farCandidates[i]);

    bool skip = false;
    for (auto it = range.first; it != range.second; ++it)
    {
      if((it->second).log().norm() < 0.2)
      {
    skip=true;
    break;
      }
    }

    if(skip)
    {
      if(enablePrintDebugInfo && printConstraintSearchInfo)
    printf("SKIPPING %d on %d (FAR), cause we already have tried it.\n", farCandidates[i]->id(), newKeyFrame->id());
      farCandidates[i] = farCandidates.back();
      farCandidates.pop_back();
      i--;
    }
  }

这里也没太多可解释的，第3部分，估计不需要解释了，应该都能看懂。