C++ Cartographer源码中MapBuilder怎么声明与构造

这篇文章主要介绍“C++ Cartographer源码中MapBuilder怎么声明与构造”的相关知识，小编通过实际案例向大家展示操作过程，操作方法简单快捷，实用性强，希望这篇“C++ Cartographer源码中MapBuilder怎么声明与构造”文章能帮助大家解决问题。

开始一条轨迹

添加轨迹是开启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中实现:

// 工厂函数,生成接口APIstd::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_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方法, 依旧只摘取重要的部分程序:

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<PoseGraph3D*>(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<PoseGraph3D*>(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 , common::Time, //返回值为空, 5个参数列表                         transform::Rigid3d ,                         sensor::RangeData ,                         std::unique_ptr<const InsertionResult>)>; //InsertionResult保存了地图的具体数据,栅格值

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

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

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

前三个比较明确,

RangeData定义在range_data.h中:

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的结果

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<PoseGraph3D*>(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<PoseGraph3D*>(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的具体实现:

// 2d的完整的slamstd::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,    const int trajectory_id, mapping::PoseGraph3D* 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::PoseGraph3D>>(      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

关于“C++ Cartographer源码中MapBuilder怎么声明与构造”的内容就介绍到这里了，感谢大家的阅读。如果想了解更多行业相关的知识，可以关注编程网行业资讯频道，小编每天都会为大家更新不同的知识点。