LVI-SAM坐标系外参分析与代码修改，以适配各种数据集

0.前言

天下苦LVI-SAM外参配置久矣！

LVI-SAM是LIO-SAM和VINS-Mono的结合体，代码本身并不是特别复杂，相较于另外两个代码只是增加了结合部分。LIO-SAM和LVI-SAM作者使用的是同一套传感器设备，但是外参配置晦涩，导致让很多人都不知道如果换了数据集或者自己的设备，该如何设定外参，而且基本上换了数据集全部都跑不下来。导致这一状况的有多个原因：

• 作者使用的IMU比较特殊：四元数输出的yaw/pitch/roll不是按照IMU坐标系的z/y/x轴动态旋转得到的，所以params_lidar.yaml文件中有extrinsicRot/extrinsicRPY两个外参。但是一般来说大多数IMU这两个应该是对应的，此时这两个外参数就是互为转置的关系。该问题并不难解决，通过看LIO-SAM的README文件应该大家都会明白是怎么回事；
• 作者使用的传感器安装外参比较特殊：作者在传感器安装时尽量考虑了机械位置的平行，所以最后多个传感器设备之间旋转外参有很多伪单位阵（这是我为了方便表述自己起的名字，即各个轴就是旋转90度或者180度的关系，旋转矩阵中都是1或者-1）。这样就导致作者为了方便写代码把一些外参在代码中进行了简化，也就是把有的旋转外参写死到了代码中，所以导致换到其他设备上会导致代码完全无法运行；
• 作者在yaml文件中定义的外参并不清晰：比如params_lidar.yaml中的extrinsicRot到底是R_lidar_imu还是R_imu_lidar？以及代码中的imu2lidar/lidar2imu是从后往前读还是从前往后读？
• 配置文件中外参多余，且关系不满足闭环：LVI-SAM的配置文件中有LIO-SAM的外参，比如params_lidar.yaml中的extrinsicRot，这部分和LIO-SAM是一样的，也可以通过先测试LIO-SAM确定这个参数到底怎么给；有VINS-Mono的外参数，比如params_camera.yaml中的extrinsicRotation，这个和VINS-Mono的定义是一样的，就是R_imu_cam，没有歧义；但是最让人困惑的是params_camera.yaml中的lidar_to_cam_tx/y/z和lidar_to_cam_rx/y/z，这些参数使用VINS-Mono和LIO-SAM的外参再计算，但是却怎么都对不上。

此外，尽管 GitHub 上有一些修改的代码，比如 LVI_SAM_fixed、LVI-SAM-modified、LVI-SAM-RoBoat。但是这些代码仍旧没有解决外参使用的问题，甚至外参比源代码变得更多、更难懂；也没有解决外参定义比较模糊的问题，比如extrinsicRot到底是R_lidar_imu还是R_imu_lidar？

因此本文主要有如下贡献：

• 仔细分析LVI-SAM的传感器坐标系定义与外参修改；
• 仔细分析LVI-SAM代码中的各种坐标系定义，以及视觉特征点深度注册中的坐标系变换；
• 给出我修改后的代码：该代码重新定义了各种传感器坐标系变量，让其见名知意；修改了外参配置，只需要给出T_imu_lidar即lidar -> imu的欧式变换外参和T_imu_camera即camera -> imu的欧式变换外参；同时解决了所说的所有问题，目的是为了切换数据集或使用自己的设备时，能够很容易的将LVI-SAM跑起来，因此本代码命名为LVI-SAM-Easyused。

代码链接：https://github.com/Cc19245/LVI-SAM-Easyused

1.原作者传感器件坐标系定义与外参修改

参考博客：【SLAM】LVI-SAM解析——综述

LIO-SAM/LVI-SAM原作者使用的传感器坐标系如下图所示。

1.1.博客作者的讲解（仅供参考）

红色是相机坐标系，蓝色是lidar坐标系，绿色是LVI-SAM的坐标系，橙色是VINS的坐标系，也就是IMU坐标系。官方配置文件中params_camera.yaml里的lidar_to_cam_XX外参指蓝色和绿色之间的外参，并不是蓝色和红色之间的外参。

此外，Feature_tracker_node的get_depth()中给特征点赋予lidar深度时，忽略了cam和lidar之间的平移，即image特征的单位球和点云的单位球球心不统一，分别是cam和IMU，rotation是统一的，都是为lidar的R。

1.2.LIO-SAM的README中作者对其传感器配置的解释

准备IMU数据：

• IMU 要求。 与最初的 LOAM 实现一样，LIO-SAM 仅适用于 9 轴 IMU，它提供滚动、俯仰和偏航估计。 横滚和俯仰估计主要用于以正确的姿态初始化系统。 使用 GPS 数据时，偏航估计会在正确的航向处初始化系统。 理论上，像 VINS-Mono 这样的初始化程序将使 LIO-SAM 能够与 6 轴 IMU 一起工作。 （新：liorf 增加了对 6 轴 IMU 的支持。）系统的性能在很大程度上取决于 IMU 测量的质量。 IMU 数据速率越高，系统精度越好。 我们使用 Microstrain 3DM-GX5-25，它以 500Hz 的频率输出数据。 我们建议使用至少提供 200Hz 输出速率的 IMU。 注意Ouster激光雷达内部IMU是6轴IMU。

