LIO-SAM中imuConvert函数解析(lidar坐标系与imu坐标系的转换)

刘乐去哪儿了

已于 2023-09-04 22:53:37 修改

阅读量524

点赞数

分类专栏：激光SLAM算法文章标签： c++ 算法计算机视觉

于 2023-09-03 21:44:26 首次发布

本文链接：https://blog.csdn.net/qq_37423490/article/details/132655534

版权

激光SLAM算法专栏收录该内容

6 篇文章 0 订阅

订阅专栏

在这里插入图片描述

LIO-SAM中imuConvert函数解析

概述

该函数主要作用为lidar坐标系与imu坐标系的转换：
1、将imu获得的角速度值及加速度值转换到lidar坐标系
2、将imu获得的姿态角转换为lidar的姿态角，即将导航系(西北地)到imu系(左前下)的姿态矩阵转换为导航系(西北地)到lidar系(前左上)的姿态矩阵

详解

先看源码：

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
    {
        sensor_msgs::Imu imu_out = imu_in;
        // rotate acceleration
        Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
        acc = extRot * acc;
        imu_out.linear_acceleration.x = acc.x();
        imu_out.linear_acceleration.y = acc.y();
        imu_out.linear_acceleration.z = acc.z();
        // rotate gyroscope
        Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
        gyr = extRot * gyr;
        imu_out.angular_velocity.x = gyr.x();
        imu_out.angular_velocity.y = gyr.y();
        imu_out.angular_velocity.z = gyr.z();
        // rotate roll pitch yaw
        Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
        Eigen::Quaterniond q_final = q_from * extQRPY;
        imu_out.orientation.x = q_final.x();
        imu_out.orientation.y = q_final.y();
        imu_out.orientation.z = q_final.z();
        imu_out.orientation.w = q_final.w();

        if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
        {
            ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
            ros::shutdown();
        }

        return imu_out;
    }

1、extRot 为lidar坐标系到imu坐标系的旋转矩阵(注意这里说的是坐标系的旋转矩阵，不是坐标的旋转矩阵)；
2、extQPRY 是imu方向(左前下)到lidar方向(前左上)的旋转矩阵。
3、代码中 acc = extRot * acc 表示获取lidar坐标系下加速度向量的取值
4、q_from表示导航坐标系到imu坐标系的四元数，或者说“导航系(西北地)到imu系(左前下)的旋转矩阵”
5、q_final表示导航坐标系到lidar坐标系的四元数，或者说“导航系(西北地)到lidar系(前左上)的旋转矩阵”
$P_n=qfinal*P_l \\其中P_n表示点P在导航系下的坐标，P_l表示点P在lidar坐标系下的坐标，并且P_n的xyz不是lidar的前左上对应的北西上，而是对应西北地$

注意

1、上述提到的所有旋转矩阵都是坐标系之间的旋转矩阵，例如A坐标系到B坐标系的旋转矩阵R可理解为张成B坐标系的三组单位正交基在A系下的投影（如果不好理解可学习一下旋转矩阵的定义， $P_2 = R*P_1 可以理解为点P在R表示的坐标系里的坐标值为P_1，在全局坐标系里的坐标为P_2$ ）
2、参数文件中给出的extRPY是lidar方向到imu方向的旋转矩阵，代码里的extQRPY是extRPY的转置
3、q_final = q_from * extQRPY 右乘是因为欧拉角旋转采用的是内旋