Cartographer学习记录：cartographer源码3D SLAM部分（一）

不甘平凡的平凡之人

已于 2022-03-01 21:52:27 修改

阅读量1.3k

点赞数 1

文章标签： 3d 自动驾驶人工智能

于 2022-02-28 16:43:40 首次发布

本文链接：https://blog.csdn.net/smartfoolish/article/details/123183449

版权

在经历过官方数据集和自录数据集的验证后，我们可以清楚的看到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类的成员函数依次解读，其中各文件之间相关的一些地方我也会指出来方便各位小伙伴理解，希望能够在记录的过程中不断纠错和进步。