cartographer 占据概率相关

xiaoma_bk

已于 2022-03-29 16:12:59 修改

阅读量4k

点赞数 4

分类专栏： # Cartographer 文章标签：人工智能

于 2018-07-20 19:34:31 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/81133495

版权

Cartographer 专栏收录该内容

27 篇文章 18 订阅

订阅专栏

1. 所处

probablity_Value.h/cc

probability_grid.h/c

probability_grid_range_data_insert.h/cc

2.相关公式

2.1 基本概念

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

s= 1:表示该点被occupied的
s=0:表示该点是Free的

那么free的概率为： $p(s=0)=1-p(s=1)$

对于同一个点用两个值表达比较麻烦，而且也不便于概率值的更新。所以，这里会引入一个新的变量Odd(s)——用两个概率的比值表示： $Odds(s) = \frac{p(s=1)} {p(s=0)}$ 这种情况下，Odd(s)值等于1时，表征一半对一半，该点被occupied和free的概率各为0.5；

如果Odd(s)值大于1，表征该点被occupied的概率更大；Odd(s)越大，occupied的概率越大.

2.2 更新模型

$Odds(s) = \frac{p(z|s=1)} {p(z|s=0)}Odds(s)$

$\frac{p(z|s=1)} {p(z|s=0)}$ 为：传感器的测量模型

$p(z|s=1)$ 代表: 在实际状态为occupied的条件下，测量结果是 z 的概率

2.3 模型加速

事先计算好更新模型：

$S_{free}=log{\frac{p(z=0|s=1)}{p(z=0|s=0)}}$

$S_{hit}=log{\frac{p(z=1|s=1)}{p(z=1|s=0)}}$

两边取对数则变为相加,则占据概率图里通常使用的更新模型

$log(odds(s|z))=log(\frac{p(z|s=1)}{p(z|s=0)})+log(odds(s))$

3.cartographer

3.1 准备

首先确定几个常量

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是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].
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);
}

为了尽量避免浮点运算

将[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向浮点数映射：

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

3.2 转换

将一个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].
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);
}

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

// 该函数的含义是，对于一个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;
}

同时，这个函数也在列表中增加了一个元素，这个元素对应着当当前cell是unknown情况下我们如何给该cell附初值。这种情况下直接把 odd(s_z) 转成概率值，再转成ProbabilityValue就可以了。这样，所有情况都可以直接查表而得。

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

      hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.hit_probability()))),
      miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
          Odds(options.miss_probability()))) 

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