关于cartographer做多传感器融合优化的思考（一）

最近打工接触了快一年的2d slam工作，因为先前毫无这方面的知识储备，一直都理解不了为什么cartographer在开启IMU和轮速计数据输输入后，依旧无法在退化环境中工作，按理来说有了IMU和轮速计，完全可以用惯导的思路做位姿推算，但cartographer就是不行，遇到长走廊就呆在原地不动😓。

正好最近也在准备解决这个问题，就看源码完整过了一遍cartographer 2d 建图的工作流程，初步猜测这个问题的原因是cartographer根本就没有充分利用IMU和轮速计的数据做位姿推算，甚至后端都没有任何关于这两个传感器的迹象，简直无大语💧。

下面是我在cartographer中看到涉及到和位姿推算的流程，核心代码主要集中在local_trajectory_builder_2d里，同时涉及到global_trajectory_builder和pose_extrapolator：

IMU和里程计数据更新，一路往下，存入extrapolator_里，并按一定策略对数据进行丢弃；
点云数据更新，数据先进入到global_trajectory_builder中，然后再从AddSensorData到AddAccumulatedRangeData；
AddSensorData中先对点云做各种滤波处理，AddAccumulatedRangeData将处理后的点云传递到前端匹配部分；
前端匹配先使用当前时间作为参数调用extrapolator_.ExtrapolatePose预估当前位姿；
将extrapolator_预估的三维空间pose投影到二维平面；
执行点云自适应体素滤波，并将点云转换到PointCloud类型；
使用预估的二维pose和点云在已有的地图上做匹配，成功后将匹配的二维pose转换回三维空间的pose后传给extrapolator_.AddPose作为下一次推算的先验；
使用匹配的二维pose对点云做坐标变换，并转换成内部的PointCloud类型；
最后将PointCloud和转换的三维pose传入InsertIntoSubmap，更新子图，并返回匹配关系和子图到global_trajectory_builder中更新pose_graph_；

从上面这流程里可以看出，主要就是一个 "预估位姿 -> 点云和地图匹配新位姿 -> 更新先验位姿" 大循环。

预估位姿：基于先验位姿，使用轮速计估算位移，使用IMU估算旋转；

点云和地图匹配：在预估位姿附近做匹配，得到新位姿；

更新先验：将新位姿作为下一次预估的先验值；

问题也就出在点云和地图匹配到更新先验位姿这一步，点云匹配出来的新位姿没有任何处理就直接作为下一次推算的先验值，如果是退化环境或者是别的什么原因导致匹配的结果是错误的，后续全部的位姿也都是错的了。这也是为什么一进到细长走廊cartographer的定位就停在原地的原因，直到雷达能扫描到走廊的另一头才重新恢复移动，但这时候一切都已经错了，还没有任何纠正补救的措施。

想要解决这个问题，切入点我想应该就是匹配到更新先验这里，需要用卡尔曼滤波之类的东西对预估位姿和匹配位姿做一下融合再作为新位姿更新先验值。

不过到底行不行还要等后面做实验验证。