栅格占用地图

---

1. 概率栅格地图基本情况

• 概率栅格法是一种近似描述，易于创建和维护，对某个栅格的感知信息可直接与环境中某个区域对应，机器人对所测得的障碍物具体形状不敏感，特别适于处理超声、激光测量数据。但当在大型环境中或栅格单元划分比较细时，栅格法计算量迅速增长，需要大量内存单元，使计算机的实时处理变得很困难。
• 基于栅格地图的环境地图模型通常建立在笛卡尔直角坐标系中：将机器人的初始位姿点定义为（0,0，0°）。以机器人初始位姿点定义为坐标原点，机器人的方向定义为 X 轴的正方建立笛卡尔直角坐标系；同时，根据经验确定需要探索的环境地图的尺寸$a_{Total}\ast b_{Total}$。
  
• 对于栅格地图，首先将整个环境分割成边长相同的矩形栅格，栅格的边长根据机器人尺寸和对环境表示的需要进行选取。然后，通过机器人在环境中的不断运动，获得环境中每个栅格的信息并赋以一定值来表示这个栅格被占用的概率。每个栅格代表一个矩形的区块，用一个在(0,1)范围内的值来指示这个区块被占用的概率，表明它所对应的物理位置是否有障碍物存在。

---

2. 栅格占用概率更新方法

栅 格 地 图 中 栅 格 概 率 更 新 方 法 有 许 多 ， 如 Bayesian 方 法 、Dempster-Shafa 方法、运动影射直方图算法等。其中，Bayesian 方法比较成熟。

2.1 Bayesian 方法

传感器模型产生 $p(s|OCC)$ 形式的条件概率，其中$OCC$表示栅格被占用，$s$表示传感器返回值。代表对应栅格确实被占用的情况下，返回值为传感器实际测量值的概率。但是在实际应用中，更关心的是返回值是实际测量值时，栅格被占用的概率，即$p(OCC|s)$。

显然， $p(s|OCC)\ne p(OCC|s)$ 。但是根据Bayesian规则，有：

$$p(OCC|s)=\frac{p(s|OCC)P(OCC)}{p(s|OCC)P(OCC)+p(s|EMP)P(EMP)}$$

其中，$EMP$代表栅格空闲。 $p(s|OCC)$ 和 $p(s|EMP)$ 由传感器模型得到。 $P(OCC)$和$P(EMP)$是先验概率，是根据已有信息计算得到，例如移动机器人在探测某个房间之前，已经知道其中30%的地面被障碍物覆盖。但是，实际中是没有先验概率的，所以测量前一般令其等于50%。

2.2 栅格占用概率求解

假设机器人传感器探测范围为$[d_{min},d_{max})]$,夹角范围为$[-{\varphi }_{min},{\varphi }_{min}]$。机器人工作环境区域为$a_{Total}\ast b_{Total}$的矩形区域。

假设每个栅格状态为 x G ∈ { E M P , O C C } x_{G}\in \left \{ EMP,OCC \right \} xG​∈{EMP,OCC}，分别代表存在与不存在障碍物。每个状态对应一个概率值，由机器人根据传感信息更新。

根据贝叶斯规则，令初始时刻所有栅格状态值 p { x G ( 0 ) = E M P } = p { x G ( 0 ) = O C C } = 0.5 p\left \{ x_G(0)=EMP \right \} = p\left \{ x_G(0)=OCC \right \} =0.5 p{xG​(0)=EMP}=p{xG​(0)=OCC}=0.5。当运动到第$k$步时，传感器观测到前方粗在障碍物，且传感器读数等于$Z(k)=[d(k),\varphi (k){]}^T$，如下图。

假设传感器距离观测值和角度观测值相互独立，下面根据$Z(k)$计算传感器观测范围内栅格状态的概率值。

首先机器人根据自身位姿$X_r(k)$估计哪些栅格被传感器覆盖，对这些栅格的概率值进行计算和更新，其它栅格的状态不变。然后，分别利用距离返回值$d(k)$和角度返回值$\varphi (k)$计算栅格$i$中存在障碍物的概率为：

其中，$d_{G_i}$和${\varphi }_{G_i}$分别表示栅格i 的中心点相对于机器人的距离和夹角。

