cartographer_node这个节点的所有操作，都交给了MapBuilderBridge的一个对象map_builder_bridge_来完成。因此，接下来我们要来看看在MapBuilderBridge中都进行了哪些操作
MapBuilderBridge定义在/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h中，实现由.../mapbuild_brige.cc完成。
包含以下成员：逐块儿查看一下.
1：跟显示相关。
维持原样就好了。这几个函数也不是MapBuilderBridge的成员函数，而只是一些工具函数
2. 与LandMark相关的
一般参数设置里，use_landmarks = false，所以，landmar这部分功能在目前的代码中是没有起作用的。但cartographer已经考虑了加入landmark的途径。调用map_builder_->pose_graph()->GetLandmarkPoses()可获取Landmark的Pose;
3、构造函数
只是做了一下实例化赋值
4、LoadState函数
调用了map_builder_的成员函数LoadState来加载一个.pbstream文件。map_builder_是接口MapBuilderInterface的实例化对象，而根据是2d还是3d情况，其具体实现会略有不同。
5、AddTrajectory
比较重要的函数：AddTrajectory。添加一条Trajectory
调用了map_builder_->AddTrajectoryBuilder，这已经是cartographer项目中的代码了。也就是说我们快把cartographer_ros的衣服剥的差不多了。可以看到传入的数据有传感器的id，trajectory的一些配置参数，以及一个仿函数对象
这里这个map_builder变量就是cartographer::mapping::MapBuilderInterface的一个实例
其中，MapBuilderBridge::OnLocalSlamResult则如下定义
6. 同样地，接下来的这三个函数也都是调用了map_builder_的成员函数来处理
7. HandleSubmapQuery
处理submap查询的，在cartographer_node中被kSubmapQueryServiceName这个Service调用。
调用map_builder_->SubmapToProto(submap_id, &response_proto);这个函数查询信息，结果放到response这个变量中。
8. GetSubmaplist
这个函数是在往kSubmapListTopic这个Topic上发布数据时被Node::PublishSubmapList调用的，用来获取Submap的列表：
该函数主要也是通过调用map_builder_->pose_graph()->GetAllSubmapPoses()来获取列表信息。
9.GetTrajectoryStates
GetTrajectoryStates用来返回一个TrajectoryStates变量组成的unordered_map这个容器。TrajectoryStates这个结构体的定义如上，见MapBuilderBridge的成员函数。
10.GetTrajectoryNodeList
通过map_builder_->pose_graph()->GetTrajectoryNodePoses()来获取TrajectoryNode的pose的。map_builder_->pose_graph()->constraints()获取Constraints
太长 286到384行 就不放截图了
11.GetConstraintList()
其他跳过
13.sensor_bridge
返回一个SensorBridge的指针。
下面来看一下sensor_bridge这个类
在处理传感器的数据时，都是通过调用map_builder_bridge_的一个类成员sensor_bridge_ptr来处理的。在阅读MapBuilderBridge的源码后我们可以看到跟我们最初的理解略有出入，sensor_bridge_ptr是在MapBuilderBridge中定义的一个SensorBridge类型的局部变量，MapBuilderBridge中一个函数MapBuilderBridge::sensor_bridge可以返回指定trajectory_id的SensorBridge变量。然后在SensorBridge中处理相关传感器的信息。以IMU为例，在cartographer_node中由Node::HandleImuMessage函数处理，该函数实际调用的是map_builder_bridge_->sensor_bridge_ptr->HandleImuMessage。
接下来看一下SensorBridge中具体处理各个传感器的函数
1. 构造函数等
构造函数的参数赋值给成员变量。
但需要注意的是成员变量中有一个TrajectoryBuilderInterface型的一个指针变量。继续跟踪代码我们可以发现，cartographer中各种消息都统一调用了这个成员类的虚函数AddSensorData().
实际上是虚函数加重载
而CollatedTrajectoryBuilder继承了这个类并实现了AddSensorData()函数。这两个类都定义在cartographer中的mapping文件夹下。CollatedTrajectoryBuilder的构造函数说明通过统一调用HandleCollatedSensorData()函数，来轮询处理kImu(IMU消息)、kRangefinder(测距消息，不仅仅是激光)、kOdometer(里程计消息)等。所以，我们到后面可以再看这两个类
2. 处理里程计的函数
一个预处理的工具函数，并非SensorBridge的成员变量；
需要注意的是TfBridge这个类，他的实例化对象是tf_bridge_，tf_bridge_是SensorBridge的一个成员变量。我们通过查看TfBridge可以看到，作者在设计的时候，通过TfBridge把不同传感器的差异进行了一下屏蔽，不管什么传感器，都会对每一帧数据的位姿进行估计，而这个历史的位姿数据就以TfBridge的形式存起来。这样在使用的时候也可以通过TfBridge查询某一个传感器在某一个历史时刻的位姿。
这里同样也是，代码通过TfBridge查询了一下历史数据（54行：const auto sensor_to_tracking = tf_bridge_.LookupToTracking(）），然后把这个作为参数传给了TrajectoryBuilder的AddSensorData来做处理（68行： trajectory_builder_->AddSensorData(））。前面我们已经介绍过了，TrajectoryBuilder也是为不同的传感器提供了统一的处理接口。我们可以在TrajectoryBuilder的具体实现里再看具体做了什么操作。但我们可以合理猜测，比如，里程计的数据是在原来数据的基础上再做一个累加，作为新的值同样保存到TfBridge里。
以里程计数据为例，我们可以梳理一下传感器数据整个的处理流程：
传感器的ROS节点/Playbag广播到Topic上相关数据的Message---->cartographer_node中启动的StartTrajectory这个服务会订阅传感器数据---->接收到该数据由相应的处理函数处理，比如Node::HandleImuMessage---->该处理函数实际调用是MapBuilderBridge中的一个SensorBridge变量进行处理---->调用了TrajectoryBuilder的虚函数AddSensorData()---->CollatedTrajectoryBuilder继承TrajectoryBuilder并具体实现AddSensorData()函数
3、4、分别处理GPS和IMU的数据 和上面类似，跳过
5、处理激光、多激光、点云数据
初看似乎没有调用trajectory-->AddSensorData函数。但仔细研究会发现他们之间彼此存在调用关系。最根上是HandleRangefinder这个函数：这个函数调用了trajectory_builder_->AddSensorData来处理， 如：
网友评论