任务动机：提高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.１cm的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）建图效果好。