orb特征 稠密特征_比对占有栅格以及基于ORB-SLAM2构建栅格地图的一种方法 杨栋梁2019.9.22...

项目任务：弄明白基于ORB-SLAM2构建栅格地图方法原理

（基于ORB-SLAM2构建栅格地图方法来源：https://github.com/skylook/ORB_SLAM2-gridmap ）

完成情况：

（注意：对于文章中所涉及的基于ORB-SLAM2构建栅格地图方法的讲解请见：

Freeme21：基于ORB-SALM实时网格地图构建 杨栋梁2019.9.15​zhuanlan.zhihu.com

所涉及的方法是链接中最终所述的ORB-SLAM2 Gridmap部分）

首先，完整了解链接中的给出的基于ORB-SLAM2构建栅格地图的方法，然后，查阅资料了解具体原理细节，这里通过与占有栅格法比对发现：

1、关于栅格地图。

栅格地图则把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。

上面看到的都是栅格图，所以，对于栅格图来说最重要的是栅格如何划分以及每个栅格占有概率是多少。

2、占有栅格原理

二维激光雷达传感器会扫描出空间中某一高度的平面，相当于给整个环境来一刀，得到一个切面（内侧）。栅格图中的线条对应环境中障碍物的内侧边界。（注意：从ORB-SLAM2到栅格的方法中提及了关键帧投影到XZ平面，将Y作为高度与此类似）。

雷达扫描过程中，光线撞到（hit）的地方所处坐标位置就对应一种灰度值，没有撞到（miss）的地方所处坐标位置对应另外的灰度值。栅格图中，一个格子某一时刻只有两种状态，要么被光线撞到了要么没有；但是该格子可能多次被撞到或多次没被撞到。如果某个格子多次被撞到了，说明它很有可能是被占用了（有障碍物的意思）。因此，可以用概率的方式来表示某个格子有多大的可能被占用了。（注意：从ORB-SLAM2到栅格的方法使用了Bresenham算法在关键帧中从相机出发向特征点画射线，原理类似激光，这里特征点会存在与分割好的网格中，所谓的特征点在线上就是类似于激光碰到该特征点所在网格内的障碍物，因为我们认为障碍物附近特征点较多）

坐标转换

X是真实世界中的坐标，i为离散化了的地图（栅格地图）中的坐标，r为一格的长度，1/r表示分辨率，显然我们有：i=ceil(x/r)。

然后，计算相机以及障碍物在网格中的位置，根据这两个坐标可以使用Bresenham算法来计算非障碍物格点的集合。（这里再次提及Bresenham算法）

（注意，从ORB-SLAM2到栅格的方法中，每个关键帧中，连接相机和地图点的射线，射线终点，即特征点位置加占有计数器是因为是大概率障碍所在，射线经过的网格加访问计数器，对于每个关键帧计数，求取每个网格之中占有概率，通过与阈值的比较判断是否占有，从而构建栅格地图）

占有栅格法：

第一个问题，咱们可以利用图像的灰度值来表达概率，只要有一张“灰度 -- 概率”对应表。如下图所示，纵坐标可以换算为八位的灰度值，横坐标为概率值。图中曲线为单调函数，这样就能从灰度图中直观的看出概率大小（颜色越深代表可能性越大/小）。一般情况下，纯黑色（灰度值为0）概率为0，代表占用；灰色（灰度值为127）概率为0.5，代表未知；纯白色（灰度值为255）概率为1，代表空闲。

在实际环境中，栅格只有两种状态，要么occupied，要么free。假设已知格子x被hit一次，格子occupied的概率是Po(x) = 0.9,free的概率是Pf(x)= 1 - Po(x) = 0.1；格子x被miss一次，格子occupied的概率是Po(x) = 0.2,free的概率是Pf(x)= 1 - Po(x) = 0.8。如果格子x被连续hit三次，miss一次，那么格子x的概率Po(x)该如何计算？

这里就要用到贝叶斯滤波：

咱们对格子x的概率换一种表达方式，用概率对数的形式：

p(x) / p(¬x) =p(x)/(1 − p(x)) // 该事件发生的概率除以该事件不发生的概率。（1）

l(x) := log(p(x)/(1 − p(x))) //对上式取对数，以自然常数e为底数。（2）

换了这种表达方式后，格子x的置信度（对应原来的概率）取值范围变成(−∞, ∞),同时避免了概率接近0或者1引起的截断问题。

上图曲线就是（2）的体现

横坐标表示概率值或置信度，纵坐标表示图像灰度值。

从（2）式中可得，p(x) = 1 - 1 / (1 + exp{l(x)}) （3）

回到原来的问题，如果格子x被连续hit三次，miss一次，置信度要如何计算？

首先给出结论，计算套路如下：

具体的贝叶斯滤波推导这里不做赘述。

咱们来计算下格子x被hit三次，miss一次的occupied概率：

初值L0 = log(0.5 / (1-0.5)) = 0.

L1 = L0 + log(0.9 / (1-0.9)) - L0 = log9;

L2 = L1 + log(0.9 / (1-0.9)) - L0 = log9 + log9 ;

L3 = L2 + log(0.9 / (1-0.9)) - L0 = log9 + log9 + log9 ;

L4 = L3 + log(0.2 / (1-0.2)) - L0 = log9 + log9 + log9 - log4 ;

即L4 = log(9*9*9/4) ≈ 5.2。

由于式子（3）： p(x) = 1 - 1 / (1 + exp{l(x)})

因此，此时格子x被occupied的概率P(x) ≈ 0.9945，其对应的灰度值就是L4*某系数。

从L4的计算过程可知，如果格子x被hit一次，加log9，被miss一次，减log4；看起来，求状态的概率的过程就是加加减减。

3、总结

基于ORB-SLAM2构建栅格地图方法中，首先利用ROS将从ORB-SLAM2中获取的关键帧地图点打包，通过ROS从monopub节点采用发布订阅方式发送给monosub节点。然后monopub处理关键帧，先对关键帧根据分辨率划分网格，从每个关键帧中相机位置出发向地图点利用Bresenham算法画射线，射线经过的网格加访问计数器，终点地图点所在网格加一占有计数器，通过统计所有关键帧中计数器数据，利用概率公式算出栅格占用概率，从而根据阈值构建栅格地图。

4、启发

占有栅格视角来自车载相机，我们可使用无人机对车以及地图点能进行定位，获取车距离特征点的信息，与车载相机视角中特征点匹配，用以获取具体时间车和障碍物的距离，不过实现会十分困难，尚且有待研究。从ORB-SLAM2到栅格的方法并未使用贝叶斯滤波，求解网格占有概率公式比较简单，那么我们可以将贝叶斯滤波融合进去，相邻关键帧，所做射线如果有出现同一网格的地图点（不一定是同一个点只是都在同一个网格）被连接就看做是占有栅格中发生连续碰撞的情况，使用贝叶斯滤波，会让概率计算更加可信。不过特征点太密集也应该考虑其中，针对此问题，将分辨率设置成特征点提取时的大小，即每个网格只有一个特征点或者没有特征点。