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的位姿更新，下一节进行介绍。