cartographer 工程化设计1：底层算法的创建与参数初始化

一、简述

我们所熟悉的SLAM算法包括前端里程计、后端优化、回环检测。
但是对于一个完整的工程，第一个任务就是将SLAM算法完整的构建出来，并能方便的配置参数。
完整SLAM算法的封装是在MapBuilder类中完成的。
前端里程计的创建和初始化在MapBuilder::AddTrajectoryBuilder()中完成。
后端优化和回环检测的构造以及初始化则在MapBuilder类的构造函数中完成。

二、算法参数的载入

2.1 存放参数的文件在哪

在node_main.cc的Run()函数的最开始就对算法参数进行了读取：

// 根据Lua配置文件中的内容, 为node_options, trajectory_options 赋值
  std::tie(node_options, trajectory_options) =   // std::tie：创建左值引用的 tuple，或将 tuple 解包为独立对象。 
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

这里通过LoadOptions()函数读取lua文件的参数，并将其加载到了node_options，trajectory_options中。
FLAGS_configuration_directory， FLAGS_configuration_basename 是 lua文件的地址参数，这两个参数是通过gflags定义的，在本文件最上方有：

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.");

即在gflags种定义了变量configuration_directory，configuration_basename，
gflags是一种命令行解析工具，即参数可以从外部传入并对gflags内部定义的同名变量进行赋值，这两个参数是在.launch文件中传入的，例如打开cartographer_ros/cartographer_ros/launch/backpack_2d.launch，可以看到：

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d.lua"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

而在gflags中定义的变量XX，在程序中通过FLAGS_XX进行读取，即通过FLAGS_configuration_directory， FLAGS_configuration_basename来访问。
可知：
FLAGS_configuration_directory = $(find cartographer_ros)/configuration_files
FLAGS_configuration_basename = backpack_2d.lua
那么，lua文件的地址就确定了。

2.2 lua文件的结构

打开backpack_2d.lua参数文件可以看到，先通过头文件引入了另外两个lua文件：

include "map_builder.lua"
include "trajectory_builder.lua"

此时，另外两个头文件的参数也被引入到了backpack_2d.lua中，
"map_builder.lua"是src/cartographer/configuration_files/map_builder.lua：

include "pose_graph.lua"

MAP_BUILDER = {
  use_trajectory_builder_2d = false,
  use_trajectory_builder_3d = false,
  num_background_threads = 4,
  pose_graph = POSE_GRAPH,
  collate_by_trajectory = false,
}

可见，引入了"pose_graph.lua"文件的POSE_GRAPH参数以及MAP_BUILDER参数。

"trajectory_builder.lua"是src/cartographer/configuration_files/trajectory_builder.lua：

include "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"

TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
--  pure_localization_trimmer = {
--    max_submaps_to_keep = 3,
--  },
  collate_fixed_frame = true, -- 是否将数据放入阻塞队列中
  collate_landmarks = false,  -- 是否将数据放入阻塞队列中
}

可见，在TRAJECTORY_BUILDER中同时引入了2D轨迹构建参数TRAJECTORY_BUILDER_2D，和3D轨迹构建参数TRAJECTORY_BUILDER_3D。

此后通过options定义了一些新的参数。

options = {
...
}

然后对从其他lua文件中引入的参数进行修改：

MAP_BUILDER.use_trajectory_builder_2d = true     // 设置为了2D建图
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10

2.3 lua文件数据的读取

LoadOptions()传入lua文件的地址，并在内部读取lua参数文件

// 创建元组tuple,元组定义了一个有固定数目元素的容器, 其中的每个元素类型都可以不相同
  // 将配置文件的内容填充进NodeOptions与TrajectoryOptions, 并返回
  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
                         CreateTrajectoryOptions(&lua_parameter_dictionary));

其中，CreateNodeOptions() 读取lua文件数据并返回NodeOptions对象，
CreateTrajectoryOptions() 读取lua文件数据并返回 TrajectoryOptions对象。

三、算法创建

3.1 后端算法的创建与参数初始化

后端算法的创建在MapBuilder类的构造函数中完成。

在读取了lua文件中的参数后，接着便可基于这些参数创建具体的算法了。
首先构造MapBuilder对象是在Node::Run()中，所传入的参数数据是node_options.map_builder_options。

 // MapBuilder类是完整的SLAM算法类
 // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
 auto map_builder =
     cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);

内部：

// 工厂函数
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

也就是使用参数构造了一个MapBuilder对象。
进入构造函数看看内部构造了什么…

