目录

• 开始一条轨迹
• MapBuilderBridge类的AddTrajectory函数
• MapBuilder类的AddTrajectoryBuilder函数
• 总结

前面已经谈到了Cartographer中添加轨迹的方法和传感器的数据流动走向。 我们注意到，在添加轨迹的时候，除了添加位姿估计器还有采样器，订阅回调函数之外，最重要的是通过map_builder_bridge添加了一条轨迹，其他的都是为它服务的。咱们这节详细看看MapBuilder这个Cartographer的核心。

开始一条轨迹

添加轨迹是开启Cartographer的大门. 顾名思义, 添加轨迹就是AddTrajectory. 我们最先接触到的AddTrajectory函数是Node类里面的. 这个函数除了我们之前详细提到的添加传感器等功能之外,还有一个核心函数:

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

这一行调用了Map_builder_bridge_的AddTrajectory添加一条轨迹. map_builder_bridge_从何而来呢?在Node类的构造函数中有个初始化列表,用于构造MapBuilderBridge这个类.

map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)

我们看到MapBuilderBridge构造函数有三个参数,其中一个(中间一个)就是std::unique_ptr<cartographer::mapping::MapBuilderInterface> ,也就是MapBuilderInterface这个类的指针. MapBuilderInterface这个类和MapBuilder是父子关系, 所以这个地方实际上构造的是MapBuilder这个类. 这个map_builder又是那来的呢? 他在Cartographer的入口程序: node_main.cc中, 就出现了,

  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//在map_builder.cc中实现，工厂函数
                                                                                //在这里,实例化一个MapBuilder, 而MapBuilder是MapBuilderInterface的子类
                                                                                //MapBuilder的AddTrajectoryBuilder实例化了CollatedTrajectoryBuilder 

CreateMapBuilder这个函数是在map_builder.cc中实现:

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

可见CreateMapBuilder就是返回了构造好了的MapBuilder类. 而cpp允许子类用父类代替实现多态, 所以上面用MapBuilderInterface也不会出错.

再回到node_main.cc中的CreateMapBuilder, 这个意思就是用传入的选项构造一个MapBuilder类的对象并返回这个对象,然后再到node.cc中, 把node_main.cc中的CreateMapBuilder作为Node的构造函数的变量传给Node构造出map_builder_bridge_这个变量(MapBuilderBridge类). 在调用Node的AddTrajectory的时候, 通过调用map_builder_bridge_.AddTrajectory完成一条轨迹的添加.

MapBuilderBridge类的AddTrajectory函数

回到map_builder_bridge_.AddTrajectory, 下面摘取了 MapBuilderBridge::AddTrajectory主要的部分:

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) 
{
    // Step: 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
    const int trajectory_id = map_builder_->AddTrajectoryBuilder(
    expected_sensor_ids, trajectory_options.trajectory_builder_options,
    [this]() {
    OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
    });
    // Step: 为这个新轨迹 添加一个SensorBridge
    sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
    trajectory_options.num_subdivisions_per_laser_scan,
    trajectory_options.tracking_frame,
    node_options_.lookup_transform_timeout_sec,
    tf_buffer_,
    map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder
    ......
    return trajectory_id;
}

传入的参数有一个std::set<...Sensor_id>类型的变量,std::set是一个容器,可以简单理解为键值对,而键就是值,值就是键.(比较基础不细说啦).另一个就是从node_main.cc就跟着我们的TrajectoryOptions. 也就是配置文件读取的内容. 返回值很简单,就是新建轨迹的编号. Cartographer允许有多个轨迹同时维护,而且后面我们会发现, Cartographer定位其实就是把建好的地图和定位作为两个不同的轨迹实现的, 这个我们在这个系列的最后会说.

咱们看一下expected_sensor_ids的面目,他是一个结构体, 在Cartographer部分的trajectory_builder_interface.h中实现:

  struct SensorId {
    // c++: 限域枚举 enum class 
    enum class SensorType {
      RANGE =,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };
    SensorType type;  // 传感器类型
    std::string id;   // topic的名字
    bool operator==(const SensorId& other) const {
      return std::forward_as_tuple(type, id) ==
             std::forward_as_tuple(other.type, other.id);
    }
    bool operator<(const SensorId& other) const {
      return std::forward_as_tuple(type, id) <
             std::forward_as_tuple(other.type, other.id);
    }
  };

其中比较有用的是它规定了一个传感器的类型与一个对应的topic的名字. 传感器的类型是一个限域枚举(枚举类). 总之作用就是联系topic与topic对应的传感器类型, 以便后续维护.

Node的AddTrajectory实际上是调用的MapBuilderBridge的AddTrajectory. 咱们看看map_builder_bridge中的AddTrajectory. 这个函数分为三个步骤:

