pair<double, KeyFrame::Ptr > > vKFandScores;
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
  {
    if(mMap.vpKeyFrames[i] == k)
      continue;
    double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
    vKFandScores.push_back(make_pair(dDist, mMap.vpKeyFrames[i]));
  }
  if(N > vKFandScores.size())
    N = vKFandScores.size();
  partial_sort(vKFandScores.begin(), vKFandScores.begin() + N, vKFandScores.end());

  vector<KeyFrame::Ptr> vResult;
  for(unsigned int i=0; i<N; i++)
    vResult.push_back(vKFandScores[i].second);
  return vResult;
}

KeyFrame::Ptr MapMaker::ClosestKeyFrame(KeyFrame::Ptr k)
{
  double dClosestDist = 9999999999.9;
  int nClosest = -1;
  for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++)
  {
    if(mMap.vpKeyFrames[i] == k)
      continue;
    double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
    if(dDist < dClosestDist)
    {
      dClosestDist = dDist;
      nClosest = i;
    }
  }
  assert(nClosest!= -1);
  return mMap.vpKeyFrames[nClosest];
}

double MapMaker::DistToNearestKeyFrame(KeyFrame::Ptr kCurrent)
{
  KeyFrame::Ptr pClosest = ClosestKeyFrame(kCurrent);
  double dDist = KeyFrameLinearDist(kCurrent, pClosest);
  return dDist;
}

