cartographer 处理imu数据流程

cartographer_ros
入口文件：node_main.cc
入口函数main,如下图：

初始化节点：cartographer_node；
Run函数加载配置文件LoadOptions()；根据配置文件处理所订阅的Topic。例如IMU

注意cartographer是否接受imu数据，取决于if判断的条件，这些条件是在“backpack_2d.lua”中所配置的。接着会交给HandleImuMessage函数（定义在sensor_bridge.cc中）处理，如下图：

函数中会对frame_id作一个简单check,然后交给函数trajectory_builder_->AddImuData处理（在trajectory_builder.h中直接实现了），如下图：

而AddSensorData又是一个纯虚函数，它是在继承该父类的子类中去实现的，如下图：

通过搜索知道这个继承关系：class CollatedTrajectoryBuilder : public TrajectoryBuilder 

在这个里面实现了。

而 sensor_collator_->AddSensorData中，  sensor::Collator sensor_collator_;在map_builder.h中定义了。继而找到在collator.cc中找到了实现，如下图：

这里就会把IMU数据加入队列啦。std::unique_ptr是标准库的智能指针，

注意到在collator.h中一个函数：

实现如下：

它在CollatedTrajectoryBuilder的构造函数中调用了，传入的回调函数名为HandleCollatedSensorData，处理整理后的传感器数据，到这里就开始进入关键正题啦！

这个函数会把sensor data的rate获取打印出来，同时对数据类型进行区分处理。继续拿imu数据进行跟踪。wrapped_trajectory_builder_->AddImuData，该类型是：

在头文件cartographer/mapping_2d/local_trajectory_builder.h中，知道GlobalTrajectoryBuilder类继承了GlobalTrajectoryBuilderInterface类，如图：

自然就能找到GlobalTrajectoryBuilder::AddImuData的具体实现了，如下图：

这个函数就意味着地图构建从全局优化转到本地局部优化上来了。

第一步：

上面这个函数卡尔曼滤波根据时间预测pose，不断更新imu数据，设置最大倾斜角阈值20度，超过则2D SLAM will fail。

第二步：图优化

OptimizationProblem类是实现SPA（统计过程调整）闭环方法的。

上面这几个就是很关键的函数，之后再具体分析。

网上相关文章链接：

http://mp.weixin.qq.com/s/LdbFp-Zvkr02-_25ILb16g

http://blog.csdn.net/u010566411/article/category/6467677

http://www.cnblogs.com/wenhust/tag/SLAM/（强烈推荐）

http://blog.csdn.net/zyh821351004/article/details/52421005

http://blog.csdn.net/jsgaobiao/article/details/53116042

http://blog.csdn.net/roadseek_zw/article/details/53316177

论文: 
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters 
博客： 
http://blog.csdn.net/heyijia0327/article/details/40899819 pf原理讲解 
http://blog.csdn.net/u010545732/article/details/17462941 pf代码实现 
http://www.cnblogs.com/yhlx125/p/5634128.html gmapping分析 
http://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search gmapping 分析 
其他 ： 
http://ishare.iask.sina.com.cn/f/24615049.html