• 开启一条新轨迹
• 给新轨迹添加传感器
• 保存轨迹的参数配置

第一步程序如下:

  // Step: 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      // lambda表达式 local_slam_result_callback_
      [this](const int trajectory_id, 
             const ::cartographer::common::Time time,
             const Rigidd local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        // 保存local slam 的结果数据个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });

这一块很复杂,一开始看很可能看不透,所以值得仔细剖析. 在第三部分剖析.

第二部分程序如下:

  // Step: 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( //sensor_bridges_是一个unorderedmap
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, 
      tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // map_builder是MapBuilder的实例化,  在map_builder.h中实现,
                                                          // 返回当前轨迹的CollatedTrajectoryBuilder的指针.
                                                          // CollatedTrajectoryBuilder就是前端后端绑在一起的,传给了SensorBridge

作用是为第一步添加的轨迹, 联合一个传感器相关处理与维护器. 这一块将在传感器的数据分发器部分详细解读. 咱们这解主要挖掘MapBuilder相关部分,而不是Sensor部分.

MapBuilder类的AddTrajectoryBuilder函数

新建轨迹通过调用map_builder的AddTrajectoryBuilder方法, 依旧只摘取重要的部分程序:

/**
 * @brief 创建一个新的 TrajectoryBuilder 并返回它的 trajectory_id
 * 
 * @param[in] expected_sensor_ids 所有需要的topic的名字的集合
 * @param[in] trajectory_options 轨迹的参数配置
 * @param[in] local_slam_result_callback 需要传入的回调函数
 *        实际上是map_builder_bridge.cc 中的 OnLocalSlamResult() 函数
 * @return int 新生成的轨迹的id
 */
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) 
{
    std::unique_ptr<LocalTrajectoryBuilderD> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilderD>( //建立一个local_trajectory_builder
          trajectory_options.trajectory_builder_d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraphD*>(pose_graph_.get()));
    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( //NOTE:MapBuilder::AddTrajectoryBuilder使用的是CollatedTrajectoryBuilder
        trajectory_options, sensor_collator_.get(), trajectory_id, 
        expected_sensor_ids,
        // 将D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilderD(  //全局轨迹构建器
                                          //CreateGlobalTrajectoryBuilderD是global_trajectory_builderd的方法,
                                          //继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类
            std::move(local_trajectory_builder), //前端构建器
            trajectory_id, //
            static_cast<PoseGraphD*>(pose_graph_.get()), //后端位姿图
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
}

先看传入的参数. 传入的有三个参数, 前面两个为Sensor_id和配置参数, 咱们之前都详细说到过.

重点要看的是最后一个参数. 最后一个传入的参数是函数指针,也就是一个回调函数的地址(c++的基础内容), 在上级直接使用的是一个lambda函数, lambda表达式我觉得可以定义为"一次性函数", 这里不深入讨论了. 这个函数的类型的具体实现还是在trajectory_builder_interface中:

  // A callback which is called after local SLAM processes an accumulated
  // 'sensor::RangeData'. If the data was inserted into a submap, reports the
  // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
  using LocalSlamResultCallback =
      std::function<void(int /* trajectory ID */, common::Time, //返回值为空,个参数列表
                         transform::Rigidd /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>; //InsertionResult保存了地图的具体数据,栅格值

咱们一一把这个函数和他的lambda表达式对照着看看

LocalSlamResultCallback有5个参数, 分别是: 1. 轨迹的id, 时间戳, cartographer自己定义的位姿结构体(类似eigen但是拥有更多方法), 激光雷达给的结果, 还有地图的栅格值.

咱么看一下这几个传入的参数:

前三个比较明确,

RangeData定义在range_data.h中:

/**
 * @brief local_slam_data中存储所有雷达点云的数据结构
 * 
 * @param origin  点云的原点在local坐标系下的坐标
 * @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit
 * @param misses  是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点
 */
struct RangeData {
  Eigen::Vectorf origin;
  PointCloud returns;
  PointCloud misses; // local坐标系下的坐标
};

这个结构体表示一帧激光点云的信息: 1. 当前雷达在哪里扫的(相对于local坐标系), 2. 有效激光点和无效激光点.

这里涉及到Cartographer中坐标的关系(我被绕了好久才搞懂~.~), 这里引用一下大佬的图

这里要注意的是global coordinate和 local coordinate之间的关系, 就是没有直接关系. 经过我的实验, 发现Cartographer一开始的时候这俩(local和global)坐标系是重合的, 只有在经历回环之后他们才会产生偏移. 而且local coordinate永远是固定的, 只有global才会变. 这只是我的实验得到的, 如果有错请大家一定要指出.

PointCloud类在point_cloud.h, 定义了点云结构, 包含了雷达一帧数据的所有数据点 与 数据点对应的强度值, 比较简单, 就不细说了.

咱们再看第二个参数:InsertionResult

  struct InsertionResult {
    NodeId node_id;
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
  };

这就是一个简单的结构体: 1. 节点的id, 2. 某个数据(下面会说), 3. 子图(比较复杂, 后面说...)

看一下这个结构体第二个参数, 在trajectory_node.h中

  struct Data {
    common::Time time;
    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;
    // Used for loop closure inD: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;
    // Used for loop closure inD.
    sensor::PointCloud high_resolution_point_cloud;
    sensor::PointCloud low_resolution_point_cloud;
    Eigen::VectorXf rotational_scan_matcher_histogram;
    // The node pose in the local SLAM frame.
    transform::Rigidd local_pose;
  };

这个Data结构体就是Cartographer重要的数据结构之一, 包含着: 前端匹配所用的数据(去重力后的点云)与计算出的local坐标系下的位姿.

再看第二个参数:submap

也就是子图, 可以先认为是很多个单帧点云形成的,也是Cartographer主要的数据类型之一, 主要有三个功能:1. 保存在local坐标系下的子图的坐标, 2. 记录插入到子图中雷达数据的个数, 3. 标记这个子图是否是完成状态.

讲submap需要的篇幅比较长, 之后单独拿出来说.

咱们在返回到最前面,去看看map_builder_bridge的AddTrajectory调用的map_builder_的AddTrajectoryBuilder的参数的lambda函数

  // lambda表达式 local_slam_result_callback_
  [this](const int trajectory_id, 
         const ::cartographer::common::Time time,
         const Rigidd local_pose,
         ::cartographer::sensor::RangeData range_data_in_local,
         const std::unique_ptr<const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult>) 
    // 保存local slam 的结果数据个参数实际只用了4个
    OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
  }

有关lambda函数的知识请参阅知识

这里我们看到,这个lambda表达式捕获了自己MapBuilderBridge这个类, 让我们能用其变量与方法.捕获，其实就是将局部自动变量保存到 Lambda 表达式内部.

传入的参数有5个: 1. 轨迹id, 2. 时间戳, 3. 扫描匹配计算出的在local坐标系下的位姿, 4. 扫描匹配使用的雷达数据,还有一个没有用上的InsertionResult. 然后把前四个参数传给OnLocalSlamResult.

所以咱们再看看OnLocalSlamResult这个函数,这个函数就定义在map_builder_bridge.cc下面, 作用是保存local SLAM的结果

/**
 * @brief 保存local slam 的结果
 * 
 * @param[in] trajectory_id 当前轨迹id
 * @param[in] time 扫描匹配的时间
 * @param[in] local_pose 扫描匹配计算出的在local坐标系下的位姿
 * @param[in] range_data_in_local 扫描匹配使用的雷达数据
 */
void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const Rigidd local_pose,
    ::cartographer::sensor::RangeData range_data_in_local) {
  std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
      std::make_shared<LocalTrajectoryData::LocalSlamData>(
          LocalTrajectoryData::LocalSlamData{time, local_pose,
                                             std::move(range_data_in_local)});
  // 保存结果数据
  absl::MutexLock lock(&mutex_);
  local_slam_data_[trajectory_id] = std::move(local_slam_data);
  // todo: local_slam_data_[trajectory_id].loc
}