/**
 * @brief 保存配置参数, 根据给定的参数初始化线程池, 并且初始化pose_graph_与sensor_collator_
 * 
 * @param[in] options proto::MapBuilderOptions格式的 map_builder参数
 */
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), thread_pool_(options.num_background_threads()) { // param: num_background_threads
  CHECK(options.use_trajectory_builder_2d() ^
        options.use_trajectory_builder_3d());

  // 2d位姿图(后端)的初始化
  if (options.use_trajectory_builder_2d()) {
    pose_graph_ = absl::make_unique<PoseGraph2D>(
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem2D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  }
  // 3d位姿图(后端)的初始化
  if (options.use_trajectory_builder_3d()) {
    pose_graph_ = absl::make_unique<PoseGraph3D>(
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem3D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  } 

  // 在 cartographer/configuration_files/map_builder.lua 中设置
  // param: MAP_BUILDER.collate_by_trajectory 默认为false
  if (options.collate_by_trajectory()) {
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    // sensor_collator_初始化, 实际使用这个
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }
}

重点有：
1、创建了一个线程池。2、创建了后端优化器。3、sensor_collator_

3.2 前端算法的创建

3.2.1 创建过程

前端里程计是在MapBuilder::AddTrajectoryBuilder()内构造完成的，函数长这样：

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback)

MapBuilder::AddTrajectoryBuilder()的调用过程是：
node_main.cc:Run()->Node::StartTrajectoryWithDefaultTopics()->Node::AddTrajectory()
->map_builder_bridge_.AddTrajectory()->map_builder_->AddTrajectoryBuilder()

其中第二个参数是算法的参数集合proto::TrajectoryBuilderOptions，传入的数据是 trajectory_options.trajectory_builder_options

在MapBuilder::AddTrajectoryBuilder()中，会根据配置参数选择构造2D前端或是3D前端，
内部实现如下：

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
   略 ...
    // 3d的轨迹
  if (options_.use_trajectory_builder_3d()) {
 略 ...
  } 
  // 2d的轨迹
  else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
	// CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
  }
略 ...
  }

可见，首先创建2D前端LocalTrajectoryBuilder2D对象local_trajectory_builder，接着通过CreateGlobalTrajectoryBuilder2D(…)函数将2D前端对象和后端优化对象以及回调函数等等打包成GlobalTrajectoryBuilder对象：

std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback,
    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
  return absl::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback, pose_graph_odometry_motion_filter);
}

最后再将GlobalTrajectoryBuilder对象以及sensor_collator_等等成分打包成CollatedTrajectoryBuilder，放入trajectory_builders_中。

3.2.2 前端算法的相关参数

前面已经提到，传入到MapBuilder::AddTrajectoryBuilder()行参trajectory_options的是trajectory_options.trajectory_builder_options，下面我们该弄清楚保存到里面的是些什么数据。
首先该数据的创建是在LoadOptions()中的CreateTrajectoryOptions()完成的，如下：

TrajectoryOptions CreateTrajectoryOptions(
    ::cartographer::common::LuaParameterDictionary* const
        lua_parameter_dictionary) {
  TrajectoryOptions options;
  // trajectory_builder.lua 里的 TRAJECTORY_BUILDER参数 
  options.trajectory_builder_options =
      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
          lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
 ..........

因此实际trajectory_options.trajectory_builder_options 保存的就是trajectory_builder.lua 里的 TRAJECTORY_BUILDER参数。
打开trajectory_builder.lua，可以看到TRAJECTORY_BUILDER参数：

TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,      -- 2D相关参数
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,      -- 3D相关参数   
-- 纯定位保留的子图数量  
--  pure_localization_trimmer = {
--    max_submaps_to_keep = 3,     
--  },
  collate_fixed_frame = true, -- 是否将数据放入阻塞队列中
  collate_landmarks = false,  -- 是否将数据放入阻塞队列中
}

其内部又包含了2D前端相关参数trajectory_builder_2d，和3D前端相关参数trajectory_builder_3d，这些参数实际上是保存在其他lua文件中的，并通过头文件引入进来，2D前端相关参数放置在 src/cartographer/configuration_files/trajectory_builder_2d.lua中，

前端子图构建的相关参数

  -- 子图相关的一些配置
  submaps = {
    num_range_data = 90,          -- 一个子图里插入雷达数据的个数的一半
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID", -- 地图的种类, 还可以是tsdf格式
      resolution = 0.05,
    },
    range_data_inserter = {
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
      -- 概率占用栅格地图的一些配置
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      -- tsdf地图的一些配置
      tsdf_range_data_inserter = {
        truncation_distance = 0.3,
        maximum_weight = 10.,
        update_free_space = false,
        normal_estimation_options = {
          num_normal_samples = 4,
          sample_radius = 0.5,
        },
        project_sdf_distance_to_scan_normal = true,
        update_weight_range_exponent = 0,
        update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
        update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
      },
    },
  },