任务动机:提高Cartographer前端匹配建图方法的鲁棒性。
任务描述:针对Cartographer前端匹配中Ceres优化时初始值不准确的情形,构造多分辨率map,使当前帧点云分别和多分辨率map匹配,计算得到准确的位姿变换矩阵,进而提升建图的鲁棒性。
1. Cartographer前端
Cartographer前端:当前帧点云和栅格地图通过使用扫描匹配(scan_match)方法进行位姿估计。扫描匹配就是想办法把当前帧点云和栅格地图对齐,对齐过程中的旋转和平移就是它们的相对位姿。
扫描匹配的实现方式先用相关性扫描匹配求初值,再用基于优化的扫描匹配进一步优化。相关性扫描匹配的思路非常简单,就是暴力搜索:给定当前帧点云和一个栅格地图,把当前帧点云放在栅格地图的每个格子上,同时旋转当前帧点云,计算点云与栅格地图的重合程度,重合程度最高的位姿就是它们的相对位姿。 基于优化的扫描匹配是构建非线性最小二乘优化问题并求解。但是基于优化的扫描匹配非常依赖于初值的准确度,如果相关性扫描匹配计算的初值偏差较大,基于优化的扫描匹配容易陷入局部最优,匹配结果误差变大。 针对cartographer前端匹配中Ceres优化时初始值不准确的情形,构造多分辨率map,使当前帧点云分别和多分辨率map匹配,计算得到准确的位姿变换矩阵,进而提升建图的鲁棒性。
2. 基于多分辨率map优化的扫描匹配
2.1 构造多分辨率map
首先构造多分辨率map。构造分辨率为0.05cm,0.1cm和0.2cm的三层map。
假设有一张4×4的地图,我们降低其分辨率,每2×2的格子合并成一个,得到一张2×2的地图,再降低其分辨率,得到一张1×1的地图。这样,地图被分成了三层,如下图所示。
图1. 多分辨率map的构建
代码实现:修改submap_2d.cc代码如下。
``
`
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
const sensor::RangeData& range_data) {
if (multi_2_submaps_.empty() && multi_4_submaps_.empty()) {
AddMultiSubmap(range_data.origin.head<2>());
}
if (submaps_.empty() ||
submaps_.back()->num_range_data() == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}
for (auto& submap : submaps_) {
submap->InsertRangeData(range_data, range_data_inserter_.get());
}
for (auto& multi_2_submap : multi_2_submaps_) {
multi_2_submap->InsertRangeData(range_data, range_data_inserter_.get());
}
for (auto& multi_4_submap : multi_4_submaps_) {
multi_4_submap->InsertRangeData(range_data, range_data_inserter_.get());
}
if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
submaps_.front()->Finish();
}
return submaps();
}
`
``
``
`
void ActiveSubmaps2D::AddMultiSubmap(const Eigen::Vector2f& origin) {
if (submaps_.size() >= 2) {
multi_2_submaps_.clear();
multi_4_submaps_.clear();
}
multi_2_submaps_.push_back(absl::make_unique<Multi2Submap2D>(
origin,
std::unique_ptr<Grid2D>(
static_cast<Grid2D*>(Create2ResolutionGrid(origin).release())),
&conversion_tables_));
multi_4_submaps_.push_back(absl::make_unique<Multi4Submap2D>(
origin,
std::unique_ptr<Grid2D>(
static_cast<Grid2D*>(Create4ResolutionGrid(origin).release())),
&conversion_tables_));
}
`
``
2.2 多分辨率map的匹配
先在分辨率0.2cm的map中计算得到最优的位姿T1;把T1做为分辨率0.1cm的map匹配的初值,计算得到最优的位姿T2;把T2做为分辨率0.05cm的map匹配的初值,计算得到最优的位姿T3。
代码实现:修改local_trajectory_builder_2d.cc代码如下。
``
`
auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
{
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
*matching_multi_4_submap->grid(), pose_observation.get(),
&summary);
ceres_scan_matcher_.Match(pose_prediction.translation(), *pose_observation,
filtered_gravity_aligned_point_cloud,
*matching_multi_2_submap->grid(), pose_observation.get(),
&summary);
ceres_scan_matcher_.Match(pose_prediction.translation(), *pose_observation,
filtered_gravity_aligned_point_cloud,
*matching_submap->grid(), pose_observation.get(),
&summary);
}
if (pose_observation) {
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
const double residual_distance =
(pose_observation->translation() - pose_prediction.translation())
.norm();
kScanMatcherResidualDistanceMetric->Observe(residual_distance);
const double residual_angle =
std::abs(pose_observation->rotation().angle() -
pose_prediction.rotation().angle());
kScanMatcherResidualAngleMetric->Observe(residual_angle);
}
`
``
3. 实验验证
为了验证Cartographer中local slam的建图效果,把POSE_GRAPH.optimize_every_n_nodes = 0。
针对初始值不好的场景,把local slam中的TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false。
用2个科力的bag(01.bag和02.bag)建图验证:
01.bag:Cartographer现有的local_slam(不加real time csm,不加loop)的建图效果如下所示:
多分辨率map的local_slam(不加real time csm,不加loop)的建图效果如下所示:
对比可知,多分辨率map的local_slam建的地图,墙边重影较少。
02.bag:Cartographer现有的local_slam(不加real time csm,不加loop)的建图效果如下所示:
多分辨率map的local_slam(不加real time csm,不加loop)的建图效果如下所示:
对比可知,Cartographer现有的local_slam的建图出现严重的扭曲,而多分辨率map的local_slam的建图效果较好。
4. 结论
通过01.bag和02.bag建图效果对比,多分辨率map的local_slam(不加real time csm,不加loop)比Cartographer现有的local_slam(不加real time csm,不加loop)建图效果好。