BLAM源码解析（三）—— 定时器总揽大局

上一节介绍了激光数据的回调，最终被依次推入到容器之中，存储在pcl_pcld_queue。那么激光数据的使用是怎么实现的，就是这一节要介绍的内容。

回想第一节我们介绍的定时器estimate_update_timer_，一秒进行20次的回调，进入回调函数BlamSlam::EstimateTimerCallback，接下来介绍此函数。

BlamSlam::EstimateTimerCallback

此函数位于src\blam_slam\src\BlamSlam.cc，内容如下：

void BlamSlam::EstimateTimerCallback(const ros::TimerEvent& ev) {
  // Sort all messages accumulated since the last estimate update.
  synchronizer_.SortMessages();

  // Iterate through sensor messages, passing to update functions.
  MeasurementSynchronizer::sensor_type type;
  unsigned int index = 0;
  while (synchronizer_.GetNextMessage(&type, &index)) {
    switch(type) {

      // Point cloud messages.
      case MeasurementSynchronizer::PCL_POINTCLOUD: {
        const MeasurementSynchronizer::Message<PointCloud>::ConstPtr& m =
            synchronizer_.GetPCLPointCloudMessage(index);

        ProcessPointCloudMessage(m->msg);
        break;
      }

      // Unhandled sensor messages.
      default: {
        ROS_WARN("%s: Unhandled measurement type (%s).", name_.c_str(),
                 MeasurementSynchronizer::GetTypeString(type).c_str());
        break;
      }
    }
  }

  // Remove processed messages from the synchronizer.
  synchronizer_.ClearMessages();
}

下面逐行进行介绍：

synchronizer_.SortMessages();

从上一次位姿更新后，排序激光消息。实现位于src\measurement_synchronizer\src\MeasurementSynchronizer.cc，内容为：

void MeasurementSynchronizer::SortMessages() {
  sensor_ordering_.clear();  // 清空排序后的容器

  // Accumulate all new messages in a single list.
  unsigned int ii = 0;
  for (pcld_queue::const_iterator it = pending_pclds_.begin();  // 无视此循环
       it != pending_pclds_.end(); ++it, ++ii) {
    TimestampedType::Ptr p = TimestampedType::Ptr(
        new TimestampedType((*it)->msg->header.stamp.toSec(), POINTCLOUD, ii));
    sensor_ordering_.push_back(p);  
  }

  ii = 0;
  for (pcl_pcld_queue::const_iterator it = pending_pcl_pclds_.begin();  
       it != pending_pcl_pclds_.end(); ++it, ++ii) {// 循环激光回调容器
    ros::Time stamp;
    stamp.fromNSec((*it)->msg->header.stamp*1e3);
    TimestampedType::Ptr p = TimestampedType::Ptr(
        new TimestampedType(stamp.toSec(), PCL_POINTCLOUD, ii));
    sensor_ordering_.push_back(p);//将激光对应的时间戳信息推进排序容器
  }

  // Sort the list by time.
  std::sort(sensor_ordering_.begin(), sensor_ordering_.end(),
            MeasurementSynchronizer::CompareTimestamps);

  pending_index_ = 0;
}

里面pending_pclds_容器是哪来的呢？发现并没有在其他地方用到，所以直接不用管上面那个循环。直接看关于pending_pcl_pclds_的循环，将激光对应的时间戳信息推进排序容器，然后将排序容器sensor_ordering_进行排序，排序规则为：

* 时间小的在前

* 若时间相等，POINTCLOUD类型在前，PCL_POINTCLOUD类型在后

感觉第二条规则用不上。最后初始化pending_index_ = 0;

回到void BlamSlam::EstimateTimerCallback，继续：

// Iterate through sensor messages, passing to update functions.
  MeasurementSynchronizer::sensor_type type;
  unsigned int index = 0;
  while (synchronizer_.GetNextMessage(&type, &index)) {
    switch(type) {

      // Point cloud messages.
      case MeasurementSynchronizer::PCL_POINTCLOUD: {
        const MeasurementSynchronizer::Message<PointCloud>::ConstPtr& m =
            synchronizer_.GetPCLPointCloudMessage(index);

        ProcessPointCloudMessage(m->msg);
        break;
      }

      // Unhandled sensor messages.
      default: {
        ROS_WARN("%s: Unhandled measurement type (%s).", name_.c_str(),
                 MeasurementSynchronizer::GetTypeString(type).c_str());
        break;
      }
    }
  }

此部分开启激光数据处理迭代。GetNextMessage函数，传入index为0，依次取出排序容器中的激光数据索引，这个索引用来在激光回调容器中寻找对应索引的激光数据，按照时间进行取出。取出的激光数据被赋予m，使用ProcessPointCloudMessage处理点云。

BlamSlam::ProcessPointCloudMessage

首先使用过滤器对点云进行过滤

  filter_.Filter(msg, msg_filtered);

filter_就是我们第一节所介绍的初始化过的过滤器。Filter函数中实施了几种点云过滤方法，包括：

（1）random_filter的随机滤波

（2）grid.filter的体素滤波

（3）sor.filter的统计滤波

（4）rad.filter的半径滤波

（5）由于源程序对具有nan的点云不适配，而后加入的NaN去除：

  if (!points->is_dense)
  {
    // points_filtered->is_dense = false;
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*points_filtered,*points_filtered, indices);
  }

这几种滤波根据实际情况进行选择，参数位于src\point_cloud_filter\config\parameters.yaml。随机滤波不介绍了，体素滤波可调节分辨率，越小保留点越多，后续的计算量也越大；统计滤波过多消耗计算量，不推荐打开；半径滤波有利于去除离散点，可以考虑，但当点云较稀疏时可不开。

接下来就是重要的，使用ICP进行位姿更新了。

  if (!odometry_.UpdateEstimate(*msg_filtered)) {

PointCloudOdometry::UpdateEstimate

此函数实现关于基于ICP的位姿更新，下一节进行介绍。