Method detectLoop

pose_graph/src/pose_graph/PoseGraph.cpp:170–224

}

int PoseGraph::detectLoop(Keyframe* keyframe, int frame_index) {
  cv::Mat compressed_image;

  if (keyframe->bowVec.empty() || keyframe->featVec.empty()) {
    // Feature vector associate features with nodes in the 4th level (from leaves up)
    // We assume the vocabulary tree has 6 levels, change the 4 otherwise
    voc->transform(keyframe->brief_descriptors, keyframe->bowVec);
  }

  float min_score = 1.0;
  for (std::map<Keyframe*, int>::iterator mit = keyframe->mConnectedKeyFrameWeights.begin();
       mit != keyframe->mConnectedKeyFrameWeights.end();
       mit++) {
    // BowVector neigh_vec;

    if (mit->first->bowVec.empty() || mit->first->featVec.empty())
      voc->transform(mit->first->brief_descriptors, mit->first->bowVec);

    float score = voc->score(keyframe->bowVec, mit->first->bowVec);
    // std::cout << "Score in neigh frames: " << score << " with Id: " << mit->first->index << std::endl;
    if (score < min_score) min_score = score;
  }

  // std::cout<< "Min BoW Score: "<< min_score << std::endl;

  // first query; then add this frame into database!
  DBoW2::QueryResults ret;
  db.query(keyframe->bowVec, ret, 4, frame_index - 50);
  db.add(keyframe->brief_descriptors);

  bool find_loop = false;
  cv::Mat loop_result;

  for (unsigned int i = 0; i < ret.size(); i++) {
    if (ret[i].Score > 0.60 * min_score) {
      find_loop = true;
      // std::cout<< "Query KF: "<< frame_index<< " candidate kf: "<< ret[i].Id << std::endl;
    }
  }

  // Note:  Returns depending on the highest score
  if (find_loop && frame_index > 50) {
    int best_index = -1;
    float best_score = 0.0;
    for (unsigned int i = 0; i < ret.size(); i++) {
      if (best_index == -1 || (ret[i].Score > best_score && ret[i].Score > 0.60 * min_score)) {
        best_index = ret[i].Id;
        best_score = ret[i].Score;
      }
    }
    return best_index;
  } else {
    return -1;
  }
}

void PoseGraph::optimize4DoFPoseGraph() {
  while (true) {