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;
}

}  // namespace

const 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为Unknown
  result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) + kUpdateMarker);
  // 从第2个开始，到32767
  for (int cell = 1; cell != 32768; ++cell) {
    result.push_back(ProbabilityToValue(ProbabilityFromOdds(
                         odds * Odds((*kValueToProbability)[cell]))) +
                     kUpdateMarker);
  }
  return result;
}

注意：

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模型的介绍、边界的更新，等等。

参考：【图解 cartographer】 之地图概率更新过程_栅格地图更新-CSDN博客