使用SensorBridge转换IMU数据

在上一章中，我们已经了解了SensorBridge是如何将ROS系统中激光传感器的数据转换成Cartographer的数据结构。本文中我们来看一下它是如何处理IMU数据的。

下面是node对象在回调函数中处理IMU消息的代码片段。首先通过map_builder_bridge_对象的接口获取当前跟踪轨迹所用的SensorBridge对象，然后通过该对象将ROS的系统消息转换成Cartographer的ImuData数据类型(第二行中通过C++11的auto关键字自动分析数据类型)。接着在第4行中将IMU的数据提供给位姿估计器，最后通过SensorBridge对象将数据喂给Cartographer。

        auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
        auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
        if (imu_data_ptr != nullptr)
            extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
        sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);

下面我们来依次查看一下第2行中的函数ToImuData和第5行的HandleImuMessage。

1. 转换数据结构

        $ rosmsg show sensor_msgs/Imu -r
        # 一些说明性的文字 
        Header header
        
        geometry_msgs/Quaternion orientation
        float64[9] orientation_covariance
        
        geometry_msgs/Vector3 angular_velocity
        float64[9] angular_velocity_covariance
        
        geometry_msgs/Vector3 linear_acceleration
        float64[9] linear_acceleration_covariance

在分析函数ToImuData之前，让我们先来看一下ROS系统中IMU的数据结构。如下面右侧的代码片段所示，我们可以通过工具rosmsg来查看该消息的数据结构。

其中，字段header是ROS消息通用的消息头，记录了消息的序列号、时间戳、以及参考坐标系。字段orientation通过四元数来描述机器人姿态， orientation_covariance则是机器人姿态的协方差矩阵，用于描述姿态估计的不确定性。

类似的字段angular_velocity和angular_velocity_covariance分别描述了机器人运动的角速度及其不确定性，而字段linear_acceleration和linear_acceleration_covariance则分别描述了机器人运动的线加速度和它的协方差矩阵。

一般情况下，上述字段中的角速度是由陀螺仪测量得到的，加表用来测量线加速度。而描述机器人姿态的四元数通常是由角速度积分得到。

        struct ImuData {
            common::Time time;
            Eigen::Vector3d linear_acceleration;    
            Eigen::Vector3d angular_velocity;
        };

左边是Cartographer定义在头文件imu_data.h中的结构体ImuData，它只有三个字段分别记录了数据产生的时间，机器人的线加速度和角速度。并没有记录机器人的姿态，因为在Cartographer中有专门的class来根据传感器的原始数据结算位姿。

下面是SensorBridge对象的成员函数ToImuData的代码片段，在函数的一开始先检查线加速度和角速度的协方差矩阵的第一个元素。根据ROS系统的定义，如果协方差矩阵的第一个元素为-1意味着所对应的传感器数据无效。接着将ROS消息头中的时间戳转换成Cartographer中的时间，并通过坐标变换对象tf_bridge_从ROS系统中查询IMU传感器相对于机器人坐标系的坐标变换关系。

        std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(const sensor_msgs::Imu::ConstPtr& msg) {
            CHECK_NE(msg->linear_acceleration_covariance[0], -1) << "错误信息, 线加速度信息不可用";
            CHECK_NE(msg->angular_velocity_covariance[0], -1) << "错误信息，角速度信息不可用";

            const carto::common::Time time = FromRos(msg->header.stamp);
            const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(msg->header.frame_id));
            if (sensor_to_tracking == nullptr)
                return nullptr;

最后检查一下IMU坐标系相对于机器人坐标系的平移距离，以保证IMU坐标系原点应当尽量与机器人坐标系重合。否则，将传感器测量的线加速度转换到机器人坐标系下就会出现比较大的偏差，对于位姿估计而言是不利的。检查通过之后，就可以构建一个ImuData的对象将经过坐标变换的传感器数据填充到该对象中，并返回。

            CHECK(sensor_to_tracking->translation().norm() < 1e-5) << "错误信息，IMU坐标系原点应当尽量与机器人坐标系重合";
            return carto::common::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
                                                                    time,
                                                                    sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
                                                                    sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
        }

2. 提供数据给地图构建器

下面是SensorBridge将IMU数据喂给轨迹跟踪器的函数实现。其过程十分简单，就是成功调用刚刚分析的函数ToImuData之后，直接通过轨迹跟踪器的接口AddSensorData填塞数据。

        void SensorBridge::HandleImuMessage(const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) {
            std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
            if (imu_data != nullptr)
                trajectory_builder_->AddSensorData(sensor_id, carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration, imu_data->angular_velocity});
        }

3. 完

本文我们详细分析了ROS系统和Cartographer中IMU数据的表示方式，并研究了数据转换函数ToImuData。最后看到SensorBridge对象直接将转换后的IMU数据喂给了路径跟踪器。