• IMU 对准。 LIO-SAM 将 IMU 原始数据从 IMU 帧转换为 Lidar 帧，遵循 ROS REP-105 约定（x - 向前，y - 左，z - 向上）。 为了使系统正常运行，需要在“params.yaml”文件中提供正确的外部转换。 之所以有两个 extrinsics，是因为我的 IMU (Microstrain 3DM-GX5-25) 加速度和姿态有不同的坐标。 根据您的 IMU 制造商，您的 IMU 的两个外部参数可能相同也可能不同。 以我们的设置为例：

  • 我们需要设置 x-z 加速度和陀螺仪负读数来转换得到激光雷达坐标系中的 IMU 数据，这由“params.yaml”中的“extrinsicRot”指示。
  • 姿态读数的转变可能略有不同。 IMU的姿态测量q_wb通常表示IMU坐标系中的点到世界坐标系（如ENU）的旋转。 但是，该算法需要 q_wl，即从激光雷达到世界的旋转。 所以我们需要从激光雷达到 IMU 的旋转q_bl，其中 q_wl = q_wb * q_bl。 为方便起见，用户只需在“params.yaml”中提供q_lb作为“extrinsicRPY”（如果加速度和姿态具有相同的坐标，则与“extrinsicRot”相同）。注意：这里作者的解释有误，如果加速度和姿态坐标系相同的话，其实extrinsicRPY和extrinsicRot并不相同，而是互为转置。

1.3.IMU坐标系详解

1.为什么需要9-axis的IMU？

因为需要知道roll/pitch/yaw的欧拉角，其中前两个角度要得到正确的姿态来初始化系统，而yaw则是为了对齐GPS的时候直接指北。

2.IMU坐标系到底是怎么回事？两个外参是什么意思？

• extrinsicRot：这个是R_lidar_imu，也就是imu -> lidar的旋转。这个参数主要是为了把IMU测量的原始加速度、角速度数据转到lidar坐标系下，然后在lidar坐标系下积分直接得到lidar坐标系的位姿。作者这样做的原因是LIO-SAM中以lidar为主要坐标系，比如在后端lidar的scan-to-map优化中求的就是lidar坐标系的位姿，因此作者干脆就在积分的时候把IMU数据直接转到lidar坐标系下积分。假设加速度/角速度表示为mea，则该向量是在IMU坐标系下表示的，即$me{a}^{imu}$（上标表示该向量在哪个坐标系下表示）。现在想把加速度/角速度转成在lidar坐标系下表示，则有$me{a}^{lidar}=R\_lidar\_imu\ast me{a}^{imu}$。

对应代码如下：

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{	
		.......
		acc = extRot * acc;     // extRot就是配置文件中的extrinsicRot
		gyr = extRot * gyr;     // extRot就是配置文件中的extrinsicRot
		.......
}
1
2
3
4
5
6
7

• extrinsicRPY：这个参数不仅和IMU与lidar之间的安装外参数有关，还和IMU本身的性质有关。对于绝大多数IMU来说（即输出的yaw/pitch/roll按照IMU坐标系的z/y/x轴动态旋转得到），该参数就是R_imu_lidar，也就是lidar -> imu旋转。但是对于作者使用的IMU来说并不是这样，详细分析如下：

首先要明白这个参数是干什么的，该参数使用的语句为

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{	
		.......
		Eigen::Quaterniond q_final = q_from * extQRPY;     // extQRPY就是配置文件中的extrinsicRPY
		.......
}
1
2
3
4
5
6

其实这个参数就是把IMU输出的四元数，也就是IMU的姿态，变成lidar的姿态。假设世界坐标系是world，则代码中q_from = R_world_imu，而我们想要的是R_world_lidar，所以转换关系就是R_world_lidar = R_world_imu * R_imu_lidar，所以可以发现代码中的extQRPY = R_imu_lidar，即lidar -> imu的旋转。

注意：上面说的是大多数IMU的情况，对于作者使用的IMU来说比较特殊。作者的IMU输出的yaw/pitch/roll并不是按照IMU坐标系的z/y/x轴动态旋转得到。如果按照欧拉角的定义，即绕坐标轴逆时针旋转得到正的角度的话，作者的IMU是yaw绕着-z轴转、pitch绕着+x轴转、roll绕着+y转。这就意味着作者使用的IMU输出的四元数，实际是下图的红色坐标系的姿态，因为按照这里坐标系的z/y/x轴逆时针动态旋转得到的yaw/pitch/roll才都是正数。为了方便后面讲解，我们将下面的红色坐标系定义为{quat}坐标系，即IMU的quaternion姿态坐标系；而下图绿色的坐标系就是IMU输出的加速度、角速度的坐标系，我们仍然称其为{imu}坐标系。则代码中的q_from = R_world_quat，而我们想要的是R_world_lidar，所以转换关系就是R_world_lidar = R_world_quat * R_quat_imu * R_imu_lidar，所以可以发现此时代码中的extQRPY = R_quat_imu * R_imu_lidar = R_quat_lidar，即lidar -> quat（lidar到红色坐标系）的旋转。

• 对于大多数IMU来说，{quat}系就是{imu}系，即上图的红色和绿色是同一个坐标系，则R_quat_imu = I_3，则extQRPY = R_quat_imu * R_imu_lidar = R_imu_lidar，即lidar -> imu的旋转，这和上面讲解的是一致的。
• 而对于作者使用的IMU来说，如上图所示，{quat}系和{imu}是两个坐标系，所以有：

