hdl_graph_slam源码解析（五）

5. loop detection

hdl_graph_slam算法中的回环检测部分主要是在loop_detector.hpp代码中定义并实现的，主要分为两部分，分别是：从历史关键帧中寻找可能的回环、对可能的回环进行点云配准确定回环。因此，整体的代码结构并不复杂：

std::vector<Loop:Ptr> detect(const std::vector<Keyframe::Ptr>& keyframes,const std::deque<Keyframe::Ptr>& new_keyframes, hdl_graph_slam::GraphSLAM& graph_slam){
	std::vector<Loop::Ptr> detected_loops;
	for(const auto& new_keyframe:new_keyframes){
		//在历史关键帧中寻找可能的回环(candidates)
		auto candidates = find_candidates(keyframes, new_keyframe);
		//分别将可能的回环(candidates)与new_keyframe配准，得到检测的回环
    	auto loop = matching(candidates, new_keyframe, graph_slam);
		if(loop){
			detected_loops.push_back(loop);
		}
	}
	return deteted_loops;
}

在寻找可能回环(candidates)的时候，主要遵循两个原则：第一个准则是两个关键帧之间的运动距离大于某一个阈值，注意这里是距离而不是位移哦，这一标准通常意味着从时序上比较相近的关键帧不是我们需要检测的回环。例如，在当前时刻有一个关键帧$k_1$，可能1s后又有一个关键帧$k_2$，关键帧$k_1$和$k_2$的点云可能比较相似，这一标准可以避免将$k_1$和$k_2$误检测为关键帧。第二个准则是两个关键帧之间估计的位移小于某一个阈值，也就是对应着紫色圆圈了。这一准则可以减少需要进行配准的关键帧数量，从而加快计算。话不多说，继续看源码吧。

5.1 find_candidates

//寻找可能的回环(candidates)
std::vector<Keyframe::Ptr> find_candidates(const std::vector<KeyFrame::Ptr>& keyframes, const KeyFrame::Ptr& new_keyframe) const {
	if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) {
      return std::vector<KeyFrame::Ptr>();
    }
    std::vector<Keyframe::Ptr> candidates;
    candidates.reserve(32);
    
    for(const auto& k : keyframes) {
      // 准则1:关键帧之间运动的距离大于某一个阈值
      if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) {
        continue;
      }

      const auto& pos1 = k->node->estimate().translation();
      const auto& pos2 = new_keyframe->node->estimate().translation();

      // 准则2:关键帧之间位姿估计的位移小于某个阈值
      double dist = (pos1.head<2>() - pos2.head<2>()).norm();
      if(dist > distance_thresh) {
        continue;
      }
      candidates.push_back(k);
    }
    return candidates;
}

5.2 matching

//点云配准以寻找正确的回环
Loop::Ptr matching(const std::vector<KeyFrame::Ptr>& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam){
	//no candidate, then return nullptr
	if(candidate_keyframes.empty()) {
      return nullptr;
    }
    registration->setInputTarget(new_keyframe->cloud);		//pcl中点云配准算法,设置目标点云
    double best_score = std::numeric_limits<double>::max();
    KeyFrame::Ptr best_matched;
    Eigen::Matrix4f relative_pose;
    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    //配准并得到两关键帧之间的相对位姿
    for(const auto& candidate : candidate_keyframes) {
      registration->setInputSource(candidate->cloud);
      //利用估计的关键帧相对位姿作为配准初始值
      Eigen::Matrix4f guess = (new_keyframe->node->estimate().inverse() * candidate->node->estimate()).matrix().cast<float>();
      guess(2, 3) = 0.0;
      registration->align(*aligned, guess);
      double score = registration->getFitnessScore(fitness_score_max_range);
      //判断配准是否收敛且配准评分
      if(!registration->hasConverged() || score > best_score) {
        continue;
      }

      best_score = score;
      best_matched = candidate;
      relative_pose = registration->getFinalTransformation();
    }
    if(best_score > fitness_score_thresh) {
      std::cout << "loop not found..." << std::endl;
      return nullptr;
    }
    
    last_edge_accum_distance = new_keyframe->accum_distance;
    return std::make_shared<Loop>(new_keyframe, best_matched, relative_pose);
}