相机点云无法从costmap清除障碍，甚至无法生成障碍

相机点云无法从costmap清除障碍，甚至无法生成障碍 | 沉默杀手

测试的要求: 不能把地面加入costmap，但是稍高于地面的物体能加入。

不断调整 max_obstacle_height, min_obstacle_height, obstacle_range, raytrace_range 四个参数，但是costmap发现生成的障碍总是清除不掉。

换了另一个相机和雷达，发现没有这个问题. 后来发现有时甚至不能生成障碍了。

清除障碍的函数重点是ObservationBuffer::purgeStaleObservations()其中的 observation_list_，它又来自bufferCloud函数中的observation_cloud，后者又来自 global_frame_cloud。

相机发布的点云是在相机坐标系，在bufferCloud函数里用pcl_ros::transformPointCloud转换到代价地图的全局坐标系(也就是yaml中指定的global_frame，一般关注的是local costmap。) 得到 global_frame_cloud。然后按如下条件筛选出 observation_cloud

for (unsigned int i = 0; i < cloud_size; ++i)
{
  if (global_frame_cloud.points[i].z <= max_obstacle_height_
      && global_frame_cloud.points[i].z >= min_obstacle_height_)
  {
    observation_cloud.points[point_count++] = global_frame_cloud.points[i];
  }
}

因此 min_obstacle_height 和 max_obstacle_height是在代价地图全局坐标系下的值。

在bufferCloud函数中加入代码，把observation_cloud发布出来

sensor_msgs::PointCloud2 observation_ros_cloud;
pcl::toROSMsg(observation_cloud, observation_ros_cloud);
observation_cloud_pub.publish( observation_ros_cloud );

在构造函数里加入

ros::NodeHandle n;
n.param("publish_observation_cloud",  pub_observation_cloud_,  false );
observation_cloud_pub = n.advertise < sensor_msgs::PointCloud2 > ("observation_cloud", 2);

这样可以观察最终生成的障碍来自什么样的点云。比如下面两个点云的对比

最后查啊查啊，终于发现和其他相机的区别不在参数，而在于我之前修改相机驱动时的滤波，滤得太狠了。于是修改驱动，y和z方向的点云不能太少，终于可以清除成功了。

问题的根源在于滤波后的点太少而且稀疏，导致raytrace机制不能清除障碍。 点云滤波不能直接滤到自己需要的范围，比如即使你实际需要1m的距离，驱动里也不能只给1m，竖直y方向也不能太小，体素滤波的体素不能太大，一般取0.01，这个对点云数量的影响也很大。