1. 整体介绍
这部分是整个系统的核心,因为这个系统就是建图的呀,建图的核心就是综合所有信息进行优化呀。我们前面几节介绍的所有的信息都会发送到这个节点中来供它使用。这部分的代码在文件apps/hdl_graph_slam_nodelet.cpp
中。
这部分的代码看着挺多,如果我们能够对它所有的函数分类,就很容易梳理它的内容了。
我们刚才提到,它需要接收之前几节介绍的所有的信息,并加入概率图中,所以我们以信息为切入点,去梳理它的内容。
我们先看都包含哪些信息:
- 前端里程计传入的位姿和点云
- gps信息
- imu信息
- 平面拟合的参数信息
处理一个信息包括以下步骤:
- 在对应的callback函数中接收信息,并放入相应的队列
- 根据时间戳对队列中的信息进行顺序处理,加入概率图
四条信息,每条信息都执行两个步骤,这就已经包含了本文件大部分的代码。
剩下的就是一些其他内容,我们进一步梳理:
- 执行图优化,这是一个定时执行的函数,闭环检测也在这个函数里
- 生成全局地图并定时发送,即把所有关键帧拼一起,得到全局点云地图,然后在一个定时函数里发送到rviz上去
- 在rviz中显示顶点和边,如果运行程序,会看到rviz中把概率图可视化了
至此,整个文件的核心框架就梳理完了,剩下的一些细枝末节的东西应该都能看懂。
下面我们就按照这个框架对各部分内容进行梳理,这次梳理我们遵照以下原则:
- 和消息相关的代码我们之前在各自对应的章节中都梳理过了,这里只是使用,所以他们的代码就不贴出来了,不然会把本节内容搞得繁琐,反而不利于整体理解
- 概率图可视化的代码就不解释了,和核心算法无关
- 和算法无关的一些ros操作相关的东西,也不在这里解释了
2. 代码详解
2.1 前端里程计消息处理
- 接收消息:
cloud_callback
- 处理消息队列:
flush_keyframe_queue
2.2 gps消息
- 接收消息:
nmea_callback
+navsat_callback
+gps_callback
- 处理消息队列:
flush_gps_queue
2.3 imu消息
- 接收消息:
imu_callback
- 处理消息队列:
flush_imu_queue
2.4 地面拟合参数
- 接收消息:
floor_coeffs_callback
- 处理消息队列:
flush_floor_queue
2.5 执行图优化
函数名是optimization_timer_callback
,这个函数除了执行图优化以外,还做了以下两点:
- 闭环检测
// 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));
}
检测闭环并加到了概率图中,当然是在优化之前。
- 生成简化版关键帧
还记得我们在关键帧那一节介绍的简化版关键帧(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();
2.6 生成地图并定时发送
生成地图并定时发送很明显包括两步:生成地图、定时发送,哈哈。
- 生成地图
就是使用简化版关键帧进行拼接,这部分代码在文件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;
}
- 定时发送
这部分内容在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);
}
3. 整体思考
OK,整个hdl_graph_slam系统的介绍到这里就结束了,前后一共八篇文章。
最后总结一下吧,虽然hdl_graph_slam的性能其实不是很好,具体原因在于帧间匹配和闭环检测的精度都不够,它在这两个环节的缺点我们在对应的章节已经解释过了。
但是,我想说的是对于一套开源系统来讲,不能指望它拿来即用,小的缺点很容易改,都已经指出问题了,就那几行代码,改一改还不容易吗。我们更应该关心它的架构,这套系统的代码模块画做的很好,思路很清晰,整个系统对于对多信息源的融合做的很好,按照这种架构,再多来几个传感器也没关系,这就是系统的扩展性。
综上,我认为对开发者来讲(注意这句话),这是一套不错的开源系统。