原理
在把一帧的scan插入到子图之前，scan 相对子图的位姿是由 ceres scan matcher 优化获得的。由于这是一个实时的局部优化，需要一个好的初始位姿估计。前面的real time CSM就是把位姿估计器传来的预测值更新为一个好的初值，如果没有real time CSM，就还用位姿估计器传来的预测值作初值。Ceres Scan Matcher以初值作为先验，并找到最佳的点，该点就是通过scan match获得的在子地图中的位置，实现方式是 interpolating the submap and sub-pixel aligning the scan. 前端的两个scan matcher其实都是 scan to map 问题，让当前观测和已知环境最匹配
scan matcher 负责在地图中找到一个scan pose，使得在该位姿下所有 hit 点在 local map中的占用概率之和最大，于是转化为一个最小二乘问题：
式中的 就是我们需要求的位姿，它将scan坐标系中的点 (hk)全部转化到 Submap坐标系下， 让点 (hk)最大概率地匹配到Submap。 由于栅格中原本存储的就是空闲的概率，其实从栅格查出来的就是 。 相对地，real time scan match是占据概率之和最大，其实二者本质差不多，只是CSM换成最小二乘形式了，为了更精确。
在代码里实现的时候，又加上了旋转和平移的残差，防止点云匹配的结果和初值差太多。其实只要初值够好，这两个残差可以去掉。
这种方式的速度很快，计算复杂度高，鲁棒性好，但不能修正比子图分辨率大很多的误差。如果传感器和时间戳是合理的，只使用Ceres Scan Matcher通常是最好的选择。
Msmooth
Msmooth是一个将local子图中的各点概率进行平滑处理的函数，能够提供比栅格分辨率更好的精度。实质是双三次插值算法，该函数的输出结果为(0, 1)以内的数，实际返回了Submap中的占用概率，如果趋近1，那是很好了。
提高Scan matcher的频率可以获得更好优化结果，但计算量加大；或者用IMU来估计 scan match 中的旋转角度
ceres匹配对初值要求相当高，匹配后的结果会考虑其与初始位置偏差进行权重化，说明 cartographer认为其匹配后的值应该与初值相差不大。 一旦初值离真实值过远，很容易陷入局部最优解，导致优化失败。这也是为什么ceres对线速度与角速度有限制。
1 | auto pose_observation = absl::make_unique<transform::Rigid2d>(); |
Match 函数
CeresScanMatcher2D类主要函数只有Match：根据初值，将点云hit点和栅格对齐，返回观测值1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52/*
input:
1. 上一个 scan 的位姿 target_translation
2. 经过 RealTimeCorrelativeScanMatcher2D 处理过的
当前的 scan 的位姿的初始估计 initial_pose_estimate
3. 当前 scan 点云（2D） point_cloud
4. local map 概率分布栅格图 grid
output: 更新的位姿估计 pose_estimate
*/
void CeresScanMatcher2D::Match(
const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const
{
// 优化的变量（3维）
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle() };
ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.);
// 点云匹配残差块
problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, grid ),
nullptr /* loss function */, ceres_pose_estimate );
// 省略 GridType::TSDF
CHECK_GT(options_.translation_weight(), 0.);
// ceres 的套路都在函数里定义，很简单 AutoDiff
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr, ceres_pose_estimate);
CHECK_GT(options_.rotation_weight(), 0.);
// ceres 的套路都在函数里定义，很简单
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]),
nullptr, ceres_pose_estimate);
// ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}ceres scan matcher 将上面的最小二乘问题分解成了三个代价函数：点云与栅格的匹配残差、平移的残差、角度的残差。其中，点云匹配的权重总和为1，平移的权重和旋转的权重都是手动给的。
三个对应的核函数都是 nullptr， ceres的linear_solver_type是DENSE_QR
TranslationDeltaCostFunctor2D
两个Cost Function的实现，就是保证平移和旋转最小的代价函数1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor2D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Vector2d& target_translation)
{
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
2 /* residuals */,
3 /* pose variables */>(
new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
}
template <typename T>
bool operator()(const T* const pose, T* residual) const {
residual[0] = scaling_factor_ * (pose[0] - x_);
residual[1] = scaling_factor_ * (pose[1] - y_);
return true;
}
private:
// Constructs a new TranslationDeltaCostFunctor2D from the given
// 'target_translation' (x, y).
explicit TranslationDeltaCostFunctor2D(
const double scaling_factor, const Eigen::Vector2d& target_translation)
: scaling_factor_(scaling_factor),
x_(target_translation.x()),
y_(target_translation.y()) {}
// 省略: 禁止拷贝构造函数 和 赋值运算符
const double scaling_factor_;
const double x_;
const double y_;
};
这个代价函数比较简单，都是套路，RotationDeltaCostFunctor2D也是同理
平移量的残差计算，当前位姿逼近目标位姿 (位姿估计器输出的预测值xy)。目标函数
residual[0] = scaling_factor_ * (pose[0] - x_); residual[1] = scaling_factor_ * (pose[1] - y_);逼近目标角度，目标函数
scaling_factor_ * (pose[2] - angle_)这里是逼近之前的位姿，所以要求先验位姿比较准确，经试验发现ceres输出的位姿和预测值相差极小，疑似存在问题，可以将这两个ceres优化去掉，但地图匹配必须保留
统计残差
观测值 - 预测值1
2
3
4
5
6
7
8
9
10
11
12
13if (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);
}
