【图解 cartographer】 之地图概率更新过程

前言：
最近一直在研究建图，对google的开源SLAM框架 Cartographer 进行了源码梳理，发现很多巧妙的算法设计，结合原论文 《Real-time Loop Closure in 2D LIDAR SLAM》 的理论简介，才恍然理解其核心目的就是实现实时回环；为了达到实时的效果，其引入了一些其他方法来进行计算加速。理解其中的思想方法，整个过程有点漫长但很有意思。Cartographer 中工程设计中可参考的点很多，本篇先把其概率地图更新部分的流程进行说明，用图示意的方式对其思路进行展示，强化自我理解。

地图更新的目的： 根据最新采集到的雷达点，在与地图匹配后，把雷达点插入到当前地图中，其本质是对栅格地图概率的更新。

地图更新的方式：

1. 逐一对地图中栅格进行概率更新，一般用对数 $Odd(x)$ 进行相乘或者 $Log(Odd(x))$ 相加减，因此当地图较大时，大量的乘法计算会影响地图更新的效率；所以为了提升计算效率，其他大部分SLAM实现中地图更新采用 $Log(Odd(x))$ 相加减的方式。
2. Cartographer针对地图更新过程，采用以空间换时间的策略，主要是建立地图更新对照表，然后更新过程就直接变成了查表操作，连加减法都省去了，牺牲内存空间，节约时间。

其具体操作流程如下，地图要更新的点主要是激光所扫到的范围：

如上图所示，当第1个栅格中的值是700时，通过查表可立即获得更新后的值892，然后直接填入第1个栅格中，即可完成栅格概率更新。那本质上说，更新表是存放的有序更新后的值，而表的序号就是更新前的值，因此该问题的重点是如何构建这张表。这里也许注意，Cartographer子图中存放的既不是概率也不是赔率odd，而是概率的映射[0~65536]。

注： 上图的值只做流程展示参考，不与真实计算挂钩。

在代码中构建这两张表的入口在代码在类LaserFanInserter的构造函数中：

// laser_fan_inserter.cc
LaserFanInserter::LaserFanInserter(const proto::LaserFanInserterOptions& options): options_(options),hit_table_(mapping::ComputeLookupTableToApplyOdds(mapping::Odds(options.hit_probability()))),miss_table_(mapping::ComputeLookupTableToApplyOdds(mapping::Odds(options.miss_probability()))) {}

其调用probability_values.cc中ComputeLookupTableToApplyOdds函数，可以看到，这里分别构造了两张表hit_table_和miss_table_，这就是前面提到的地图概率更新表。地图中点的更新状态有三种：hit、miss、free(不更新)，options.hit_probability()和options.miss_probability()为激光雷达点击中(hit)和穿过(miss)的概率，是Lua配置数据输入：

options.hit_probability() = 0.55;
options.miss_probability() = 0.49;

我们可以看到在probability_values.h和probability_values.cc中实现了整个表的构建：

// probability_values.h
// 概率值转Odds
inline float Odds(float probability) {return probability / (1.f - probability);
}
// Odds转概率值
inline float ProbabilityFromOdds(const float odds) {return odds / (odds + 1.f);
}// 概率值的范围为 [0.1, 0.9]
constexpr float kMinProbability = 0.1f;
constexpr float kMaxProbability = 1.f - kMinProbability;// Clamps probability to be in the range [kMinProbability, kMaxProbability].
inline float ClampProbability(const float probability) {return common::Clamp(probability, kMinProbability, kMaxProbability);
}constexpr uint16 kUnknownProbabilityValue = 0;
constexpr uint16 kUpdateMarker = 1u << 15;  // = 32768// Converts a probability to a uint16 in the [1, 32767] range.
inline uint16 ProbabilityToValue(const float probability) {const int value =common::RoundToInt((ClampProbability(probability) - kMinProbability) *(32766.f / (kMaxProbability - kMinProbability))) +1;// DCHECK for performance.DCHECK_GE(value, 1);DCHECK_LE(value, 32767);return value;
}extern const std::vector<float>* const kValueToProbability;// Converts a uint16 (which may or may not have the update marker set) to a
// probability in the range [kMinProbability, kMaxProbability].
inline float ValueToProbability(const uint16 value) {return (*kValueToProbability)[value];
}

头文件中主要实现了基本概率、赔率、对应整数的转换函数，概率的范围为[0.1, 0.9]，对应value的范围为[1， 32767]，若value为0表示地图为unknow状态，也就是初始灰色状态。kValueToProbability为从Value对应的概率值的表，为计算地图更新hit和miss表做准备。

