cartographer Code Reading
flow chart:
Some concepts: submaps, be understood as: a map is a mosaic formed gradually, a map is a submap.
Laser data (range data) filtered through a voxel (Voxel Filter) and the adaptive filter matches the current submap (submap). The matching process requires odometer (Odometry Pose) provides an initial position (PoseEstimate). Determining an initial position of the odometer can be used alone, it may be used odometer and gyroscopes (an IMU, if not an IMU, odometer can only use) fused manner. If the odometer has not it? Laser can be used directly with the map matching (relatively complicated algorithm). ( Insertion, while interposing two active submaps. Each time two submap is active, and the current submap a submap ).
After the laser matching, ready to be inserted submap, but not all post-match laser must insert submap, only the movement for some time after the laser is inserted after the match or, for example, you need a sports filter (Motion Filter), does not meet the filter movement the device will be discarded (dropped).
The above process can be understood as partially SLAM, or a front end: the process of collecting sensor information, the map is formed, the following describes the basic idea of the global optimization
After partial SLAM formation results InsertionResult, including rangde_data, the so-called node and the current submaps; the results into constraints (constraints) pose_graph further computing node and submap, and other constraints (odom, landmark), and according to the parameter , options_.optimize_every_n_nodes () decide to run a global optimization when.
The basic structure of the code:
mapBuilder: implement the entire map building, including front-end and back-end local slam global slam.
轨迹（trajectory): 可以理解为一次SLAM 从起点到终点过程中的机器人行走轨迹，建图中可以通过startTrajectory和finishTrajectory控制。在轨迹生成的过程中，完成sensor到sumap的生成，以及pose_graph的构建。TrajectoryBuilder(globalTrajectory类)主要通过sensor_collator(localTrajecory类)和pose_graph构成 。sensor_collator实现局部地图构建，最终结果传递给pose_graph；
节点图(poseGraph)：具体参考图优化的知识。简单理解图优化（如果没接触过图优化，下面估计看不懂，后续会详细讲）：每个插入的激光和生成sumap，以及landmark（后续讲）都可以理解为图优化的一个节点，建图过程中，生成这些点之间的关系，这些关系便是图中的线，最终优化，就是调整点的位置，得到最优值。可以理解为PoseGraph主要实现全局优化（global slam）功能。
代码流程：最终代码运行通过ROS node 方式实现。node中对应topic和service订阅和发布等功能通过MapBuilderBridge类实现。MapBuilderBridge顾名思义，实现ROS节点代码和cartographer功能代码之间的桥接，也可以理解为对cartographer主体代码的接口封装。cartographer主体代码主要功能通过构建地图MapBuilder类实现。此外，MapBuilderBridge 还包含sensorBridge类，实现传感信息ros格式和cartographer自定义格式之间的转换，这些传感器主要包括scan,imu,odom,tf 等。
MapBuilder 类源码注释：
// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure.
// 人话：把局部SLAM中submap，node等连接起来，形成 PoseGraph,以便回环优化。
class MapBuilder : public MapBuilderInterface{}总结：
整个SLAM 过程主要通过TrajectoryBuilder 完成，实现sensor到submap的构建，以及pose_graph 的构建，最后通过pose_graph 实现最终全局优化（finalOptimization)。
其它代码说明：
mapping ：实现建图功能
以下四个文件夹都是mapping中要用到的库函数。
common: 时间戳处理，数据统计，参数读取，线程池构建，任务队列等。
io： 读写文件，读写地图，数据类型（proto，pcd，地图grid）转换等。
sensor： range（激光），cloud，imu，odom数据类型封装。
transform： 坐标系转换。
scan处理流程
从scan处理过程理解代码结构。采取至上而下的方式。为了便于理解，以下代码中，删去了大部门源码，仅仅留存我们需要关注的部分。
cartographer_ros/node.cc
// ros node中处理订阅到的激光数据
void Node::HandleLaserScanMessage(const sensor_msgs::LaserScan::ConstPtr& msg) {
map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLaserScanMessage(sensor_id, msg);
}cartographer_ros/sensor_bridge.cc
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud)
{
carto::sensor::TimedPointCloud ranges getfrom(points);
HandleRangefinder( ranges)
{
//trajectory_builder_：通过CollatedTrajectoryBuilder实现
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
carto::sensor::TransformTimedPointCloud(ranges)});
}
}
}cartographer/mapping/internal/collated_trajectory_builder.cc *****
class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface{
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
}
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
// sensor_collator_:通过Collator实现
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}cartographer/sensor/internal/collator.cc
void Collator::AddSensorData(const int trajectory_id,
std::unique_ptr<Data> data) {
QueueKey queue_key{trajectory_id, data->GetSensorId()};
queue_.Add(std::move(queue_key), std::move(data));
}cartographer/sensor/internal/Ordered_multi_queue.cc
void OrderedMultiQueue::Add(const QueueKey& queue_key,
std::unique_ptr<Data> data) {
auto it = queues_.find(queue_key); // 插入数据
it->second.queue.Push(std::move(data)); // 并处理 通过call_back 函数处理数据
Dispatch();//调用一次Add()就要调用一次Dispatch()
}接下来，寻找处理数据的函数call_back 具体实现形式。从下往上回溯。
由于call_back 函数是在collator.cc 中Collator类的函数AddTrajectory定义的。
cartographer/sensor/internal/collator.cc
void Collator::AddTrajectory(
const int trajectory_id,
const absl::flat_hash_set<std::string>& expected_sensor_ids,
const Callback& callback) { // call_back 函数在此引入
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = QueueKey{trajectory_id, sensor_id};
queue_.AddQueue(queue_key,
[callback, sensor_id](std::unique_ptr<Data> data) {
callback(sensor_id, std::move(data));
});
queue_keys_[trajectory_id].push_back(queue_key);
}
}cartographer/mapping/internal/collated_trajectory_builder.cc *****
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator),
collate_landmarks_(trajectory_options.collate_landmarks()),
collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) {
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_id_strings,
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data)); // call_back 函数在此实现
});
}
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}
// 到此，对data的处理过程： call_back 调用HandleCollatedSensorData ，再调用AddToTrajectoryBuilder ， 传入 wrapped_trajectory_builder_
wrapped_trajectory_builder_ 的具体实现形式是在初始化CollatedTrajectoryBuilder过程中声明的。
也就是在定义trajectory_builder_ 时，定义的。trajectory_builder_ 定义在cartographer_ros/sensor_bridge.cc 中。 具体通过 map_builder.cc 实现：
int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback) {
....
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D( // call_back函数的具体形式在此定义
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback)));
....
}到此，基本完成了从下到上对call_back 函数的最终，最后，通过CreateGlobalTrajectoryBuilder2D，由GlobalTrajectoryBuilder2D类实现。
总结
从ros node 在调用函数处理scan信息，最终压入队列queue，该过程中，同时定义了queue的call_back 函数，具体实现流程如下：
从node.cpp -> map_builder_bridge.cc(sensor_bridge <- map_builder) -> collated_trajectory_builder.cc ->collator.cc -> queue.cc
主要涉及的变量： map_builder_bridge <- trajectory_builder < queue_
同样的，对于imu和odom里程计数据，处理方式相似。接下来就需要研究这个核心类。看看scan怎么处理的。
参考上面程序 cartographer/mapping/internal/collated_trajectory_builder.cc，可知，处理data的方式，通过
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()) 实现，这里的wrapped_trajectory_builder_ 即为 GlobalTrajectoryBuilder 类型。
sensor/internal/dispatchable.h
class Dispatchable : public Data {
public:
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}显然，data的处理方式是通过GlobalTrajectoryBuilder -> AddSensorData(...)函数来实现的。
cartographer/mapping/internal/global_trajectory_builder.cc
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph3D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback) {
return absl::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback);
}
// 因此组成global_trajectory_builder主要有三个变量：
// 1. local_trajectory_builder
// 2. pose_graph
// 3. local_slam_result_callback
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
matching_result = local_trajectory_builder_->AddRangeData(
sensor_id, timed_point_cloud_data);
auto node_id = pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps);
insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
node_id, matching_result->insertion_result->constant_data,
std::vector<std::shared_ptr<const Submap>>(
matching_result->insertion_result->insertion_submaps.begin(),
matching_result->insertion_result->insertion_submaps.end())});
}
if (local_slam_result_callback_) {
local_slam_result_callback_(
trajectory_id_, matching_result->time, matching_result->local_pose,
std::move(matching_result->range_data_in_local),
std::move(insertion_result));
}
}
// 通过该函数我们知道，对激光信号处理主要经过了四个过程（可以与流程图对照）：
// 1. local_trajectory_builder_ addRangeData() 结合里程计数据进行激光匹配
// 2. 把匹配结果加入全局优化图pose_graph 中
// 3. 插入当前局部地图submap中
// 4. 执行 local_slam_result_callback 函数。 local_slam_result_callback 在调用CreateGlobalTrajectoryBuilder2D 时候定义。CreateGlobalTrajectoryBuilder2D在map_builder_调用，map_builder 被 map_builder_bridge 调用。 最终，local_slam_result_callback的定义在map_builder_bridgecartographer_ros/map_builder_bridge.cc
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)});
local_slam_data_[trajectory_id] = std::move(local_slam_data);
}
// 最终把局部slam结果传到local_slam_data_ 这个变量中，便于用户调阅查看。以上便事local_slam的过程。
pose_graph
pose_graph map_builder.cc variables are defined, responsible for global optimization problem (back-end) global optimization process with the front local_slam, at the same time, achieved through multi-threaded approach can be simply understood as:.. map_builder in the scan data to establish acceptable local_trajectory_builder map building process, the while building global optimization (optimization_problem) corresponding thread, back-end optimization.
cartographer/mapping/internal/2d/pose_graph_2d.h
PoseGraph2D(
const proto::PoseGraphOptions& options, // 配置参数
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,//全局优化主体
common::ThreadPool* thread_pool // 处理全局优化的线程池(可以是一个线程或多个线程)
);Simple look, thread pool to use:
cartographer/common/thread_pool.h
// 下面代码为源码简化版
class ThreadPool : public ThreadPoolInterface {
public:
explicit ThreadPool(int num_threads);//初始化一个线程数量固定的线程池。
std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task) //添加想要ThreadPool执行的task，
// 插入tasks_not_ready_,如果任务满足执行要求，直接插入task_queue_准备执行
LOCKS_EXCLUDED(mutex_) override;
private:
void DoWork();//每个线程初始化时,执行DoWork()函数. 与线程绑定
std::deque<std::shared_ptr<Task>> task_queue_ GUARDED_BY(mutex_); // 准备执行的task
task可能有依赖还未完成
GUARDED_BY(mutex_);
};
// 线程池初始化时,每个线程与DoWork()函数绑定,也就是线程在后台不断执行DoWork()函数.
// DoWork()函数负责处理 task_queue_ 内的task(函数)
// task 通过 ThreadPool::Schedule 函数压入.
//总结: ThreadPool初始化后,通过Schedule 压入内部任务队列,并安装队列顺序执行任务.After understanding the above, you can enter the pose_graph see how optimization_problem for background execution by the ThreadPool.
// AddNode的主要工作是把一个TrajectoryNode的数据加入到PoseGraph维护的trajectory_nodes_这个容器中。并返回加入的节点的Node. 在global_trajectory_builder.AddSensorData()中调用.
NodeId PoseGraph2D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {e
// 生成的optimized_pose就是该节点的绝对位姿
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
const NodeId node_id = trajectory_nodes_.Append(
trajectory_id, TrajectoryNode{constant_data, optimized_pose});
// 节点数加1.
++num_trajectory_nodes_;
const SubmapId submap_id =
submap_data_.Append(trajectory_id, InternalSubmapData());
submap_data_.at(submap_id).submap = insertion_submaps.back();
// 检查insertion_submaps的第一个submap是否已经被finished了。
// 如果被finished，那么我们需要计算一下约束并且进行一下全局优化了。
// 这里是把这个工作交到了workItem中等待处理(通过线程池thread_pool处理)
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) { //该部分用到lamda表达式结果作为addWorkItem()输入参数
ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
// 上述都做完了，返回node_id.
return node_id;
}
// 该函数实现 node 和 submaps 插入 pose_graph, 并把优化工作交给线程池处理.
// 我们需要进一步查看 AddWorkItem 和 ComputeConstraintsForNode;
void PoseGraph2D::AddWorkItem( // std::function<outType(inType)> f
const std::function<WorkItem::Result()>& work_item) {
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_ == nullptr) { //当work_queue_为空时... 由于work_queue_ 大部分时候非空,为了先看基本逻辑,该调节暂且认为不成立.所以,AddWorkItem函数主要执行 下面push_back部分
work_queue_ = absl::make_unique<WorkQueue>();
auto task = absl::make_unique<common::Task>();
task->SetWorkItem([this]() { DrainWorkQueue(); });
thread_pool_->Schedule(std::move(task));
}
const auto now = std::chrono::steady_clock::now();
work_queue_->push_back({now, work_item}); // AddWorkItem 每次执行主要部分
}
// 通过以上两个函数,可知,每次addNode时,会把ComputeCocnstraintsForNode(计算约束)函数压入work_queue_. 到此,可以猜测work_queue_的处理函数一定被塞入线程执行. 因此,我们下一步寻找,被压入线程池执行的函数(task)
//work_queue_ = {{time1,ComputeCocnstraintsForNode1},{time2,ComputeCocnstraintsForNode2}, ... }
// 通过搜索,发现,仅有一处thread_pool_->Schedule(task),压入的函数为: DrainWorkQueue()
// Process pending tasks in the work queue on the calling thread, until the
// queue is either empty or an optimization is required.
void PoseGraph2D::DrainWorkQueue() {
bool process_work_queue = true;
size_t work_queue_size;
while (process_work_queue) {
std::function<WorkItem::Result()> work_item;
{
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_->empty()) {
work_queue_.reset();
return; // work_queue_ 空的时候, thread_pool中DrainWorkQueue执行完,等待下一次addNode时候,再次执行thread_pool_->Schedule(task),再次执行.
}
work_item = work_queue_->front().task;
work_queue_->pop_front();
work_queue_size = work_queue_->size();
}
process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
// 执行work_item() 也就是ComputeCocnstraintsForNode 并判断是否需要运行优化
// 如果需要需要优化,则work_item() = WorkItem::Result::NotRunOptimization, while退出,
// 如果work_item()都不需要全局优化,则直到work_queue_为空,下面的优化函数不会执行.
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
// We have to optimize again. when constraint_builder done
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder2D::Result& result) {
HandleWorkQueue(result);// 执行全局优化.
});
}
总结:我们已经明白全局规划的程序思路. 从addNode开始.每次addNode后,会把ComputeCocnstraintsForNode以workItem的形式,压入work_queue_中,交给DrainWorkQueue依次执行.当没有任务时(比如初始化时),DrainWorkQueue作为task的形式被压入到thread_pool中进行处理. 最后,当需要执行全局优化时(这个调节可以由ComputeCocnstraintsForNode结果控制),进行全局优化(HandleWorkQueue).