RTABMAP论文和代码中栅格地图部分研读

1、ROS中常见的SLAM算法汇总

2、RTABMAP的ROS节点框图

3、局部占据栅格地图生成流程图

根据参数“Grid/FromDepth”、“Grid/3D”和输入topic设置，局部占据网格会以不同的方式生成，结果是2D或3D，如图所示。例如，如果参数“Grid/FromDepth”为false, 同时rtabmap节点订阅一个激光扫描主题，则创建一个局部2D占用网格。2D局部占用网格比3D网格需要更少的内存，因为需要节省的维度少了一个(例如z)，而且叠加的障碍物可以减少到只有一个障碍物单元格。但是，局部2D占据网格不能用于生成3D全局占据网格，而局部3D占据网格可以用于生成2D和3D全局占据网格。这取决于应用程序需要什么样的全局地图以及可用的处理能力。请注意，如果\Grid/FromDepth“为false，并且没有订阅激光扫描和点云主题，则不会计算网格。图7中的矩形框描述如下：

• 二维光线追踪：对于激光射程内的每一条光线，都会在网格上画一条线，将传感器和被光线击中的障碍物之间的空白单元格全部填满。假设射线与地面平行。这种方法可以非常快速地生成2D局部占用网格，并且在2D-lidar映射中是默认的。
• 点云深度图像：将输入的深度图像(立体图像的视差图像)根据传感器帧和摄像机标定进行三维空间投影。然后在机器人基础帧中转换点云。
• 滤波和地面分割：点云由体素网格进行下采样，体素大小等于固定网格单元大小。然后从点云中分割出地平面：计算点云的法线，然后在固定的最大角度“\Grid/MaxGroundAngle”范围内，所有法线平行于z轴（向上）的点作为地面，其他点则为障碍物。
• 投影：如果\Grid/3D”为false，则三维地面和障碍物点云投影在地平面上（例如，x-y平面）。体素网格过滤器再次应用于合并在同一单元格中投影的点。二维光线跟踪可以用来消除障碍物和相机之间的空白。如果不使用二维光线跟踪，并且点云没有任何点分割为地面，则传感器和障碍物之间的占用栅格中不会设置空单元。
• 三维光线跟踪：从机器人参考中的单个局部占用栅格创建OctoMap。OctoMap进行3D光线跟踪，并检测相机和占用的单元格之间的空单元格。OctoMap被转换回具有空单元格、地面单元格和障碍单元格的局部占据网格格式。

4、全局地图集成

图8展示了可以从图7的局部占据网格中组装的全局地图输出。在节点中保存3D局部占用网格可以提供最大的灵活性，因为它们可以用来生成所有类型的地图。但是，如果只需要一个2D全局占用网格地图，那么在组装局部地图时，在节点中保存已经投影的局部网格可以节省内存（每个点两个数字，而不是三个）和时间（点已经投影到2D）。利用地图的图形，将每个局部占用网格转化为相应的姿态。当一个新的节点被添加到地图中时，新的局部占用网格与全局占用网格相结合，清除并添加障碍物。当循环闭合发生时，全局地图应根据地图中所有节点的所有新优化姿势重新组合。这一过程是必需的，以便在环路闭合之前被错误地清除的障碍物可以被重新包括在内。点云输出集合局部地图的所有点，并以标准传感器msgs/PointCloud2 14 ROS格式发布。体素网格过滤用于合并重叠曲面。生成的点云是一种方便的可视化和调试格式，并且易于与第三方应用程序集成。

5、RTABMAP的一些默认参数

具体含义可以查看论文相应部分和代码文件Parameters.h

6、RTABMAP处理占据栅格地图流程

1）在obstacles_detection.cpp中处理点云回调callback函数，获取frame_ID到cloud_ID的变换关系localTransform，map_ID到frame_ID的变换关系pose，将cloud_ID中的点云转换到frame_ID中：

inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);

2）调用OccupancyGrid.hpp中segmentCloud函数实现地面和障碍物点云分割的功能：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
inputCloud,
pcl::IndicesPtr(new std::vector<int>),
pose,
cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
ground,
obstacles,
&flatObstacles);

3）先根据位姿pose（frame_ID机器人帧在map_ID地图帧坐标系中的位姿）将姿态分解成欧拉角rpy，仅使用roll和pitch对点云（frame_ID机器人帧中的坐标）进行坐标转换（projMapFrame为false，不使用位移相关信息，本质上还是在frame_ID机器人帧），然后调用util3d_mapping.hpp中segmentObstaclesFromGround函数来实现分割：

util3d::segmentObstaclesFromGround<PointT>(
cloud,
indices,
groundIndices,
obstaclesIndices,
normalKSearch_,
maxGroundAngle_,
clusterRadius_,
minClusterSize_,
flatObstaclesDetected_,
maxGroundHeight_,
flatObstacles,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));

其中，viewPoint是cloud_ID（相机帧）在frame_ID（机器人帧）坐标系中的坐标。

4）调用util3d_filtering.cpp中normalFiltering函数分割地面：

pcl::IndicesPtr flatSurfaces = normalFiltering(
cloud,
indices,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalKSearch,
viewPoint);

实际是调用normalFilteringImpl函数实现：
normalFilteringImplpcl::PointXYZ(cloud, indices, angleMax, normal, normalKSearch, viewpoint);

点云分割与地面检测：
OccupancyGrid.hpp – segmentCloud函数

1. 对点云进行体素化与降采样。调用pcl的setLeafSize()实现。
   • rtabmap: cloud_voxel_size: 0.05f, gridCellSize = 0.05f. ICP配准中没有启用voxel(parameters.h)
2. 根据当前位姿，将点云从相机坐标系转换至世界坐标系。
   • 调用rtabmap的util3d::transformPointCloud()实现。
3. 机器人范围检测与环境高度检测
   • 分别采用util3d::cropBox和util3d::passThrough方法实现，在util3d_filtering.cpp中。
4. 检测地面点云
   • util3d::segmentObstaclesFromGround，来自util3d_mapping.hpp
   • 使用util3d::normalFitering方法滤波获取地面点云，指标为点的法线与向量（0，0，1）的夹角大小，默认值为45°。首先，通过pcl::NormalEstimationOMP方法，使用KdTree作为搜索方法，并通过setViewPoint方法设置视角，根据公式计算点云所有点的法向量。通过pcl::getAngle3D获得点云每个点法向量与地面垂直向量的夹角。实现方法在util3d_filtering.cpp
   • 提取聚类分离地面与平坦障碍物，方法为util3d::extractClusters，来自util3d_filtering.cpp。具体算法为pcl::EuclideanClusterExtraction
5. 对地面点云滤波，分离地面与非地面点云
   通过地面与障碍物高度排除三维空间外点，采用passThrough直通滤波器方法滤波，从之前的步骤获取这两种点云的下标值。或者通过cropbox方法直接通过移动机器人footprint范围来排除三维空间外点。
6. 生成栅格地图
   • util3d::occupancy2DFromGroundObstacles，来自util3d_mapping.cpp