约束计算

闭环检测的策略:搜索闭环,通过匹配检测是否是闭环,采用了分支定界法。

前已经述及PoseGraph的内容,此处继续。位姿图类定义了pose_graph::ConstraintBuilder constraint_builder_对象。

1.ConstraintBuilder类图

定义了SubmapScanMatcher的键值对。

 // Map of already constructed scan matchers by 'submap_id'.
std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_ GUARDED_BY(mutex_);

SubmapScanMatcher结构体定义如下:

   struct SubmapScanMatcher
{
const ProbabilityGrid* probability_grid;
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher>
fast_correlative_scan_matcher;
};

注意ConstraintBuilder::ComputeConstraint方法,MaybeAddConstraint和MaybeAddGlobalConstraint都调用了该方法。

 void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, bool match_full_submap,
const mapping::TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id); // The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'filtered_gravity_aligned_point_cloud' in node j,
// - the initial guess 'initial_pose' for (map <- node j),
// - the result 'pose_estimate' of Match() (map <- node j).
// - the ComputeSubmapPose() (map <- submap i)
float score = .;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); // Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
// 2. Prune if the score is too low.
// 3. Refine.
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, );
CHECK_GE(submap_id.trajectory_id, );
} else {
return;
}
} else {
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
return;
}
}
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
} // Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher->probability_grid,
&pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset(new Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP}); if (options_.log_matches()) {
std::ostringstream info;
info << "Node " << node_id << " with "
<< constant_data->filtered_gravity_aligned_point_cloud.size()
<< " points on submap " << submap_id << std::fixed;
if (match_full_submap) {
info << " matches";
} else {
const transform::Rigid2d difference =
initial_pose.inverse() * pose_estimate;
info << " differs by translation " << std::setprecision()
<< difference.translation().norm() << " rotation "
<< std::setprecision() << std::abs(difference.normalized_angle());
}
info << " with score " << std::setprecision() << . * score << "%.";
LOG(INFO) << info.str();
}
}

这里出现了scan_matching::FastCorrelativeScanMatcher,另一种扫描匹配的方法。论文中介绍的分支定界法就在这个类中实现。

以上FastCorrelativeScanMatcher::Match和FastCorrelativeScanMatcher::MatchFullSubmap方法都调用了FastCorrelativeScanMatcher::MatchWithSearchParameters方法。

FastCorrelativeScanMatcher::MatchWithSearchParameters调用了FastCorrelativeScanMatcher::BranchAndBound方法。

Tips:总结一下出现的几种扫描匹配的方法?

RealTimeCorrelativeScanMatcher

FastCorrelativeScanMatcher

 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
SearchParameters search_parameters,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, float min_score, float* score,
transform::Rigid2d* pose_estimate) const
{
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits()); const std::vector<Candidate> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
const Candidate best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);//分支定界法
if (best_candidate.score > min_score) {
*score = best_candidate.score;
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return true;
}
return false;
}

2.分支定界法

FastCorrelativeScanMatcher::BranchAndBound,......

 Candidate FastCorrelativeScanMatcher::BranchAndBound(
const std::vector<DiscreteScan>& discrete_scans,
const SearchParameters& search_parameters,
const std::vector<Candidate>& candidates, const int candidate_depth,
float min_score) const
{
if (candidate_depth == )
{
// Return the best candidate.
return *candidates.begin();
} Candidate best_high_resolution_candidate(, , , search_parameters);
best_high_resolution_candidate.score = min_score;
for (const Candidate& candidate : candidates)
{
if (candidate.score <= min_score) { break; }
std::vector<Candidate> higher_resolution_candidates;
const int half_width = << (candidate_depth - );
for (int x_offset : {, half_width})
{
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x) {
break;
}
for (int y_offset : {, half_width}) {
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y) {
break;
}
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - ),
discrete_scans, search_parameters,
&higher_resolution_candidates);
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - ,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}