# R_quat_imu, 即下图的  绿色IMU系 -> 红色quat系 的旋转，这个只和IMU有关
R_quat_imu: [0,    1,     0,
                             1,    0,     0,
                             0,    0,   -1];                       
1
2
3
4

而作者给的配置文件中的参数为：

# R_lidar_imu，也就是imu -> lidar的旋转，即下图 绿色IMU系 -> lidar系的旋转，只和安装位置有关
extrinsicRot: [-1,    0,    0, 
                              0,    1,    0, 
                              0,    0,   -1]
# R_quat_lidar，也就是lidar -> quat的旋转，即下图 lidar系 -> 红色quat系的旋转，和安装位置以及IMU自身有关
extrinsicRPY: [0,     1,    0, 
                           -1,     0,    0, 
                            0,      0,    1]     
1
2
3
4
5
6
7
8

计算验证结果为：

# 计算验证结果：R_quat_lidar =    R_quat_imu * R_imu_lidar
                                                                    [ 0,   1,   0,              [ -1,    0,    0,
                                                             =      1,   0,   0,      *          0,    1,     0,  
                                                                     0,   0,   -1]                  0,    0,    -1]
                                                                   [ 0,   1,   0,
                                                             =     -1,   0,   0,
                                                                      0,   0,   1]
1
2
3
4
5
6
7

可以看到结果是可以和作者给的配置文件的参数对应的。

1.4.params_lidar.yaml中LIO外参修改

1.4.1.作者给的参数注释问题

再次回顾原作者给出的配置文件中的参数含义：

• extrinsicRot: R_lidar_imu，也就是imu -> lidar的旋转，其中{imu}是加速度、角速度输出结果所在的坐标系，也就是前面的图中绿色的坐标系；
• extrinsicRPY: R_quat_lidar，也就是lidar -> quat的旋转，其中{quat}是IMU输出姿态表示的坐标系，也就是前面的图中红色的坐标系。

再看作者给出的配置文件中的参数，可以发现，作者的注释存在歧义！其中的extrinsicTrans/extrinsicRot是IMU坐标系（上图绿色）和LiDAR之间的外参，因为extrinsicRot = R_lidar_imu，即imu -> lidar的旋转；所以我们可以同理类推得到extrinsicTrans = t_lidar_imu，即imu -> lidar的平移。也就是这两个变量组合在起来表示的是T_lidar_imu，即imu -> lidar的坐标变换，而作者给出的注释是lidar -> imu，显然是不对的。

# Extrinsics (lidar -> IMU)
extrinsicTrans: [0.0, 0.0, 0.0]
extrinsicRot: [-1, 0, 0, 0, 1, 0, 0, 0, -1]
extrinsicRPY: [0, 1, 0, -1, 0, 0, 0, 0, 1]
1
2
3
4

更新：注意这里LIO-SAM和LVI-SAM代码的外参含义并不一样。这是因为开发LVI-SAM的时候，作者用的还是自己之前版本的LIO-SAM代码，而LIO-SAM开源后GitHub上有人提PR对代码进行了修改，这就导致LVI-SAM中LIO部分的代码和LIO-SAM的代码并不一样。尤其是外参部分，他们的对比如下：

/********************************************  LIO-SAM 代码  ********************************************/
// R_lidar_imu
 extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
// R_lidar_quat
extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
// t_lidar_imu
 extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
 // R_quat_lidar
 extQRPY = Eigen::Quaterniond(extRPY).inverse();

// 把IMU坐标系的加速度、角速度 转成 lidar坐标系的加速度、角速度：mea_lidar = R_lidar_imu * mea_imu
 acc = extRot * acc;
 gyr = extRot * gyr;
// 把IMU坐标系的姿态 转成 lidar坐标系的姿态：R_world_lidar = R_world_quat * R_quat_lidar
 Eigen::Quaterniond q_final = q_from * extQRPY;


/********************************************  LVI-SAM 代码  ********************************************/
// R_lidar_imu
extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
// R_quat_lidar
extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
// t_lidar_imu
extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
// R_quat_lidar
extQRPY = Eigen::Quaterniond(extRPY);

// 把IMU坐标系的加速度、角速度 转成 lidar坐标系的加速度、角速度：mea_lidar = R_lidar_imu * mea_imu
 acc = extRot * acc;
 gyr = extRot * gyr;
// 把IMU坐标系的姿态 转成 lidar坐标系的姿态：R_world_lidar = R_world_quat * R_quat_lidar
 Eigen::Quaterniond q_final = q_from * extQRPY;
 
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
30
31
32
33
34

可以看到，LIO-SAM和LVI-SAM的配置文件中，关于extrinsicRPY的定义恰好是相反的。

1.4.2.自己修改代码

1.外参配置文件修改
为了清晰的定义外参，我将后面的LIO和VIO参数都定义为以IMU坐标系（上图绿色）为中心坐标系，即所有配置文件中给出的外参都是T_imu_xxx，即xxx -> imu的欧式变换。

