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

news2025/1/12 8:41:56

文章目录

  • 0.前言
  • 1.原作者传感器件坐标系定义与外参修改
    • 1.1.博客作者的讲解(仅供参考)
    • 1.2.LIO-SAM的README中作者对其传感器配置的解释
    • 1.3.IMU坐标系详解
    • 1.4.params_lidar.yaml中LIO外参修改
      • 1.4.1.作者给的参数注释问题
      • 1.4.2.自己修改代码
  • 2.LVI-SAM中的坐标系定义
    • 2.1.ROS中常见坐标系定义
      • 2.1.1.map坐标系
      • 2.1.2.odom坐标系
      • 2.1.3.base_link坐标系
    • 2.2.LVI-SAM中的传感器坐标系
      • 2.2.1.map/odom/base_link坐标系
      • 2.2.2.lidar_link坐标系
      • 2.2.3.vins_world坐标系
      • 2.2.4.vins_body坐标系
      • 2.2.5.vins_camera坐标系
      • 2.2.6.vins_body_ros坐标系(==难理解==)
  • 3.视觉前端用LiDAR点云进行深度注册
    • 3.1.前左上(FLU)坐标系
    • 3.2.为什么要用FLU坐标系
    • 3.3.前端深度注册逻辑
      • 3.3.1.累积vinsworld坐标系下的LiDAR点云
      • 3.3.2.点云投影到相机系下做深度注册

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/imu2lidar是从后往前读还是从前往后读?
  • 配置文件中外参多余,且关系不满足闭环: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/zlidar_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代码中的各种坐标系定义,以及视觉特征点深度注册中的坐标系变换;
  • 给出我修改后的代码:该代码重新定义了各种传感器坐标系变量,让其见名知意;同时解决了所说的所有问题,目的是为了切换数据集或使用自己的设备时,能够很容易的将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”相同)。

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_imumeaimu

对应代码如下:

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

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

# 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,也就是quat -> lidar的旋转,其中{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坐标系(上图绿色)为中心坐标系,即所有配置文件中给出的外参都是xx -> 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逆时针绕着哪个坐标轴旋转输出的欧拉角为正数,比如对于原作者使用的IMU,yaw/pitch/roll输出正的欧拉角分别是逆时针绕着-z/+x/+y旋转。

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

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_lidarR_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];绕着-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的代码中也有相似的操作

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^{vins_world} = T\_vinsworld\_cFLU * P^{cFLU} Pvinsworld=T_vinsworld_cFLUPcFLU,从而可以对点云累加。

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^{vins_world} 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_vinsworldPvinsworld
  3. 将相机坐标系下的LiDAR点云和视觉特征点做关联,进行深度注册。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/94532.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

Redis常见面试题(一)

目录 1、Redis是什么? 2、Redis有哪些应用场景? 3、Redis有什么优势? 4、Redis为什么这么快? 5、Redis主要消耗什么物理资源? 6、Redis为什么把所有数据放到内存中? 7、Redis命令是原子性的吗? 8、Redis磁盘快照操作是原子的吗? 9、Redis怎么测试连通性? 10、…

【图像去噪】均值+中值+空间+高斯滤波图像去噪【含GUI Matlab源码 763期】

⛄一、图像去噪及滤波简介 1 图像去噪 1.1 图像噪声定义 噪声是干扰图像视觉效果的重要因素&#xff0c;图像去噪是指减少图像中噪声的过程。噪声分类有三种&#xff1a;加性噪声&#xff0c;乘性噪声和量化噪声。我们用f(x,y&#xff09;表示图像&#xff0c;g(x,y&#xff0…

四十六——

四十六、JavaScript——对象 一、对象 数据类型&#xff1a;原始值&#xff1a; 1. 数值 Number 2. 大整数 BigInt 3. 字符串 String 4. 布尔值 Boolean 5. 空值 Null 6. 未定义 Undefinded 7. 符号 Symbol 除了七种原始值之外&#xff0c;后面所用到的数据类型&#xff0c;都…

jsp+ssm计算机毕业设计茶园文化交流平台论文【附源码】

项目运行 环境配置&#xff1a; Jdk1.8 Tomcat7.0 Mysql HBuilderX&#xff08;Webstorm也行&#xff09; Eclispe&#xff08;IntelliJ IDEA,Eclispe,MyEclispe,Sts都支持&#xff09;。 项目技术&#xff1a; JSPSSM mybatis Maven等等组成&#xff0c;B/S模式 Mave…

jsp+ssm计算机毕业设计超市收银系统论文【附源码】

项目运行 环境配置&#xff1a; Jdk1.8 Tomcat7.0 Mysql HBuilderX&#xff08;Webstorm也行&#xff09; Eclispe&#xff08;IntelliJ IDEA,Eclispe,MyEclispe,Sts都支持&#xff09;。 项目技术&#xff1a; JSPSSM mybatis Maven等等组成&#xff0c;B/S模式 Mave…

【LeetCode101. 对称二叉树】—— 二叉树遍历

101. 对称二叉树 给你一个二叉树的根节点 root &#xff0c; 检查它是否轴对称。 示例 1&#xff1a; 输入&#xff1a;root [1,2,2,3,4,4,3] 输出&#xff1a;true示例 2&#xff1a; 输入&#xff1a;root [1,2,2,null,3,null,3] 输出&#xff1a;false提示&#xff1a; …

【探索Spring底层】12.谈谈代理创建器与代理创建时机

文章目录1. 前言2. 谈谈代理创建器3. 代理创建时机是什么时候4. 浅谈Order的失效场景4. 浅谈Order的失效场景5. 高级切面如何转为低级切面1. 前言 Spring中有两种切面&#xff0c;一种是Aspect&#xff0c;另一种是Advisor 其中Aspect是高级切面&#xff0c;Advisor是低级切面…