其中LocalSlamData结构体定义在map_builder_bridge.h中, 包含了时间,位姿与雷达数据

// LocalSlamData中包含了local slam的一些数据, 包含当前时间, 当前估计的位姿, 以及累计的所有雷达数据
struct LocalSlamData {
  ::cartographer::common::Time time;
  ::cartographer::transform::Rigidd local_pose;
  ::cartographer::sensor::RangeData range_data_in_local;
};

代码数据类型的深挖告一段落, 咱们必须要回头去看看MapBuilder类的AddTrajectoryBuilder函数了. 咱们以2D地图为例:

std::unique_ptr<LocalTrajectoryBuilderD> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_d_options()) {
  // local_trajectory_builder(前端)的初始化
  local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilderD>( //建立一个local_trajectory_builder
      trajectory_options.trajectory_builder_d_options(),
      SelectRangeSensorIds(expected_sensor_ids));
}
DCHECK(dynamic_cast<PoseGraphD*>(pose_graph_.get()));
// CollatedTrajectoryBuilder初始化
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( //NOTE:MapBuilder::AddTrajectoryBuilder使用的是CollatedTrajectoryBuilder
    trajectory_options, sensor_collator_.get(), trajectory_id, 
    expected_sensor_ids,
    // 将D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
    CreateGlobalTrajectoryBuilderD(  //全局轨迹构建器
                                      //CreateGlobalTrajectoryBuilderD是global_trajectory_builderd的方法,
                                      //继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类
        std::move(local_trajectory_builder), //前端构建器
        trajectory_id, //
        static_cast<PoseGraphD*>(pose_graph_.get()), //后端位姿图
        local_slam_result_callback, pose_graph_odometry_motion_filter)));

