中用table_miss_table和hit_table等概率解释

一、

可参考概率机器人书中的二值贝叶斯滤波器 知乎 https://zhuanlan.zhihu.com/p/74003207

如果状态是静态的，那么置信度就只是测量值的函数:注意置信度不是p(x)而是基于已有所有观测的p(x|z1:t)

ut控制数据对于二值状态没有影响，所以忽略。

置信度通常是以对数差异比log odds ratio的形式实现的。 状态x的差异比odds被定义为正状态与反状态的概率值之比：

对数差异比是对上式的对数运算：

在测量数据不断变化的环境下，通过如下算法对差异比进行更新：

注意：Carto用的是差异比Odd，Carto中用的不是对数差异比log odds ratio【加法更新】，而是Odd【乘法更新】

Carto前端网格概率更新的整体思想：

1、对于当前activesubmap中某个网格如果被某次激光帧hit或miss时，取出当前网格的存储的CorrespondenceCost概率的int值调用事先生成的ApplyLookupTable，查询并更新该网格CorrespondenceCost概率值。该网格当次被it就查询hit_table,被miss就查询miss_table.

2、主要涉及两个函数ComputeLookupTableToApplyCorrespondenceCostOdds和ApplyLookupTable

A、ComputeLookupTableToApplyCorrespondenceCostOdds只在系统构造range_data_inserter时使用一次。即构造range_data_inserter时根据配置给定hit_probability和miss_probability分别构造hit_table和miss_table。

      range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },

注：很重要！

hit_probability的物理意义是p(s=1|z=1)，代表观测到一次hit时的占据概率probability，故正常该是大于0.5，好比0.55.他和p(s=0|z=1)之和为1；是传感器特性，代表该传感器给出hit的可信度，事先在配置文献中给定值。

miss_probability的物理意义是p(s=1|z=0)，代表观测到一次miss时的占据概率probability，故正常该是小于0.5，好比0.49；注意miss_probability并不是p(s=0|z=1)，也不是p(s=0)故两者之和不为1.代表该传感器给出miss的可信度，事先在配置文献中给定值。

故上述两者代表：该传感器观测给出一个hit/miss时，该网格被占据的概率是多少！

在Carto中probability特指占据概率，从来不是指free概率（CorrespondenceCost）

(1)占据概率为0的未知网格

A.如果观测到一次hit则概率就变了p(s=1|z=1)=0.55，故第一次hit观测后的Odd=0.55/(1-0.55)，也等于后面更新累积要用的常值C(z=1);很重要

B.如果观测到一次miss则概率就变了p(s=1|z=0)=0.49，第一次miss观测后的Odd=0.49/(1-0.49)，也等于后面更新累积要用的常值C(z=0);很重要

也就是乘法更新公式中：第一项为1，最后一项也为1。【下式中第一项为0，最后一项为0】

用log时第一项为0，最后一项也为0.

最后一项为1的原因？？？，我猜是这样的：p(s=1)代表该网格占据的概率，由于没有观测信息，故p(s=1)=0.5，故

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

所以，两者相除可以得到：

(2)占据概率为p(s=1|z[1:n-1])的网格

A.如果新观测到一次hit则概率就更新为p(s=1|z[1:n]));

只需要在原有概率上乘以常数C(z=1)=0.55/(1-0.55)即可！！！！

上述公式推到要用到两个基本公式：

B.如果新观测到一次miss则概率就更新为p(s=1|z[1:n]));

只需要在原有概率上乘以常数C(z=1)=0.49/(1-0.49)即可！！！！

从上面我们可以看出概率更新变成了对上次的概率的常数乘法，而且Carto将乘法简化成查询lookup表了。

具体实现：

std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
    float odds) {
  std::vector<uint16> result;  
  result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
                       ProbabilityFromOdds(odds))) +
                   kUpdateMarker);//kUpdateMarker用于判断该值在此次激光观测中是否被更新，避免单帧激光的多根射线反复更新某格子。Raycasts中先计算hit，再计算miss
  for (int cell = 1; cell != 32768; ++cell) {
    result.push_back(
        CorrespondenceCostToValue(
            ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
                odds * Odds(CorrespondenceCostToProbability(
                           (*kValueToCorrespondenceCost)[cell]))))) +
        kUpdateMarker);
  }
  return result;

2、注意：单次range，某分辨率网格只更新一次，优先级为先遍历returns中的hit，再分别遍历returns和misses中的miss。为避免重复更新，采用+kUpdateMarker作为当前range是否被更新的标志位Marker，并同时记录在update_index中【方便后期遍历，减少消耗】。

二、

索引0都代表未知，其对应的value

hit_table中索引0代表hit之前为未知，hit一次观测之后该网格CorrespondenceCost概率int值

miss_table中索引0代表miss之前为未知，miss一次观测之后该网格CorrespondenceCost概率int值

bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,//cell_index是将要更新的栅格单元索引
 const std::vector<uint16>& table) {//table是更新过程中将要查的表

由于ProbabilityGrid::ApplyLookupTable中会根据网格cell_index的xy坐标从correspondence_cost_cells())[flat_index]找到该网格的CorrespondenceCost概率int值，再查询该表hit_table或miss_table，

整个Carto代码中对submap中每个网格只存储correspondence_cost_cells_的Free概率值。

std::vector<uint16> correspondence_cost_cells_;//存储概率值，这里的概率值是Free的概率值

三、

所有在新来一个range时，更新submap概率的基本原理是：

1、利用Odd将概率更新转换成乘法【如果是logOdd就是加法】

2、为提高乘法效率就将乘法转换成两个查询表

3、某个点新观测为hit就用该点之前的概率进行查表hit_table，得到更新后的概率

4、某个点新观测为free就用该点之前的概率进行查表free_table，得到更新后的概率

注意：
A.无论这个点在rviz或pgm被定义为occ还是free，其在Carto中存储的都是free的概率，
即：p(s=0|z1,...zn)
B.因为free的概率和占据的概率是唯一对应的：p(s=0|z1,...zn) = 1.0 - p(s=1|z1,...zn)
C.个人觉得只存一个值的原因有两个
（1）节省资源：次要
（2）避免混淆：因为概率中0代表的是未知，free的概率范围为（0.1 ～0.9）对应整数（1~32767），
避免1.0-的时候出问题，都以free为准

四、几个1-1映射/转换

Carto中有以下几种映射/转换：

1、hit_table 和 miss_table，也就是某个网格被hit一次或被miss一次的 概率更新查询表

2、correspondence_cost和Probability，也就是某个网格free和occ的唯一转换，ProbabilityToCorrespondenceCost

3、占据Probability 和 Odd的一一转换，注意Odd只能用Probability进行一一转换。也就是correspondence_cost要转成Probability后再进行转换。ProbabilityFromOdds 或Odds

4、概率float向uint16的转换，（0.1 ～0.9）对应整数（1~32767）

kValueToCorrespondenceCost
CorrespondenceCostToValue
// 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)