对于LIO的外参来说，将配置文件中的参数定义为T_imu_lidar，并把原来作者给的外参注释掉，对应的yaml文件如下：

  #####################  注释掉原始的外参，不再使用 ##############################
  # # Extrinsics (lidar -> IMU)   
  # extrinsicTrans: [0.0, 0.0, 0.0]
  # extrinsicRot: [-1, 0, 0, 0, 1, 0, 0, 0, -1]
  # extrinsicRPY: [0, 1, 0, -1, 0, 0, 0, 0, 1]
  
  ###################### extrinsic between IMU and LiDAR  ###########################
  ###################### T_IMU_LiDAR, LiDAR -> IMU       ###########################
  # t_imu_lidar
  extrinsicTranslation: [0.0, 0.0, 0.0]    
  # R_imu_lidar
  extrinsicRotation: [-1,  0,  0, 
                      0,   1,  0, 
                      0,   0,  -1]
  yawAxis: "-z"      # 绕着哪个轴逆时针转动，输出yaw角度为正
  pitchAxis: "+x"    # 绕着哪个轴逆时针转动，输出pitch角度为正
  rollAxis: "+y"     # 绕着哪个轴逆时针转动，输出roll角度为正
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

• 其中extrinsicTranslation = t_imu_lidar，extrinsicRotation = R_imu_lidar，他们一起组成了T_imu_lidar。
• 下面的yawAxis/pitchAxis/rollAxis则反映了所使用的IMU逆时针绕着哪个坐标轴旋转输出的欧拉角为正数，比如对于原作者使用的IMU，yaw/pitch/roll输出正的欧拉角分别是逆时针绕着-z/+x/+y旋转。注意：这个参数并不是安装外参，而是IMU本身的性质，和如何安装没有任何关系！

2.代码修改
这部分主要讲解如何从上述的配置文件中转化成我们想要的外参，这部分代码在lidar_odometry/utility.h的ParamServer类构造函数中，代码如下：

nh.param<vector<double>>(PROJECT_NAME+ "/extrinsicTranslation", t_imu_lidar_V, vector<double>());
nh.param<vector<double>>(PROJECT_NAME+ "/extrinsicRotation", R_imu_lidar_V, vector<double>());
t_imu_lidar = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(t_imu_lidar_V.data(), 3, 1);
Eigen::Matrix3d R_tmp = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(R_imu_lidar_V.data(), 3, 3);
ROS_ASSERT(abs(R_tmp.determinant()) > 0.9);   // 防止配置文件中写错，这里加一个断言判断一下
R_imu_lidar = Eigen::Quaterniond(R_tmp).normalized().toRotationMatrix();
R_lidar_imu = R_imu_lidar.transpose();

//; yaw/pitch/roll的欧拉角绕着哪个轴逆时针旋转，结果为正数。一般来说是绕着+z、+y、+x
std::string yaw_axis, pitch_axis, roll_axis;   
nh.param<std::string>(PROJECT_NAME + "/yawAxis", yaw_axis, "+z");
ROS_ASSERT(yaw_axis[0] == '+' || yaw_axis[0] == '-');
nh.param<std::string>(PROJECT_NAME + "/pitchAxis", pitch_axis, "+y");
ROS_ASSERT(pitch_axis[0] == '+' || pitch_axis[0] == '-');
nh.param<std::string>(PROJECT_NAME + "/rollAxis", roll_axis, "+x");
ROS_ASSERT(roll_axis[0] == '+' || roll_axis[0] == '-');
ROS_ASSERT(yaw_axis[1] != pitch_axis[1] && yaw_axis[1] != roll_axis[1] && pitch_axis[1] != roll_axis[1]);

//; 旋转的欧拉角坐标系(quat) -> IMU角速度、加速度坐标系(imu) 的旋转
Eigen::Matrix3d R_imu_quat;   
std::unordered_map<std::string, Eigen::Vector3d> col_map;
col_map.insert({"+x", Eigen::Vector3d( 1,  0,  0)}); 
col_map.insert({"-x", Eigen::Vector3d(-1,  0,  0)});
col_map.insert({"+y", Eigen::Vector3d( 0,  1,  0)}); 
col_map.insert({"-y", Eigen::Vector3d( 0, -1,  0)});
col_map.insert({"+z", Eigen::Vector3d( 0,  0,  1)}); 
col_map.insert({"-z", Eigen::Vector3d( 0,  0, -1)});
R_imu_quat.col(2) = col_map[yaw_axis];
R_imu_quat.col(1) = col_map[pitch_axis];
R_imu_quat.col(0) = col_map[roll_axis];
ROS_ASSERT(abs(R_imu_quat.determinant()) > 0.9);  

//; R_quat_lidar = R_quat_imu * R_imu_lidar
Eigen::Matrix3d R_quat_lidar = R_imu_quat.transpose() * R_imu_lidar;  
Q_quat_lidar = Eigen::Quaterniond(R_quat_lidar).normalized();
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
30
31
32
33
34
35

首先t_imu_lidar和R_imu_lidar部分很简单，直接读取了转换成Eigen格式即可。

主要的内容在根据yaw/pitch/roll的旋转轴计算R_imu_quat的部分。其实这部分也很简单，我们知道R_imu_quat从左到右的三列分别就是{quat}坐标系的xyz三个轴在{imu}坐标系下的表示。而yaw角度对应的是{quat}坐标系的z轴，也就对应R_imu_quat的第3列。其绕着{imu}系的哪个轴旋转，就决定了这一列该填什么。比如如果它绕着-x旋转，则第3列就是[-1, 0, 0]^T；绕着+y旋转，则第3列就是[0, 1, 0]^T；绕着-z旋转，则第3列就是[0, 0, -1]^T。然后pitch角度对应R_imu_quat的第2列，roll角度对应R_imu_quat的第1列，这两列怎么填的计算方法和前面说的一样。

其余还有很多修改部分，这里不再赘述，感兴趣的读者可以去看我修改后的代码。

