这篇文章主要介绍“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中实现:
// 工厂函数,生成接口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_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<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 /* 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<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的完整的slam
std::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怎么声明与构造”的内容就介绍到这里了，感谢大家的阅读。如果想了解更多行业相关的知识，可以关注云搜网行业资讯频道，小编每天都会为大家更新不同的知识点。