cartographer 处理imu数据流程

最新推荐文章于 2024-07-28 11:11:59 发布

最新推荐文章于 2024-07-28 11:11:59 发布

阅读量6.4k

点赞数 4

注意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

关注