Cartographer行人点云过滤建图

任务动机：使用Cartographer在人群密集的环境中建立相对干净的地图。

任务描述：在使用Cartographer建图时，存在环境中行人较多，或者建图时机器人周围人员一直跟随，导致最终建图后地图上存在许多行人噪点，本文主要从点云原始数据过滤解决这一问题。

1. Cartographer原有的功能过滤行人

        Cartographer在构建submap以及整体map时，实质是对栅格地图概率的更新，每一个node在栅格概率上的累加，可以去除建图时一般移动物体所产生的噪点。例如建图时，前面走过几个行人，甚至车辆，只要不是长时间停留在雷达探测范围内，栅格地图的概率会及时更新，最终生成的地图不会带有行人和车辆。但是实际建图中经常会出现多人跟在机器人周围建图或恰巧机器人旋转时周围人为移动，旋转时node快速生成，导致该部分submap中存在行人噪点，如果后期不重复走这条路径，最终生成的map会存在相关噪点，影响地图美观，以及路径规划是被视为障碍物。

2. obstacle_detector论文介绍

        如果从数据源上就能把行人等噪点滤除，加上上述的概率更新的作用，那么就可以建立一个相对干净的地图，这里推荐使用obstacle_detector，源码网址为https://github.com/tysik/obstacle_detector，顾名思义，其功能为障碍物检测，具体细节可以看相关论文和源码，这里只做简单介绍。

        论文中分为五大部分引言 、障碍物提取、障碍物跟踪、实验结果和结论。引言部分介绍了当前通过点云对障碍物的检测的几种方法，其中包括：通过几何特征检测障碍物稀疏的点云（铁丝网、树叶）、密集的点云（墙壁、树干）、凸面点云（家具、汽车）、凹面点云（洞、笼子）等；通过颜色反射或者不透明度检测障碍物，不透明的（木头、岩石）、反光的（镜子、抛光金属）、透明的（各种玻璃）；通过物质不同性质区分的障碍物，材料（实心箱子、织物窗帘等）、虚拟（编程限制、机械限制等）；通过不同时间的变化检测障碍物，静止（沙发、树）、可移动（椅子、垃圾桶）、移动的（行人、机器人）。

        论文中第二章介绍了不考虑运动状态下障碍物检测方法，

        1. 通过特定的距离阈值判断方法将点云分类为不同的组合（博文中的图均引用原论文）

        2. 将组内最远两点不满足要求的点云组合继续拆分。

        3. 用线段图形构建分割数据  

        4. 构建好的线段数据根据需要合并

        5. 检测圆（这里的圆即代表需要检测的障碍物），通过检测出的点云构建圆形，圆半径可视为障碍物大小。

        6. 圆合并（即可能为相同的障碍物点云合并）

        论文第三章障碍物跟踪和以后的内容，不做进一步探究，这里只对原论文原理做大致梳理和概括，公式和代码不去细致推理，抛砖引玉，具体细节需要读者进一步对源码和论文进行探究，后续更多的功能和更广泛的应值得挖掘和思考。

3. obstacle_detector源码使用

        源码下载至自己的工作空间内可以直接编译成功（多台机器测试都是catkin_make直接成功），运行demo.launch小试牛刀，bag为源码中附带的，rviz中观察到的圆形就是检测到的障碍物体将launch文件中的路径换成自己的bag，scan话题修改为相对应的雷达数据话题名称，运行可以观察效果。其中绿色圆形就是检测出的障碍物。

        编译该源码生成了四个可执行文件，我们只需要用到obstacle_extractor_node，其余合并点云，点云发布，以及障碍物追踪可以后续根据需求继续添加。

对源码做一定的修改

        在obstacle_extractor.cpp中添加发布filter_scan话题

filter_scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>("filter_scan", 50);

        在雷达数据的回调函数scanCallback中添加赋值全局变量

raw_scan_ = *scan_msg ;

        最后在发布obstacles_msg时同时发布filter_scan ，publishobstacles函数中添加如下代码 ：

sensor_msgs::LaserScan filter_scan;

filter_scan = raw_scan_;

for (const Circle & c: circles_) {

  if (c.center.x > p_min_x_limit_ && c.center.x < p_max_x_limit_ &&

    c.center.y > p_min_y_limit_ && c.center.y < p_max_y_limit_) {

    CircleObstacle circle;

    circle.center.x = c.center.x;

    circle.center.y = c.center.y;

    circle.velocity.x = 0.0;

    circle.velocity.y = 0.0;

    circle.radius = c.radius;

    for (auto a: c.point_sets) {

      for (int i = 0; i < raw_scan_.ranges.size(); ++i)

      {

        double x = cos(raw_scan_.angle_min + i * raw_scan_.angle_increment) * raw_scan_.ranges[i];

        double y = sin(raw_scan_.angle_min + i * raw_scan_.angle_increment) * raw_scan_.ranges[i];

        if ((x - c.center.x) * (x - c.center.x) + (y - c.center.y) * (y - c.center.y)

          <
          ((c.radius - p_radius_enlargement_) * (0.1 + c.radius - p_radius_enlargement_)))

        {

          filter_scan.ranges[i] = 0;

        }

      }

    }

    circle.true_radius = c.radius - p_radius_enlargement_;

    obstacles_msg -> circles.push_back(circle);

  }

}

filter_scan_pub_.publish(filter_scan);

        launch文件以及相关参数如下：

<launch>

  <param name="use_sim_time" value="false"/> 

  <node name="nodelet_manager" pkg="nodelet" type="nodelet" args="manager" output="screen">

    <param name="num_worker_threads" value="20"/>

  </node>

  <node name="obstacle_extractor" pkg="nodelet" type="nodelet" args="load obstacle_detector/ObstacleExtractor nodelet_manager">

    <param name="active"               value="true"/>

    <param name="use_scan"             value="true"/>

    <param name="use_pcl"              value="false"/>

    <param name="circles_from_visibles"  value="true"/>   

    <param name="discard_converted_segments" value="true"/>

    <param name="transform_coordinates"  value="true"/>  

    <param name="min_group_points"     value="5"/>     

    <param name="max_group_distance"   value="0.1"/>    

    <param name="distance_proportion"  value="0.02628"/>

    <param name="max_split_distance"   value="0.2"/>   

    <param name="max_merge_separation" value="0.2"/>     

    <param name="max_merge_spread"     value="0.2"/>     

    <param name="max_circle_radius"    value="0.5"/>     

    <param name="radius_enlargement"   value="0.2"/>     

    <param name="frame_id"             value="laser"/>

  </node>

</launch>

        编译运行成功后，即可通过obstacle_extractor节点接受雷达发出的scan数据，经过滤后发出filter_scan数据，在cartographer的运行launch中加入

<remap from="/ scan" to="/filter_scan" />

        即可测试使用过滤后的点云建图。

4. obstacle_detector使用测试结果

        首先对比过滤后的点云与原始点云，蓝色点云为原始点云，白色点云为过滤出的去的障碍物点，左图中除了最左边点云为实际物体，其余白色点云均为行人，右图中没有行人，将环境中较小的立柱检测为新人过滤。

 

        使用filter_scan实际建图效果对比：

        未使用过滤点云的建立地图（左图）中有明显行人噪点，使用过滤后的点云建图（右图）中几乎没有行人噪点。