Month: May 2025

在cartographer系统中，激光点云作为定位和建图的核心数据，在系统中不同的类对象中的流转和变换(包括坐标变换和数据类型的变换)是最频繁和复杂的，这篇文章将向大家梳理一下cartographer系统中多种类型传感器数据的统一预处理，包括传感器数据类型从标准ROS消息到cartographer内部处理所需数据类型的转换，消息多队列缓存和分发派遣机制等，如对本文理解和分析的过程有疑问或发现有问题的地方欢迎联系作者讨论并校正，谢谢。 1、首先在cartographer_ros的节点中，接收到的点云消息为标准通用的点云消息定义（根据物理传感器的不同，点云的标准消息有三类: sensor_msgs::PointCloud2，sensor_msgs::LaserScan，sensor_msgs::MultiEchoLaserScan，sensor_msgs 包用来专门存放消息的标准定义格式，其中msg.header.stamp定义了点云帧扫描的起始时间)，ROS节点收到点云消息后，在SensorBridge类对象中调用了三个函数名相同(函数名均为ToPointCloudWithIntensities，具体的实现参考msg_conversion.cc代码)但参数不同的重载函数将上述三种不同的点云数据转换成为一致的cartographer内部的PointCloudWithIntensities类型对象，该类型对象结构含有成员变量points和intensities，其中points的每一个点云点含有时间信息（TimedCloudPoint，时间为相对于点云扫描帧结束时间的偏移，为负值），intensities为点云点对应的强度信息，为浮点值数组。 以sensor_msgs::PointCloud2点云消息为例，SensorBridge类的函数HandlePoint2Message对接收到的消息进行处理，其中会通过函数调用ToPointCloudWithIntensities(*msg)进行的转换，在sensor_msgs::PointCloud2类型转换的工程中，用到了较为常用的pcl点云处理库，先将数据转换成pcl::PointCloud<PointXYZT>数据类型（在pcl::PointCloud数据类型汇总，点云点的时间戳为相对于点云帧扫描开始的时间偏移，为正值），然后基于该类型数据转换为PointCloudWithIntensities类型。 2、SensorBridge中通过函数HandleRangefinder将TimedPointCloud数据（PointCloudWithIntensities的points成员变量）转化为TimedPointCloudData类型的数据对象，该转换涉及到了点云数据从传感器坐标系到机器人本体坐标系（车身坐标系）的坐标变换，该变换定义为sensor_to_tracking->cast<float>()，而tracking_frame一般在lua配置文件中定义为“base_link”（一般为车体底盘后面中间位置）。同时TimedPointCloudData的time成员变量值为点云帧扫描结束时间，其origin成员变量定义了Lidar传感器相对于tracking_frame的位置偏移（ sensor_to_tracking->translation().cast<float>()）。将转换后的数据通过调用trajectory_builder_->AddSensorData转入到cartographer的核心库进行后续的算法处理。具体的变换代码为： 3、上面的trajectory_builder_变量的类型为CollatedTrajectoryBuilder，在MapBuilderBridge::AddTrajectory函数里通过调用map_builder_->AddTrajectoryBuilder函数来生成，在map_builder.cc代码文件里的实现片段可供参考： 其中map_builder对象在node_main.cc文件里通过调用cartographer::mapping::CreateMapBuilder(node_options.map_builder_options)进行的初始化），CollatedTrajectoryBuilder类为local_trajectory_builder（局部SLAM，前端）和PoseGraphND（全局SLAM，后端）的对象的集成封装，为SLAM前后端的集成入口。trajectory_builders_为vector数组容器，因此cartographer系统默认支持多个trajectory的构建。在cartographer核心库的实现中，不同的传感器数据将会在一起集中进行队列缓存管理和进一步分发。相关的数据队列缓存管理和分发的对象为sensor_collator_ ，其内部实现中使用了多个队列来分别缓存不同的传感器消息，并基于类似归并排序的思想从多个队列中取出最早的sensor数据进行分发(Dispatch)。 综合上述的描述，数据的处理流转主要表现为：CollatedTrajectoryBuilder::AddSensorData(…)->CollatedTrajectoryBuilder::AddData(…)->sensor_collator_->AddSensorData(…)->OrderedMultiQueue::Add()->OrderedMultiQueue::Dispatch()->HandleCollatedSensorData()->data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get())->trajectory_builder->AddSensorData(sensor_id_, data_);->local_trajectory_builder_->AddRangeData()。 当数据流转到LocalTrajectoryBuilderND的AddRangeData函数时，就基本到达了SLAM算法的核心部分，包括外推器的姿态估计，ScanMatch的局部位姿优化(Local SLAM)，以及全局位姿优化(PoseGraph )等过程中的Lidar数据的进一步的流转和变换，限于篇幅，将在下一篇文章中向大家介绍。 References