Cartographer源码阅读(9):图优化的前端——闭环检测的更多相关文章

  1. Cartographer源码阅读(1):程序入口

    带着几个思考问题: (1)IMU数据的使用,如何融合,Kalman滤波? (2)图优化的具体实现,闭环检测的策略? (3)3D激光的接入和闭环策略? 1. 安装Kdevelop工具: http://b ...

  2. Cartographer源码阅读(3):程序逻辑结构

    Cartographer早期的代码在进行3d制图的时候使用了UKF方法,查看现有的tag版本,可以转到0.1.0和0.2.0查看,包含kalman_filter文件夹. 文件夹中的pose_track ...

  3. Cartographer源码阅读(5):PoseGraph位姿图

    PoseGraph位姿图 mapping2D::PoseGraph类的注释: // Implements the loop closure method called Sparse Pose Adju ...

  4. Cartographer源码阅读(6):LocalTrajectoryBuilder和PoseExtrapolator

    LocalTrajectoryBuilder意思是局部轨迹的构建,下面的类图中方法的参数没有画进去. 注意其中的三个类:PoseExtrapolator类,RealTimeCorrelativeSca ...

  5. Cartographer源码阅读(4):Node和MapBuilder对象2

    MapBuilder的成员变量sensor::Collator sensor_collator_; 再次阅读MapBuilder::AddTrajectoryBuilder方法.首先构造了mappin ...

  6. Cartographer源码阅读(8):imu_tracker

    IMU的输入为imu_linear_acceleration 和  imu_angular_velocity 线加速和角速度.最终作为属性输出的是方位四元数.  Eigen::Quaterniond ...

  7. Cartographer源码阅读(2):Node和MapBuilder对象

    上文提到特别注意map_builder_bridge_.AddTrajectory(x,x),查看其中的代码.两点: 首先是map_builder_.AddTrajectoryBuilder(...) ...

  8. Cartographer源码阅读(7):轨迹推算和位姿推算的原理

    其实也就是包括两个方面的内容:类似于运动模型的位姿估计和扫描匹配,因为需要计算速度,所以时间就有必要了! 1. PoseExtrapolator解决了IMU数据.里程计和位姿信息进行融合的问题. 该类 ...

  9. 【原】SDWebImage源码阅读(四)

    [原]SDWebImage源码阅读(四) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 SDWebImage中主要实现了NSURLConnectionDataDelega ...

随机推荐

  1. [elk]elasticsearch实现冷热数据分离

    本文以最新的elasticsearch-6.3.0.tar.gz为例,为了节约资源,本文将副本调为0, 无client角色 https://www.elastic.co/blog/hot-warm-a ...

  2. iOS开发下载文件速度计算

    当我们写下载界面的时候,需要向用户展示每秒下载多少KB,这个时候就需要计算速度.如下: 我用的是AFNetworking来做下载的,我们拿AFHTTPRequestOperation来举列,AFHTT ...

  3. [转]让iframe自适应高度-真正解决

    原文地址:https://www.cnblogs.com/rogge7/p/7762052.html 需求:实现 iframe 的自适应高度,能够随着页面的长度自动的适应以免除页面和 iframe 同 ...

  4. 框架源码系列九:依赖注入DI、三种Bean配置方式的注册和实例化过程

    一.依赖注入DI 学习目标1)搞清楚构造参数依赖注入的过程及类2)搞清楚注解方式的属性依赖注入在哪里完成的.学习思路1)思考我们手写时是如何做的2)读 spring 源码对比看它的实现3)Spring ...

  5. (原)kenel开机logo的制作

    今天项目需要,需要制作一个kernel的开机logo,所以在rk3288的平台上进行测试一番. 第一步:配置kernel:选上CONFIG_LOGO_LINUX_CLUT224选项 make menu ...

  6. java中获取两个时间中的每一天

    引入下面方法即可: /** * 获取两个时间中的每一天 * @param bigtimeStr 开始时间 yyyy-MM-dd * @param endTimeStr 结束时间 yyyy-MM-dd ...

  7. 赵雅智_Swift(4)_断言

    可选能够让你推断值是否存在.你能够在代码中优雅地处理值缺失的情况.然而,在某些情况下,假设值缺失或者值并不满足特定的条件.你的代码可能并不须要继续执行.这时.你能够在你的代码中触发一个断言(asser ...

  8. jQuery UI =>jquery-ui.js中sortable方法拖拽对象位置偏移问题

    今天要处理sortable方法处理的对象,拖拽的时候,位置偏移的问题. 按理应该是鼠标在哪,对象就跟着在哪的 百度了一下问题,http://blog.csdn.net/samed/article/de ...

  9. C#二分查找算法设计实现

    C#二分查找算法设计实现 1.介绍 二分查找也称折半查找(Binary Search),它是一种效率较高的查找方法.但是,折半查找要求线性表必须采用顺序存储结构,而且表中元素按关键字有序排列.(记住了 ...

  10. nginx相关知识

    一.负载均衡与反向代理 server { listen 9008; server_name localhost; location / { proxy_pass http://web_server; ...