LIOSAM 工程是怎么跑起来的

SLAM / LIOSAM

liosam 工程是怎么跑起来的

LIOSAM 的中 Topic 与 node 的关联图，咋一看非常之多，十分骇人，但是从捋出重要 Topic 之后，就简练的多了，关注以下这些：

/lio_sam/mapping/odometry_incremental：节点

rosgraph-more-topics

rosgraph-less-topics

后端优化中雷达位姿的初值获取

LIOSAM 中的 lidar_link 其实也就是base_link，它与机器人场景中的机器人、自动驾驶场景的车辆是类似的。

在优化问题中，一个好的初值是非常重要的。LIOSAM在启动过程中，不同的状态下位姿初值获取选用了不同的方案。

第一帧

当接收到第一帧点云，将IMU原始数据中的角度值作为初始姿态，位置直接用（0，0，0）。位姿初始化在文件 mapOptmization.cpp 中， ， 代码如下，

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
//// @file： LIO-SAM/src/mapOptmization.cpp
void updateInitialGuess()
{
    // save current transformation before any processing
    incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
    static Eigen::Affine3f lastImuTransformation;

    // initialization
    if (cloudKeyPoses3D->points.empty())// lidar trajectory
    {
        transformTobeMapped[0] = cloudInfo.imuRollInit;// 时间早于 point_cloud, 且最接近一帧的 imu 数据中的RPY
        transformTobeMapped[1] = cloudInfo.imuPitchInit;
        transformTobeMapped[2] = cloudInfo.imuYawInit;
        if (!useImuHeadingInitialization)
            transformTobeMapped[2] = 0;

        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
    }

    // use imu pre-integration estimation for pose guess
    ...
    // use imu incremental estimation for pose guess (only rotation)
    ...
}

上述 updateInitialGuess() 函数中：

位姿保存在变量 transformTobeMapped[] 中，且只对角度赋值，位置值保留变量的初始化值；代码如下；

角度的赋初值：cloudInfo.imuRollInit / imuPitchInit / imuYawInit 的获取在文件 imageProjection.cpp 中，存于消息 cloud_info 中与其他消息一并发布；代码如下；

另还有两个参数 incrementalOdometryAffineFront 和 lastImuTransformation，lastImuTransformation 用于下一帧的雷达初始化（详见下一小节）；incrementalOdometryAffineFront在后续生成平滑的 lidar Odometry 中有应用。

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
27
28
29
//// @file： LIO-SAM/src/mapOptmization.cpp
void allocateMemory()
{    
    ...
    for (int i = 0; i < 6; ++i){
        transformTobeMapped[i] = 0;
    }
    ...
}

//// @file： LIO-SAM/src/imageProjection.cpp    
void imuDeskewInfo()
{
    ...        
    for (int i = 0; i < (int)imuQueue.size(); ++i)
    {
        sensor_msgs::Imu thisImuMsg = imuQueue[i];
        double currentImuTime = thisImuMsg.header.stamp.toSec();
        
        // get roll, pitch, and yaw estimation for this scan
        // 持续更新 rpy 直到 IMU 时间为小于雷达时间且相距最近
        if (currentImuTime <= timeScanCur)
            imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
        if (currentImuTime > timeScanEnd + 0.05)
            break;
    }    
    ...
}

上述 imuDeskewInfo() 中的 imuRPY2rosRPY() 函数（line 23）定义在 utility.h 中，函数功能简单，只将 IMU_raw 中的 orientation 作格式转换，不多赘述。

第二帧

收到第二帧点云后，判断其不满足时间间隔阈值，直接返回。

第三帧

收到点云后，进入函数 updateInitialGuess() 中，不是第一帧后只需要关注后面的两个判断条件，cloudInfo.imuAvailable, cloudInfo.odomAvailable。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
//// @file： LIO-SAM/src/mapOptmization.cpp
void updateInitialGuess()
{
    // save current transformation before any processing
    incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);

    // initialization
    if (cloudKeyPoses3D->points.empty()){...}
    
    // use imu pre-integration estimation for pose guess
    if (cloudInfo.odomAvailable == true){...}

    // use imu incremental estimation for pose guess (only rotation)
    if (cloudInfo.imuAvailable == true){...}
}

