Cartograoher算法学习与源码阅读（1）

Cartographer:

算法的基本思路：

Scan是激光扫描的单帧数据，通过累加Scan来构建局部地图（Submap）。Cartographer的实现并没有采用滤波方法，而是采用了Graph-based SLAM的模型进行Pose estimation，具体的实现是用了Ceres scan matching(Ceres是Google自家的库) 。

这样构造出来的很多很多submap是会产生累计误差的，

利用SPA方法优化scan和submap的pose。

通过Loop closing(方法是pixel-accurate scan matching)来消除这些误差，完成闭环。

 

ROS部分的基本流程：

Cartographer的总框架流程图为：

    分为本地SLAM和全局SLAM

Voxel Filter：体素滤波器,对每一个体素voxel,采用第一个point代替所有的points

Adaptive Filter:自适应体素滤波

 

cartographer代码结构：

common：定义了基本数据结构以及一些工具的使用接口。

sensor：定义了雷达数据及点云等相关的数据结构。

transform：定义了位姿的数据结构及其相关的转换。

kalman_filter:

主要通过kalman滤波器完成对IMU、里程计及基于雷达数据的估计位姿的融合，进而估计新进的laser scan的位姿。

状态量 15维

里程计和雷达数据是观测量

利用IMU数据预测里程计和雷达数据，与里程计与雷达数据的真实测量数据做差，利用最小方差补偿IMU15维的状态量。

 

mapping：定义了上层应用的调用接口以及局部submap构建和基于闭环检测的位姿优化等的接口。

mapping_2d和mapping_3d：对mapping接口的不同实现。

 

程序详细流程：

1.Cartographer_node从雷达，IMU及里程计中获取原始数据，在C++中的数据接口为mapping/global_trajectory_builder_interface.h这是一个抽象类，定义了不少纯虚函数

2.  mapping_2d/globle_trajectory_builder这个类作为上面接口的派生类，继承了上述原始数据。

（1）AddImuData用于接收处理上层应用传递的IMU数据。

（2）AddOdometerPose用于接收处理上层应用传递的里程计数据。

（3）AddHorizontalLaserFan用于接收处理上层应用传递的雷达数据。并在本地SLAM完成以后将数据传入全局SLAM中，具体函数是sparse_pose_graph_->AddScan

 

3.  然后原始数据在local_trajectory_builder中被处理，主要是构建局部地图submap。

（1）      首先需要做一个pose tracker（位姿跟踪器），目的是计算scan的方向与位置

（2）  BuildProjectedLaserFan将scan的点云投影至地面并进行体素滤波

 

（3）  ScanMatch

① 自适应体素滤波 ② ceres_scan_matcher_.Match利用ceres库非线性优化

Ceres的初始位姿值为位姿预测值的地平面投影。Probability_grid概率网格

4.  ceres_scan_matcher_.Match分析

构造约束条件，然后调用ceres库完成优化。

方法：双三次线性插值法。

5.  如果该scan被成功插入到某个submap中，那么该scan被插入后的相关信息则被传递给sparse_pose_graph_对象用于基于闭环检测的全局位姿优化

 

 

程序中的一些关键的数据结构：

1． Rigid2——Rigid2是对二维刚性变换【旋转与平移】的封装,SE2。

Rigid2表现为先将vector[x0,y0]旋转θ角度得到[x',y']，然后再叠加[dx,dy]得到[x1,y1]。即:x1=x'+dx,y1=y'+dy。

2． Rigid3——Rigid3是三维刚性变换.SE3。