在经历过官方数据集和自录数据集的验证后,我们可以清楚的看到cartographer的3D建图效果,下面一段时间我将开展3D SLAM源码部分的阅读和注释,在此做个记录,如果有小伙伴发现不对的地方,希望能够及时告知我,以便我及时改正。
以个人习惯而言,我阅读代码采用的策略是先看头文件,熟悉类内包括的函数及成员变量,然后对实现文件采用的是深度优先搜索+广度优先搜索的方式,深度优先搜索是指从主线程出发,对调用函数逐段深入;广度优先搜索主要指呈现形式,把深度优先搜索得到的调用放在该文件中以便后续查看方便,下面我将先从cartographer的上层ROS封装开始逐步深入阅读。
一.node_main.cc文件
该文件是cartographer_ros的主体文件,所以选择从该文件进行逐步深入阅读。
1.文件的一开始是调用Google 开源的命令行标记处理库gflags,gflags主要支持的参数类型包括bool, int32, int64, uint64, double, string等,定义参数通过DEFINE_type宏实现,参数分别为参数名,参数默认值以及提示信息,定义完成后即可通过FLAGS_name访问相应的参数,例如下面出现的“FLAGS_configuration_directory”和“FLAGS_configuration_basename”就对应此处的“configuration_directory”和“configuration_basename”。
DEFINE_bool(collect_metrics, false,
"Activates the collection of runtime metrics. If activated, the "
"metrics can be accessed via a ROS service.");
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
DEFINE_string(load_state_filename, "",
"If non-empty, filename of a .pbstream file to load, containing "
"a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
"Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(
start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
save_state_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
2.从底下的main函数出发进行阅读,main函数里面主要进行一些初始化,运行和结束操作,初始化包括InitGoogleLogging、ParseCommandLineFlags以及ros的init函数,运行是本文件内定义的run函数,结束则是ros的shutdown函数,(ps:ros前面跟的::表示全局作用域下的调用)。初始化和结束操作大致了解下作用就可以,关键是run函数,直接关系到如何从ros上层节点一点一点进入下层cartographer算法层面。
int main(int argc, char** argv) {
// 初始化Google的日志库glog
google::InitGoogleLogging(argv[0]);
// 调用gflags进行参数的初始化,第一个和第二个参数为main函数的参数基数和参数值,第三个参数名为remove_flags,如果为false,则表示会将argv的参数值按照flags进行分隔、重新排序,如果为true, gflags会移除argv中分隔过的参数
google::ParseCommandLineFlags(&argc, &argv, true);
// Google的日志库glog里提供的CHECK系列的宏,检测某个条件是否为真,形式为CHECK(condition)
// condition如果不为真,则打印后面的表达式信息以及“check failed:" #condition "”
// 然后退出程序,出错后的处理过程和FATAL比较像
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
// cartographer的主ROS节点"cartographer_node"的初始化
::ros::init(argc, argv, "cartographer_node");
// 启动ROS,显式调用该函数表示在创建任何NodeHandle实例之前启动ROS相关的线程, 网络等
::ros::start();
// 使用ROS_INFO进行glog消息的输出
cartographer_ros::ScopedRosLogSink ros_log_sink;
// 开始运行cartographer_ros
cartographer_ros::Run();
// 结束ROS相关的线程,网络等
::ros::shutdown();
}
3.Run函数流程:利用LoadOptions函数加载配置->将配置作为CreateMapBuilder的参数创建map_builder对象->将map_builder和配置node_options作为参数构造node类对象->node类对象订阅topic信息开始制定路径->node类对象结束所有处于活动状态的轨迹并执行全局优化。
Run函数中的重要的调用函数包括CreateMapBuilder()、StartTrajectoryWithDefaultTopics()、FinishAllTrajectories()、RunFinalOptimization(),加载功用的一些函数就不做过多的介绍。
// cartographer_ros命名空间
namespace cartographer_ros {
namespace {
void Run() {
// 定义tf发布的时间间隔
constexpr double kTfBufferCacheTimeInSeconds = 10.;
// 实例化tf2_ros::Buffer对象tf_buffer
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
// 开启监听tf的独立线程
tf2_ros::TransformListener tf(tf_buffer);
// node_options结构体,包含地图帧名设置、接收tf的timeout时间设置、子图发布周期设置、位姿发布周期设置、路径发布周期设置
NodeOptions node_options;
// TrajectoryOptions结构体,包含跟踪帧名设置、发布帧名设置、里程计帧名设置、雷达扫描数量设置、点云数量设置等等
TrajectoryOptions trajectory_options;
// 补充——c++11引入的std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple
// 将LoadOptions 获取到的lua文件内参数值分别赋给 node_options 和 trajectory_options
// LoadOptions 函数在node_options.h 中定义
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
// MapBuilder类是cartographer内完整的SLAM算法类包含前端(TrajectoryBuilders,scan to submap)与后端(用于查找回环的PoseGraph)
// CreateMapBuilder函数利用lua文件内设置的参数值传感指向MapBuilder类实例化对象以便后续嗲用相关成员函数
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
// 补充——c++11引入的std::move 是将对象的状态或者所有权从一个对象转移到另一个对象,
// 只是转移,没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能
// 右值引用是用来支持转移语义的,转移语义可以将资源(堆,系统对象等)从一个对象转移到另一个对象,这样能够减少不必要的临时对象的创建、拷贝以及销毁,能够大幅度提高C++应用程序的性能
// 在程序中对临时对象的维护(创建和销毁)对性能有严重影响.
// Node类的初始化,将ROS的topic传入MapBuilder
// Node 在/cartographer_ros/cartographer_ros/cartographer/node.h 中定义
// 在该构造函数中订阅了很多传感器的topic,收集传感器数据
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
// 根据前面DEFINE_type定义决定是否加载pbstream地图文件
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
// 使用默认topic(在node_constants.h文件中定义相关话题名称)开始制定轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
// ROS 消息回调处理函数,ros::spinonce()表示只调用一次订阅的消息,后续程序还可以继续执行,若想要一直订阅则需要加上循环,所以相对spin()具有灵活性,可以控制频率和消息池大小;
::ros::spin();
// 结束所有处于活动状态的轨迹
node.FinishAllTrajectories();
// 当所有的轨迹结束时,再执行一次全局优化
node.RunFinalOptimization();
// 如果DEFINE_type定义save_state_filename非空,就保存pbstream文件
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}
} // namespace
} // namespace cartographer_ros
接下来的文章将先结合CreateMapBuilder()函数和node的构造函数进行突破,逐段深入解读,然后按照StartTrajectoryWithDefaultTopics()、FinishAllTrajectories()、RunFinalOptimization()的顺序对node类的成员函数依次解读,其中各文件之间相关的一些地方我也会指出来方便各位小伙伴理解,希望能够在记录的过程中不断纠错和进步。