技術標籤：三個點在同一個半圓的概率

說明：
在閱讀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呢？如果有讀者比較清楚這裡是怎麼回事，煩請留言告知。

關於該公式的理解請見評論中 杜圖圖 的留言。我覺得他的理解應該是正確的。