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

在这里插入图片描述

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 = q f i n a l ∗ P l 其中 P n 表示点 P 在导航系下的坐标, P l 表示点 P 在 l i d a r 坐标系下的坐标,并且 P n 的 x y z 不是 l i d a r 的前左上对应的北西上,而是对应西北地 P_n=qfinal*P_l \\其中P_n表示点P在导航系下的坐标,P_l表示点P在lidar坐标系下的坐标,并且P_n的xyz不是lidar的前左上对应的北西上,而是对应西北地 Pn=qfinalPl其中Pn表示点P在导航系下的坐标,Pl表示点Plidar坐标系下的坐标,并且Pnxyz不是lidar的前左上对应的北西上,而是对应西北地

注意

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

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值