做slam怎么能少得了闭环。
闭环检测的代码在文件include/hdl_graph_slam/loop_detector.hpp中,整个流程分布的很清晰,我们直接进入看代码的环节了
1. 整体流程
在调用闭环检测功能时,实际是调用的函数detect,所以我们直接看这个函数,就可以看出它的流程
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) {
auto candidates = find_candidates(keyframes, new_keyframe);
auto loop = matching(candidates, new_keyframe, graph_slam);
if(loop) {
detected_loops.push_back(loop);
}
}
return detected_loops;
}
它的输入是:
1)keyframes:已有的关键帧
2)new_keyframes:新增的关键帧
3)graph_slam:这个参数没有被用到,估计作者可能是原计划要用,但后来没用到,也忘了删了
它的输出是:
1)std::vector<Loop::Ptr>:所有检测成功的匹配关系
具体来讲,包括它是由哪两帧匹配的,匹配计算得到的相对位姿是多少。
这个看Loop这个结构提的定义就明白了
struct Loop {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using Ptr = std::shared_ptr<Loop>;
Loop(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Matrix4f& relpose)
: key1(key1),
key2(key2),
relative_pose(relpose)
{}
public:
KeyFrame::Ptr key1;
KeyFrame::Ptr key2;
Eigen::Matrix4f relative_pose;
};
它包含的就是上面我们所提到的这三个信息。
由于每次检测可能有多个新增的关键帧要参与,有可能检测到多对匹配关系,所以这里用一个vector把所有关系同时输出。
弄明白输入输出,这个函数的功能就可以概括了:
从已有的关键帧中,检测与新增的关键帧是否有闭环关系,如果有,则返回所有检测结果。
而且上面的代码还告诉了我们)进行闭环检测的步骤:
1)检测满足条件的关键帧,以进行下一步匹配
2)对检测到的关键帧进行匹配,找出匹配精度最好的一帧,并返回匹配关系
3)把第2)步找到的匹配关系放入容器中
4)循环上面三步,直到所有新的关键帧遍历完
上面四步中,第3)和第4)部就是几行语句而已,需要展开介绍的是第1)和第2)步,下面我们就分别对这两步进行介绍。
2. 检测满足条件的关键帧
这个功能被封装在了函数find_candidates中,我们看它的代码
std::vector<KeyFrame::Ptr> find_candidates(const std::vector<KeyFrame::Ptr>& keyframes, const KeyFrame::Ptr& new_keyframe) const {
// too close to the last registered loop edge
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) {
// traveled distance between keyframes is too small
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();
// estimated distance between keyframes is too small
double dist = (pos1.head<2>() - pos2.head<2>()).norm();
if(dist > distance_thresh) {
continue;
}
candidates.push_back(k);
}
return candidates;
}
从上面的代码中我们可以看到,所谓的满足条件,指的是满足以下条件:
1)新的关键帧离上一个被闭环的帧“里程距离”不能过小
注意,这里特意强调了是“里程距离”,我们举个例子,来弄明白这个概念。
比如第1帧往前走了1米得到了第2帧,第2帧往后走了1米得到了第3帧,那么第1帧和第3帧的“直线距离”就是0,但是总的里程是2米,所以他们的“里程距离”就是2。闭环应该找“直线距离”尽量进而“里程距离”尽量远的,所以这里才要求“里程距离”不能过小。
其实我觉得给每一个关键帧一个编号,编号顺序递增,用编号计算一个“编号距离”应该更简单,不用每次还要计算“里程距离”这么麻烦。
弄明白了它的原理,我们就清楚它的作用了,就是把闭环关系弄的稀疏一些,别整那么多,如果来了是个关键帧,没一帧都弄了一个闭环边添加到图里,也没那个必要,所以有了这一步以后,我们会在所有新的关键帧中取一小部分建立闭环关系,既能起到约束作用,又不至于把模型弄的复杂。
2)已有关键帧离新的关键帧的“里程距离”不能过小
3)已有关键帧和新的关键帧之间的“直线距离”要足够小
能够同时满足这三个条件的就可以进入下一步的匹配环节了。
3. 闭环匹配
上一步索引出来的关键帧还要通过这一步的检验,才能够算是真正成功实现了闭环匹配。这一步在函数matching中,我们还是直接看代码
Loop::Ptr matching(const std::vector<KeyFrame::Ptr>& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam) {
if(candidate_keyframes.empty()) {
return nullptr;
}
registration->setInputTarget(new_keyframe->cloud);
double best_score = std::numeric_limits<double>::max();
KeyFrame::Ptr best_matched;
Eigen::Matrix4f relative_pose;
std::cout << std::endl;
std::cout << "--- loop detection ---" << std::endl;
std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl;
std::cout << "matching" << std::flush;
auto t1 = ros::Time::now();
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);
std::cout << "." << std::flush;
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();
}
auto t2 = ros::Time::now();
std::cout << " done" << std::endl;
std::cout << "best_score: " << boost::format("%.3f") % best_score << " time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
if(best_score > fitness_score_thresh) {
std::cout << "loop not found..." << std::endl;
return nullptr;
}
std::cout << "loop found!!" << std::endl;
std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl;
last_edge_accum_distance = new_keyframe->accum_distance;
return std::make_shared<Loop>(new_keyframe, best_matched, relative_pose);
}
从上面的代码中我们可以看到,它的核心流程可以概括如下:
1)对每一个关键帧都和新帧进行点云匹配,看匹配精度满足闭环的基本要求
2)如果满足,则在所有满足条件的关键帧中找出匹配精度最高的那一帧
4. 一点思考
在上面的流程中,我认为还是有一些不合理的地方
1)匹配点云的时候还是只对两帧进行匹配,这一点我们在讲前端里程计的时候就提到过,我认为把关键帧的前后几帧都找出来,拼成一个局部地图再去匹配应该精度会更高,毕竟两帧点云匹配太稀疏了。
2)对所有满足条件的关键帧都做一次点云匹配太浪费时间了,帧间匹配还是挺耗时间的,当地图稍微大一些,有几千个关键帧的时候,一个闭环检测要匹配多少次,这肯定不行。我认为找一个距离最近的关键帧,然后以它为中心取前后几帧构建一个局部地图去匹配就行了,精度满足就建一条闭环边,不满足就不要了,等下一次。
评论(0)
您还未登录,请登录后发表或查看评论