[人工智能]【SLAM】LIO-SAM解析——获得高频的lidar里程计TransformFusion(6)

系列文章链接：

【SLAM】LIO-SAM解析——流程图(1)

【SLAM】LIO-SAM解析——数据预处理imageProjection(2)

【SLAM】LIO-SAM解析——特征提取featureTrack(3)

【SLAM】LIO-SAM解析——IMU预计分IMU-Preintegration(4)

【SLAM】LIO-SAM解析——后端优化MapOptimization(5)

【SLAM】LIO-SAM解析——里程计融合transformFusion(6)

知识点：

如何合并一个低频但高精度的里程计和一个高频但低精度的里程计，输出一个高频高精度的里程计。(虽然我觉得代码里IMUPreintegration输出的里程计已经很高频且高精度了)

这是LIO-SAM里最小的一个类，它虽然和IMUPreintegration在同一个文件下，但是这个类所处的环节是最靠后的，所以分开讲。
LIO-SAM涉及到多个不同的坐标系，首先是lidar系，大部分运算都是在这个系上；baselink是指body系，一般和IMU坐标系重合。

6.1 综述

TransformFusion订阅了来自mapOptimization的lidar里程计和来自IMUPreintegration的IMU里程计。它的作用是什么呢?

以当前低频lidar里程计为基准，找到与它最近的一个IMU里程计和时间最新的一个IMU里程计，用这两个IMU里程计求出位姿的变化量，再作用到lidar位姿上，得到一个高频的里程计。

这个高频的里程计在LIO-SAM内部没有被用到，它与IMU里程计的区别是什么呢？

还记得mapOptimization发布了2个里程计吗，一个是没做处理的，另一个是与IMU数据加权融合后的IMUPreintegration特供版里程计；
前者是在这里生成高频的lidar里程计，也就是TransformFusion的输出，
后者是在IMUPreintegration与高频的IMU数据进行预积分+图优化生成高频的IMU里程计，也就是IMUPreintegration的输出；

作者在代码里搞出了这么多种数值差不了太多的里程计，第一次看着实非常的晕...

它的构造函数：

    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());

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

6.2 lidarOdometryHandler()

把订阅的lidar里程计保存在buffer里，这个位姿是lidar->world的表示：

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

6.3 imuOdometryHandler()

当收到IMU里程计的时候，先放到buffer里去，在把比lidar里程计时间早的IMU里程计都删掉：

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

        // 从imu里程计队列中删除当前（最近的一帧）激光里程计时刻之前的数据
        if (lidarOdomTime == -1)
            return;
        while (!imuOdomQueue.empty())
        {
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }

求出最新时刻的IMU里程计和与lidar里程计最近的一个IMU里程计之间的位姿变换

        // 最近的一帧激光里程计时刻对应imu里程计位姿
        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
        // 当前时刻imu里程计位姿
        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
        // imu里程计增量位姿变换
        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;

把这个位姿变换作用到lidar里程计获得最新时刻的lidar里程计，此时lidar里程计变成高频的了：

        // 最近的一帧激光里程计位姿 * imu里程计增量位姿变换 = 当前时刻imu里程计位姿
        Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);

发布这个高频的lidar里程计，虽然作者管它也叫IMU里程计，但是我这里把它称为高频的lidar里程计，与之前的IMU里程计作为区别：

        // 发布当前时刻里程计位姿
        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)

由于刚刚发布的里程计是lidar->world的位姿，这里发布一个body->world的位姿：

        // 发布tf，当前时刻odom与baselink系变换关系
        static tf::TransformBroadcaster tfOdom2BaseLink;
        tf::Transform tCur;
        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
        if(lidarFrame != baselinkFrame)
            tCur = tCur * lidar2Baselink;
        tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
        tfOdom2BaseLink.sendTransform(odom_2_baselink);

发布path：

        static nav_msgs::Path imuPath;
        static double last_path_time = -1;
        double imuTime = imuOdomQueue.back().header.stamp.toSec();
        // 每隔0.1s添加一个
        if (imuTime - last_path_time > 0.1)
        {
            last_path_time = imuTime;
            geometry_msgs::PoseStamped pose_stamped;
            pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
            pose_stamped.header.frame_id = odometryFrame;
            pose_stamped.pose = laserOdometry.pose.pose;
            imuPath.poses.push_back(pose_stamped);
            // 删除最近一帧激光里程计时刻之前的imu里程计
            while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
                imuPath.poses.erase(imuPath.poses.begin());
            if (pubImuPath.getNumSubscribers() != 0)
            {
                imuPath.header.stamp = imuOdomQueue.back().header.stamp;
                imuPath.header.frame_id = odometryFrame;
                pubImuPath.publish(imuPath);
            }
        }
    }
};

完结。