cartographer源码分析

     

重新捋一捋cartographer的代码：

一、cartographer的代码分为两部分：ROS中的cartographer_ROS包也是调用了cartographer的算法，cartographer的代码分为两部分：

代码从博客https://www.cnblogs.com/hitcm/p/5939507.html中的github下载：

（1）https://github.com/hitcm/cartographer.git

（2）https://github.com/hitcm/cartographer_ros.git

涉及主要论文：

          1、 Real-Time Loop Closure in 2D LIDAR SLAM , ICRA 2016

          2、Efficient Sparse Pose Adjustment for 2D Mapping (SPA：Sparse Pose Adjustment)

          3、 Real-Time Correlative Scan Matching (BBS：Branch-and-bound scan matching)

二、从cartographer_ROS进入cartographer算法：

在cartographer_ros中：

                  main()——》Run()——》node.Initialize()——》HandleSensorData()——》AddHorizontalLaserFan(cartographer_ros)——》AddHorizontalLaserFan()进入cartographer算法

 

三、在cartographer中，AddHorizontalLaserFan()函数（global_trajectory_builder.cc） 分为两大步：

（1）进行激光帧插入 前端匹配，调用流程：

——》AddHorizontalLaserFan()——》ScanMatch()——》real_time_correlative_scan_matcher_.Match（）

                                                                                  ——》ceres_scan_matcher_.Match（）

（2） 插入成功则进行回环检测和后端优化  ，就是SPA，调用流程：

——》AddScan()——》ComputeConstraintsForScan()——》MaybeAddConstraint()——》ComputeConstraint（）

                   ——》fast_correlative_scan_matcher->Match——》MatchWithSearchParameters（）——》BranchAndBound()

 

void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
    const common::Time time, const sensor::LaserFan3D& laser_fan)
{
  //1、进行激光帧插入 前端匹配
  std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
      local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan); //前端匹配——帧节匹配的过程

  //2、SPA  插入成功则进行回环检测和后端优化
  if (insertion_result != nullptr)
  {
    sparse_pose_graph_->AddScan(
        insertion_result->time,                                             //激光帧的时间
        insertion_result->tracking_to_tracking_2d,                          //把激光数据转换到平面转换矩阵
        insertion_result->laser_fan_in_tracking_2d,                         //平面坐标系中的激光数据
        insertion_result->pose_estimate_2d,                                 //滤波器估计出来的机器人最新位姿
        kalman_filter::Project2D(insertion_result->covariance_estimate),    //滤波器估计出来的机器人位姿的方差
        insertion_result->submaps,                                          //所有的submap
        insertion_result->matching_submap,                                  //本次用来进行scan-match的submap
        insertion_result->insertion_submaps);                               //插入了激光数据的submap 就是submap(size-1) 和 submap(size-2)
  }
}

四、先进入前端匹配AddHorizontalLaserFan()

（1）进入ScanMatch（分为4步）：

1、得到ukf预测的位姿和协方差, 相当于滤波器中的预测位姿
2、在ukf的预测位姿的基础上，通过scanmatch匹配的位姿叫做观测位姿。     
    ScanMatch()函数内部会更新滤波器，滤波器更新完毕之后，得到机器人的最新的估计位姿和方差
void LocalTrajectoryBuilder::ScanMatch(*)
{
   //帧节匹配的流程分为4个步骤：
  //1、得到一个地图。         用来进行scan-match对应的submap