C++ Cartographer源码中关于MapBuilder的声明与构造

目录

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

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

开始一条轨迹

添加轨迹是开启Cartographer的大门. 顾python名思义, 添加轨迹就是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: 1 开始一条新的轨迹, 返回新轨迹的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: 2 为这个新轨迹 添加一个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++11: 限域枚举 enum class 
    enum class SensorType {
      RANGE = 0,
      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: 1 开始一条新的轨迹, 返回新轨迹的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 Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });

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

第二部分程序如下:

  // Step: 2 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( //sensor_bridges_是一个unorderedmap
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_optijavascriptons.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<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( //建立一个local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(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,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局轨迹构建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类
            std::move(local_trajectory_builder), //前端构建器
            trajectory_id, //
            static_cast<PoseGraph2D*>(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, //返回值为空, 5个参数列表
                         transform::Rigid3d /* 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::Vector3f 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 in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;
    // Used for loop closure in 3D.
    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::Rigid3d 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 Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult>) 
        // 保存local slam 的结果数据 5个参数实际只用了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 Rigid3d 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::Rigid3d local_pose;
      ::cartographer::sensor::RangeData range_data_in_local;
    };

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

    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>( //建立一个local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(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,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局轨迹构建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类
            std::move(local_trajectory_builder), //前端构建器
            trajectory_id, //
            static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿图
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

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

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

// 只返回传感器类型是RANGE的topic的集合
std::vector<std::string> SelectRangeSensorIds(
    const 开发者_JS开发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的具体实现:

// 2d的完整的slam
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);
}

这个函数没处理啥, 直接返回了由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 for 2D and 3D.
// 使用 sensor::CollatorInterface 整理传感器数据, 
// 然后将其传递到2D和3D通用的 mapping::TrajectoryBuilderInterface
// 处理传感器数据, 使其按照时间排列, 然后传入GlobalTrajectoryBuilder

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

总结

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

到此这篇关于C++ Cartographer源码中关于MapBuilder的声明与构造的文章就介绍到这了,更多相关C++ MapBuilder的声明与构造内容请搜索我们以前的文章或继续浏览下面的相关文章希望大家以后多多支持我们！