一、总体如是说
该些是整座控制系统的核心理念，因为那个控制系统是建图的呀，建图的核心理念是综合性大部份重要信息展开强化呀。他们后面一节如是说的大部份的重要信息单厢推送至那个结点中去供它采用。该些的标识符在文档apps/hdl_graph_slam_nodelet.cpp中。
该些的标识符看著挺多，假如他们能对它大部份的表达式进行分类，就很难剖析它的文本了。
他们这边提及，它须要转交以后一节如是说的大部份的重要信息，并重新加入机率图中，所以他们以信息为突破口，去剖析它的文本。
他们先看都包涵什么样重要信息：
1）后端鸭岛传至的体位和点云
2）gps重要信息
3）Imu重要信息
4）正方形插值的模块重要信息
处置两个重要信息包括下列关键步骤：
1）在相关联的callback表达式中转交重要信息，并放进适当的堆栈
2）依照时间戳对堆栈中的重要信息展开次序处置，重新加入机率图
三条重要信息，每一重要信息都继续执行两个关键步骤，这就已经包涵了本文档绝大部分的标识符。
剩的是许多其它文本，他们更进一步剖析：
1）继续执行图强化，这是一个间歇继续执行的表达式，生态圈检验也在那个表达式里
2）聚合自上而下世界地图并间歇推送，即把大部份sizes拼一同，获得自上而下点云世界地图，接着在两个间歇表达式里推送至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）聚合简化版sizes
还记得他们在sizes那一节如是说的简化版sizes（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）聚合世界地图
是采用简化版sizes展开拼接，该些标识符在文档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);
}
=============分割线===========
OK，整座hdl_graph_slam控制系统的如是说到这里就结束了，前后一共八篇文章。最后总结一下吧，虽然hdl_graph_slam的性能其实不是很好，具体原因在于帧间匹配和生态圈检验的精度都不够，它在这两个环节的缺点他们在相关联的章节已经解释过了。但是，但是，但是，我想说的是对于一套开源控制系统来讲，不能指望它拿来即用，小的缺点很难改，都已经指出问题了，就那几行标识符，改一改还不难吗。他们更如果关心它的架构，这套控制系统的标识符模块画做的很好，思路很清晰，整座控制系统对于对多重要信息源的融合做的很好，按照这种架构，再多来几个传感器也没关系，这是控制系统的扩展性。综上，我认为对开发者来讲（注意这句话），这是一套不错的开源控制系统。
