cartographer 代码思想解读(11)- 顶层map_bulider类解析
从本博客开始分析cartographer中的后端代码,无闭环的SLAM的算法代码和思想已基本分析完成。本节开始首先引入顶层接口类map_bulider类,实际上SLAM的顶层接口,并由cartographer_ros调用,不详细分析,主要是可说明cartographer的主要成员,包括前面分析的local slam。
map_builder成员
const proto::MapBuilderOptions options_; // 配置信息
common::ThreadPool thread_pool_; // 线程
std::unique_ptr<PoseGraph> pose_graph_; // 用于闭环, 包括ID,位置, 约束, 即每个submap的相关信息,所有submap一起进行闭环
std::unique_ptr<sensor::CollatorInterface> sensor_collator_; //Sensor 收集器
std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
trajectory_builders_; // trajectory build 序列, 每个元素可认为是维护了一个submap
// 即整个vector则维护了所有的submap
std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
all_trajectory_builder_options_; // 每个trajectory 对应的 配置信息
all_trajectory_builder_options_:所有轨迹信息的配置信息
sensor_collator_:用于收集传感器数据的。
trajectory_builders_: 前端核心类,即local slam的维护类,其维护了前端SLAM中的轨迹。这里需要指出不一定仅有一条轨迹,cartographer的高明之处。但一般情况下一次SLAM即仅有一条轨迹。
pose_graph_: 后端核心关键类;用于后端闭环检测和优化实现类。
thread_pool_:是线程池,可采用多线程并行处理,主要在后端中使用。
其中trajectory_builders_即是前10部分分析的前端slam内容。pose_graph_则为另一个最为重要的类,后端优化类。
MapBuilder构建函数
// 构建函数
// 包含两个全局类
// pose_graph_: 用于闭环;
// trajectory_builders_: 用于local slam
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.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_);
}
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_);
}
if (options.collate_by_trajectory()) {
sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
} else {
sensor_collator_ = absl::make_unique<sensor::Collator>();
}
}
MapBuilder构建函数中主要构建了pose_graph_和sensor_collator_两个类,其中pose_graph_分为2d和3d两种配置。sensor_collator_是用来收集所需要传感器类型的的封装类,用来存储关心的传感器类型数据并村放入queue缓存。
其中sensor_collator_也包括两类,根据配置进行选择,但是cartographer_ros的configurtion中没有进行指示定义,在cartographer 中的configurtion_file中的mapbuilder.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,
}
表明默认为false。
关键核心函数AddTrajectoryBuilder
AddTrajectoryBuilder是mapbuilder中最为关键的接口函数,其主要功能是创建一个新的轨迹,即SLAM中维持的一条的连续轨迹,依据此轨迹进行记录相关信息(牛逼的地方,可创建多条,暂时未用过,估计可用于地图扩展)。即用于localslam。
int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback)
接口包含三个内容,
expected_sensor_ids:期望的传感器种类,即仅处理期望的sensor data,如laser、imu等,其他的不关心;
trajectory_options:轨迹的相关配置信息
local_slam_result_callback:local slam结果返回回调函数
// 3d
if (options_.use_trajectory_builder_3d()) {
// 省略
} else {
// 2d 处理, 构建local规划器,没有闭环能力,实际应该是每个submap的local slam
// 仅选择距离传感器描述信息,用于slam
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids));
}
// 定义闭环节点
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
// 将每个局部规划器,放入队列中, 包含规划器描述,需要校准的sensor, 当前id, 期望的sensor类型
// 每个局部规划器需create 全局规划器,局部slam采用回调方法
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback)));
}
根据2d和3d slam分成两部分处理,这里仅分析2d情况。
其核心便是构建一个LocalTrajectoryBuilder2D即轨迹跟踪器,前面10节分析的内容均是LocalTrajectoryBuilder2D功能,包括位置估计、相关匹配、优化匹配、submap构建以及更新等,即完成了除闭环优化外的所有功能。从代码可看出,即接口仅是配置信息和传感器信息。
而trajectory_builders_则是维护和存储所有轨迹类,包括配置信息,trajectory_id,传感器类型,和全局轨迹跟踪器。其中CreateGlobalTrajectoryBuilder2D为local slam与后端优化结合的桥梁,下节将详细分析。
//判断是否为纯定位模式,如果是,内部可以不用闭环处理
MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
pose_graph_.get());
cartographer也支持纯定位模式,因此如果是纯定位模式模式,后端处理经会有一定区别,即无需闭环处理。
if (trajectory_options.has_initial_trajectory_pose()) {
//获取配置中的初始位置
const auto& initial_trajectory_pose =
trajectory_options.initial_trajectory_pose();
// 闭环pose_graph_设置初始位置,即当前ID,与初始ID的位置约束
pose_graph_->SetInitialTrajectoryPose(
trajectory_id, initial_trajectory_pose.to_trajectory_id(),
transform::ToRigid3(initial_trajectory_pose.relative_pose()),
common::FromUniversal(initial_trajectory_pose.timestamp()));
}
如果该轨迹有初始pose,应当获取给定的初始位置,初始位置主要用于建图和闭环中作为初始固定位置使用。而slam中其所有节点以及所有submap的位置均是相对初始位置的相对位置,而优化也是将相对位置进行优化。
// 定义所有期望的即设置的 sensor种类配置转换成proto
proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
for (const auto& sensor_id : expected_sensor_ids) {
*options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
}
// 轨迹配置和传感器配置融合
*options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
trajectory_options;
// 将配置内容放入规划器队列配置中
all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
return trajectory_id;
将传感器及其配置信息进行序列化,进行存储,最后返回新建立trajectory_id。由代码可看出,轨迹可能不止一个,因此每个轨迹需要存储自身的配置信息,故创建一个新的轨迹时,判断下配置信息和轨迹的个数是否相等。
FinishTrajectory 完成并结束一条轨迹
void MapBuilder::FinishTrajectory(const int trajectory_id) {
sensor_collator_->FinishTrajectory(trajectory_id);
pose_graph_->FinishTrajectory(trajectory_id);
}
此方法是针对某一轨迹实现轨迹结束命令,即trajectory_id的轨迹不再增加的新的节点,也就是不再继续更新,也可以认为是此轨迹的slam结束。如此此轨迹的不再接收新的传感器信息,同时后端节点也将停止增加。
其他函数接口
和slam的算法基本无关,主要是存储和读取的功能。其目的是可将slam的中间数据进行存储,同时也可从文件中读回了。
总结
本节内容,没有算法本质内容,主要是引入轨迹的概念,因为后续的闭环代码中trajectory会一直存在。正常情况下的建图,仅有会创立一个轨迹,直到slam结束。已有地图时进行纯定位时,可完成关闭当前轨迹,新创建一条新的轨迹进行定位。同时也可创建一条新的轨迹扩展已有的地图。
同时本节中引入了关键核心类pose_graph_,即后端优化处理,前端SLAM过程中产生的节点信息和submap信息都将传递给pose_graph_,实现整个SLAM过程。