这里kUpdateMarker 为已更新后的标志量，是为了防止miss和hit重复对地图更新，其值为32768，为uint16 的最高位，其操作的方式是，更新时加上，更新完了后减去。

// probability_values.cc
// 0 is unknown, [1, 32767] maps to [kMinProbability, kMaxProbability].
float SlowValueToProbability(const uint16 value) {CHECK_GE(value, 0);CHECK_LE(value, 32767);if (value == kUnknownProbabilityValue) {// Unknown cells have kMinProbability.return kMinProbability;}const float kScale = (kMaxProbability - kMinProbability) / 32766.f;return value * kScale + (kMinProbability - kScale);
}// 直接巧妙计算Value转概率对照表，重复2次，区别了带marker和不带marker
const std::vector<float>* PrecomputeValueToProbability() {std::vector<float>* result = new std::vector<float>;// Repeat two times, so that both values with and without the update marker// can be converted to a probability.for (int repeat = 0; repeat != 2; ++repeat) {for (int value = 0; value != 32768; ++value) {result->push_back(SlowValueToProbability(value));}}return result;
}}  // namespaceconst std::vector<float>* const kValueToProbability = PrecomputeValueToProbability();

在计算ComputeLookupTableToApplyOdds更新表之前，这里先利用PrecomputeValueToProbability计算了一个kValueToProbability 表，也是为了加速计算更新表：

注意：

1. 这里直接巧妙计算Value转概率对照表，重复2次，区别了带marker和不带marker，但都可以同时获得相同的概率值。

std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {std::vector<uint16> result;// 计算Value=0时更新后的Value，可直接更新，因为更新前=0为Unknownresult.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) + kUpdateMarker);// 从第2个开始，到32767for (int cell = 1; cell != 32768; ++cell) {result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds * Odds((*kValueToProbability)[cell]))) +kUpdateMarker);}return result;
}

$$M_{new}(x)=clamp[odd{s}^{-1}(odds(M_{old}(x))\cdot odds(p_{hit}))]$$

注意：

1. 这里的更新表第一个数是直接存入的，直接用传入的赔率值计算，这其实是在计算未知区域的更新Value，也是地图中当前值为0的区域；
2. 更新后的Value都加上了kUpdateMarker标志，该标志将在下次地图更新之前被清除。清除操作在probability_grid.h文件中的StartUpdate()函数中。

到此，我们已经成功建立了更新查询的hit和miss表，如下开始介绍其地图更新的核心操作，如下laser_fan_inserter.cc文件中：

// laser_fan_inserter.cc
void LaserFanInserter::Insert(const sensor::LaserFan& laser_fan,ProbabilityGrid* const probability_grid) const {// 去除上一次更新后留下的kUpdateMarker标志CHECK_NOTNULL(probability_grid)->StartUpdate();// By not starting a new update after hits are inserted, we give hits priority// (i.e. no hits will be ignored because of a miss in the same cell).// 利用激光的CastRays模型获得his和miss栅格在grid_map上的索引表，然后根据前面构建的更新表进行更新// hit和miss一次调用ApplyLookupTable()进行更新CastRays(laser_fan, probability_grid->limits(),[this, &probability_grid](const Eigen::Array2i& hit) {probability_grid->ApplyLookupTable(hit, hit_table_);},[this, &probability_grid](const Eigen::Array2i& miss) {// 可根据外部参数关闭miss区域更新if (options_.insert_free_space()) {probability_grid->ApplyLookupTable(miss, miss_table_);}});
}

注意：

1. 这里的CastRays()传入的是一个函数指针；
2. 这块后面可以单独拎出来讲解。

// probability_grid.h
bool ApplyLookupTable(const Eigen::Array2i& xy_index,const std::vector<uint16>& table) {DCHECK_EQ(table.size(), mapping::kUpdateMarker);const int cell_index = GetIndexOfCell(xy_index);uint16& cell = cells_[cell_index];if (cell >= mapping::kUpdateMarker) {return false;}update_indices_.push_back(cell_index);cell = table[cell];  // 真正的更新操作DCHECK_GE(cell, mapping::kUpdateMarker);UpdateBounds(xy_index);return true;
}

到这里，地图概率更新的过程已经梳理完成，这其中还有很多具体的细分点，比如：MapLimit的计算、CastRays模型的介绍、边界的更新，等等。

好了，今天就简单写到这里。