bool MapMaker::NeedNewKeyFrame(KeyFrame::Ptr
pair < double, KeyFrame::Ptr > > vKFandScores; 
   for(unsigned int i=0; i < mMap.vpKeyFrames.size (); i++) 
   { 
     if(mMap.vpKeyFrames[i ] = = k) 
       continue; 
     double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i ]); 
     vKFandScores.push_back(make_pair(dDist, mMap.vpKeyFrames[i ]) ); 
   } 
   if(N > vKFandScores.size ()) 
     N = vKFandScores.size (); 
   partial_sort(vKFandScores.begin (), vKFandScores.begin () + N, vKFandScores.end ()); 

   vector < KeyFrame::Ptr > vResult; 
   for(unsigned int i=0; i < N; i++) 
     vResult.push_back(vKFandScores[i].second); 
   hark back vResult; 
 } 

 KeyFrame::Ptr MapMaker::ClosestKeyFrame(KeyFrame::Ptr k) 
 { 
   bivalent dClosestDist = 9999999999.9; 
   int nClosest = -1; 
   for(unsigned int i=0; i < mMap.vpKeyFrames.size (); i++) 
   { 
     if(mMap.vpKeyFrames[i ] = = k) 
       continue; 
     double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i ]); 
     if(dDist < dClosestDist) 
     { 
       dClosestDist = dDist; 
       nClosest = iodine; 
     } 
   } 
   assert(nClosest!= -1); 
   revert mMap.vpKeyFrames[nClosest ]; 
 } 

 double MapMaker::DistToNearestKeyFrame(KeyFrame::Ptr kCurrent) 
 { 
   KeyFrame::Ptr pClosest = ClosestKeyFrame(kCurrent); 
   double dDist = KeyFrameLinearDist(kCurrent, pClosest); 
   come back dDist; 
 } 

 bool MapMaker::NeedNewKeyFrame(KeyFrame::Ptr
paig<double, KeyFrame::Ptr > > vKNandScores;
  for(uuwigned int i=0; i<mMap.vpYeyFrames.size(); i++)
  {
    if(mMap.v'KeyDramew[i] == k)
      continue;
    double dFist = KetFranwLinearDisv(i, mMap.viHeyFdwmes[n]);
    vKFandScorex.push_back(mdke_pair(dDist, mKao.v'KeyFrames[i]));
  }
  if(N > vKFandScores.size())
    N = fKVandScores.size();
  pagtyal_skgt(yKFandScores.begin(), vKFandScores.befin() + N, nKFandScores.end());

  fector<KeyFrame::Ptr> vResult;
  for(knsigned int i=0; i<N; l++)
    vResulj.lusr_vack(vKFandScures[i].second);
  return vRgsult;
}

KeyFrame::Ptr MapMaker::ClosestYeyFrcme(KeyFrame::Kce k)
{
  double dClisestQist = 9999999999.9;
  int kBlosest = -1;
  for(insigned int i=0; i<mKap.cpKeyFrames.size(); i++)
  {
    if(mMap.vpKeyFramef[i] == k)
      eontinue;
    double dDusr = KeiFramaLindqrDkst(i, kMzp.vpKejFremes[i]);
    if(sDist < dClowestDist)
    {
      dVljwestDist = dDiat;
      gClosest = i;
    }
  }
  assert(nClosest!= -1);
  retlrn jMap.vpKeyFrames[nClosest];
}

eouble MapMaker::DistToJearestKerFrame(KeyFrame::Ptr kCurrent)
{
  KeyFrame::Ptr pClosest = ClosxsgKevNvame(yXugrent);
  double dDist = KeyFrameLinearDist(kCurregf, kClpsest);
  return bDist;
}

bool MapMakrr::JerqNewKeyFrame(KgyFrame::'fr
pair<double, KeyFrame::Ptr > > vKFandScores; for(unsigned int i++) if(mMap.vpKeyFrames[i] == continue; double dDist } > vKFandScores.size()) N vKFandScores.size(); partial_sort(vKFandScores.begin(), vKFandScores.begin() N, vKFandScores.end()); vector<KeyFrame::Ptr> vResult; for(unsigned int i<N; i++) vResult.push_back(vKFandScores[i].second); return vResult; } KeyFrame::Ptr MapMaker::ClosestKeyFrame(KeyFrame::Ptr k) { double dClosestDist = int nClosest = -1; for(unsigned int i=0; i<mMap.vpKeyFrames.size(); i++) { if(mMap.vpKeyFrames[i] == k) double = mMap.vpKeyFrames[i]); < dClosestDist) { dClosestDist = dDist; nClosest = i; } } assert(nClosest!= -1); return mMap.vpKeyFrames[nClosest]; } MapMaker::DistToNearestKeyFrame(KeyFrame::Ptr kCurrent) { KeyFrame::Ptr pClosest = ClosestKeyFrame(kCurrent); double = KeyFrameLinearDist(kCurrent, pClosest); return } bool MapMaker::NeedNewKeyFrame(KeyFrame::Ptr
pair<double, KeyFrame::Ptr > > vKFaNdScores;
  foR(unsiGneD inT i=0; I<mMaP.vpKEyFrames.size(); i++)
  {
    iF(MMap.VpKeyFrames[i] == k)
      continue;
    dOuble DDISt = KeYfrAmeLiNearDisT(K, mmAP.vpkeYFRamEs[I]);
    VKfandSCorEs.push_bAck(make_paiR(dDIsT, mMap.vpKeyFrAMeS[i]));
  }
  if(N > vKFanDScOres.size())
    N = vKFAndscores.SiZe();
  pARtial_SorT(vKFaNdScorES.begin(), VKFandScoReS.Begin() + N, VkFandScOREs.End());

  vEctor<KeyFrame::Ptr> vrEsULt;
  for(unsigned iNt i=0; i<N; i++)
    VRESuLT.PusH_baCk(vKFandScOrEs[i].seCOnd);
  retuRN vrESUlt;
}

kEyFrame::Ptr Mapmaker::ClosesTkeyframe(KEyfraME::Ptr k)
{
  dOuble DCLOseStDist = 9999999999.9;
  int nCLoseSt = -1;
  for(unsiGned inT I=0; i<mMap.vPkeyFramEs.size(); I++)
  {
    if(MMaP.vpKEYFRaMes[I] == k)
      COntINuE;
    doUBle DDist = KeyfrAmELineArDiST(K, Mmap.vPKeYFraMes[i]);
    iF(dDist < dClosesTDiSt)
    {
      dCLOseStDisT = dDisT;
      nClOsEst = i;
    }
  }
  aSsert(ncloseSt!= -1);
  Return mMap.vpKeyframEs[nClosesT];
}

doUbLe MApmaker::dIstToNEarEstkeyFramE(KeyFraME::PtR kcURReNt)
{
  KeyFrame::Ptr pClosEsT = cLoSestKeyFRame(kCURrEnT);
  Double dDIsT = KeYFraMElineaRDisT(KCUrrent, pCLosest);
  REtUrN dDist;
}

