博客转载自：https://blog.csdn.net/zhubaohua_bupt/article/details/77093334
绘制二维障碍栅格地图-局部地图的绘制 ：http://blog.csdn.net/zhubaohua_bupt/article/details/72923373 讲述了如何利用深度相机去绘制单帧图像看到的局部地图。但是只有局部地图是不够用的，在一般的地面机器人的路径规划中，往往需要整个场景的栅格地图，这样，就可以为机器人，在场景里所有可达的目标位置，提供路径规划数据。本篇博客是接上述那篇博客讲述的。本篇博客内容为：在已知局部栅格地图后，怎么通过相机在不同时刻的位姿，把多帧局部地图绘制成全局栅格地图。比如，这是一个局部地图：
不断融合后，形成的全局地图：
关键技术点： 1: 坐标转换: 局部栅格地图根据相机位姿，把二维栅格点转换到不断融合的全局地图 2. 地图融合: 不断融合的全局栅格地图，和新的一帧局部栅格地图的融合
坐标转换
坐标转换是把t时刻的局部地图点，经过相机t时刻的位姿，投射到全局地图中。栅格地图的应用对象是地面机器人，它的运动一般是二维的，所以，在坐标转换过程中，我们只需要考虑相机当前坐标系与初始坐标系（一般用它来作为全局参考系）的偏航角Yaw与t时刻相机中心在全局中心的位置。以下默认坐标系(Z轴：相机前方向，Y轴：相机下方向，X轴：相机左方向)
偏航角的计算：如果我们知道了t时刻相机相对于全局参考坐标系的位姿T(R，t)之后，只需要姿态R，就可以转换得到偏航角Yaw（不明白就搜旋转矩阵转RPY角）
公式如下
其中， R为t时刻相机相对于参考系的旋转矩阵，R31为旋转矩阵第三行第一列的元素，其他类推。
t时刻相机中心在全局中心的位置:
地图融合
这一部分讲述的是，把局部地图投影到全局地图后，怎么和已有的全局地图进行融合，形成一个新的全局地图。我们知道，栅格地图至少有三种状态（可通行（无障碍占据）、有障碍占据、未知），融合是解决这样一个具体问题：在全局栅格地图中，某一个栅格（称为g）肯定是这些状态中的一种，当局部地图上的点伴随着它自己的状态，投影到栅格g上时，怎么解决栅格g状态更新的问题。常用的融合策略是概率融合（搜概率栅格图）。在《局部地图》这篇博客中，我们画的局部地图其实就是概率栅.格图，初始时，用像素值128表示当前栅格不确定（所用像素值的范围0-255），然后，如果当前栅格是障碍，我们把它置0，否则就是255。
融合方法1: 概率统计
全局栅格地图上栅格的值，我们不让它是3值的（3状态），而是让它们分布在0-255，栅格对应的值小于128时，越小是障碍的概率越大，栅格对应的值大于128时，越大是障碍的概率越小。如此，我们就可利用如下思想融合地图：
如此，我们就可这样融合地图： IF plocalGrid==obstacle and plocalGrid -value1>=0 Then pglobleGrid-=value1; ELSE IF plocalGrid== PassableandpglobleGrid +value2<=255 Then pglobleGrid +=value2; Endif
value1和value2是一个经验值，比如可以都取5。注:pglobleGrid为局部栅格plocalGrid投影到全局栅格上后，在全局栅格上的位置。栅格地图的像素值在0~255之间。这个方法虽然比较暴力，但也是基于统计的，效果可行。
融合方法2: 贝叶斯滤波
更普通的方法是利用贝叶斯滤波对栅格地图融合。先给出公式
这个公式乍一看有点复杂，但稍微经过解释，非常容易理解。下面我们来看看，这个公式表述了一个怎样的融合方法。首先解释一下公式中的符号含义：
z : 二值化观测值（每一次的观测值来源于局部地图，只有两种状态：占据、空闲）
融合t+1时刻的观测值后，栅格i被障碍占据的概率。
融合t时刻的观测值后，栅格i被障碍占据的概率。
单纯的从t+1时刻的观测值，判断的栅格i被障碍占据的概率
一般取常数，比如
注意：这里
不是一个完备事件，也就是说两者之和不一定为1。说到这里，讲的可能还不是特别清楚，我们举个融合的例子来帮助理解。
通过这两个例子，也就是新观测不同的时候，全局地图融合的过程，你是否能理解这个公式呢？
地图融合注意事项
在融合地图时，我们只融合当前局部地图能看到的范围，我们不能往全局地图里融合未知的范围，否则就会出错。举个例子，比如你的全局地图刚才看到了一面墙后面的场景，当前局部地图看到的是相机至这面墙之间的空间，墙后面的看不到（对于局部地图是未知区域），这个时候，如果把未知融合到全局地里，肯定不是我们想要的。最后，地图构建的效果见顶端。