【cartographer without ros】五、陀螺仪Imu数据转换

上一节介绍了cartographer中的OdometryData数据相关转换。

本节将介绍陀螺仪Imu数据转换为cartographer中的ImuData数据。还有cartographer中的ImuData数据和Ros的sensor_msgs::Imu数据相互转换，方便引入Rosbag进行储存和读取。

目录

1：ImuData数据类型

2：Imu数据转换cartographer数据

3：Imu数据转换Ros数据

4：cartographer数据转换Ros数据

5：Ros数据转换cartographer数据

---

1：ImuData数据类型

【imu_data.h】中查看数据结构：

//时间
common::Time time;
//线加速度
Eigen::Vector3d linear_acceleration;
//角速度
Eigen::Vector3d angular_velocity;

---

2：Imu数据转换cartographer数据

cartographer::sensor::ImuData GetImuData()
{
    cartographer::sensor::ImuData imu_data;

    //计算
    //线加速度(含重力)
    acc[0] = *((float *)&handle_buf[22]);
    acc[1] = *((float *)&handle_buf[26]);
    acc[2] = *((float *)&handle_buf[30]);

    //角速度(陀螺仪I的输出)
    gyo[0] = *((float *)&handle_buf[82]);
    gyo[1] = *((float *)&handle_buf[86]);
    gyo[2] = *((float *)&handle_buf[90]);

    //欧拉角
    eular[0] = *((float *)&handle_buf[146]);
    eular[1] = *((float *)&handle_buf[150]);
    eular[2] = *((float *)&handle_buf[154]);

    Eigen::Vector3d linear_acceleration(((double)acc[0] * (-9.8)), ((double)acc[1] * (-9.8)), ((double)acc[2] * (-9.8)));
    Eigen::Vector3d angular_velocity(((double)gyo[0] * 3.1415926 / 180), ((double)gyo[1] * 3.1415926 / 180), ((double)gyo[2]) * 3.1415926 / 180);
    Eigen::Vector3d imu_angle(((double)eular[0]), ((double)eular[1]), ((double)eular[2]));

    imu_data_.time = cartographer::common::NowTime();
    imu_data_.linear_acceleration = linear_acceleration;
    imu_data_.angular_velocity = angular_velocity;
    return imu;
}

---

3：Imu数据转换Ros数据

【cartographer_ros】五： 发布和订阅陀螺仪Imu信息

---

4：cartographer数据转换Ros数据

sensor_msgs::Imu ToRosMsg(cartographer::sensor::ImuData imu_data)
{
    sensor_msgs::Imu imu;

    imu.header.stamp = TimeRosFromCarto(imu_data.time);
    imu.header.frame_id = "imu";
    imu.angular_velocity.x = imu_data.angular_velocity.x();
    imu.angular_velocity.y = imu_data.angular_velocity.y();
    imu.angular_velocity.z = imu_data.angular_velocity.z();
    imu.linear_acceleration.x = imu_data.linear_acceleration.x();
    imu.linear_acceleration.y = imu_data.linear_acceleration.y();
    imu.linear_acceleration.z = imu_data.linear_acceleration.z();
    return imu;
}

---

5：Ros数据转换cartographer数据

cartographer::sensor::ImuData ToCartoSensor(sensor_msgs::Imu imu)
{
    cartographer::sensor::ImuData imu_data;
    imu_data.time = TimeCartoFromRos(imu.header.stamp);
    imu_data.angular_velocity = Eigen::Vector3d(imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z);
    imu_data.linear_acceleration = Eigen::Vector3d(imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z);
    return imu_data;
}

【完】

---

下一节会介绍路标Landmark数据在cartographer中的发布。