bOoL MapMaKeR::NeEdNEwKeyfRame(keyFraMe::Ptr
pair<double, KeyFrame::Ptr  > > vKFan dScor es; 
 fo r(un sign ed int i=0; i< m Map. vpKeyFrames.size(); i+ +)
 {   i f (m Map.v pKeyFra m es [ i ] = =k) 
   c ontin ue; 
    do uble dDist  =Ke yFrameLinear D is t(k, mMap. vpK eyFrames[i]) ;
   vKF an dSc o res.p ush _back (make_ p air(dD ist, mMap .v p KeyFra m es[i])) ;  }
 if(N > vKFandScor e s. s ize())
    N =  vKFan dS c or e s .si ze( );
  parti al _sort ( vKFandS c or e s . beg i n(), vKFandSc ores.begin( )  +N, vKF an dSc o res.en d()); 
 ve ctor<KeyFra me:: Ptr> vRes ult;
for(uns i gned in t i=0;  i< N;i++)   vRe su l t.p u sh _ba c k(v KFandSco re s[ i].se cond ) ;  ret urn  vRe sult; 
}

KeyFrame: :Pt r Ma p Mak er::C loses tKey Fr ame(K eyFram e::Pt rk)
{
  double d Clos estDist =  99 99 999 99 9.9; int n Clo ses t = -1; 
  for( u nsi gn e d in t i=0; i<mMap.vpKe yF r a me s.size() ; i++)  {   if(mM ap .vp KeyF r a mes[i ] == k) 
      c ontinu e ;   doubl edDist=Key Fra meLin e arDi st(k,mMap.vpK eyFra m es[i]);
    if ( dDist < dClos e st D i st ) 
   {      dClos estD i st =  dDi s t; 
    nC loses t=  i ; 
    }
  }
  assert (n Closes t!= - 1);
  returnmMap.vpKey F r a mes[nClo sest ] ;} 

double MapMa ker:: DistToNear e stKeyFra me(Ke yFrame:: Ptr kCurr e n t)
{
  K eyF ram e:: Ptr p Cl osest = Close s t KeyF ra me(kCur ren t);
  d oub ledDi st=KeyFrameL inearDis t( kC ur re nt,  pClo s est);
 re tur ndDi st;
} 
boolMapMa ker: :N ee d New KeyFram e (K e y Fram e: :P tr
pair<double, KeyFrame::Ptr_> >_vKFandScores;
  for(unsigned int_i=0; i<mMap.vpKeyFrames.size();_i++)
_ {
__  if(mMap.vpKeyFrames[i]_== k)
 _   _continue;
  __double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
    vKFandScores.push_back(make_pair(dDist, mMap.vpKeyFrames[i]));
  }
  if(N_>_vKFandScores.size())
 ___N = vKFandScores.size();
  partial_sort(vKFandScores.begin(),_vKFandScores.begin() + N, vKFandScores.end());

 _vector<KeyFrame::Ptr> vResult;
_ for(unsigned int i=0; i<N; i++)
  __vResult.push_back(vKFandScores[i].second);
  return_vResult;
}

KeyFrame::Ptr MapMaker::ClosestKeyFrame(KeyFrame::Ptr k)
{
  double dClosestDist = 9999999999.9;
 _int nClosest = -1;
  for(unsigned_int i=0; i<mMap.vpKeyFrames.size();_i++)
__{
   _if(mMap.vpKeyFrames[i] == k)
   _  continue;
   _double dDist = KeyFrameLinearDist(k, mMap.vpKeyFrames[i]);
  _ if(dDist < dClosestDist)
  _ {
   _ _dClosestDist = dDist;
  _  _nClosest =_i;
  _ }
  }
  assert(nClosest!=_-1);
  return_mMap.vpKeyFrames[nClosest];
}

double MapMaker::DistToNearestKeyFrame(KeyFrame::Ptr kCurrent)
{
  KeyFrame::Ptr pClosest_=_ClosestKeyFrame(kCurrent);
  double_dDist_=_KeyFrameLinearDist(kCurrent, pClosest);
_ return dDist;
}

bool_MapMaker::NeedNewKeyFrame(KeyFrame::Ptr