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 .

请我喝杯咖啡吧~

支付宝
微信