因为$d(k)$和$\varphi (k)$相互独立，则基于当前机器人位姿和观测值计算的栅格$i$中存在障碍物的概率为：

同理可知，基于当前机器人位姿和观测值计算的栅格$i$中不存在障碍物的概率为：

又因为：

且在没有观测$Z(k)$的条件下， p { x G i ( k ) = O C C ∣ X r ( k ) } = p { x G i ( k ) = E M P ∣ X r ( k ) } p\left \{ x_{G_{i}}(k)=OCC|X_{r}(k) \right \} = p\left \{ x_{G_{i}}(k)=EMP|X_{r}(k) \right \} p{xGi​​(k)=OCC∣Xr​(k)}=p{xGi​​(k)=EMP∣Xr​(k)}，则由（5-5）得到：

由（5-9）得：

将(5-9)代入(5-4)，整理得到：

而栅格$i$中不存在障碍物的概率为：

基于当前机器人位姿和传感器观测值计算出第k步栅格状态的概率之后，利用它更新第k−1步的栅格状态概率值，得到最终的第k 步栅格状态，下面给出更新方法。

因为

其中，

又因为

由(5-13)和（5-14），得：



当栅格概率大于$p_{max}$时，认为它被障碍物覆盖；当栅格概率小于$p_{min}$时，认为它属于自由区，其中$p_{max}$和$p_{min}$为给定阈值。

2.3 占用栅格地图的C++实现

参考：https://blog.csdn.net/zhao_ke_xue/article/details/109040676

---

3.基于gmapping的栅格地图构建

3.1 原理

TODO

3.2 gmapping的使用

Gmapping是一个基于2D激光雷达使用RBPF（Rao-Blackwellized Particle Filters）算法完成二维栅格地图构建的SLAM算法。

• pros：可以实时构建室内环境地图，在小场景中计算量少，且地图精度较高，对激光雷达扫描频率要求较低。
• cons：随着环境的增大，构建地图所需的内存和计算量就会变得巨大，所以gmapping不适合大场景构图。一个直观的感受是，对于200x200米的范围，如果栅格分辨率是5cm，每个栅格占用一个字节内存，那么每个粒子携带的地图都要16M的内存，如果是100粒子就是1.6G内存。

功能包的安装：
二进制方式安装gmapping算法功能包

sudo apt-get install ros-kinetic-gmapping       #根据您ROS的版本，自行更改即可

关于gmapping 算法功能包中的话题和服务如下图所示：

gmapping需要订阅机器人关节变换话题/tf和激光雷达扫描数据话题/scan，将会发布栅格地图话题/map。

关于/tf又分为两个部分，gmapping功能包中的TF变换如下图所示：

gmapping算法的必备条件，/tf、/odom、/scan，就可以通过算法发布/map了。

在启动gmapping算法之前，启动ROS小车以及激光雷达传感器之后，你的TF树应该大致如下图所示，这样才能满足启动gmapping算法的必备条件

rosrun rqt_tf_tree rqt_tf_tree

接下来，更改参数

启动算法的gmapping.launch文件如下，这里需要根据各自的情况，对下面文件中注释的部分进行更改，更改内容根据自己的情况。

该启动的launch文件放在任意一个功能包的launch的文件夹中都可以，也可以新建一个功能包，因为按照二进制方式安装gmapping算法是没有这个文件的。

<launch>
    <arg name="scan_topic" default="scan" />                  <!-- 根据自己发布scan名称进行修改 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="base_frame" value="base_footprint"/>     <!-- 根据自己的基座标系名称进行修改 -->
        <param name="odom_frame" value="odom"/>               <!-- 根据自己的里程计坐标系名称进行修改 -->
        <param name="map_update_interval" value="4.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

最后，启动算法

使用下面的命令，即可启动gmapping算法

roslaunch robot_test gmapping.launch      #这里的gmapping.launch放在robot_test功能包下

此时，在运行的ROS节点关系如下图所示，其中/mbot_bringup为手写的功能包节点主要用来发布TF和odom以及与下位机通信。

保存地图命令如下：

rosrun map_server map_saver -f map

生成map.pgm、 map.yaml两个文件，其中map.yaml文件内容如下：

