录制室内数据集跑 LIO-SAM

录制室内数据集跑 LIO-SAM

LIO-SAM 仅支持 9 轴 IMU，但是实验室仅有 6 轴 IMU，好在原作者推荐了支持 6 轴 IMU 的版本

https://github.com/YJZLuckyBoy/liorf

作者还是做了非常多的优化的，respect！

下面的博客对于配置文件的修改有非常详尽的介绍，参照修改即可

IMU+激光雷达融合使用LIO-SAM建图学习笔记——详细、长文、多图、全流程

💡 P_IinL.x、P_IinL.y、P_IinL.z 和 q_ItoL.x、q_ItoL.y、q_ItoL.z、q_ItoL.w 取的是最后五次迭代结果的平均值

最终的配置文件如下

liorf:

  # Topics
  pointCloudTopic: "velodyne_points"               # Point cloud data
  imuTopic: "zedm/zed_node/imu/data"                         # IMU data
  odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  gpsTopic: "gps/fixz"                   # GPS odometry topic from navsat, see module_navsat.launch file

  # Frames
  lidarFrame: "base_link"
  baselinkFrame: "base_link"
  odometryFrame: "odom"
  mapFrame: "map"

  # GPS Settings
  useImuHeadingInitialization: false           # if using GPS data, set to "true"
  useGpsElevation: false                      # if GPS elevation is bad, set to "false"
  gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
  poseCovThreshold: 25.0                      # m^2, threshold for using GPS data
  
  # Export settings
  savePCD: true                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  savePCDDirectory: "/Documents/LIO-SAM/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

  # Sensor Settings
  sensor: velodyne                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox' or 'robosense'
  N_SCAN: 16                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
  Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
  downsampleRate: 1                           # default: 1. Downsample your data if too many points(line). i.e., 16 = 64 / 4, 16 = 16 / 1
  point_filter_num: 1                         # default: 3. Downsample your data if too many points(point). e.g., 16: 1, 32: 5, 64: 8
  lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be used
  lidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used

  # IMU Settings
  imuType: 0                                  # 0: 6-axis  1: 9-axis
  imuRate: 200.0
  imuAccNoise: 3.0451604105922320e-02
  imuGyrNoise: 2.1139892533843861e-03
  imuAccBiasN: 7.9660177633820517e-04
  imuGyrBiasN: 4.3749578117779117e-06
  imuGravity: 9.80511
  imuRPYWeight: 0.01

  # Extrinsics: T_lb (lidar -> imu)
  extrinsicTrans: [0.0264051, -0.00276252, 0.021908333]
  extrinsicRot: [0.99992251, 0.00791289, 0.00961021,
                -0.00786348, 0.99995573, -0.00516815,
                -0.00965068, 0.00509218, 0.99994047]

  # This parameter is set only when the 9-axis IMU is used, but it must be a high-precision IMU. e.g. MTI-680
  extrinsicRPY: [0, -1, 0,
                 1, 0, 0,
                 0, 0, 1]

  # voxel filter paprams
  mappingSurfLeafSize: 0.2                      # default: 0.4 - outdoor, 0.2 - indoor
  mappingCornerLeafSize: 0.1
  odometrySurfLeafSize: 0.2

  # robot motion constraint (in case you are using a 2D robot)
  z_tollerance: 1000                            # meters
  rotation_tollerance: 1000                     # radians

  # CPU Params
  numberOfCores: 4                             # number of cores for mapping optimization
  mappingProcessInterval: 0.0                  # seconds, regulate mapping frequency

  # Surrounding map
  surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
  surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
  surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
  surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
  surroundingKeyframeMapLeafSize: 0.5           # downsample local map point cloud

  # Loop closure
  loopClosureEnableFlag: true
  loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
  historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
  historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
  historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
  loopClosureICPSurfLeafSize: 0.5               # downsample icp point cloud  
  historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment

  # Visualization
  globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
  globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
  globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density

# Navsat (convert GPS coordinates to Cartesian)
navsat:
  frequency: 50
  wait_for_datum: false
  delay: 0.0
  magnetic_declination_radians: 0
  yaw_offset: 0
  zero_altitude: true
  broadcast_utm_transform: false
  broadcast_utm_transform_as_parent_frame: false
  publish_filtered_gps: false

# EKF for Navsat
ekf_gps:
  publish_tf: false
  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  frequency: 50
  two_d_mode: false
  sensor_timeout: 0.01
  # -------------------------------------
  # External IMU:
  # -------------------------------------
  imu0: imu_correct
  # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
  imu0_config: [false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, true,
                true,  true,  true]
  imu0_differential: false
  imu0_queue_size: 50 
  imu0_remove_gravitational_acceleration: true
  # -------------------------------------
  # Odometry (From Navsat):
  # -------------------------------------
  odom0: odometry/gps
  odom0_config: [true,  true,  true,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_differential: false
  odom0_queue_size: 10

  #                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
  process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]

数据包录制好、配置文件修改好以后，就可以直接进行建图

在启动文件中取消对 GPS 模块的使用，如下

<launch>

    <arg name="project" default="liorf"/>
    
    <!-- Parameters -->
    <rosparam file="$(find liorf)/config/redwallBot.yaml" command="load" />

    <!--- LOAM -->
    <include file="$(find liorf)/launch/include/module_loam.launch" />

    <!--- Robot State TF -->
    <include file="$(find liorf)/launch/include/module_robot_state_publisher.launch" />

    <!--- Run Navsat -->
    <!-- <include file="$(find liorf)/launch/include/module_navsat.launch" /> -->

    <!--- Run Rviz-->
    <include file="$(find liorf)/launch/include/module_rviz.launch" />

</launch>

建图效果还可以，估计的轨迹也与实际的轨迹很吻合