栅格地图、障碍物地图与膨胀地图（障碍物地图（二））

上一篇大致看完了障碍物地图的初始化内容以及对于传感器数据的处理，我们知道在该部分算法维护了一个ObservationBuffer，其中存储了一段时间内的点云数据。每次新的数据进来后，还会根据设定的时间参数observation_keep_time抛弃比较久远的障碍物点云。但是在看的过程中，我们也产生了一些疑惑，比如ObservationBuffer本身维护的是一系列世界坐标系下的pointcloud，它跟栅格地图之间是如何结合的？又比如在维护ObservationBuffer时，每一帧点云都是通过Observation类函数完成的。在其中传入了当时点云产生时其传感器在世界坐标系下的位置origin，以及还有两个似乎一直没用过的参数obstacle_range与raytrace_range。带着这些疑惑我们重新回到obstacle_layer这个类函数中，继续学习其中剩下的内容。

1、updateBounds

在obstacle_layer类函数中，我们会发现它除了初始化与点云函数的订阅外，还有一个很重要的函数updateBounds。这个函数首先确定了是否需要更新地图的原点：

if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);

这行代码比较有意思，感觉它相当于是一个用来决定当前地图是否锁定机器人视角的参数，如果是的话地图的中心会始终随着机器人的位置变化。正常来说它应该是false，静态地图本身也不会随着机器人的位置变化而变化。
然后是获取了下地图的上下界：

  useExtraBounds(min_x, min_y, max_x, max_y);

接下来的函数是获取了两个vector:

  bool current = true;
  std::vector<Observation> observations, clearing_observations;
  // get the marking observations
  current = current && getMarkingObservations(observations);
  // get the clearing observations
  current = current && getClearingObservations(clearing_observations);
  // update the global current status
  current_ = current;

这里维护的是两个Observation类的数据。这两个之间咋一看似乎是互斥的，但是实际上它们的数据来源是一样的。为什么这么说呢，首先看一下getMarkingObservations这个函数，它的作用呢是返回marking_buffers_的值，这个buffer的值本身是在上一篇中初始化函数时执行的这么一个步骤：

// create an observation buffer
    observation_buffers_.push_back(
        boost::shared_ptr < ObservationBuffer
            > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
                                     max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
                                     sensor_frame, transform_tolerance)));

    // check if we'll add this buffer to our marking observation buffers
    if (marking)
      marking_buffers_.push_back(observation_buffers_.back());

    // check if we'll also add this buffer to our clearing observation buffers
    if (clearing)
      clearing_buffers_.push_back(observation_buffers_.back());

在代码创建完成ObservationBuffer指针后，marking_buffers_实际上存储了该指针，所以操作marking_buffers_其实约等于操作了ObservationBuffer。因为marking_buffers_和 observation_buffers_共享了相同的ObservationBuffer 对象。注意到下面的clearing_buffers_也进行了相同的操作。

然后我们再回到上面的代码中，再看一下getClearingObservations这个函数，而这个函数其实也就是返回了clearing_buffers_中的所有点云值。所以说这两个函数其实殊涂同归，最终都是执行的ObservationBuffer::getObservations这个函数。那么为什么这里要获取两个相同的东西呢？继续往下看。

下面一个函数跟我们解释了clearing_observations中数据的使用方式

  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
   
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }

这个地方调用了一个raytraceFreespace函数，这个函数的详细解释在第三部分，它的作用是清理出传感器到被测物体之间的栅格，设置为空闲。

在根据clearing_observations将传感器到点云之间的栅格清理完毕后，下一步自然是要操作这些障碍物点云了，所以这里就用到了observations的点云，算法在后面对其进行了遍历，取出每一帧点云，首先判断它到传感器之间的距离，注意到这里用到了一个参数obstacle_range_，这个上一章中提到Observations类时提到了，但是当时没有使用到，后面在这里我们发现它被使用了，它的作用是过滤掉那些点云距离传感器原点超过该值的点。然后对剩下的点求取其在地图下的栅格位置index，最后将该栅格的值设置为LETHAL_OBSTACLE（254：障碍物）。

// place the new obstacles into a priority queue... each with a priority of zero to begin with
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
   
    const Observation& obs = *it;

    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);

    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");