2.LVI-SAM中的坐标系定义

2.1.ROS中常见坐标系定义

2.1.1.map坐标系

ROS中常见坐标系定义，一般可以认为就是静止的世界坐标系。

2.1.2.odom坐标系

ROS中常见坐标系定义，一般也可以认为就是静止的世界坐标系。

之所以除了map之外又定义一个odom，其实是考虑基于地图地位的情况。此时地图的坐标系就是map，而里程计初始化的时候未必在地图坐标系的原点，所以就可以在初始化时刻的位置定义一个odom坐标系，这样后面里程计的位姿就可以是相对odom系的。而如果想计算在map系下的位姿，只需要知道odom和map之间的变换即可。

而在slam中由于是同时定位和建图，所以一般map和odom都是初始化时刻的起始坐标系，因此我们可以把两者都认为是静止的世界坐标系。

2.1.3.base_link坐标系

ROS中常见坐标系定义，一般认为就是运动物体的坐标系， 或者叫车体坐标系。

一般我们可以简化，认为base_link坐标系就是body坐标系，或者说就是IMU坐标系。具体怎么定义都可以，只要是运动物体上一个固定的坐标系即可。

注意：在LVI-SAM中，base_link指的是LiDAR坐标系，因为LIO-SAM是以LiDAR坐标系为主坐标系的。代码可以搜索tfOdom2BaseLink.sendTransform(odom_2_baselink);，这个是在IMU回调函数中发布预测的位姿，但是在之前由于把IMU数据转到了LiDAR坐标系下，所以这里发布的实际是LiDAR坐标系的位姿。或者搜索cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, "base_link");也可以发现这个地方是发布LiDAR点云，给的相对坐标系是base_link，这也说明base_link就是LiDAR坐标系。

2.2.LVI-SAM中的传感器坐标系

给出LVI-SAM运行时的tf树，其中只有红色圈出的是需要看的。剩下的坐标系没有用，代码中没有明确发布这些坐标变换。

2.2.1.map/odom/base_link坐标系

• map：静止的世界坐标系；
• odom：静止的世界坐标系，和map系重合，也是代码中LIO的世界坐标系；
• base_link：IMU频率的LiDAR坐标系，上面已经解释过了。

2.2.2.lidar_link坐标系

后端优化频率的LiDAR坐标系，这个就是在后端做了LiDAR的scan-to-map优化之后，得到的比较低频、但是更加准确的位姿。它和base_link坐标系是同一个坐标系，只不过发布频率不同。

2.2.3.vins_world坐标系

VINS-Mono的世界坐标系。这个坐标系和odom并不是同一个坐标系，而是作者特地把odom的坐标系绕着Z轴转了180度，然后得到了vins_world坐标系，感觉就是为了方便区分VIO和LIO的轨迹吧，不然没必要多此一举。

但是这个坐标系也不是一成不变的，而是可以通过配置文件调整是否让这个坐标系变化，也就是配置文件中的align_camera_lidar_estimation参数。如果它是1，则vins_world坐标系是运动的，主要就是因为vins漂移更大。 所以为了把vins和lio-sam估计的位姿对齐，作者就以IMU坐标系为媒介，把vins的漂移转移到vins_world的移动上，从而让VIO预测的位姿和LIO预测的位姿不会相差太大。在VINS-Fusion融合GPS的代码中也有相似的操作。

其实这个坐标系到底是否变化，对算法表现没有影响，只是对可视化结果有影响。如果保持不动，由于vins漂移比liosam大，所以会导致rviz显示的视觉的特征点点云（rviz中紫色和白色点云）和lidar建图的点云有较大的位置差。而如果让vinsworld坐标系保持运动，也就是时刻把vins估计的位姿和liosam估计的位姿对齐，从而把vins的累计漂移转移到vinsworld系相对odom系的运动上，那么可视化结果看起来视觉特征点的点云和lidar建图的点云就可以匹配上了。这二者的对比可以见下图，左图是保持vinsworld系不动，右图是动态计算vinsworld系和odom系的关系，可以看到左图的视觉特征点点云已经明显漂掉了，而右图仍然可以跟lidar的点云对齐。

但是如果保持vinsworld不动（即配置文件中align_camera_lidar_estimation=0），那么vins估计的漂移就很大了，这样不会给liosam预测的位姿带来很大的误差吗？其实并不会，因为liosam预测的位姿并不是用发来的里程计的绝对位姿，而是用这次发来的里程计位姿和上次发来的里程计位姿计算出增量位姿，然后加到上次优化后的位姿上，从而得到本次预测的优化前的位姿。所以尽管vins全局有很大的漂移，但是局部预测的增量位姿还是准确的，所以并不会对liosam后端优化的预测位姿造成影响。

2.2.4.vins_body坐标系

原来VINS-Mono中就有的，lvi-sam没有改动。这个就是VINS-Mono的IMU系。

2.2.5.vins_camera坐标系

原来VINS-Mono中就有的，lvi-sam没有改动。发布vins_camera相对vins_body的坐标系变换，也就是VIO外参，就得到了相机系在vins_world下的位姿。

2.2.6.vins_body_ros坐标系（难理解）

lvi-sam新加的，是IMU频率下的“vins_body”坐标系，坐标系的位置是IMU坐标系的原点，但是姿态是LiDAR坐标系的姿态（即把IMU姿态乘以R_imu_lidar）。该坐标系有两个作用：

