cartographer代码流程整理

代码解析地址
https://zhuanlan.zhihu.com/p/48010119
一、代码目录结构
(1) cartographer_ros

(2) cartographer





二、测试命令
2D：
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=
${HOME}/Downloads/cartographer_paper_deutsches_museum.bag
3D：
roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=
${HOME}/Downloads/b3-2016-04-05-14-14-00.bag
三、cartographer_ros代码分析
a.主函数入口
node_main.cc
main()->Run()
{
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
node.StartTrajectoryWithDefaultTopics(trajectory_options);
::ros::spin();
node.FinishAllTrajectories();
node.RunFinalOptimization();
}

Node::Node()
{
1.注册并发布了5个Topic, 并为5个Topic分别设置了定时器函数，在定时器函数中定期向Topic上广播数据：
map_builder_bridge_.GetSubmapList();
map_builder_bridge_.GetTrajectoryNodeList();
map_builder_bridge_.GetLandmarkPoseList()；
map_builder_bridge_.GetConstraintList()；
map_builder_bridge_.GetTrajectoryStates()；
2.发布了4个Service，并为4个Service分别设置了句柄函数，而句柄函数也是通过调用map_builder_bridge_的成员函数来处理的。
map_builder_bridge_.HandleSubmapQuery()
map_builder_bridge_.AddTrajectory()
map_builder_bridge_.FinishTrajectory()
map_builder_bridge_.SerializeState()
}

b.订阅传感器发布的消息
node.cc
LaunchSubscribers()->HandleLaserScanMessage()->
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg)->
sensor_bridge.cc
->HandleLaserScan()->HandleRangefinder()->
trajectory_builder_->AddSensorData()

::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;

其它传感器调用方式基本相同：
HandleMultiEchoLaserScanMessage()
HandlePointCloud2Message()
HandleImuMessage()
HandleOdometryMessage()
HandleNavSatFixMessage()
HandleLandmarkMessage()

c.建图入口
node.cc
有两处地方调用了AddTrajectory()，该函数是建图的入口函数。
(1)
Node::Node()
{
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
}
HandleStartTrajectory()->AddTrajectory()
{
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, trajectory_id);
}

(2)
node.StartTrajectoryWithDefaultTopics(trajectory_options)
-> AddTrajectory()
{
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, trajectory_id);
}
d.建图过程
node.cc
AddTrajectory()->
map_builder_bridge.cc
AddTrajectory()->map_builder_->AddTrajectoryBuilder()

std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder_;
四、cartographer代码分析
通过MapBuilderBridge和SensorBridge这两个类的过渡，我们正式从cartographer_ros转到了cartographer部分了。从之前部分我们可知，MapBuilderBridge和SensorBridge主要调用了MapBuilderInterface和TrajectoryBuilderInterface这两个类的成员函数来处理。所以，在cartographer部分，我们将从这两个部分入手。
a．建图逻辑
1.当刚初始化一个TrajectoryBuilder时：
初始时，用处发出AddTrajectory的服务请求–>cartographer_node调用MapBuilderBridge::AddTrajectory函数，同时订阅传感器的Topic–>该函数调用MapBuilder::AddTrajectory–>该函数中生成一个LocalTrajectoryBuilder2D,然后通过CreateGlobalTrajectoryBuilder把LocalTrajectoryBuilder2D的参数和回调函数传给GlobalTrajectoryBuilder–>以GlobalTrajectoryBuilder为参数构造了CollatedTrajectoryBuilder,并给CollatorInterface中的AddTrajectory函数注册回调函数的实际地址。
2. 在初始化完成后，每次传感器往Topic上广播Message后：
传感器的ROS节点/Playbag广播到Topic上相关数据的Message---->cartographer_node中启动的StartTrajectory这个服务会订阅传感器数据---->接收到该数据由相应的处理函数处理，比如Node::HandleImuMessage---->该处理函数实际调用是MapBuilderBridge中的一个SensorBridge变量进行处理---->调用了TrajectoryBuilder的虚函数AddSensorData()---->CollatedTrajectoryBuilder或GlobalTrajectoryBuilder继承TrajectoryBuilder并具体实现AddSensorData()函数–>这些函数通过LocalTrajectoryBuilder2D将传感器数据压入相应的数据队列–>LocalTrajectoryBuilder2D通过AddRangeData等函数处理传感器数据。
3. 被动地等待调用AddRangeData来处理队列中的数据
上面这个流程中有传感器数据来临时的做法都是调用了AddSensorData()函数把数据压入了数据队列，那么程序是在什么地方调用AddRangeData来处理队列中的数据的呢？
对于IMU数据和里程计数据都是压入数据队列后被动地等待AddRangeData函数调用PoseExtrapolator来做处理，但当RangeData数据发布时，最后在处理RangeData的AddSensorData中，并不是把数据压入队列，而是在这其中调用了LocalTrajectoryBuilder2D的AddRangeData函数处理。所以，cartographer算法是以激光、点云数据为核心的，IMU、里程计等数据都是起到一定的辅助作用。这就是整个程序的逻辑思路。
b.建图入口
map_builder.cc
std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder_;
map_builder_->AddTrajectoryBuilder()
{
std::unique_ptr local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder = absl::make_unique(
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids));
}
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
trajectory_builders_.push_back(absl::make_unique(
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, pose_graph_odometry_motion_filter)));
}

