三个点在同一个半圆的概率_Cartographer源码阅读之附 1—probability_values.h/c：占据概率相关...

说明：
在阅读cartographer源码过程中，我觉得有必要详细介绍一下栅格地图中一个pixel坐标处的occupied probability以及如何根据新的传感器数值更新它。但又不想让这部分内容干扰到主线逻辑，所以另起一篇文章，以附件的形式分析一下cartographer中是如何表示occupied probability以及在/mapping/probability_values.h和.../ http:// probability_values.cc 中提供的一些工具函数。

1. 占据栅格图(Occupancy Grid Map)更新公式推导

编程我不擅长，但推公式我很擅长，所以我就把这块儿的公式讲解一下吧，也都是很基础的内容。

对于栅格化地图中的一个cell, 或者说一个pixel，它的状态

可能有两个状态：

: 表示该点被occupied的

: 表示该点是Free的

通常，我们用一个概率

来表示该pixel被占据(occupied)的概率。那么，该点Free的概率就是

.

但是，对于同一个点用两个值表达比较麻烦，而且也不便于概率值的更新。所以，这里会引入一个新的变量Odd(s)——用两个概率的比值表示：

这种情况下，Odd(s)值等于1时，表征一半对一半，该点被occupied和free的概率各为0.5；

如果Odd(s)值大于1，表征该点被occupied的概率更大；Odd(s)越大，occupied的概率越大。范围为1～+

；

如果Odd(s)值小于1，表征该点free的概率更大；Odd(s)越小，free的概率越大。范围为0~1。

那么对于这个cell, 新来了一个测量值

——即传感器测得该点为occupied或Free, 那么我们如何根据该测量值来更新该点的状态呢？

分为两种情况讨论：

Case 1：假设之前我们对该点没有任何信息，那么我们会直接把测量值赋给该点：

Case 2: 假设测量值来之前，该点已有一个测量值odd(s).

我们要求的实际上是一个条件概率：

即，在存在观测值

的条件下，该点的实际状态为

的概率与该点的实际状态为

的比值。

那么，根据贝叶斯的条件概率公式，我们有：

所以，我们可以得到：

其中，

即为上一次的状态，即odd(s). 所以，我们可以得到更新模型为：

其中，

表征的是传感器的测量模型。

的含义是在实际状态为occupied的条件下，测量结果是

的概率。同样，

的含义是在实际状态为free的条件下，测量结果是

