一、整体介绍

这部分是整个系统的核心，因为这个系统就是建图的呀，建图的核心就是综合所有信息进行优化呀。我们前面几节介绍的所有的信息都会发送到这个节点中来供它使用。这部分的代码在文件apps/hdl_graph_slam_nodelet.cpp中。

这部分的代码看着挺多，如果我们能够对它所有的函数分类，就很容易梳理它的内容了。

我们刚才提到，它需要接收之前几节介绍的所有的信息，并加入概率图中，所以我们以信息为切入点，去梳理它的内容。

我们先看都包含哪些信息：

1）前端里程计传入的位姿和点云

2）gps信息

3）Imu信息

4）平面拟合的参数信息

处理一个信息包括以下步骤：

1）在对应的callback函数中接收信息，并放入相应的队列

2）根据时间戳对队列中的信息进行顺序处理，加入概率图

四条信息，每条信息都执行两个步骤，这就已经包含了本文件大部分的代码。

剩下的就是一些其他内容，我们进一步梳理：

1）执行图优化，这是一个定时执行的函数，闭环检测也在这个函数里

2）生成全局地图并定时发送，即把所有关键帧拼一起，得到全局点云地图，然后在一个定时函数里发送到rviz上去

3）在rviz中显示顶点和边，如果运行程序，会看到rviz中把概率图可视化了

至此，整个文件的核心框ros架就梳理完了，剩下的一些细枝末节的东西应该都能看懂。

下面我们就按照这个框架对各部分内容进行梳理，这次梳理我们遵照以下原则：

1）和消息相关的代码我们之前在各自对应的章节中都梳理过了，这里只是使用，所以他们的代码就不贴出来了，不然会把本节内容搞得繁琐，反而不利于整体理解

2）概率图可视化的代码就不解释了，和核心算法无关

3）和算法无关的一些ros操作相关的东西，也不在这里解释了

二、代码详解

1. 前端里程计消息处理

还记得那两个步骤吗？

1）接收消息：cloud_callback

2）处理消息队列：flush_keyframe_queue

2. gps消息

1）接收消息：nmea_callback + navsat_callback + gps_callback

2）处理消息队列：flush_gps_queue

3. imu消息

1）接收消息：imu_callback

2）处理消息队列：flush_imu_queue

4. 地面拟合参数

1）接收消息：floor_coeffs_callback

2）处理消息队列：flush_floor_queue

5. 执行图优化

函数名是optimization_timer_callback，这个函数除了执行图优化意外，还做了以下两点：

1）闭环检测

// loop detection
    std::vector<Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
    for(const auto& loop : loops) {
      Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
      Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
      auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
      graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("loop_closure_edge_robust_kernel", "NONE"), private_nh.param<double>("loop_closure_edge_robust_kernel_size", 1.0));
    }

检测闭环并加到了概率图中，当然实在优化之前

2）生成简化版关键帧

还记得我们在关键帧那一节介绍的简化版关键帧（KeyFrameSnapshot）吗？

它从KeyFrame类中取出位姿（当然是优化之后的位姿）和点云，供地图拼接使用，这样我们得到的就是位姿经过优化的地图了。

std::vector<KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
    std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(),
      [=](const KeyFrame::Ptr& k) {
        return std::make_shared<KeyFrameSnapshot>(k);
    });

    keyframes_snapshot_mutex.lock();
    keyframes_snapshot.swap(snapshot);
    keyframes_snapshot_mutex.unlock();

6. 生成地图并定时发送

生成地图并定时发送很明显包括两步：生成地图、定时发送，哈哈

1）生成地图

就是使用简化版关键帧进行拼接，这部分代码在文件src/hdl_graph_slam/map_cloud_generator.cpp中，更具体点说，就是里面的generate函数，其实就是个拼接的过程。

pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const {
  if(keyframes.empty()) {
    std::cerr << "warning: keyframes empty!!" << std::endl;
    return nullptr;
  }

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());

  for(const auto& keyframe : keyframes) {
    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
    for(const auto& src_pt : keyframe->cloud->points) {
      PointT dst_pt;
      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
      dst_pt.intensity = src_pt.intensity;
      cloud->push_back(dst_pt);
    }
  }

  cloud->width = cloud->size();
  cloud->height = 1;
  cloud->is_dense = false;

  pcl::octree::OctreePointCloud<PointT> octree(resolution);
  octree.setInputCloud(cloud);
  octree.addPointsFromInputCloud();

  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
  octree.getOccupiedVoxelCenters(filtered->points);

  filtered->width = filtered->size();
  filtered->height = 1;
  filtered->is_dense = false;

  return filtered;
}

2）定时发送

这部分内容在apps/hdl_graph_slam_nodelet.cpp文件中，简单，不解释了

void map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
    if(!map_points_pub.getNumSubscribers()) {
      return;
    }

    std::vector<KeyFrameSnapshot::Ptr> snapshot;

    keyframes_snapshot_mutex.lock();
    snapshot = keyframes_snapshot;
    keyframes_snapshot_mutex.unlock();

    auto cloud = map_cloud_generator->generate(snapshot, 0.05);
    if(!cloud) {
      return;
    }

    cloud->header.frame_id = map_frame_id;
    cloud->header.stamp = snapshot.back()->cloud->header.stamp;

    sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
    pcl::toROSMsg(*cloud, *cloud_msg);

    map_points_pub.publish(cloud_msg);
  }