本篇文章将向大家介绍cartographer系统的sensor模块的功能的代码实现，希望对感兴趣的读者朋友们有所帮助，欢迎读者朋友阅读并对说明描述不足之处提出意见和建议。 首先，cartographer库是SLAM的算法处理核心部分，其关于sensor的数据的定义主要用于内部算法的处理，而从智能设备本体接受的sensor原始数据会由cartographer_ros的节点进行接收和发送，这里将对cartographer_ros和cartographer算法库这两部分的sensor的数据表示在一起做一下说明。 如下图所示为cartographer系统（包括ros节点）代码中数据相关的类图。其中左边一列为IMU，Odometry等其他传感器的类图，右边两列为Lidar传感器相关类图，其中右上方描述了Lidar数据在系统中的流转转换流程过程。 在这里需要特别加以说明的有：在ROS节点接收通用点云数据ROS消息在SensorBridge类中转换为TimedPointCloudData类型的数据后发送给cartographer库进一步处理，TimedPointCloudData类对象数据的time成员表示为当前扫描帧点云的最晚时间，原点坐标为lidar传感器相对于tracking_frame的相对位置（一般tracking_frame定义为base_link，lua中有定义tracking_frame = “base_link”，原点定义为lidar传感器相对于base_link的位移大小，为固定的标定的值)。cartographer系统统一支持2D的LaserScan Lidar（较低成本的平面Lidar传感器）和3D Lidar传感器，对于2D LaserScan点云数据，一般会在预处理时进行切分处理(如backpack_2d.lua中定义了参数num_subdivisions_per_laser_scan = 10)，以便充分利用时间及时进行点云去畸变的计算。 在cartographer库内部，RangeDataCollator类实现了兼容多个Lidar(如backpack_3d.lua中定义了num_point_clouds = 2)传感器数据的同步融合，融合后的数据类型为TimedPointCloudOriginData，保留传感器相对于机器人本体坐标系的位移origin信息，可以计算range的大小以便进行点云的过滤操作。经过进一步的处理后的点云数据转换为RangeData，基于该数据调用ScanMatch算法优化局部位姿后对局部地图进行更新，关于整个SLAM算法流程中点云数据的坐标变换过程，ScanMatch算法实现局部位姿优化以及PoseGraph算法实现全局位姿优化等细节将在后续文章中做更加具体的说明分析。 ImuData为高频数据（频率几百到上千hz），PoseExtrapolate类实现了基于ImuData和/或OdometryData实现的机器人的位姿的较为精确的初始估计值。其中预估的大体方法为基于线速度和时间差估计位移，基于角速度和时间差积分出旋转增量估计，并且姿态外推器也基于ScanMatch的局部位姿优化结果更新time时刻更加准确的位姿，并预估出两个pose之间的匀速速度用于位移估计，在cartographer中没有采用IMU的线性加速度去进行预积分计算位姿中的translation部分(IMU的线性加速度的噪声较大，容易导致位姿漂移，而基于较为准确的位姿的变化去估计速度则较为稳定和准确，也可以基于OdometryData进行位移估计，如果有提供且数据量组足够时）。 后续文章将对cartographer的细节算法，系统架构等做进一步的介绍，欢迎关注。 References