此部分为本文的重点。相对来说,真正的 Semantic SLAM (即语义建图和 SLAM 定位相互促进)发展较晚(基本是2017年后)。
在 Bundle Adjustment (BA) 方法中,我们同时优化相机位姿和 3D 坐标位置,使得
重投影到 2D 图像的像素点与实际观测(多个相机,多个特征点)的总误差最小
。
那么如何将语义信息融合进来呢?
思路一:
同个 3D 点重投影之后,应保持语义一致
。
这又是重投影优化问题,可以加入到 BA 公式中
加强优化目标
,关键是如何
量化重投影误差
,就好像传统 BA 的重投影误差通过与实际观测的像素距离来量化。
ICRA 2017 的著名工作 Probabilistic Data Association for Semantic SLAM
[12]
使用了此思路,其量化重投影误差的方法在于:使用概率模型计算出来的
物体中心,重投影到图像上,应该接近检测框的中心
。而数据关联(到底要接近哪个检测框中心),由一组权重决定,最后 “BA” 和 “权重更新” 通过 EM 算法交替优化。
ECCV 2018 上的工作 VSO
[13]
与之类似,重投影误差通过
与目标类别的语义区域的远近
来量化。有几个细节比较巧妙,下面展开说明。
如上图所示,(a) 为语义分割图,(b) 为类别 “Car” 的区域,在(c) (d) 中,根据与 Car 区域的距离,概率值从1 (红) 变为 0 (蓝)。其他类别如Tree,也会产生此概率分布图。
其中
距离 到 概率 的转化
,利用了下方的高斯分布,(c) (d) 的不同是方差
导致的。这是在为量化重投影误差做准备。对于一个空间点P(拥有坐标
)来说,重投影之后计算得到一个概率:
其中
计算了重投影的结果
与类别 c 区域的
最近距离
,最后得到的
用于
计算重投影误差
:
权重
的存在是为了解决数据关联
,即空间点P应该以哪一个类别的区域为目标,
。其由多个相机下的
值累乘,即多个角度观测投票决定。
会加到普通的 BA 优化公式中,使用 EM 算法进行优化,E 步更新权重
,而 M 步优化三维点P坐标和相机位姿(普通的 BA 过程)。
个人理解,之所以使用高斯分布,是因为其函数有“骤降”之处,那么方差
可以起到决定阈值的作用,让
距离超过阈值的类别区域更快得到一个小权重
,多个相机的投票下,数据关联很快就能稳定下来,可以加速优化。
(为简化说明,上述公式已被简化,去除了对于多个相机、空间点的索引,详见原文)