的概率。这两个值是由传感器的测量精度、可靠性等因素决定。当一个传感器制造完成后，该值是定值，不会随时间、位置等因素改变(当然，如果你的传感器是随着时间、位置等因素而变化的，这不在我们讨论范围内，你需要一个新的传感器模型）。

为了更方便计算，可以对两边取log，可以得到：

那么，根据测量结果z是occupied或free的情况，我们可以预先求出来两个值：

则：

记：

则我们的更新模型就可以得到:

这个就是我们在占据概率图里通常使用的更新模型。

2. probability_values.h/.cc解读

可以看到，在前面公式推导中，有三个变量我们需要经常遇到：被占据的概率

、Free的概率

和两者的比值odd(s).

在cartographer的源码里，也必然涉及到很多对这三个量的相互转化以及模型更新等公式的计算。这部分工作是在/mapping/probability_values.h/.cc中完成的。我们来看看这部分代码：

cartographer中

称为Probability,后面公式中我用

表示；

称为CorrespondenceCost, 后面公式中我用

表示

Odd(s)记为了Odds.

首先，在/mapping/probability_values.h里定义了几个常量：

constexpr float kMinProbability = 0.1f;//最小概率为0.1
constexpr float kMaxProbability = 1.f - kMinProbability;//最大概率为1-0.1=0.9
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability;//最小Free概率
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;//最大Free概率

以及：

//在没有任何先验信息情况下，Occupied和Free概率值都为0
constexpr uint16 kUnknownProbabilityValue = 0;
constexpr uint16 kUnknownCorrespondenceValue = kUnknownProbabilityValue;
// 非常抱歉，之前我把左移看成了右移，所以认为kUpdateMarker是一个极小值，所以导致大家对代码的理解出现了问题。
// 左移的话kUpdateMarker是2的15次方：32768，也就是所以概率值转化成整数value之后的最大范围。所以程序中有判断是否越界
constexpr uint16 kUpdateMarker = 1u << 15;

首先提供了如下转化工具：

//由Probability计算odds
inline float Odds(float probability) {
  return probability / (1.f - probability);
}

//由odds计算probability
inline float ProbabilityFromOdds(const float odds) {
  return odds / (odds + 1.f);
}

根据之前的公式，容易推得：

//Probability转CorrespondenceCost
inline float ProbabilityToCorrespondenceCost(const float probability) {
  return 1.f - probability;
}

// CorrespondenceCost转Probability
inline float CorrespondenceCostToProbability(const float correspondence_cost) {
  return 1.f - correspondence_cost;
}

两个Clamp函数：

// Clamps probability to be in the range [kMinProbability, kMaxProbability].
// clamp函数的含义是，如果给定参数小于最小值，则返回最小值；如果大于最大值则返回最大值；其他情况正常返回参数
inline float ClampProbability(const float probability) {
  return common::Clamp(probability, kMinProbability, kMaxProbability);
}
// Clamps correspondece cost to be in the range [kMinCorrespondenceCost,
// kMaxCorrespondenceCost].
inline float ClampCorrespondenceCost(const float correspondence_cost) {
  return common::Clamp(correspondence_cost, kMinCorrespondenceCost,
                       kMaxCorrespondenceCost);
}

cartongrapher为了尽量避免浮点运算，将[kMinProbability, kMaxProbability]或[kMinCorrespondenceCost, kMaxCorrespondenceCost]之间的浮点数映射到了整数区间：[1, 32767]:

由浮点数到整数区间的映射函数为：

inline uint16 BoundedFloatToValue(const float float_value,
                                  const float lower_bound,
                                  const float upper_bound) {
  const int value =
      common::RoundToInt(
          (common::Clamp(float_value, lower_bound, upper_bound) - lower_bound) *
          (32766.f / (upper_bound - lower_bound))) +
      1;
  // DCHECK for performance.
  DCHECK_GE(value, 1);//检查是否大于等于1
  DCHECK_LE(value, 32767);//是否小于等于32767
  return value;
}

Probability和CorrespondenceCost向整数区间映射：

// Converts a probability to a uint16 in the [1, 32767] range.
inline uint16 ProbabilityToValue(const float probability) {
  return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
}

// Converts a probability to a uint16 in the [1, 32767] range.
inline uint16 ProbabilityToValue(const float probability) {
  return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
}

整数区间value向浮点数映射：

extern const std::vector<float>* const kValueToProbability;
extern const std::vector<float>* const kValueToCorrespondenceCost;

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

// Converts a uint16 (which may or may not have the update marker set) to a
// correspondence cost in the range [kMinCorrespondenceCost,
// kMaxCorrespondenceCost].
inline float ValueToCorrespondenceCost(const uint16 value) {
  return (*kValueToCorrespondenceCost)[value];
}

Probability的Value转成CorrespondenceCost的Value:

inline uint16 ProbabilityValueToCorrespondenceCostValue(
    uint16 probability_value) {
  if (probability_value == kUnknownProbabilityValue) {
    return kUnknownCorrespondenceValue;
  }//如果是Unknown值还返回unknown值。Probability和CorrespondenceCost的Unknown值都是0
  bool update_carry = false;
  if (probability_value > kUpdateMarker) {//如果该值超过最大范围：但什么情况下会导致出现该值超过范围还不清楚
    probability_value -= kUpdateMarker;//防止溢出范围
    update_carry = true;//如果存在过超出范围的行为，则将update_carry置为true
  }
  //ProbabilityValue-->Probability-->CorrespondenceCost-->CorrespondenceCostValue
  uint16 result = CorrespondenceCostToValue(
      ProbabilityToCorrespondenceCost(ValueToProbability(probability_value)));
  if (update_carry) result += kUpdateMarker;//原先减去过一个最大范围，现在再加回来
  return result;
}

同样，CorrespondenceCost的Value也可以转成Probability的Value：

inline uint16 CorrespondenceCostValueToProbabilityValue(
    uint16 correspondence_cost_value) {
  if (correspondence_cost_value == kUnknownCorrespondenceValue)
    return kUnknownProbabilityValue;
  bool update_carry = false;
  if (correspondence_cost_value > kUpdateMarker) {
    correspondence_cost_value -= kUpdateMarker;
    update_carry = true;
  }
  uint16 result = ProbabilityToValue(CorrespondenceCostToProbability(
      ValueToCorrespondenceCost(correspondence_cost_value)));
  if (update_carry) result += kUpdateMarker;
  return result;
}

接下来几个函数在http://probability_values.cc中：

将一个uint16型的value转成一个浮点数。value的范围是[1,32767]，若value为0，表示是unknown。若是[1,32767]，则映射到浮点型的范围[lower_bound, upper_bound].

// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].
//在计算时并不是用浮点数进行的计算，二是将0~1的概率值映射到1_32767的整数值
float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value,
                              const float unknown_result,
                              const float lower_bound,
                              const float upper_bound) {
  CHECK_GE(value, 0);//是否大于等于0
  CHECK_LE(value, 32767);//是否小于等于0
  if (value == unknown_value) return unknown_result;
  const float kScale = (upper_bound - lower_bound) / 32766.f;
  return value * kScale + (lower_bound - kScale);
}

