占据栅格地图（Occupancy Grid Map）
1.机器人地图的分类
地图有很多种表示方式，例如，用经纬度标识地方的世界地图，城市的地铁图，校园指引图。
第一种我们称为尺度地图（Metric Map），每一个地点都可以用坐标来表示，比如北京在东经116°23′17’‘，北纬39°54′27’'；第二种我们称为拓扑地图（Topological Map），每一个地点用一个点来表示，用边来连接相邻的点，即图论中的图（Graph），比如从地铁路线图中我们知道地铁红磡站与旺角东站和尖东站相连；第三种我们称为语义地图（Semantic Map），其中每一个地点和道路都会用标签的集合来表示，例如，有人问我中山大学教学楼E栋在哪里，我会说在图书馆正门右手边靠近图书馆的一侧。
在机器人领域，尺度地图常用于定位于地图构建（Mapping）、定位（Localization）和同时定位与地图构建（Simultaneous Localization And Mapping，SLAM），拓扑地图常用于路径规划（Path Planning），而语义地图常用于人机交互（Human Robot Interaction）。
这节课我们将介绍如何用机器人传感器数据绘制尺度地图。这有什么难点呢？首先也是最重要的一点，传感器数据有噪音。用激光传感器检测前方障碍物距离机器人多远，不可能检测到一个准确的数值。如果准确值是 2 \sqrt 2 2 米，有时会测出1.42米，有时甚至1.35米。另外，传感器数据是本地坐标系的，而机器人要构建的是一个全局的地图。最后，机器人会运动，运动也是有噪音的。总结起来就两个字，噪音。通俗点来讲，“不准”。
2.占据栅格地图
我们首先来介绍机器人Mapping用到的的传感器，它叫做激光传感器（Laser Sensor），如下图所示：
激光传感器会向固定的方向发射激光束，发射出的激光遇到障碍物会被反射，这样就能得到激光从发射到收到的时间差，乘以速度除以二就得到了传感器到该方向上最近障碍物的距离。
这样看来，似乎利用激光传感器，机器人能够很好地完成Mapping这一任务。但是我们前面提到了，传感器数据是有噪音的。例如，假如我们在此时检测到距离障碍物4米，下一时刻检测到距离障碍物4.1米，我们是不是应该把4米和4.1米的地方都标记为障碍物？又或者怎么办呢？
为了解决这一问题，我们引入占据栅格地图（Occupancy Grid Map）的概念。
我们首先来解释这里的占据率 (Occupancy) 指的是什么。在通常的尺度地图中，对于一个点， 它要么有 (Occupied状态，下面用1来表示) 障碍物，要么没有 (Free状态，下面用0来表示) 障碍物 (旁白：那么问题来了，薛定谔状态呢? )。在占据栅格地图中，对于一个点，我们用
p
(
s
=
1
)
p(s=1)
p(s=1) 来表示它是Free状态的概率，用
p
(
s
=
0
)
p(s=0)
p(s=0) 来表示它是Occupied状态的概率，当然两者的和为 1 。两个值太多了，我们引入两者的比值来作为点的状态:
Odd
(
s
)
=
p
(
s
=
1
)
p
(
s
=
0
)
\operatorname{Odd}(s)=\frac{p(s=1)}{p(s=0)}
Odd(s)=p(s=0)p(s=1) 。
对于一个点，新来了一个测量值 (Measurement，
z
∼
{
0
,
1
}
z \sim\{0,1\}
z∼{0,1} ) 之后我们需要更新它的状态。 假设测量值来之前，该点的状态为
Odd
(
s
)
\operatorname{Odd}(s)
Odd(s) ，我们要更新它为：
Odd
(
s
∣
z
)
=
p
(
s
=
1
∣
z
)
p
(
s
=
0
∣
z
)
\operatorname{Odd}(s \mid z)=\frac{p(s=1 \mid z)}{p(s=0 \mid z)}
Odd(s∣z)=p(s=0∣z)p(s=1∣z) 。 这种表达方式类似于条件概率，表示在
z
z
z 发生的条件下
s
s
s 的状态。
根据贝叶斯公式，我们有:
p
(
s
=
1
∣
z
)
=
p
(
z
∣
s
=
1
)
p
(
s
=
1
)
p
(
z
)
p
(
s
=
0
∣
z
)
=
p
(
z
∣
s
=
0
)
p
(
s
=
0
)
p
(
z
)
\begin{aligned} &p(s=1 \mid z)=\frac{p(z \mid s=1) p(s=1)}{p(z)} \\ &p(s=0 \mid z)=\frac{p(z \mid s=0) p(s=0)}{p(z)} \end{aligned}
p(s=1∣z)=p(z)p(z∣s=1)p(s=1)p(s=0∣z)=p(z)p(z∣s=0)p(s=0)
带入之后，我们得
O
d
d
(
s
∣
z
)
=
p
(
s
=
1
∣
z
)
p
(
s
=
0
∣
z
)
=
p
(
z
∣
s
=
1
)
p
(
s
=
1
)
/
p
(
z
)
p
(
z
∣
s
=
0
)
p
(
s
=
0
)
/
p
(
z
)
=
p
(
z
∣
s
=
1
)
p
(
z
∣
s
=
0
)
O
d
d
(
s
)
\begin{aligned} O d d(s \mid z) &=\frac{p(s=1 \mid z)}{p(s=0 \mid z)} \\ &=\frac{p(z \mid s=1) p(s=1) / p(z)}{p(z \mid s=0) p(s=0) / p(z)} \\ &=\frac{p(z \mid s=1)}{p(z \mid s=0)} O d d(s) \end{aligned}
Odd(s∣z)=p(s=0∣z)p(s=1∣z)=p(z∣s=0)p(s=0)/p(z)p(z∣s=1)p(s=1)/p(z)=p(z∣s=0)p(z∣s=1)Odd(s)
我们对两边取对数得:
log
O
d
d
(
s
∣
z
)
=
log
p
(
z
∣
s
=
1
)
p
(
z
∣
s
=
0
)
+
log
O
d
d
(
s
)
\log O d d(s \mid z)=\log \frac{p(z \mid s=1)}{p(z \mid s=0)}+\log O d d(s)
logOdd(s∣z)=logp(z∣s=0)p(z∣s=1)+logOdd(s)
这样，含有测量值的项就只剩下了
log
p
(
z
∣
s
=
1
)
p
(
z
∣
s
=
0
)
\log \frac{p(z \mid s=1)}{p(z \mid s=0)}
logp(z∣s=0)p(z∣s=1) 。我们称这个比值为测量值的模型(Measurement Model)，标记为 lomeas 。测量值的模型只有两种:
l
o
f
r
e
e
=
log
p
(
z
=
0
∣
s
=
1
)
p
(
z
=
0
∣
s
=
0
)
lo_{free} =\log \frac{p(z=0 \mid s=1)}{p(z=0 \mid s=0)}
lofree=logp(z=0∣s=0)p(z=0∣s=1) 和
l
o
o
c
c
u
=
log
p
(
z
=
1
∣
s
=
1
)
p
(
z
=
1
∣
s
=
0
)
lo_{occu} =\log \frac{p(z=1 \mid s=1)}{p(z=1 \mid s=0)}
looccu=logp(z=1∣s=0)p(z=1∣s=1) ，而且都是定值。
这样，如果我们用
log
O
d
d
(
s
)
\log O d d(s)
logOdd(s) 来表示位置
s
s
s 的状态
S
S
S 的话，我们的更新规则就进一步简化成 了:
S
+
=
S
−
+
l
o
m
e
a
s
S^{+}=S^{-}+lomeas
S+=S−+lomeas。其中
S
+
S^{+}
S+和
S
−
S^{-}
S−分别表示测量值之后和之前
s
s
s 的状态。
另外，在没有任何测量值的初始状态下，一个点的初始状态
S
i
n
i
t
=
log
O
d
d
(
s
)
=
log
p
(
s
=
1
)
p
(
s
=
0
)
=
log
0.5
0.5
=
0
。
S_{i n i t}=\log O d d(s)=\log \frac{p(s=1)}{p(s=0)}=\log \frac{0.5}{0.5}=0 \text { 。 }
Sinit=logOdd(s)=logp(s=0)p(s=1)=log0.50.5=0 。
经过这样的建模，更新一个点的状态就只需要做简单的加减法了。这，就是数学的魅力。
例如，假设我们设定
l
o
o
c
c
u
=
0.9
lo_{occu}=0.9
looccu=0.9 ，
l
o
f
r
e
e
=
−
0.7
lo_{free}=-0.7
lofree=−0.7 。那么，一个点状态的数值越大，就表 示越肯定它是Occupied状态，相反数值越小，就表示越肯定它是Free状态。
上图就展示了用两个激光传感器的数据更新地图的过程。在结果中，一个点颜色越深表示越肯定它是Free的，颜色越浅表示越肯定它是Occupied的。
3.TSDF（Truncated Signed Distance Function）
TSDF（Truncated Signed Distance Function）是实时3D重建经典算法，简单可并行，极大推动了实时三维重建的发展。TSDF是SDF的改进，讲取值限制在[-1,1]之间，同时仅在物体表面的限定的距离范围内进行计算，降低了计算量。
TSDF思路很朴素，用一个大的空间包围盒(volume)去包括进去待3D构建的场景，volume成多个voxel(小立体方块)：
如何计算tsdf？假设已经从深度相机获得多帧RGBD图像以及相机的位姿T。
左图中灰色小方格表示体素voxel，蓝色三角性表示视场，绿色线为截面对应的线。
第一步：计算体素
x
x
x 在世界坐标系下的坐标。记体素
x
x
x 在TSDF地图上得坐标为
(
v
x
,
v
y
,
v
z
)
\left(v_{x}, v_{y}, v_{z}\right)
(vx,vy,vz) -则该体素在世界坐标系下的位置为:
P
x
,
world
=
(
x
0
+
v
x
⋅
Voxel.
x
,
y
0
+
v
y
⋅
Voxel.
y
,
z
0
+
v
z
⋅
Voxel.
z
)
P_{x, \text { world }}=\left(x_{0}+v_{x} \cdot \text { Voxel. } x, y_{0}+v_{y} \cdot \text { Voxel. } y, z_{0}+v_{z} \cdot \text { Voxel. } z\right)
Px, world =(x0+vx⋅ Voxel. x,y0+vy⋅ Voxel. y,z0+vz⋅ Voxel. z)
第二步：计算体素在相机坐标系下的坐标。相机的旋转为
R
R
R 平移为
T
T
T ，则
P
x
,
c
a
m
e
r
a
=
R
P
x
,
w
o
r
l
d
+
T
P_{x, c a m e r a}=R P_{x, w o r l d}+T
Px,camera=RPx,world+T ；
第三步：根据相机内参
K
K
K 计算体素在相机坐标系下
z
z
z 向深度
cam
z
(
x
)
\operatorname{cam}_{z}(x)
camz(x) :
第四步：重投影计算沿光心经过体素
x
x
x 到达物体表面
p
p
p 点对应的
z
z
z 向深度：因为RGBD的信息 已经知道，所以根据
P
x
,
c
a
m
e
r
a
P_{x, c a m e r a}
Px,camera 可以直接查到
p
p
p 的深度值
depth
(
pic
(
x
)
)
\operatorname{depth}(\operatorname{pic}(x))
depth(pic(x)) ；
第五步：得到sdf:
s
d
f
i
(
x
)
=
depth
i
(
p
i
c
(
x
)
)
−
cam
z
(
x
)
s d f_{i}(x)=\operatorname{depth}_{i}(p i c(x))-\operatorname{cam}_{z}(x)
sdfi(x)=depthi(pic(x))−camz(x)
第六步：截断：
t
s
d
f
i
(
x
)
=
max
(
−
1
,
min
(
1
,
s
d
f
i
(
x
)
t
)
)
t s d f_{i}(x)=\max \left(-1, \min \left(1, \frac{s d f_{i}(x)}{t}\right)\right)
tsdfi(x)=max(−1,min(1,tsdfi(x))) 即将sdf截断在
[
−
t
,
t
]
[-\mathrm{t}, \mathrm{t}]
[−t,t] 内，sdf超 过这个范围的voxel点不用计算，直接得1或者-1。
第七步：将当前帧得到的值合并到已有的volume。通过将多帧合并成一个TSDF，既可以提升精度 又可以补全单帧缺失的信息。
TSDFi
(
x
)
=
W
i
−
1
(
x
)
T
S
D
F
i
−
1
(
x
)
+
w
i
(
x
)
t
s
d
f
i
(
x
)
W
i
−
1
(
x
)
+
w
i
(
x
)
W
i
(
x
)
=
W
i
−
1
(
x
)
+
w
i
(
x
)
\begin{aligned} &\operatorname{TSDFi}(x)=\frac{W_{i-1}(x) T S D F_{i-1}(x)+w_{i}(x) t s d f_{i}(x)}{W_{i-1}(x)+w_{i}(x)} \\ &W_{i}(x)=W_{i-1}(x)+w_{i}(x) \end{aligned}
TSDFi(x)=Wi−1(x)+wi(x)Wi−1(x)TSDFi−1(x)+wi(x)tsdfi(x)Wi(x)=Wi−1(x)+wi(x)
对于需要更新的voxel
w
i
(
x
)
=
1
w_{i}(x)=1
wi(x)=1 ，没在当前视场内的点为 0 。
最后: 重复遍历所有帧，得到合并后的TSDF。
参考文章
[1] https://zhuanlan.zhihu.com/p/21738718 占据栅格地图（Occupancy Grid Map）
[2] https://zhuanlan.zhihu.com/p/487277749 TSDF地图算法分析 实时三维重建
5016
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
