Cartographer的算法执行流程梳理

Cartographer的理论基础

Cartographer涉及到的图优化理论已经有前辈做了详细的描述。可以参考下面的博文。
graph slam tutorial : 从推导到应用1
graph slam tutorial ：从推导到应用２
graph slam tutorial ：从推导到应用3
参考论文：
Real-Time Correlative Scan Matching
Google Cartographer SLAM 原理 （Real-Time Loop Closure in 2D LIDAR SLAM 论文详细解读）这篇博文还对 Cartographer中用到的分支定界算法做了介绍。

Cartographer_ros

图片来源：https://congleetea.github.io/blog/2019/02/20/slam-cartographer-introduction/

上图为一个Cartographer_ros的运行示例。我们分析算法的主要执行流程是从数据流动的方向来分析的。cartographer的ros封装负责了传感器数据的接收预处理和结果数据的发布。

数据输入
node.cc文件中定义了各类传感器数据的接收回调函数。如下所示：

  // The following functions handle adding sensor data to a trajectory.
  void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
                             const nav_msgs::Odometry::ConstPtr& msg);
  void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
                              const sensor_msgs::NavSatFix::ConstPtr& msg);
  void HandleLandmarkMessage(
      int trajectory_id, const std::string& sensor_id,
      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
  void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
                        const sensor_msgs::Imu::ConstPtr& msg);
  void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
                              const sensor_msgs::LaserScan::ConstPtr& msg);
  void HandleMultiEchoLaserScanMessage(
      int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
  void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
                                const sensor_msgs::PointCloud2::ConstPtr& msg);

node.cc文件中的传感器数据处理回调函数都调用了sensorbridge.cc里的函数。sensorbridge.cc中的函数主要是将ros消息的数据格式转成Cartographer包中使用的形式，然后调用Cartographer包的传感器数据处理函数。这样传感器数据就流向了Cartographer包。

数据输出
node.cc文件中的数据发布函数都调用了map_builder_bridge.cc中的函数。

  void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
  void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event);
  void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
  void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
  void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);

map_builder_bridge.cc中的函数将直解调用cartographe包中pose_graph_2d.cc文件的接口，返回结果数据。

  std::map<int /* trajectory_id */,
           ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
  GetTrajectoryStates();
  cartographer_ros_msgs::SubmapList GetSubmapList();
  std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
      LOCKS_EXCLUDED(mutex_);
  visualization_msgs::MarkerArray GetTrajectoryNodeList();
  visualization_msgs::MarkerArray GetLandmarkPosesList();
  visualization_msgs::MarkerArray GetConstraintList();

Cartographer的主要算法流程

https://google-cartographer-ros.readthedocs.io/en/latest/algo_walkthrough.html这篇官方文档对Cartographer的算法做了整体介绍并提供了参数调整的建议。具有重要的参考价值。

上图就是cartographer算法的执行流程。最左边的框框里就是所有传感器数据的输入口。global_trajectory_builder.cc文件中的重载函数AddSensorData就是传感器数据输入的接口。其中最主要的数据流就是激光数据。因为在2d中IMU和里程计数据都是可选的。所以我们看激光数据的流动就可以摸清算法的主要流程。AddSensorData函数中又分了local-slam和global-slam的处理函数。其中AddRangeData函数会将激光数据与子图进行匹配得到激光在当前子图中的位姿。这其实就产生了一个图优化中的节点。而AddNode函数会将前面得到的节点加入到pose-graph中，并进行回环检测和后端优化。

AddRangeData函数在local_trajectory_builder.cc文件中。它主要执行了下面三个步骤：
1）通过位姿插值器对激光数据进行运动畸变去除。
2）然后根据重力向量将数据点投影到平面。
3）调用AddAccumulatedRangeData函数进行scanMatch。
下面一个个步骤来看。

激光数据畸变去除
这里获取一帧激光数据中每束激光的时间戳。将获取到的时间戳代入到位姿插值器中获取对应时间激光器所在的位姿。然后根据各个激光器的位姿，对激光束末端点的坐标进行转换。而位姿插值器中是假设一帧激光数据的时间段内，激光器是匀速运动的。平移的增量就是用线速度乘上间隔时间得到。这里的线速度优先用里程计数据计算的。如果没有里程计就用之前匹配得到的位姿来计算。旋转的插值分两种情况，有IMU数据和没有IMU数据。没有IMU数据时就优先用里程计计算的角速度乘上时间间隔得到旋转增量。没有里程计就用匹配位姿计算出的角速度。有IMU数据就用IMU的数据来计算旋转增量。
这里需要注意一下，里程计的数据、IMU的数据和scanmacter得到的位姿都会更新到PoseExtrapolator中。也正是这些数据的更新才可以使插值器能预测未来某一时刻的位姿。

激光数据投影到平面上
这里主要是靠IMU的数据得到重力向量，如果没有IMU就设定一个虚假的重力向量。利用该重力向量将激光数据点投影到水平面上。并且使用固定的voxlfilter进行滤波，去除冗余的点。

scanmatcher
scanmatcher操作主要在AddAccumulatedRangeData函数中被调用。首先通过插值器给出一个当前时刻的预测位姿。激光数据则再进行一次Adaptive Voxel Filter滤波。将预测的位姿和滤波后的激光数据传入到ScanMatch函数中。ScanMatch函数中会先使用real_time_correlative_scan_matcher得到一个匹配位姿（这一步是可配置是否执行的）。将该位姿传入ceres_scan_matcher中进行最终的匹配。real_time_correlative_scan_matcher中的匹配其实就是设定一个搜索窗口。然后枚举窗口中的每一个位姿，得到每一个位姿的似然得分，返回一个得分最高的位姿（这里还没有用多分辨率地图）。最终匹配的位姿会更新到位姿插值器中。激光点云会转换到估计出来的位姿中．即Submap坐标系中，再将其插入到子图中。

到这里local-slam就完成了。下面的global-slam就在Addnode函数里执行。

实现global-slam的主要函数都在pose_graph_2d.cc文件中。
pose_graph_2d.cc文件中的Addnode函数调用AppendNode函数将匹配得到的节点加入到轨迹和子图中。
然后后台调用ComputeConstraintsForNode()来计算约束。该函数会进行下列几个步骤：
1）计算当前node与插入的子图之间的约束。
2）将当前帧激光数据和以往所有的子图进行匹配，这其实是回环检测了。
3）如果当前节点的加入促使一个新的子图完成。则需将以往所有节点与该子图计算约束。这也可以看成一次回环检测。
计算约束是通过调用ComputeConstraint完成的。该函数中会判断是进行窗口局部搜索还是全子图的搜索。
最终会调用FastCorrelativeScanMatcher2D类中的match函数来进行匹配。fast-CSM使用了多分辨率地图和分支定界算法来加速搜索。其他部分则和realtime-CSM是一样的。约束计算完后会返回是否进行优化的标志位（kRunOptimization和kDoNotRunOptimization）。然后调用HandleWorkQueue()函数。该函数包含了优化操作。
到这里global-slam也就完成了。

总的来说，global-slam这部分的工作就是计算各节点的顺序约束和回环约束。然后调用ceres的优化函数计算最小二乘目标函数。

cartographer配置参数的说明

关注公众号《首飞》回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2，机器人学等机器人行业常用技术资料。