把[1,32767]之间的所有value预先计算出来其映射到[lower_bound, upper_bound]这个区间的对应浮点值，存到一个浮点型向量中：

// 把[1,32767]之间的所有value预先计算出来其映射到[lower_bound, upper_bound]这个区间
// 的对应浮点值，存到一个浮点型向量中：
std::unique_ptr<std::vector<float>> PrecomputeValueToBoundedFloat(
    const uint16 unknown_value, const float unknown_result,
    const float lower_bound, const float upper_bound) {
  auto result = common::make_unique<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(SlowValueToBoundedFloat(
          value, unknown_value, unknown_result, lower_bound, upper_bound));
    }
  }
  return result;
}

下面两个函数通过调用上面这个公式将Value值映射到Probability或CorrespondenceCost：

std::unique_ptr<std::vector<float>> PrecomputeValueToProbability() {
  return PrecomputeValueToBoundedFloat(kUnknownProbabilityValue,
                                       kMinProbability, kMinProbability,
                                       kMaxProbability);
}

std::unique_ptr<std::vector<float>> PrecomputeValueToCorrespondenceCost() {
  return PrecomputeValueToBoundedFloat(
      kUnknownCorrespondenceValue, kMaxCorrespondenceCost,
      kMinCorrespondenceCost, kMaxCorrespondenceCost);
}

然后，把预先计算出来的Probability和CorrespondenceCost放到在probability_values.h中定义的两个向量kValueToProbability和kValueToCorrespondenceCost中。这样，以后直接以value为索引值查表就可以获得其对应的probability或correspondenceCost。

const std::vector<float>* const kValueToProbability =
    PrecomputeValueToProbability().release();

const std::vector<float>* const kValueToCorrespondenceCost =
    PrecomputeValueToCorrespondenceCost().release();

接下来ComputeLookupTableToApplyOdds这个函数的功能比较难理解。我们这里做一个简单的解释。假设我们现在有一个cell，这里存着一个ProbabilityValue, 如果这时候我们通过传感器测量得到了一个新的观测结果

，那么我们应该如何更新这个cell的ProbabilityValue呢？还记得我们之前写的概率模型更新公式吗？