MapBuilder的AddTrajectoryBuilder开启了Cartographer的前端和后端! 可以从上面的程序看出.

第一部分, 首先通过absl::make_unique<LocalTrajectoryBuilder2D>构建了一个2D的local SLAM, 啥是local SLAM? 在Cartographer中就是所谓的前端. 传入的参数有配置参数, 以及SelectRangeSensorIds的返回值. 我们去看看SelectRangeSensorIds干了些啥

// 只返回传感器类型是RANGE的topic的集合
std::vector<std::string> SelectRangeSensorIds(
    const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
  std::vector<std::string> range_sensor_ids;
  for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
    if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
      range_sensor_ids.push_back(sensor_id.id);
    }
  }
  return range_sensor_ids;
}

发现SelectRangeSensorIds返回了传感器类型是range的topic集合, 也就是各种激光雷达的所有topic. LocalTrajectoryBuilder2D具体内容将在前端部分细讲.

第二部分, 为CollatedTrajectoryBuilder初始化. 这一部分一层套一层, 咱们从最底层分析.

里面有个函数CreateGlobalTrajectoryBuilder2D, 顾名思义, Global就是全局的意思, 就是前端加后端的意思. 咱们看看传入的参数就知道为啥是前端加后端了.

第一个参数std::move(local_trajectory_builder), 通过std::move把local_trajectory_builder, 也就是前端的构建器, 当成参数传递给了CreateGlobalTrajectoryBuilder2D函数.

第二个参数, 轨迹id就不多说了

第三个参数是pose_graph_, 也就是位姿图, 在Cartographer中也就是后端. 把整个后端的构建器的指针传给了CreateGlobalTrajectoryBuilder2D函数.

第四个参数, local_slam_result_callback, 也就是前面大费周章写的MapBuilderBridge的AddTrajectory的lambda表达式(作用是保存local SLAM的结果).

第五个参数是pose_graph_odometry_motion_filter, 既运动过滤器, 用来把太小的运动给过滤掉. (可能后端没有使用, 还没深究).

看完参数,咱们找找CreateGlobalTrajectoryBuilder2D在哪,干了啥. 在global_trajectory_builder.cc中我们可以找到CreateGlobalTrajectoryBuilder2D的具体实现:

//d的完整的slam
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilderD(
    std::unique_ptr<LocalTrajectoryBuilderD> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraphD* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback,
    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
  return absl::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilderD, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback, pose_graph_odometry_motion_filter);
}

这个函数没处理啥, 直接返回了由Local(前端)和PoseGraph(后端)组合出来的GlobalTrajectoryBuilder. 而GlobalTrajectoryBuilder有着自己个构造函数, 起到了初始化赋值的作用. 所以我们可以说GlobalTrajectoryBuilder才是完整的SLAM, 链接了前端和后端.

另外提一嘴, 由于LocalTrajectoryBuilder和GlobalTrajectoryBuilder都继承自mapping::TrajectoryBuilderInterface, 都有相同的父类, 所以他们之间是可以通过多态互相调用彼此的成员函数的. (CreateGlobalTrajectoryBuilder2D在map_builder.cc中使用的时候并没有添加命名空间)

GlobalTrajectoryBuilder返回了整个前端和后端, 联合trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, 这些参数, 一起传给了CollatedTrajectoryBuilder.

对于CollatedTrajectoryBuilder这个类, 源程序中注释已经很明确(如下), 他就是联合和传感器的前端后端SLAM, 所以他叫Collated(收集并综合). 他同local与global, 依然继承于TrajectoryBuilderInterface

// Collates sensor data using a sensor::CollatorInterface, then passes it on to
// a mapping::TrajectoryBuilderInterface which is common forD and 3D.
// 使用 sensor::CollatorInterface 整理传感器数据, 
// 然后将其传递到D和3D通用的 mapping::TrajectoryBuilderInterface
// 处理传感器数据, 使其按照时间排列, 然后传入GlobalTrajectoryBuilder

总结一下: CollatedTrajectory = GlobalTrajectory + Sensor = LocalTrajectory + PoseGraph + Sensor

总结

MapBuilder通过联合LocalTrajectory, PoseGraph, 还有Sensor, 开启了整个Cartographer的前端与后端.