image: map.pgm                               #图像文件名字
resolution: 0.050000                         #图像分辨率
origin: [-13.800000, -1.000000, 0.000000]    #机器人在开始建图时在地图图片上的姿态
negate: 0                                    #对立面
occupied_thresh: 0.65                        #被占据的临界值
free_thresh: 0.196                           #自由状态的临界值

---

4. 栅格地图的改进

4.1 八叉树地图

• Motivation：栅格占用地图需要对地图进行密集切分，势必造成内存占用过大。实际一些环境中，障碍物所占的空间相比自由空间是要少的，此时就可以采用八叉树地图（Octo-map）。

原理见下图：

八叉树功能包octomap的使用：

• 基本实现思路：将点云数据通过ROS发布到某个topic上面比如"/outputCloud",再启动 octomap 节点将数据读入该topic并发布到另一个新的topic 上面去。最后在rviz 里面接收这个新topic 达到实时显示的目的.

1. 安装octomap
以 “indigo” 为例，安装命令如下：

sudo apt-get install ros-indigo-octomap-ros #安装octomap
sudo apt-get install ros-indigo-octomap-msgs
sudo apt-get install ros-indigo-octomap-server

安装octomap 在 rviz 中的插件

sudo apt-get install ros-indigo-octomap-rviz-plugins

这个时候启动rviz，就会多出一个octo开头的模组。

2. 发布点云数据
首先跑ORB生成稠密的点云文件，把这个点云文件加载然后通过一个topic发布出去:

/**
*
* 函数功能:读取pcl点云文件并发布到topic上去
* maker: crp 
* data: 2016-6-8
*/

#include<iostream>
#include<string>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <vector>

#include<ros/ros.h>  
#include<pcl/point_cloud.h>  
#include<pcl_conversions/pcl_conversions.h>  
#include<sensor_msgs/PointCloud2.h>  
#include<pcl/io/pcd_io.h>

using namespace std;

int main (int argc, char **argv)  
{  
	std::string topic,path,frame_id;
    int hz=5;

	ros::init (argc, argv, "publish_pointcloud");  
	ros::NodeHandle nh;  

    nh.param<std::string>("path", path, "/home/crp/catkin_ws/test.pcd");
	nh.param<std::string>("frame_id", frame_id, "camera");
	nh.param<std::string>("topic", topic, "/pointcloud/output");
    nh.param<int>("hz", hz, 5);
   
	ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> (topic, 10);  

	pcl::PointCloud<pcl::PointXYZ> cloud;  
	sensor_msgs::PointCloud2 output;  
	pcl::io::loadPCDFile (path, cloud);  
	pcl::toROSMsg(cloud,output);// 转换成ROS下的数据类型 最终通过topic发布

	output.header.stamp=ros::Time::now();
	output.header.frame_id  =frame_id;

	cout<<"path = "<<path<<endl;
	cout<<"frame_id = "<<frame_id<<endl;
	cout<<"topic = "<<topic<<endl;
	cout<<"hz = "<<hz<<endl;

	ros::Rate loop_rate(hz);  
	while (ros::ok())  
	{  
		pcl_pub.publish(output);  
		ros::spinOnce();  
		loop_rate.sleep();  
	}  
	return 0;  
}  

我们通过如下命令单独启动点云发布节点:

rosrun publish_pointcloud publish_pointcloud

此时可以看到发布的点云数据的topic.可以使用rostopic echo 来检查是否有数据输出。

然后再打开新的终端运行RVIZ:

rosrun rviz rviz

• 点击add 按钮添加"PointCloud2模块"
• 设置topic为"/pointcloud/output"
• 设置FixedFram为"camera"

设置完成以后你可以看到界面中会显示出topic 发布的点云数据,如下图：

3. Octomap 实时显示
写一个launch文件去启动 octomap_server ,创建 octomaptransform.launch 文件,填入下面代码:

<launch>
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.05" />

    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <param name="frame_id" type="string" value="/camera" />

    <!-- max range / depth resolution of the kinect in meter -->
    <param name="sensor_model/max_range" value="100.0" />
    <param name="latch" value="true" />

    <!-- max/min height for occupancy map, should be in meters -->
    <param name="pointcloud_max_z" value="1000" />
    <param name="pointcloud_min_z" value="0" />

    <!-- topic from where pointcloud2 messages are subscribed -->
    <remap from="/cloud_in" to="/pointcloud/output" />
 
  </node>
