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

阿里云国内75折 回扣 微信号：monov8
阿里云国际，腾讯云国际，低至75折。AWS 93折 免费开户实名账号 代冲值 优惠多多 微信号：monov8 飞机：@monov6

文章目录

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和IMUrotation是统一的都是为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坐标系下表示的即 m e a i m u mea^{imu} meaimu上标表示该向量在哪个坐标系下表示。现在想把加速度/角速度转成在lidar坐标系下表示则有 m e a l i d a r = R _ l i d a r _ i m u ∗ m e a i m u mea^{lidar} = R \_lidar \_ imu * mea^{imu} mealidar=R_lidar_imu∗meaimu。

对应代码如下

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{	
		.......
		acc = extRot * acc;     // extRot就是配置文件中的extrinsicRot
		gyr = extRot * gyr;     // extRot就是配置文件中的extrinsicRot
		.......
}
  • 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
		.......
}

其实这个参数就是把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 -> quatlidar到红色坐标系的旋转。

在这里插入图片描述

  • 对于大多数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];

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

# 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]

计算验证结果为

# 计算验证结果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.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.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角度为正
  • 其中extrinsicTranslation = t_imu_lidarextrinsicRotation = R_imu_lidar他们一起组成了T_imu_lidar。
  • 下面的yawAxis/pitchAxis/rollAxis则反映了所使用的IMU逆时针绕着哪个坐标轴旋转输出的欧拉角为正数比如对于原作者使用的IMUyaw/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();

首先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_linkIMU频率的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);

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

实际上这个地方的代码是作者直接取巧了这里的本意并不是要发布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;
	    }
}

可以看到代码中使用了点的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 l i d a r P^{lidar} Plidar通过外参(L_C_TX, L_C_TY, L_C_TZ, L_C_RX, L_C_RY, L_C_RZ)转到相机的FLU坐标系下表示即变成 P c F L U P^{cFLU} PcFLU。这里可以发现如果不理解代码在配置文件中是很难把这个参数给对的。
  3. 通过第1步监听的相机的FLU坐标系到世界坐标系的位姿T_vinsworld_cFLU把相机FLU坐标系下的点云 P c F L U P^{cFLU} PcFLU变成vinsworld坐标系下的点云 P v i n s w o r l d = T _ v i n s w o r l d _ c F L U ∗ P c F L U P^{vinsworld} = T\_vinsworld\_cFLU * P^{cFLU} Pvinsworld=T_vinsworld_cFLU∗PcFLU从而可以对点云累加。

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

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

  1. 监听相机的FLU坐标系到世界坐标系的位姿T_vinsworld_cFLU。由于作者使用的设备的FLU坐标系和LiDAR坐标系刚好平行所以直接监听vins_body_ros即可。
  2. 把之前累积的vinsworld坐标系下的点云 P v i n s w o r l d P^{vinsworld} Pvinsworld转到当前的相机FLU坐标系下即 P c F L U = T _ c F L U _ v i n s w o r l d ∗ P v i n s w o r l d P^{cFLU} = T\_cFLU\_vinsworld * P^{vinsworld} PcFLU=T_cFLU_vinsworld∗Pvinsworld。
  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);

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;

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;

代码中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);

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

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

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

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

2.把之前累积的vinsworld坐标系下的点云 P v i n s w o r l d P^{vinsworld} Pvinsworld转到当前的相机FLU坐标系下即 P c F L U = T _ c F L U _ v i n s w o r l d ∗ P v i n s w o r l d P^{cFLU} = T\_cFLU\_vinsworld * P^{vinsworld} PcFLU=T_cFLU_vinsworld∗Pvinsworld

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

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

4.多种数据集测试

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

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

修改LVI-SAM代码适配M2DGR、UrbanNavDataset、KITTI和自己的数据集

阿里云国内75折 回扣 微信号：monov8
阿里云国际，腾讯云国际，低至75折。AWS 93折 免费开户实名账号 代冲值 优惠多多 微信号：monov8 飞机：@monov6
返回列表

上一篇：2022全国职业技能大赛大数据技术与应用赛项赛题环境准备

下一篇：音频转文字怎么转？三个方法教你音频转文字

“LVI-SAM坐标系外参分析与代码修改，以适配各种数据集” 的相关文章

初学者掌握IP地址1年前 (2023-02-02)
【UIButton和UIImageView的区别 Objective-C语言】1年前 (2023-02-02)
AcWing257 关押罪犯1年前 (2023-02-02)
（10）go-micro微服务发送邮件1年前 (2023-02-02)
C++ 序列化和反序列化1年前 (2023-02-02)
Earthdata批量下载MODIS遥感影像的方法1年前 (2023-02-02)
Matplotlib绘制散点图与条形图1年前 (2023-02-02)
学习笔记——Spring中的注解；Spring中装配对象的注解；使用注解配置对象中属性1年前 (2023-02-02)
C#11新特性整理1年前 (2023-02-02)
学习笔记——SpringMVC文件上传与下载1年前 (2023-02-02)

阿里云国际版