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)givenz)=\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更新公式：
  $$logod{d}^{+}=logoddmeas+logod{d}^{-}$$

1.2.3 OGM Log-Odd 更新原理

• 每个Cell的值为Log-Odd值
• Log-Odd更新公式：$logod{d}^{+}=logoddmeas+logod{d}^{-}$
  
• 更新规则($logodd+=logoddmeas$)：
  • 只更新被观察到的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)
  $$logoddocc=log\frac{p(z=1|m_{x,y}=1)}{p(z=1|m_{x,y}=0)}$$
• 情况２：Cell (z=0)
  $$logoddfree=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(odd{s}^{-1}(odds(M_{old}(x))\bullet odds(p_{hit})))$$
  • 为miss更新：
    $$M_{new}(x)=clamp(odd{s}^{-1}(odds(M_{old}(x))\bullet odds(p_{miss})))$$
  • 因为$p_{hit}>0.5$且$p_{miss}<05$所以有：
    $$odds(p_{hit})>1odds(p_{miss})<1$$