(1) 发布里程计给LIO-SAM后端scan-to-map做优化的位姿初值（实际只用到了姿态，LIO-SAM一直都是只用预测的姿态，而不用预测的平移）。对应代码如下，注意其中作者的变量命名并不符合实际，以我的注释为准。

//; R_imu_lidar，即lidar -> imu（绿色）旋转
tf::Quaternion q_cam_to_lidar(0, 1, 0, 0); 
//; R_odom_lidar = R_odom_imu * R_imu_lidar
tf::Quaternion q_odom_ros = q_odom_cam * q_cam_to_lidar;
tf::quaternionTFToMsg(q_odom_ros, odometry.pose.pose.orientation);
pub_latest_odometry_ros.publish(odometry);
1
2
3
4
5
6

(2) 发布tf变换给VINS-Mono前端视觉深度注册变换LiDAR点云。代码如下：

// q_odom_ros就是R_odom_lidar
tf::Transform t_w_body = tf::Transform(q_odom_ros, tf::Vector3(P.x(), P.y(), P.z()));
tf::StampedTransform trans_world_vinsbody_ros = tf::StampedTransform(
		t_w_body, header.stamp, "vins_world", "vins_body_ros");
br.sendTransform(trans_world_vinsbody_ros);
1
2
3
4
5

实际上，这个地方的代码是作者直接取巧了，这里的本意并不是要发布lidar的tf坐标系，而是想发布相机的前左上(FLU)的tf坐标系。只不过恰好对于作者的设备来说lidar坐标系和相机的FLU坐标系平行，所以这里直接一个参数两个用途了。

关于这里为什么要用相机的FLU坐标系，具体讲解见下一章节。

3.视觉前端用LiDAR点云进行深度注册

3.1.前左上(FLU)坐标系

前面刚说过，vins_body_ros坐标系发布的是IMU频率下的、位置在IMU原点、姿态是lidar姿态的位姿。并且说了这里的本意并不是要发布lidar的tf坐标系，而是想发布相机的前左上(FLU)的tf坐标系。

那么什么是相机的前左上坐标系呢？如下图所示，对坐标系定义如下：

• 正常来说相机的坐标系xyz三轴顺序应该是右下前，也就是下图的青色坐标系，我们定义为{c}系；
• 相机的FLU坐标系即重新定义xyz让他们分别指向前、左、上，就是下图的黄色坐标系，我们定义为{cFLU}系；
• 下图蓝色就是lidar坐标系，也是前左上的坐标系，定义为{lidar}系。

注意：

• 什么叫前左上？这个是对于LiDAR或者相机来说的，因为这两个传感器有前向的定义。比如上图LiDAR的图标velodyne就是前向，相机的镜头前方也是前向，然后再根据y/z分别是左/上确定另外两个轴。
• 这个前向是只和传感器有关的，跟安装位置无关，比如上图的相机如果绕Z轴旋转90度安装，则它的前向还是它的镜头前方，跟它怎么安装没有任何关系。这也就意味着，上图作者使用的相机的FLU坐标系恰好和LIDAR的坐标系平行只是因为安装的巧合，所以才导致了前面说的可以共用lidar坐标系和相机的FLU坐标系的位姿（姿态完全相同，平移较小忽略）。

3.2.为什么要用FLU坐标系

那么问题来了，对视觉前端特征点用lidar点云进行深度注册，为什么要使用相机的FLU坐标系呢？

其实这个也是作者为了写代码方便，为了复用自己之前写过的代码。

在LeGO-LOAM中，作者为了去除点云的噪点，要使用BFS广度优先搜索算法对点云进行聚类。为了组织点云，会把点云投影到一个矩阵上，这里称为range图像。而投影过程中为了计算点云中的某个点属于这个range图像的行列位置，需要利用坐标计算角度，代码如下：

// 3. project depth cloud on a range image, filter points satcked in the same region
float bin_res = 180.0 / (float)num_bins; // currently only cover the space in front of lidar (-90 ~ 90)
cv::Mat rangeImage = cv::Mat(num_bins, num_bins, CV_32F, cv::Scalar::all(FLT_MAX));

for (int i = 0; i < (int)depth_cloud_local->size(); ++i)
{
	    PointType p = depth_cloud_local->points[i];
	    // filter points not in camera view
	    if (p.x < 0 || abs(p.y / p.x) > 10 || abs(p.z / p.x) > 10)
	        continue;
	    // find row id in range image
	    float row_angle = atan2(p.z, sqrt(p.x * p.x + p.y * p.y)) * 180.0 / M_PI + 90.0; // degrees, bottom -> up, 0 -> 360
	    int row_id = round(row_angle / bin_res);
	    // find column id in range image
	    float col_angle = atan2(p.x, p.y) * 180.0 / M_PI; // degrees, left -> right, 0 -> 360
	    int col_id = round(col_angle / bin_res);
	    // id may be out of boundary
	    if (row_id < 0 || row_id >= num_bins || col_id < 0 || col_id >= num_bins)
	        continue;
	    // only keep points that's closer
	    float dist = pointDistance(p);
	    if (dist < rangeImage.at<float>(row_id, col_id))
	    {
	        rangeImage.at<float>(row_id, col_id) = dist;
	        pointsArray[row_id][col_id] = p;
	    }
}
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

可以看到代码中使用了点的xyz坐标来计算角度，而这个角度是在lidar坐标系下计算的，也就是点的xyz坐标服从lidar的前左上坐标系定义。

