cartographer源码解析(1)

本文的目的是记录阅读cartographer的整个过程，第一步是确定其整体框架，对于具体的技术细节暂时不会深入了解。这里我专注于输入数据是如何被处理的，跟着数据走，同时借助于doxygen进行分析。

入口在node_main.cc，通过入口我们可以看到其调用了SensorBridge类，在sensor_bridge.cc中会看到对数据的处理，这里以LaserScan为例，在HandleLaserScanMessage()函数中调用laser.cc中的ToPointCloud()函数将LaserScan信息转换为点云信息。转换原理是，根据某个点的距离信息和这个点与angle_min的夹角进行计算，用到的参数有angle_min，angle_increment和ranges。当然，这个点云信息其实是伪点云信息，因为默认激光的扫描是水平的。然后在TransformAndFilterLaserFan(local_trajectory_builder.cc)中对生成的点云数据进行过滤（此时已经进入cartographer中了）。Local_trajectory_builder.cc 里有个函数Predict()，是整个track过程的第一步，用IMU数据对位姿进行初步估计。如果传感器还有odometry，结合其数据再对pose信息进行估计。如果laser的垂直角度变化超过20度，fail。然后在Local_trajectory_builder.cc里的ScanMatch中对传感器坐标进行估计，并对点云信息进行过滤（应该是考虑到计算成本问题，并且一个voxel只能有一个点）。

这里有一个关键点是，尽管已经计算出点云了，我们的laser信息到目前为止实际上还没有派上用场。

所以下一步进入ScanMatch函数(Local_trajectory_builder.cc)，首先real_time_correlative_scan_matcher_.Match函数实时scan与scan进行匹配，滑动窗口对不同pose下的scan进行匹配，然后选出得分最高的那个scan，对pose进行优化(TODO:　具体怎么做的还需要详细参详源码)。

然后就可以进行局部优化了，直接把网格信息probability_grid/最早估计的位姿信息previous_pose／经过上一步优化过的位姿信息initial_pose_estimate／点云信息point_cloud扔进ceres求解器进行优化(ceres_scan_matcher.cc)，得到优化后的位姿信息。同时获得下一步需要的卡尔曼滤波器系数covariance_observation_2d。再将相对位姿信息转换为绝对位姿信息。如果位姿变化过大，就判定这次的位姿估计是无效的。然后更新submaps_。

奇怪的是，在这里没有只看到计算卡尔曼滤波器的系数了，没看到用卡尔曼滤波器再估计一次位姿，应该是理解有误，还需要进一步理解。

然后现在看global_trajectory_builder.cc，AddRangefinderData() 转sparse_pose_graph.cc的AddScan()。这个函数的重点在ComputeConstraintsForScan()函数，这个函数进行回环检测。在RunOptimization()中进行全局优化。

到此，基本上整体上走了一遍了。

现在再来看数据输出的情况，对于submap_list，在map_builder_bridge.cc中GetSubmapList()函数，观察函数可知submap_version是end_laser_fan_index，而pose其实是submap_transform而非绝对值。

还有比较重要的tf，在node.cc里的PublishTrajectoryStates可以找到tf的广播函数。那么pose从哪儿来的呢，可以在map_builder_bridge_.GetTrajectoryStates()，也就是map_builder_bridge.cc中的GetTrajectoryStates()函数中找到相关信息。

至此，源码的第一遍算是差不多了，后续会对某些核心算法部分结合论文进行更加详细的剖析。不过水平实在不行，还有很多地方不知道作者的意图。