</launch>

这个文件里面有的frame_id 和 remap topic 的值必须和发布节点中的frame_id以及数据发布的topic一致。

接下来首先启动点云发布节点：

rosrun publish_pointcloud publish_pointcloud

其次启动了这个节点以后，我们再去启动Octomap服务节点, 正确启动以后会有几个 octomap 相关的 topic 发布: (如下图)

roslaunch publish_pointcloud octomaptransform.launch

最后在rviz 中添加一个 “OccupancyGrid”模块(三维格点). 设置topic 为"/octomap_full",即可以得到如下结果:

最后我们将所有的启动命令写入到一个launch文件中，我们在publish_pointcloud 包中的 launch 文件夹下面编辑一个名为demo.launch的文件,填入下面代码:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
 
  <node name="publish_pointcloud" pkg="publish_pointcloud" type="publish_pointcloud">
	<param name="path" value="$(find publish_pointcloud)/data/test.pcd" type="str" />
	<param name="frame_id" value="camera" type="str" />
	<param name="topic" value="/pointcloud/output" type="str" />
	<param name="hz" value="2" type="int" />
  </node>

  <!-- Load ocotmap launch -->
  <include file="$(find publish_pointcloud)/launch/octomaptransform.launch" />

  <!-- RViz -->
  <node pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find publish_pointcloud)/rviz/OctomapShow.rviz"/>

</launch>

就可以通过上面的launch文件一键启动节点以及RVIZ了。启动命令为：

roslaunch publish_pointcloud demo.launch

这样，就可以将点云数据发布到一个指定的 topic 上,然后调用 Octomap 在ROS下的srv组件进行实时转换,并发布到另外一个 Octomap topic 上去.最后通过可视化工具 rviz 进行显示Octomap。

4.2 Voxel hashing

• 体素块（Voxel Blocks）：无限均匀的网格将世界细分为体素块（voxel block），每个体素块由888个体素构成，每个体素存储一个T-SDF、颜色和权重。哈希表中的每一项都对应着一个体素块。

• Voxel Hashing的基本原理：

  • 在输入一张新的深度图片时，首先执行融合操作。根据输入的深度图分配体素块，并将块描述符插入哈希表中。这里要注意，只是分配了体素块，但是体素块内并没有存储相应的数据。
  • 接下来对依据输入的深度图和颜色数据对每个分配的体素块进行扫描，从中获得对应的SDF、颜色和权重。此外，对距离等值面太远的体素块进行移除，并释放相应的内存。
  • 在融合操作结束之后，依据估计的当前相机位姿进行光线投射从而获得等值面。获得的深度信息和颜色信息可以用在下一步的相机位姿估计当中。
  • 最后，在主机和GPU中间建立双向流，将即将使用的哈希表条目及其对应的体素块传输到主机上。之前流出的体素块也可在重新访问时流回GPU数据结构。

• Voxel Hashing的特点

  • 能够有效的压缩T-SDF的体积，在无需分层空间的数据结构的同时，保证表面的分辨率
  • 通过插入和更新操作，能够有效的把新的T-SDF数据融合到哈希表中，同时最小化哈希冲突
  • 在清理无效的体素块时，不需要重组数据结构，避免了巨大的开销

---

每个体素块由888个体素构成，每个体素存储一个T-SDF、颜色和权重。

体素的数据结构如下：

struct Voxel {
	float sdf;
	uchar colorRGB[3];
	uchar weight;
};

为了利用稀疏性，同时减少计算资源的消耗，仅仅在重建的曲面几何体周围分配。

通过世界坐标(x,y,z)计算哈希值的函数如下：
$$H(x,y,z)=(x\cdot p_1\oplus y\cdot p_2\oplus z\cdot p_3)modn$$

p1,p2,p3是三个大素数（常量），n是哈希表的大小。除了指向voxel block的指针外，哈希表中的每个条目还包含了世界坐标，和处理冲突时要用到的偏移指针

https://github.com/niessner/VoxelHashing