现在为了用lidar点云进行深度注册，也要进行lidar点云的投影，并且是要投影到相机的坐标系下，从而和相机坐标系下检测到的视觉特征点做关联。所以为了代码复用，作者直接把这段代码复制过来了。但是点云的xyz仍然是前左上坐标系，所以不修改点云坐标的最简单的方式就是把相机的坐标系也使用前左上(FLU)坐标系，这样这段代码就可以直接使用而不需要对点云坐标进行变换了。

所以深度注册这里，只需要知道相机的FLU坐标系位姿，就可以进行LIDAR点云的投影，进而关联特征点深度了。

3.3.前端深度注册逻辑

3.3.1.累积vinsworld坐标系下的LiDAR点云

这部分是feature_tracker_node.cpp中的void lidar_callback(const sensor_msgs::PointCloud2ConstPtr& laser_msg)函数，总结逻辑如下：

1. 监听相机的FLU坐标系到世界坐标系的位姿T_vinsworld_cFLU。由于作者使用的设备的FLU坐标系和LiDAR坐标系刚好平行，所以直接监听vins_body_ros即可。
2. 把收到的lidar坐标系下的点云$P^{lidar}$，通过外参(L_C_TX, L_C_TY, L_C_TZ, L_C_RX, L_C_RY, L_C_RZ)转到相机的FLU坐标系下表示，即变成$P^{cFLU}$。这里可以发现，如果不理解代码，在配置文件中是很难把这个参数给对的。
3. 通过第1步监听的相机的FLU坐标系到世界坐标系的位姿T_vinsworld_cFLU，把相机FLU坐标系下的点云$P^{cFLU}$变成vinsworld坐标系下的点云$P^{vinsworld}=T\_vinsworld\_cFLU\ast P^{cFLU}$，从而可以对点云累加。

3.3.2.点云投影到相机系下做深度注册

这部分是feature_tracker.h中的 sensor_msgs::ChannelFloat32 get_depth()函数，总结逻辑如下：

1. 监听相机的FLU坐标系到世界坐标系的位姿T_vinsworld_cFLU。由于作者使用的设备的FLU坐标系和LiDAR坐标系刚好平行，所以直接监听vins_body_ros即可。
2. 把之前累积的vinsworld坐标系下的点云$P^{vinsworld}$转到当前的相机FLU坐标系下，即$P^{cFLU}=T\_cFLU\_vinsworld\ast P^{vinsworld}$。
3. 将相机前左上坐标系cFLU下的LiDAR点云和视觉特征点做关联，进行深度注册。

3.4.以修改后的代码为例讲解前端深度注册逻辑

3.4.1.累积vinsworld坐标系下的LiDAR点云

这部分是feature_tracker_node.cpp中的void lidar_callback(const sensor_msgs::PointCloud2ConstPtr& laser_msg)函数，修改后的代码流程分析如下（为了简洁起见略去了一些不重要的代码）：

1.监听坐标变换

这里要监听的是vinsworld坐标系和相机的FLU坐标系cFLU之间的坐标变换，即transform_world_cFLU；以及相机的FLU坐标系cFLU和IMU坐标系之间的坐标变换，即transform_cFLU_imu。注意：相机的FLU坐标系cFLU就是前面说的，以相机为中心的前左上坐标系，和IMU没有任何关系。

// 0. listen to transform
static tf::TransformListener listener;
static tf::StampedTransform transform_world_cFLU;   //; T_vinsworld_camera_FLU
static tf::StampedTransform transform_cFLU_imu;    //; T_cameraFLU_imu

try{
    //? mod: 监听T_vinsworld_cameraFLU 和 T_cameraFLU_imu
    listener.waitForTransform("vins_world", "vins_cameraFLU", laser_msg->header.stamp, ros::Duration(0.01));
    listener.lookupTransform("vins_world", "vins_cameraFLU", laser_msg->header.stamp, transform_world_cFLU);
    listener.waitForTransform("vins_cameraFLU", "vins_body_imuhz", laser_msg->header.stamp, ros::Duration(0.01));
    listener.lookupTransform("vins_cameraFLU", "vins_body_imuhz", laser_msg->header.stamp, transform_cFLU_imu);
} 

double xCur, yCur, zCur, rollCur, pitchCur, yawCur;
xCur = transform_world_cFLU.getOrigin().x();
yCur = transform_world_cFLU.getOrigin().y();
zCur = transform_world_cFLU.getOrigin().z();
tf::Matrix3x3 m(transform_world_cFLU.getRotation());
m.getRPY(rollCur, pitchCur, yawCur);
//; T_vinswolrd_cameraFLU
Eigen::Affine3f transNow = pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

2. 把LiDAR本体坐标系下的点云，转到相机的FLU坐标系cFLU下表示

这个操作主要是为了下一步准备，下一步要根据相机的FoV对当前的LiDAR点云进行筛选，去掉不在相机FoV内的点。为什么要转到相机FLU坐标系下表示呢？还是前面所说的，作者为了复用代码。