【网站架构】网站系统怎么才是安全的?安全验收?等保、网络安全、SQL盲注、https、鉴权

大家好&#xff0c;欢迎来到停止重构的频道。 本期我们讨论网站系统的安全性。 安全的重要性不言而喻&#xff0c;大部分安全问题确实是安全扫描后根据指引修改就可以了。 但是仍有一些问题修改起来是特别麻烦的&#xff0c;这些问题会严重影响上线时间。 本期我们的重点不…

共享购模式简单又好玩,撑起市场的半边天,推动实体产业改造上级

在2022年1月18日&#xff0c;国家发展改革委等七部局下发《促进消费实施方案》的通知&#xff0c;确立了”消费送积分”的新形式。该政策的出台&#xff0c;表明了政府探索实施全国绿色消费积分制度&#xff0c;鼓励地方结合实际建立本地绿色消费积分制度&#xff0c;以兑换商品…

S3 Drive支持以及FIPS 140-2兼容性

S3 Drive支持以及FIPS 140-2兼容性 在Windows Arm64上运行-添加了在Microsoft Windows for Arm64上的功能。无需额外下载&#xff0c;安装程序将为您的系统选择正确的驱动程序和库。 现在符合FIPS 140-2。 现在&#xff0c;您可以使用新的CacheOnlyFiles设置阻止上载临时(或其他…

基于高分辨率时频分析的单通道地震数据自动噪声衰减方法(Matlab代码实现)

目录 &#x1f4a5;1 概述 &#x1f4da;2 运行结果 &#x1f389;3 参考文献 &#x1f468;‍&#x1f4bb;4 Matlab代码 &#x1f4a5;1 概述 记录的地震信号常常被噪声破坏。本文使用了一种基于高分辨率时频分析的单通道地震数据自动噪声衰减方法。同步压缩是一种时频重…

MySQL——保证主从一致

binlog 可以用来归档&#xff0c;也可以用来做主备同步&#xff0c;备库执行了 binlog 就可以跟主库保持一致。 MySQL 主备的基本原理 如图 1 所示就是基本的主备切换流程。 在状态 1 中&#xff0c;客户端的读写都直接访问节点 A&#xff0c;而节点 B 是 A 的备库&#xff…

Java安全--CC4

CC4 环境提一小嘴&#xff1a; CC4利用的是commons-collections4&#xff0c;所以我们需要导入新的依赖&#xff0c;地址&#xff1a;https://mvnrepository.com/artifact/org.apache.commons/commons-collections4/4.0 我们先来关注一下利用链&#xff1a; 后半段是一样的&am…

本地运行好好的 Java 程序, 一发布到线上就报错的灵异事件终于让我碰到了

说明 本文涉及的相关软件版本如下&#xff1a; mybatis 3.4.xHotSpot JDK1.8Windows 11IDEA 2022.3 先看一段 mybatis 相关的代码 今天一个朋友丢给我如下一段代码&#xff1a; 然后跟我讲为什么本地是好好的&#xff0c; 发布到线上执行就报错。 BlogMapper.java public…

【python机器学习】K-Means算法详解及给坐标点聚类实战(附源码和数据集 超详细)

需要源码和数据集请点赞关注收藏后评论区留言私信~~~ 人们在面对大量未知事物时&#xff0c;往往会采取分而治之的策略&#xff0c;即先将事物按照相似性分成多个组&#xff0c;然后按组对事物进行处理。机器学习里的聚类就是用来完成对事物进行分组的任务 一、样本处理 聚类…

技术原理|Hologres Binlog技术原理揭秘

作者&#xff1a;张高迪&#xff08;花名杳天&#xff09;&#xff0c;Hologres研发。 同传统MySQL数据库&#xff0c;Hologres支持Hologres binlog&#xff0c;记录数据库中所有数据的变化事件日志。通过Hologres binlog&#xff0c;可以非常方便灵活的实现数据之间的复制、同…

“电池黑马”瑞浦兰钧增速惊人,动储双起飞

撰稿 | 多客 来源 | 贝多财经 12月14日&#xff0c;“电池黑马”瑞浦兰钧能源股份有限公司&#xff08;以下简称“瑞浦兰钧”&#xff09;向港交所主板提交上市申请&#xff0c;摩根士丹利和中信证券为其联席保荐人。至此&#xff0c;国内动力电池装机量排名前十的企业均已上…

DB Optimizer Multiplatform SQL评测和调优IDE

DB Optimizer Multiplatform SQL评测和调优IDE 增加了对最新版本Log4j的支持。 改进了分析会话功能&#xff0c;可提前提醒用户可能有问题的SQL。 DB Optimizer可以快速发现、诊断和优化性能较差的SQL。DBOptimizer使DBA和开发人员能够在整个开发生命周期中优化SQL性能&#xf…

合并多个有序数组

合并多个有序数组题目描述思想代码实现变形题目题目描述 我们现在有多个已经有序的数组&#xff0c;我们知道每个有序数组的元素个数和总共的有序数组的个数&#xff0c;现在请设计一个算法来实现这多个有序数组的合并&#xff08;合并成一个数组&#xff09;; 例如&#xff1a…

Chrome浏览器可以用ChatGPT了?

程序员宝藏库&#xff1a;https://gitee.com/sharetech_lee/CS-Books-Store 最近这段时间想必 和我一样&#xff0c;都被chatGPT刷屏了。 在看到网上给出的一系列chatGPT回答问题的例子和自己亲自体验之后&#xff0c;的确发现它效果非常令人惊艳。 chatGPT的火热程度在开源社…