Cartographer Insert Submap

1. 基本概率

• TSDF: Truncated Signed Distance Function（截断的符号距离函数）
• Occupancy Grid Map(OGM:占用栅格地图):
  • “free space”：用于planning
  • 使用SLAM输出的位姿创建occupancy grid map
  • OGM可用于路径规划(planning)和定位(localization)
  • 又叫做根据已知的Pose建图(mapping with know poses)
  • Cell的值：表示此Cell有障碍物的概率（被占据）
  • 采用Bayesian滤波更新OGM Cell的值
• OGM的特征
  • 把真实世界离散化为cells
  • 栅格结构是刚体
  • 每个cell有占用、空闲、未知三种状态
  • 非参数化模型
  • 大的map需要大量的内存
  • 不依赖特征
  • 每个cell是一个随机变量（模型化占用情况）
• Cell的占用概率
  • Cell被占用： $p(m_i)=1$
  • Cell未被占用： $p(m_i)=0$
  • Cell未知状态： $p(m_i)=0.5$
• 每个Cell有一个对应的随机变量，这些随机变量相对独立
  • 随机变量表示cell被占用的概率
• 根据已知数据估计一个map
  • 给定Sensor数据 $z_{1:t}$和Sensor的位姿 $x_{1:t}$，估计map:
    $p(m|z_{1:t}, x_{1:t})=\prod_i p(m_i|z_{1:t}, x_{1:t})$

1.1 Log-odd概念

• Log odd的概念：计算odd(赔率)的对数
• 为什么需要Log-Odd?
  • 使用log-odds的形式，cell的值更新只使用加法(即把乘法变成了加法)
  • 避免在0或１附近，概率被截断问题 $l(x) \in (-\infty, \infty)$
    
• Odd定义：事件发生的概率与事件不发生的概率的比值
  $odd(x) = \frac{p(x)}{1-p(x)}$
• Log-odd定义：　 $l(x) =log(odd(x))= log\frac{p(x)}{1-p(x)}$
• 反向计算p(x)： $p(x) = 1- \frac {1}{1+expl(x)}$ $p(x)=\frac{odd(x)}{1+odd(x)}$

1.2 Log-Odd update (对数赔率更新法)

1.2.1 OGM概率更新原理

• 每个Cell的值为Cell被占据的概率值
• 在Bayes’ Rule下，根据传感器的测量数据更新map中的cell被占据概率
• Bayes规则： $p(m_{x,y}|z) = \frac {p(z|m_{x,y})p(m_{x,y})}{p(z)})$
• 直接计算概率是困难的，为了简化计算，不使用占据概率本身，而使用log-odd
• $Odd((m_{x,y}=1)\; given \; z) = \frac{p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}$

1.2.2 Log-Odd更新推导过程

$p(m_{x,y}=1|z) = \frac {p(z|m_{x,y}=1)p(m_{x,y}=1)}{p(z)}$
$Odd(m_{x,y}=1|z) = \frac {p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}=\frac {p(z|m_{x,y}=1)p(m_{x,y}=1)/p(z)}{p(m_{x,y}=0|z)}$

• 同理， $p(m_{x,y}=0|z) = \frac {p(z|m_{x,y}=0)p(m_{x,y}=0)}{p(z)}$
  $Odd(m_{x,y}=1|z) = \frac {p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}= \frac {{p(z|m_{x,y}=1)p(m_{x,y}=1)}}{{p(z|m_{x,y}=0)p(m_{x,y}=0)}}$
• 取对数：
  $log(Odd(m_{x,y}=1|z)) =log \frac {p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}=log \frac {{p(z|m_{x,y}=1)p(m_{x,y}=1)}}{{p(z|m_{x,y}=0)p(m_{x,y}=0)}}$
  $= log \frac{p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)} + log \frac {p(m_{x,y}=1)}{p(m_{x,y}=0)}$
• 上式左边为后验概率的Log-Odd，右边为Sensor model和先验概率，通过取对数，把右边两项相乘变成了两项相加
• 可得OGM的log-odds更新公式：
  $log \; odd^+ = log \; odd \; meas + log \; odd^-$

1.2.3 OGM Log-Odd 更新原理

• 每个Cell的值为Log-Odd值
• Log-Odd更新公式： $log \; odd^+ = log \; odd \; meas + log \; odd^-$
  
• 更新规则( $log\;odd\; += log\;odd\;meas$)：
  • 只更新被观察到的Cell
  • 被更新之后的值变成了先验值

1.2.4 Log-Odd形式的测量模型(Measurement Model)

• 测量项： $log \frac {p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)}$
• 情况1：Cell (z=1)
  $log \; odd \; occ = log \frac {p(z=1|m_{x,y}=1)}{p(z=1|m_{x,y}=0)}$
• 情况２：Cell (z=0)
  $log \; odd \; free = log \frac {p(z=0|m_{x,y}=0)}{p(z=0|m_{x,y}=1)}$
• 如果log-odd的值为０，则Odd＝１，即 $p(m_{x,y}=1)=p(m_{x,y}=0) = 0.5$
• 测量模型： $p(z|m_{x,y})$
• $p(z=1|m_{x,y}=1)$：True occupied measurement
• $p(z=0|m_{x,y}=1)$：False free measurement
• $p(z=1|m_{x,y}=0)$：False occupied measurement
• $p(z=0|m_{x,y}=0)$：True free measurement

1.3 Cartographer Submap的Cell更新过程

• 当一个Scan插入Submap时，通过以下步骤进行：
  • 为hits计算一个栅格点的集合，且为misses计算一个与hits不相交的集合
  • 对于每一个hit,插入一个最接近的grid point进入hits集合
  • 对于每一个miss,把与线段(origin,scan point)交叉的Grid Point（且不在hits集合中的）插入misses集合
  • 如果一个Grid Point从未被更新，则分配 $p_{hit}或p_{miss}给它$，当它分别位于hits集合或misses集合
  • 如果一个Grid Point已经被更新，则更新公式如下：
    $odds(p)=\frac {p}{1-p}$
  • 为hit更新：
    $M_{new}(x) = clamp(odds^{-1}(odds(M_{old}(x)) \bullet odds(p_{hit})))$
  • 为miss更新：
    $M_{new}(x) = clamp(odds^{-1}(odds(M_{old}(x)) \bullet odds(p_{miss})))$
  • 因为 $p_{hit} > 0.5$且 $p_{miss} < 05$所以有：
    $odds(p_{hit}) > 1 \quad odds(p_{miss}) < 1$