RTABMAP论⽂和代码中栅格地图部分研读
1、ROS中常见的SLAM算法汇总
2、RTABMAP的ROS节点框图
3、局部占据栅格地图⽣成流程图
根据参数“Grid/FromDepth”、“Grid/3D”和输⼊topic设置，局部占据⽹格会以不同的⽅式⽣成，结果是2D或3D，如图所⽰。例如，如果参数“Grid/FromDepth”为fal, 同时rtabmap节点订阅⼀个激光扫描主题，则创建⼀个局部2D占⽤⽹格。2D局部占⽤⽹格⽐3D⽹格需要更少的内存，因为需要节省的维度少了⼀个(例如z)，⽽且叠加的障碍物可以减少到只有⼀个障碍物单元格。但是，局部2D占据⽹格不能⽤于⽣成3D全局占据⽹格，⽽局部3D占据⽹格可以⽤于⽣成2D和3D全局占据⽹格。这取决于应⽤程序需要什么样的全局地图以及可⽤的处理能⼒。请注意，如果\Grid/FromDepth“为fal，并且没有订阅激光扫描和点云主题，则不会计算⽹格。图7中的矩形框描述如下：
⼆维光线追踪：对于激光射程内的每⼀条光线，都会在⽹格上画⼀条线，将传感器和被光线击中的障碍物之间的空⽩单元格全部填满。
假设射线与地⾯平⾏。这种⽅法可以⾮常快速地⽣成2D局部占⽤⽹格，并且在2D-lidar映射中是默认的。
点云深度图像：将输⼊的深度图像(⽴体图像的视差图像)根据传感器帧和摄像机标定进⾏三维空间投影。然后在机器⼈基础帧中转换点云。
滤波和地⾯分割：点云由体素⽹格进⾏下采样，体素⼤⼩等于固定⽹格单元⼤⼩。然后从点云中分割出地平⾯：计算点云的法线，然后在固定的最⼤⾓度“\Grid/MaxGroundAngle”范围内，所有法线平⾏
于z轴（向上）的点作为地⾯，其他点则为障碍物。
赖世雄美语从头学投影：如果\Grid/3D”为fal，则三维地⾯和障碍物点云投影在地平⾯上（例如，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的变换关系po，将cloud_ID中的点云转换到frame_ID中：
inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
2）调⽤OccupancyGrid.hpp中gmentCloud函数实现地⾯和障碍物点云分割的功能：
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.gmentCloud<pcl::PointXYZ>(
inputCloud,
pcl::IndicesPtr(new std::vector<int>),
po,
cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
ground,
obstacles,
r是什么意思&flatObstacles);
3）先根据位姿po（frame_ID机器⼈帧在map_ID地图帧坐标系中的位姿）将姿态分解成欧拉⾓rpy，仅使⽤roll和pitch对点云（frame_ID机器⼈帧中的坐标）进⾏坐标转换（projMapFrame为fal，不使⽤位移相关信息，本质上还是在frame_ID机器⼈帧），然后调⽤util3d_mapping.hpp中gmentObstaclesFromGround函数来实现分割：
util3d::gmentObstaclesFromGround<PointT>(
cloud,
indices,
groundIndices,
fuelgauge>wrapsobstaclesIndices,
normalKSearch_,
技术支持英文maxGroundAngle_,
clusterRadius_,
minClusterSize_,
flatObstaclesDetected_,
maxGroundHeight_,ms dos
flatObstacles,abandonment
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?po.z():0), 1));
其中，viewPoint是cloud_ID（相机帧）在frame_ID（机器⼈帧）坐标系中的坐标。
pth4）调⽤util3d_filtering.cpp中normalFiltering函数分割地⾯：
pcl::IndicesPtr flatSurfaces = normalFiltering(
2017奥斯卡提名
cloud,
indices,
groundNormalAngle,
Eigen::Vector4f(0,0,1,0),
normalKSearch,
viewPoint);
实际是调⽤normalFilteringImpl函数实现：
normalFilteringImplpcl::PointXYZ(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
点云分割与地⾯检测：
OccupancyGrid.hpp – gmentCloud函数
1. 对点云进⾏体素化与降采样。调⽤pcl的tLeafSize()实现。
· 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::gmentObstaclesFromGround，来⾃util3d_mapping.hpp
· 使⽤util3d::normalFitering⽅法滤波获取地⾯点云，指标为点的法线与向量（0，0，1）的夹⾓⼤⼩，默认值为45°。⾸先，通过
pcl::NormalEstimationOMP⽅法，使⽤KdTree作为搜索⽅法，并通过tViewPoint⽅法设置视⾓，根据公式计算点云所有点的法向量。通过pcl::getAngle3D获得点云每个点法向量与地⾯垂直向量的夹⾓。实现⽅法在util3d_filtering.cpp
· 提取聚类分离地⾯与平坦障碍物，⽅法为util3d::extractClusters，来⾃util3d_filtering.cpp。具体算法为pcl::EuclideanClusterExtraction
5. 对地⾯点云滤波，分离地⾯与⾮地⾯点云
通过地⾯与障碍物⾼度排除三维空间外点，采⽤passThrough直通滤波器⽅法滤波，从之前的步骤获取这两种点云的下标值。或者通过cropbox ⽅法直接通过移动机器⼈footprint范围来排除三维空间外点。
6. ⽣成栅格地图
· util3d::occupancy2DFromGroundObstacles，来⾃util3d_mapping.cpp