imuAvailable,odomAvailable 这两个参数同样来自于文件 imageProjection.cpp ：

  • 参数 cloudInfo.imuAvailable 是指是否收到用于畸变纠正的 IMU 原始数据，是一开始就有的；

  • this is test cloudInfo.imuAvailable 是指是否收到用于畸变纠正的 IMU 原始数据，是一开始就有的；

  • 参数 cloudInfo.odomAvailable 是指是否收到用于畸变纠正的 IMU odometry ，而这个 Topic 是在收到节点 mapOptmization 发出的 lidar odometry 后再下一帧处理所得（收到第一帧需要初始化）；

因此进入第三个case，if (cloudInfo.imuAvailable == true){...} 。

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
//// @file： LIO-SAM/src/mapOptmization.cpp
void updateInitialGue当前帧的位姿初值 `transFinal=transTobe * transIncre` ss()
{
    // save current transformation before any processing
    incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
    
    // initialization
    ...    
    // use imu pre-integration estimation for pose guess
    ...    
    // use imu incremental estimation for pose guess (only rotation)
    if (cloudInfo.imuAvailable == true){        
        ROS_WARN("Guess init, imuAvailable");
        Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
        Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;

        Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
        Eigen::Affine3f transFinal = transTobe * transIncre;
        pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                      transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
        return;
    }
}

这一段代码的逻辑是，通过结合上一帧的雷达位姿和两帧雷达位姿的增量来计算而得，获取当前帧的雷达位姿。

上一帧的雷达位姿初值 $T_{last_init}$ ：上一帧处理过程中存于变量 lastImuTransformation ；

上一帧雷达位姿优化值 $T_{last_opt}$ ：上一帧的优化位姿，存于 transformTobeMapped[] ，格式转换得到 transTobe ；

当前帧雷达位姿临时值 $T_{cur_temp}$ ：当前帧从 IMU原始数据中获取的位姿，存于 transBack ；

当前帧的雷达位姿初值 $T_{cur_init}$ ：也就是当前函数的待求值，前帧的位姿初值 transFinal=transTobe * transIncre >。

两帧之间的位姿增量 $T_{between}$ ：上一帧位姿初值 $T_{last-init}$ 和当前帧雷达位姿临时值 $T_{cur_temp}$ 之间的增量（transIncre）；

用公式表示为：
$$
T_{between}=T_{last_init}^{-1}T_{cur_temp}\T_{cur_init}=T_{last_opt}T_{between}
$$
得到当前帧位姿初值后，再度存于 transformTobeMapped[] 中对其进行更新，这个参数非常重要的。

并同时更新lastImuTransformation 用于下一帧的处理。

注1：前面的【当前帧雷达位姿临时值】是笔者自己起的一个名字，为区别于用于后端优化的初值，而用了临时值的说法。

注2：这部分应用到有关矩阵右乘的知识点，主要多加注意，可参考这个问题 矩阵左乘，右乘到底是什么意思？中 糖糖不是堂 的回答。

第四帧

同第二帧情形，不满足时间间隔阈值，直接返回。

第五帧

到此时，

雷达里程计的最高频率

雷达里程计并不是对每一帧点云都进行处理，这里有一个时间阈值的判断。

《后端优化中 雷达 位姿的初始化》中所说的第二帧，是指满足时间间隔要求的第二帧，实为第三帧，收到的第二帧点云与第一帧

1
2
3
4
//// @file： LIO-SAM/src/mapOptmization.cpp
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval){
    ...
}

mappingProcessInterval的默认参数是0.15s，所以在默认参数下，雷达里程计不会超过1/0.15Hz

参数配置文件： LIO-SAM/config/params.yaml

Q&A

  • 每个 Topic 具体发布的数据什么含义

  • mapOpt 中的lio_sam/mapping/odometry 与 lio_sam/mapping/odometry_incremental有什么区别与联系?

https://github.com/TixiaoShan/LIO-SAM/issues/92

  • odometry/imu 与 odometry/imu_incremental

    odometry/imu_incremental，在imagerojection发布

    odometry/imu，在transformusion中，

  • transformTobeMapped 的初始化与优化历程

  • gtsam 中 的先验因子和 Values 有什么区别

  • imgProject 中用于畸变纠正的 IMU 数据 Odom 数据

    Odom 数据 为什么用 IMU 里程计，而不用 lidar Odom？因为后者滞后

    roll，pitch，yaw 的纠正为什么用原始IMU，而不用 IMU 里程计 中的orientation？ 尚不清楚。

  • addOdomFactor() 为何首帧的噪声模型参数的yaw为 PI*PI？

Donate
  • © 2023 , Monicakaa .

请我喝杯咖啡吧~

支付宝
微信