c.MapBuilder 类
class MapBuilder : public MapBuilderInterface
map_builder_interface.cc
map_builder.cc
MapBuilder中定义了3个重要的变量：
std::unique_ptr pose_graph_;
std::unique_ptrsensor::CollatorInterface sensor_collator_;
std::vector<std::unique_ptrmapping::TrajectoryBuilderInterface>
trajectory_builders_;
其中，trajectory_builders_是一个TrajectoryBuilder的向量列表，每一个TrajectoryBuilder对应了机器人运行了一圈。这个向量列表就管理了整个图中的所有submap。pose_graph_是一个PoseGraph的智能指针，该指针用来做Loop Closure。
sensor_collator_接受传感器数据。
d. CollatedTrajectoryBuilder和GlobalTrajectoryBuilder类
collated_trajectory_builder.cc
global_trajectory_builder.cc
class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterf
ace
e. LocalTrajectoryBuilder2D类
local_trajectory_builder_2d.cc
这个类的主要作用就是局部轨迹的构建。
定义了如下变量：
scan_matching::RealTimeCorrelativeScanMatcher2D
real_time_correlative_scan_matcher_;
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
std::unique_ptr extrapolator_;

f. PoseExtrapolator类
pose_extrapolator.cc
class PoseExtrapolator : public PoseExtrapolatorInterface
该类主要作用是对IMU、里程计数据进行融合，估计机器人的实时位姿。
AdvanceImuTracker()
该函数的主要作用就是取出IMU数据队列中的数据，更新ImuTracker。
g. PoseGraph类
pose_graph.cc
class PoseGraph : public PoseGraphInterface
我们在Local Slam部分已经可过通过一帧帧的Laser Scan建立起了一个个的Submap，接下来，需要Loop Closure来进行回环检测，来消除累积误差。
PoseGraph要优化的就是所有的Submap和所有的TrajectoryNode的绝对位姿。

五．算法流程总结
node_main.cc
main()->Run()
node.cc
HandleStartTrajectory()->AddTrajectory()
map_builder_bridge.cc
AddTrajectory()
map_builder.cc
AddTrajectoryBuilder()
collated_trajectory_builder.cc
global_trajectory_builder.cc
local_trajectory_builder_2d.cc
pose_extrapolator.cc
pose_graph.cc
六、代码阅读笔记

七、cartographer和ROS的坐标系关系
https://www.cnblogs.com/long5683/p/11562823.html
https://blog.csdn.net/xiekaikaibing/article/details/80118134
https://www.cnblogs.com/jiangxinyu1/p/12458699.html
https://www.ncnynl.com/archives/201810/2778.html
https://www.jianshu.com/p/d18096c66d3a