LIO-SAM源码解读（三）：TransformFusion 和 IMUPreintegration

原创

已于 2024-09-25 18:48:58 修改 · 1.4k 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #算法 #c++ #slam

于 2021-09-09 11:39:03 首次发布

本文介绍了TransformFusion类和IMUPreintegration类在激光里程计与IMU数据融合中的应用。TransformFusion订阅并融合来自MapOptimization的激光里程计和IMU里程计，计算并发布当前时刻的IMU里程计轨迹。IMUPreintegration则通过预积分技术，结合激光里程计优化状态，并预测和发布高频率的IMU里程计信息。整个系统旨在提高机器人定位的精度和稳定性。

写在前面

TransformFusion类
功能简介：
主要功能是订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；
rviz展示IMU里程计轨迹（局部）。
订阅：
1、订阅激光里程计，来自MapOptimization；
2、订阅imu里程计，来自ImuPreintegration。
发布：
1、发布IMU里程计，用于rviz展示；
2、发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。

IMUPreintegration类
功能简介：
1、用激光里程计，两帧激光里程计之间的IMU预积分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）;
2、以优化后的状态为基础，施加IMU预积分量，得到每一时刻的IMU里程计。

订阅：
1、订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预积分量，预测每一时刻（IMU频率）的IMU里程计；
2、订阅激光里程计（来自MapOptimization）,用两帧之间的IMU预积分构建因子图，优化当前帧位姿（每个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。

发布：
1、发布IMU里程计。

定义参数：

using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

TransformFusion整体代码

public:
    std::mutex mtx;

    ros::Subscriber subImuOdometry;
    ros::Subscriber subLaserOdometry;

    ros::Publisher pubImuOdometry;
    ros::Publisher pubImuPath;

    Eigen::Affine3f lidarOdomAffine;
    Eigen::Affine3f imuOdomAffineFront;
    Eigen::Affine3f imuOdomAffineBack;

    tf::TransformListener tfListener;
    tf::StampedTransform lidar2Baselink;

    double lidarOdomTime = -1;
    deque<nav_msgs::Odometry> imuOdomQueue;


class TransformFusion : public ParamServer
{
   
   
public:
    std::mutex mtx;

    ros::Subscriber subImuOdometry;
    ros::Subscriber subLaserOdometry;

    ros::Publisher pubImuOdometry;
    ros::Publisher pubImuPath;

    Eigen::Affine3f lidarOdomAffine;
    Eigen::Affine3f imuOdomAffineFront;
    Eigen::Affine3f imuOdomAffineBack;

    tf::TransformListener tfListener;
    tf::StampedTransform lidar2Baselink;

    double lidarOdomTime = -1;
    deque<nav_msgs::Odometry> imuOdomQueue;

    /**
     * 构造函数
     */
    TransformFusion()
    {
   
   
        // 如果lidar系与baselink系不同（激光系和载体系），需要外部提供二者之间的转换关系
        if(lidarFrame != baselinkFrame)
        {
   
   
            try
            {
   
   
                // 等待3s
                tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
                // lidar系到baselink系的变换
                tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
            }
            catch (tf::TransformException ex)
            {
   
   
                ROS_ERROR("%s",ex.what());
            }
        }

        //订阅激光里程计，来自mapOptimization
        subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
        //订阅imu里程计，来自IMUPreintegration
        subImuOdometry   = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());

        //发布imu里程计，用于rviz展示
        pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
        //发布imu里程计轨迹
        pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);
    }

    /**
     * 里程计对应变换矩阵
     */
    Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
    {
   
   
        double x, y, z, roll, pitch, yaw;
        x = odom.pose.pose.position.x;
        y = odom.pose.pose.position.y;
        z = odom.pose.pose.position.z;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        return pcl::getTransformation(x, y, z, roll, pitch, yaw);
    }

    /**
     * 订阅激光里程计，来自mapOptimization
     */
    void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
   
   
        std::lock_guard<std::mutex> lock(mtx);
        //激光里程计对应变换矩阵
        lidarOdomAffine = odom2affine(*odomMsg);
        //激光里程计时间戳
        lidarOdomTime = odomMsg->header.stamp.toSec();
    }

    /**
     * 订阅imu里程计，来自IMUPreintegration
     * 1.以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿
     * 2.发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段
     */
    void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
   
   
        // static tf
        // 发布tf，map与odom系设为同一个系
        static tf::TransformBroadcaster tfMap2Odom;
        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
        tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));

        std::lock_guard<std::mutex> lock(mtx);

        // 添加imu里程计到队列
        imuOdomQueue.push_back(*odomMsg);

        // get latest odometry (at current IMU stamp)
        // 从imu里程计队列中删除当前（最近的一帧）激光里程计时刻之前的数据
        if (lidarOdomTime == -1)
            return;
        while (!imuOdomQueue.empty())
        {
   
   
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }
        // 最近的一帧激光里程计时刻对应imu里程计位姿
        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
        // 当前时刻imu里程计位姿
        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
        // imu里程计增量位姿变换
        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
        Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
        
        // publish latest odometry
        // 发布当前时刻里程计位姿
        nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
        laserOdometry.pose.pose.position.x = x;
        laserOdometry.pose.pose.position.y = y;
        laserOdometry.pose.pose.position.z = z;
        laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        pubImuOdometry.publish(laserOdometry);

        // publish tf
        // 发布tf，当前时刻odom与baselink系变换关系
        static tf::TransformBroadcaster tfOdom2BaseLink;
        tf::Transform tCur;
        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
        if(l