C++ (Cpp) KeyFrame::Distance Examples

Example #1

File: MapMakerBase.cpp Project: wavelab/mcptam

// Finds N closest KeyFrames
std::vector<KeyFrame*> MapMakerBase::NClosestKeyFrames(KeyFrame& kfSrc, unsigned int N)
{
  std::vector<std::pair<double, KeyFrame*>> vScoresAndKFs;
  for (MultiKeyFramePtrList::iterator it = mMap.mlpMultiKeyFrames.begin(); it != mMap.mlpMultiKeyFrames.end(); ++it)
  {
    MultiKeyFrame& mkf = *(*it);

    // Have to iterate through each MultiKeyFrame as well to get KeyFrames
    for (KeyFramePtrMap::iterator jit = mkf.mmpKeyFrames.begin(); jit != mkf.mmpKeyFrames.end(); jit++)
    {
      KeyFrame& kf = *(jit->second);

      if (&kf == &kfSrc)
        continue;

      double dDist = kfSrc.Distance(kf);
      vScoresAndKFs.push_back(std::make_pair(dDist, &kf));
    }
  }

  if (N > vScoresAndKFs.size())  // if we expect too many neighbors
    N = vScoresAndKFs.size();    // reduce numbe that will be returned

  // Sort the first N entries by score
  std::partial_sort(vScoresAndKFs.begin(), vScoresAndKFs.begin() + N, vScoresAndKFs.end());

  std::vector<KeyFrame*> vResult;
  for (unsigned int i = 0; i < N; i++)
    vResult.push_back(vScoresAndKFs[i].second);

  return vResult;
}

Example #2

File: MapMakerBase.cpp Project: wavelab/mcptam

// Finds nMaxNum closest KeyFrames, within a given distance, within a given region
std::vector<KeyFrame*> MapMakerBase::ClosestKeyFramesWithinDist(KeyFrame& kf, double dThreshDist, unsigned nMaxNum,
    KeyFrameRegion region)
{
  std::vector<KeyFrame*> vResult;

  std::vector<std::pair<double, KeyFrame*>> vpDistsAndKeyFrames;
  MultiKeyFrame& parent = *kf.mpParent;

  if (region == KF_ONLY_SELF)  // Only search through parent's keyframes
  {
    for (KeyFramePtrMap::iterator it = parent.mmpKeyFrames.begin(); it != parent.mmpKeyFrames.end(); it++)
    {
      KeyFrame& currentKF = *(it->second);
      if (&currentKF == &kf)
        continue;

      double dDist = kf.Distance(currentKF);
      if (dDist <= dThreshDist)
      {
        vpDistsAndKeyFrames.push_back(std::make_pair(dDist, &currentKF));
      }
    }
  }
  else  // Otherwise search all keyframes in the map
  {
    for (MultiKeyFramePtrList::iterator it = mMap.mlpMultiKeyFrames.begin(); it != mMap.mlpMultiKeyFrames.end(); ++it)
    {
      MultiKeyFrame& mkf = *(*it);

      if (&mkf == &parent && region == KF_ONLY_OTHER)
        continue;

      for (KeyFramePtrMap::iterator jit = mkf.mmpKeyFrames.begin(); jit != mkf.mmpKeyFrames.end(); ++jit)
      {
        KeyFrame& currentKF = *(jit->second);
        if (&currentKF == &kf)
          continue;

        double dDist = kf.Distance(currentKF);
        if (dDist <= dThreshDist)
        {
          vpDistsAndKeyFrames.push_back(std::make_pair(dDist, &currentKF));
        }
      }
    }
  }

  if (!vpDistsAndKeyFrames.empty())
  {
    if (nMaxNum > vpDistsAndKeyFrames.size())  // if we expect too many neighbors
      nMaxNum = vpDistsAndKeyFrames.size();    // reduce number that will be returned

    // Sort the first nMaxNum entries by score
    std::partial_sort(vpDistsAndKeyFrames.begin(), vpDistsAndKeyFrames.begin() + nMaxNum, vpDistsAndKeyFrames.end());

    for (unsigned int i = 0; i < nMaxNum; i++)
      vResult.push_back(vpDistsAndKeyFrames[i].second);
  }

  return vResult;
}