看我是怎么学SLAM（二）——3D自主探索建图源码解读

说明

　　这份源码能实现Kobuki自主探索房间并建立房间的三维地图，地图表现形式是八叉树。

源码下载与配置

步骤：

• 安装octomap_ros和rviz插件

sudo apt-get install ros-indigo-octomap*

• 源码安装

cd turtlebot_ws/src
git clone https://github.com/RobustFieldAutonomyLab/turtlebot_exploration_3d.git
catkin_make

• deb包安装：

sudo apt-get update
sudo apt-get install ros-indigo-turtlebot-exploration-3d

运行：

roslaunch turtlebot_exploration_3d minimal_explo.launch
roslaunch turtlebot_exploration_3d turtlebot_gmapping.launch
rosrun turtlebot_exploration_3d turtlebot_exploration_3d
roslaunch turtlebot_exploration_3d exploration_rviz.launch

　　最后一条指令是打开RVIZ查看地图，不打开机器人已经能够开始探索啦。

代码解读

　　主程序是在src目录下的turtlebot_exploration_3d.cpp，主要讲解这个文件。
　　1. 全局变量声明和配置
　　位置：exploration.h的21到40行
　　说明：

• 第25行的octo_reso变量定义了octomap中的最小分辨率，如果要提高octomap的分辨率可以适当减小该变量的值；
• 第28行的num_of_bay定义为高斯过程的迭代次数;
• 第31至33行是对octomap和octomap消息的变量声明，特别是cur_tree，作为后面程序中octomap的指针变量；
• 第37行声明的三维点变量kinect_orig在程序中表示机器人的实时位置；

• 第40行octomap_name_3d变量在turlebot_exploration.cpp的第38行中被初始化为Octomap3D_%m%d_%R_%S.ot的形式，并在exploration.h的第291行中，作为保存octomap的名字；
  2. 全局变量初始化
  位置： turtlebot_exploration_3d.cpp的25到72行
  说明：

• 第42至47行定义了程序中消息的订阅和发布, kinect_sub为订阅者接受kinect图像信息, GoalMarker_pub发布目标点信息, Candidates_pub发布候选点信息,
  Frontier_points_pub发布前沿点信息, pub_twist发布对机器人的控制信息,Octomap_pub发布octomap的信息;

• 第64至68行初始化octomap;

• 第69行定义下一个探测点next_vp;
  3. 更新机器人的初始位置
  位置: turtlebot_exploration_3d.cpp的74到108行
  说明:

• 第77至88行从kinect发布的消息中获取图像信息, 通过lookupTransform函数获取位置信息，并且更新机器人的位置kinect_orig;

• 第91行ros::spinOnce()是ros的回调函数, 回调在exploration.h中265行定义的kinectCallbacks函数，通过kinect传来的图像消息,更新并发送和保存octomap信息;

• 第97至103行为旋转60度，原因是twist_cmd.angular.z定义旋转角速度为0.6，旋转时间由ros::Duration(2.6)定义为2.6-1=1.6的旋转角度为(0.6×1.6) × 180/π = 55 ≈ 60；
  4. 获取并发布前沿点信息
  位置: turtlebot_exploration_3d.cpp的115到157行
  说明:

• 第115行中，通过extractFrontierPoints函数从octomap中获取前沿点，该获取前沿点的函数定义在exploration.h的111至184行;

• 第118至157行根据获取到的前沿点数据frontier_groups，创建前沿点消息Frontier_points_cubelist并将前沿点消息发布出去;
  5. 提取候选点
  位置: turtlebot_exploration_3d.cpp的159到169行
  说明:

• 在160行中，使用extractCandidateViewPoints函数从前沿点中提取候选点，该函数声明在exploration.h的188至252行;

• 在164至169行中，如果候选点的数量不够，程序终止;
  6. 为每个候选点初始化交互信息(mutual information)
  位置: turtlebot_exploration_3d.cpp的175到203行
  说明:

• 第177行使用countFreeVolume函数计算octomap中的空闲空间体积大小保存在before中，该函数定义在exploration.h的80至87行;

• 第185至196行中，使用calc_MI函数初始化交互信息数组MIs, 为每个元素赋值为该候选点的空闲空间与当前空闲空间的体积之差，保存在MIs数组中；
  7. 高斯过程回归
  位置: turtlebot_exploration_3d.cpp的207到251行
  说明:

• 第212至223行初始化高斯回归的参数;

• 第225至250行执行高斯回归过程，得到acquisition function,并计算交互信息;

• 详细可见论文:Information-Theoretic Exploration with Bayesian Optimization
  8. 发布候选点信息
  位置: turtlebot_exploration_3d.cpp的266到297行
  说明:

• 第272至294行，把每个候选点的信息加入要发布的消息CandidatesMarker_array中；

• 第295行发布候选点消息；
  9. 选取并到达目标点
  位置: turtlebot_exploration_3d.cpp的304到384行
  说明:

• 第306行，通过候选点和交互信息选取下一个目标点，该交互信息数组已经使用exploration.h中第42至50行的sort_MIs函数进行了排序;

• 第307至333行，向rviz中发布目标点信息;
• 第336行，使用定义在navigation_utils.cpp中的goToDest函数向MoveBaseClient发送目标点信息，使机器人移动到目标点;
• 第338至373行，如果到达了目标点，则更新octomap并且发送msg_octomap消息;
• 第374至382行，如果没有到达目标点，且没有更多的目标点可以选择，程序结束；
  10. 点云地图的保存
  在程序中使用kinect采集的点云信息的位置在exploration.h中，第265至296行的kinectCallbacks函数，该函数通过ros::spinOnce被调用. 该函数传入的参数cloud2_msg为ros的消息，程序266至277行经过如下转化

pcl::PCLPointCloud2 cloud2;
pcl_conversions::toPCL(*cloud2_msg, cloud2);
PointCloud* cloud (new PointCloud);
PointCloud* cloud_local (new PointCloud);
pcl::fromPCLPointCloud2(cloud2,*cloud_local);
octomap::Pointcloud hits;
ros::Duration(0.07).sleep();
while(!pcl_ros::transformPointCloud("/map", *cloud_local, *cloud, *tf_listener))
{
ros::Duration(0.01).sleep();
}

便把点云消息转化为点云数据保存在cloud指针指向的点云中；之后就可以用相应的保存点云数据函数保存点云图像，比如

pcl::io::savePCDFileASCII (cloud_name, cloud); // 保存点云

Octomap的保存和读取

环境配置

• 下载octomap源码

git clone https://github.com/OctoMap/octomap

• 编译

mkdir build
cd build
cmake ..
make

• 使用

cd octomap/bin
./octovis the .ot file to view

　　这就是这份开源代码的个人解读与扩展使用