LIO_SAM源码解析：ImageProjection—imuHandler()

ImageProjection模块的主要功能为：

1. 利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）；
2. 同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。

public:
    ImageProjection():deskewFlag(0)
    {
        // 订阅原始imu数据
        // this: 调用这个class里的返回函数，可以使用第四个参数，例如有个对象叫listener，
        // 调用该对象内部的回调函数，则传入&listener，这里调用自己的，则传入this指针
        // ros::TransportHints().tcpNoDelay() :被用于指定hints，确定传输层的作用话题的方式:无延迟的TCP传输方式
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, 
        &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿(发布Lidar坐标系下IMU数据)
        subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 
        2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅原始lidar数据
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, 
        &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
        // 发布当前激光帧运动畸变校正后的点云，有效点
        pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> 
        ("lio_sam/deskew/cloud_deskewed", 1);
        // 发布当前激光帧运动畸变校正后的点云信息
        pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> 
        ("lio_sam/deskew/cloud_info", 1);
         // 初始化
        allocateMemory();
        // 重置参数
        resetParameters();
        // pcl日志级别，只打ERROR日志
        pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
    }

解析函数一： void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
    {
        // imu原始测量数据转换到lidar系，加速度、角速度、RPY
        // 经过处理后的LIdar坐标系下的IMU
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
        // 上锁，添加数据的时候队列不可用
        std::lock_guard<std::mutex> lock1(imuLock);
        imuQueue.push_back(thisImu); 
    }



 //它主要的作用，是把IMU的信息，从IMU坐标系，转换到雷达坐标系。
    //（注意，这个函数只旋转，没有平移，和真正的雷达坐标系之间还是差了一个平移的。
    //至于为什么没有平移，先提前剧透一下，在imuPreintegration.cpp文件中，
    //还有两个imu2Lidar，lidar2imu变量，这俩变量只有平移，没有旋转。
    //作者的IMU与雷达的坐标系以及IMU提供的欧拉角的方向跟常用的有区别 特别注意IMU 的yaw角是与常见的反向的
    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);
        //************取正数***************
        //acc1(-0.45463,1.54775,-9.68648);
        //cout<<"acc1"<<acc<<endl;
        //extRot-1  0  0
        //       0  1  0
        //       0  0 -1
        acc = extRot * acc;
        //cout<<"extRot"<<extRot<<endl;
        //cout<<"acc"<<acc<<endl;
        //acc(0.45463,1.54775,9.68648);
        //********************************
        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;//个人理解应该是处理后的在Lidar下的IMU数据
    }
};

总结：IMU坐标系下的数据转换成Lidar坐标系下的数据，存储在imuQueue中。