// 3. 把lidar坐标系下的点云转到相机的FLU坐标系下表示，因为下一步需要使用相机FLU坐标系下的点云进行初步过滤
pcl::PointCloud<PointType>::Ptr laser_cloud_offset(new pcl::PointCloud<PointType>());
//; T_cFLU_lidar
tf::Transform transform_cFLU_lidar = transform_cFLU_imu * Transform_imu_lidar;
double roll, pitch, yaw, x, y, z;
x = transform_cFLU_lidar.getOrigin().getX();
y = transform_cFLU_lidar.getOrigin().getY();
z = transform_cFLU_lidar.getOrigin().getZ();
tf::Matrix3x3(transform_cFLU_lidar.getRotation()).getRPY(roll, pitch, yaw);
Eigen::Affine3f transOffset = pcl::getTransformation(x, y, z, roll, pitch, yaw);
//; lidar本体坐标系下的点云，转到相机FLU坐标系下表示
pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_offset, transOffset);
*laser_cloud_in = *laser_cloud_offset;
1
2
3
4
5
6
7
8
9
10
11
12
13

3.过滤不在相机FoV内的点云

至此我们得到的已经是相机的前左上坐标系cFLU下的点云了，那么再利用相机的FoV大小，过滤掉其他不在相机FoV范围内的点云，降低后面进一步处理的计算量。

// 4. filter lidar points (only keep points in camera view)
//; 根据已经转到相机FLU坐标系下的点云，先排除不在相机FoV内的点云
pcl::PointCloud<PointType>::Ptr laser_cloud_in_filter(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)laser_cloud_in->size(); ++i)
{
    PointType p = laser_cloud_in->points[i];
    if (p.x >= 0 && abs(p.y / p.x) <= 10 && abs(p.z / p.x) <= 10)
        laser_cloud_in_filter->push_back(p);
}
*laser_cloud_in = *laser_cloud_in_filter;
1
2
3
4
5
6
7
8
9
10

代码中if (p.x >= 0 && abs(p.y / p.x) <= 10 && abs(p.z / p.x) <= 10)这一句就是寻找在相机FOV内的点，其使用的坐标系就是相机的FLU坐标系，也就是下图中的红色坐标系。可以看到根据相机FoV进行筛选的三个判断条件：

• p.x >= 0：点在相机前方
• abs(p.y / p.x) <= 10：点在相机水平方向FoV内
• abs(p.z / p.x) <= 10：点在相机垂直方向FoV内

从这里的代码也可以看出来，这里的点云确实是相机的前左上坐标系下的，而不是普通的相机坐标系下的。

4.把过滤后的相机FLU坐标系cFLU下的点云，转到vinsworld坐标系下表示

这个操作主要是为了下一步累积点云做准备，因为只有在同一个坐标系下的点云才能累积融合。

// 5. transform new cloud into global odom frame
pcl::PointCloud<PointType>::Ptr laser_cloud_global(new pcl::PointCloud<PointType>());
//; cameraFLU坐标系下的点云，转到vinsworld系下表示
pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_global, transNow);
1
2
3
4

5.累积vinsworld坐标系下的多帧点云

累积点云是为了让点云更密，这样和视觉特征点进行关联的时候才能容易找到关联的点云，结果也更准确。

// 8. fuse global cloud
depthCloud->clear();
for (int i = 0; i < (int)cloudQueue.size(); ++i)
    *depthCloud += cloudQueue[i];
1
2
3
4

3.4.2.点云投影到相机系下做深度注册

这部分是feature_tracker.h中的 sensor_msgs::ChannelFloat32 get_depth()函数，代码只做了一点很小的修改，就是监听的坐标系变换的名字，源代码是vins_body_ros，而我修改后的代码为了清晰起见叫做vins_cameraFLU。这里给出这部分的思路和对应代码如下：

1.监听相机的FLU坐标系到世界坐标系的位姿T_vinsworld_cFLU

// 0.3 look up transform at current image time
try
{
    //? mod: 直接监听vins_camFLU坐标系在世界坐标系下的表示，这样就把VIO的动态外参包括进去了
    listener.waitForTransform("vins_world", "vins_cameraFLU", stamp_cur, ros::Duration(0.01));
    listener.lookupTransform("vins_world", "vins_cameraFLU", stamp_cur, transform);
}

double xCur, yCur, zCur, rollCur, pitchCur, yawCur;
xCur = transform.getOrigin().x();
yCur = transform.getOrigin().y();
zCur = transform.getOrigin().z();
tf::Matrix3x3 m(transform.getRotation());
m.getRPY(rollCur, pitchCur, yawCur);
Eigen::Affine3f transNow = pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

2.把之前累积的vinsworld坐标系下的点云$P^{vinsworld}$转到当前的相机FLU坐标系下，即$P^{cFLU}=T\_cFLU\_vinsworld\ast P^{vinsworld}$

// 0.4 transform cloud from global frame to camera frame
pcl::PointCloud<PointType>::Ptr depth_cloud_local(new pcl::PointCloud<PointType>());
//; 这里就是把vins_world坐标系下的点云，转到了camera的FLU坐标系下
pcl::transformPointCloud(*depthCloud, *depth_cloud_local, transNow.inverse());
1
2
3
4

3.将相机前左上坐标系cFLU下的LiDAR点云和视觉特征点做关联，进行深度注册
后面的所有代码都是在进行这部分操作，具体原理可以参考论文，比较繁琐这里就不赘述了。

4.多种数据集测试

将修改后的代码在官方数据集、M2DGR数据集、UrbanNavDataset数据集、KITTI数据集、自己的设备采集的数据集上进行了广泛测试，主要是测试根据这些数据集给的外参，写入到修改后的代码的配置文件中，代码是否可以正常运行。

经过测试这些代码在这些数据集上均可以正常运行，测试 BiliBili视频 如下：