3d激光SLAM：LIO-SAM框架---位姿融合输出

前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。

在之前的博客讲解了imu如何进行预积分，最终以imu的频率发布了imu的预测位姿里程计。

本篇博客主要讲解，最终是如何进行位姿融合输出的

Eigen::Affine3f

其中功能的核心在于 位姿间的变换，所以要了解 Eigen::Affine3f 部分的内容
Affine3f 是eighen库的 仿射变换矩阵
实际上就是：平移向量+旋转变换组合而成，可以同时实现旋转，缩放，平移等空间变换。

Eigen库中，仿射变换矩阵的大致用法为：

1. 创建Eigen::Affine3f 对象a。
2. 创建类型为Eigen::Translation3f 对象b，用来存储平移向量；
3. 创建类型为Eigen::Quaternionf 四元数对象c，用来存储旋转变换；
4. 最后通过以下方式生成最终Affine3f变换矩阵： a=b*c.toRotationMatrix();
5. 一个向量通过仿射变换时的方法是result_vector=test_affine*test_vector;

仿射变换包括：

• 平移
• 旋转
• 放缩
• 剪切
• 反射

平移（translation）和旋转（rotation）顾名思义，两者的组合称之为欧式变换（Euclidean transformation）或刚体变换（rigid transformation）；放缩（scaling）可进一步分为uniform scaling和non-uniform scaling，前者每个坐标轴放缩系数相同（各向同性），后者不同；如果放缩系数为负，则会叠加上反射（reflection）——reflection可以看成是特殊的scaling；刚体变换+uniform scaling 称之为，相似变换（similarity transformation），即平移+旋转+各向同性的放缩；

位姿融合输出

在imu预积分的节点中，在main函数里面 还有一个类的实例对象，那就是

TransformFusion TF
1

其主要功能是做位姿融合输出

最终输出imu的预测结果，与上节中的imu预测结果的区别就是：

该对象的融合输出是基于全局位姿的基础上再进行imu的预测输出。全局位姿就是 经过回环检测后的lidar位姿。

该对象的本质功能如下图

建图优化会输出两种激光雷达的位姿：

• lidar 增量位姿
• lidar 全局位姿

其中lidar 增量位姿就是 通过 lidar的匹配功能，优化出的帧间的相对位姿，通过相对位姿的累积，形成世界坐标系下的位姿
lidar全局位姿 则是在 帧间位姿的基础上，通过 回环检测，再次进行优化的 世界坐标系下的位姿，所以对于增量位姿，全局位姿更加精准

在前面提到的发布的imu的预测位姿是在lidar的增量位姿上基础上预测的，那么为了更加准确，本部分功能就预测结果，计算到基于全局位姿的基础上面。

首先看构造函数

    TransformFusion()
    {
        if(lidarFrame != baselinkFrame)
        {
            try
            {   
                tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
                tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
            }
            catch (tf::TransformException ex)
            {
                ROS_ERROR("%s",ex.what());
            }
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14

判断lidar帧和baselink帧是不是同一个坐标系
通常baselink指车体系
如果不是，
查询 一下 lidar 和baselink 之间的 tf变换 ros::Time(0) 表示最新的
等待两个坐标系有了变换
更新两个的变换 lidar2Baselink

        subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
        subImuOdometry   = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());
1
2

订阅地图优化的节点的全局位姿 和预积分节点的 增量位姿

        pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
        pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);
1
2

发布两个信息 odomTopic ImuPath

然后看第一个回调函数 lidarOdometryHandler

    void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard<std::mutex> lock(mtx);
        lidarOdomAffine = odom2affine(*odomMsg);
        lidarOdomTime = odomMsg->header.stamp.toSec();
    }
1
2
3
4
5
6

将全局位姿保存下来

• 将ros的odom格式转换成 Eigen::Affine3f 的形式
• 将最新帧的时间保存下来

第二个回调函数是 imuOdometryHandler
imu预积分之后所发布的imu频率的预测位姿

    void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
1
2

        static tf::TransformBroadcaster tfMap2Odom;
        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
1
2

建图的话 可以认为 map坐标系和odom坐标系 是重合的（初始化时刻）

tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
1

发布静态tf，odom系和map系 他们是重合的

imuOdomQueue.push_back(*odomMsg);
1

imu得到的里程计结果送入到这个队列中

        if (lidarOdomTime == -1)
            return;
1
2

如果没有收到lidar位姿 就returen

        while (!imuOdomQueue.empty())
        {
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }
1
2
3
4
5
6
7

弹出时间戳 小于 最新 lidar位姿时刻之前的imu里程计数据

        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
1
2
3

计算最新队列里imu里程计的增量

Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
1

增量补偿到lidar的位姿上去，就得到了最新的预测的位姿

        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
1
2

分解成平移 + 欧拉角的形式

        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);
1
2
3
4
5
6

发送全局一致位姿的 最新位姿

        static tf::TransformBroadcaster tfOdom2BaseLink;
        tf::Transform tCur;
        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
        if(lidarFrame != baselinkFrame)
            tCur = tCur * lidar2Baselink;
1
2
3
4
5

更新tf

        tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
        tfOdom2BaseLink.sendTransform(odom_2_baselink);
1
2

更新odom到baselink的tf

        static nav_msgs::Path imuPath;
        static double last_path_time = -1;
        double imuTime = imuOdomQueue.back().header.stamp.toSec();
        // 控制一下更新频率，不超过10hz
        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);
            // 把lidar时间戳之前的轨迹全部擦除
            while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
                imuPath.poses.erase(imuPath.poses.begin());

            // 发布轨迹，这个轨迹实践上是可视化imu预积分节点输出的预测值
            if (pubImuPath.getNumSubscribers() != 0)
            {
                imuPath.header.stamp = imuOdomQueue.back().header.stamp;
                imuPath.header.frame_id = odometryFrame;
                pubImuPath.publish(imuPath);
            }
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

发布imu里程计的轨迹

• 控制一下更新频率，不超过10hz
• 将最新的位姿送入轨迹中
• 把lidar时间戳之前的轨迹全部擦除
• 发布轨迹，这个轨迹实践上是可视化imu预积分节点输出的预测值

result

其中粉色的部分就是imu的位姿融合输出path