如上所示，其中

是我们的观测模型，根据我们观测到的是hit还是miss的情况，新的观测

只有两种情况：

这两种情况的

值在传感器确定后是已知确定的。

那么，假设，我们某一个cell的ProbabilityValue是已知的,我们用

表示，那么我们把这个值先转成概率值

,然后再转成odds值。之后通过上面的公式：

就可以计算出更新后的odds值，然后再把该值转化成概率值，最后再把概率值转化为ProbabilityValue值。

上面这个流程就是ComputeLookupTableToApplyOdds函数所做的工作，所不同的是，该函数把

之间的所有value都预先计算出来，存成一个表，那么这样在使用时就可以以当前cell的value为索引值直接查找到更新后的结果value。那么

有两种不同的情况，所以我们存两个表就可以了。

在程序运行过程中，需要不停地更新，那么这样预先计算一次之后，以后就不用再计算，直接查表就可以，能节省大量的时间。不得不说，cartographer的这个设计是一个十分有才的设计。

// 该函数的含义是，对于一个value~[1,32767], 如果有一个新的odds值的观测后，更新后的value应该是什么。
// 这里对所有可能的value都进行了计算，存在了一个列表中。odds只有两种情况，hit或misses. 
// 因此，可以预先计算出来两个列表。这样，在有一个新的odds时可根据原有的value值查表得到一个新的value值，更新
std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
  std::vector<uint16> result;
  result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) +
                   kUpdateMarker);//这个表示这个表中的第一个元素对应了如果之前该点是unknown状态，更新的value应该是什么
  for (int cell = 1; cell != 32768; ++cell) {
    result.push_back(ProbabilityToValue(ProbabilityFromOdds(
                         odds * Odds((*kValueToProbability)[cell]))) +
                     kUpdateMarker);
  }
  return result;
}
// 但在ComputeLookupTableToApplyOdds这个转化里都加了一个kUpdateMarker，相当于有了一个偏移，但为什么我没想明白

同时，这个函数也在列表中增加了一个元素，这个元素对应着当当前cell是unknown情况下我们如何给该cell附初值。这种情况下直接把

转成概率值，再转成ProbabilityValue就可以了。这样，所有情况都可以直接查表而得。

基于同样的原理，ComputeLookupTableToApplyCorrespondenceCostOdds是处理某一个cell的CorrespondenceCostValue已知时如何更新的情况：

std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
    float odds) {
  std::vector<uint16> result;
  result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
                       ProbabilityFromOdds(odds))) +
                   kUpdateMarker);
  for (int cell = 1; cell != 32768; ++cell) {
    result.push_back(
        CorrespondenceCostToValue(
            ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                odds * Odds(CorrespondenceCostToProbability(
                           (*kValueToCorrespondenceCost)[cell]))))) +
        kUpdateMarker);
  }
  return result;
}

这里特殊说明一下，Grid2D里存的都是CorrespondenceCostValue

看完这部分代码我更加不理解论文原文中的公式(3)到底是什么意思了。根据代码来看，cartographer更新cell中概率值的方法跟我们在本文第一部分的推导是一致的。但是对于公式(3):

其中

到底存储的是什么呢？从代码来看，Grid2D中存的是CorrespondenceCost。按我的理解，应该是先将CorrespondenceCost转成Odds（我们暂时忽略CorrespondenceCost与CorrespondenceCostValue的相互转化），然后跟

相乘，最后再转成CorrespondenceCost。但是这个公式中

到底表示什么意思呢？如果表示取倒数，那对

取倒数是什么意思呢？如果是表示

的逆过程——即求一个odds值所对应的CorrespondenceCost是什么，那也应该是先让

与

相乘后再求

啊。

所以，我合理地怀疑这个公式是不是存在typos呢？如果有读者比较清楚这里是怎么回事，烦请留言告知。

关于该公式的理解请见评论中 杜图图 的留言。我觉得他的理解应该是正确的。