search

TixiaoShan / LIO-SAM

LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
BSD 3-Clause "New" or "Revised" License
3.3k stars 1.23k forks source link

Not enough features! Only 0 edge and 0 planar features available. #326

Closed JokerJohn closed 2 years ago

JokerJohn commented 2 years ago

Maybe I have solved this problem.

I try to adjust LIO_SAM system with 6-axis imu before, such as stim300, it works very well. When I use the senor combination of Pandar xT32 and BMI085(6-axis MEMS), I faced many problems. As the below picture shows, Not enough features! Only 0 edge and 0 planar features available. I can guarantee that the extrinsic parameters from lidar to imu are correct.

image

image

I modify the imageProjection.cpp to add the support of pandar lidar as below. Because the pandar lidar axis is different from velodyne and ouster.

      // Convert to Velodyne format
      pcl::moveFromROSMsg(currentCloudMsg, *tmpPandarCloudIn);
      laserCloudIn->points.resize(tmpPandarCloudIn->size());
      laserCloudIn->is_dense = tmpPandarCloudIn->is_dense;
      double time_begin = tmpPandarCloudIn->points[0].timestamp;
      for (size_t i = 0; i < tmpPandarCloudIn->size(); i++) {
        auto &src = tmpPandarCloudIn->points[i];
        auto &dst = laserCloudIn->points[i];
        dst.x = src.y;
        dst.y = src.x * -1;
        dst.z = src.z;
        dst.intensity = src.intensity;
        dst.ring = src.ring;
        //dst.tiSme = src.t * 1e-9f;
        dst.time = src.timestamp - time_begin; // s
      }
    } else {
      ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
      ros::shutdown();
    }

I try to debug the imu sensor, but the roll is almost near PI.

x: 0.172744, y: -0.313069, z: 9.789951
IMU gyro: 
x: 0.105997, y: 0.002131, z: 0.003729
IMU roll pitch yaw: 
roll: 3.141593, pitch: -0.000000, yaw: -1.570796

end time from pcd and size: 0.100178003311, 58084
IMU acc: 
x: 0.172266, y: -0.299790, z: 9.814834
IMU gyro: 
x: 0.102162, y: 0.004368, z: 0.000746
IMU roll pitch yaw: 
roll: 3.141593, pitch: -0.000000, yaw: -1.570796

[ WARN] [1648758938.083301867]: Not enough features! Only 0 edge and 0 planar features available.
IMU acc: 
x: 0.141521, y: -0.320127, z: 9.809570
IMU gyro: 
x: 0.112602, y: 0.000959, z: 0.004155
IMU roll pitch yaw: 
roll: 3.141593, pitch: -0.000000, yaw: -1.570796

IMU acc: 
x: 0.118193, y: -0.338071, z: 9.790908
IMU gyro: 
x: 0.114626, y: -0.001065, z: 0.005966
IMU roll pitch yaw: 
roll: 3.141593, pitch: -0.000000, yaw: -1.570796

can anyone help me? Thanks a lot!

my params.yaml:


  # Topics
  pointCloudTopic: "/hesai/pandar"               # Point cloud data
  imuTopic: "/alphasense/imu"

  odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  gpsTopic: "gps_odom"                   # GPS odometry topic from navsat, see module_navsat.launch file

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

  # GPS Settings
  useGps: false
  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: 0.02                      # m^2, threshold for using GPS data

  # Export settings
  savePCD: true                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  savePCDDirectory: "/home/xchu/ramlab_ws/src/LIO-SAM/data/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
  scene_name: "map"

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

  #  sensor: velodyne                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
  #  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. i.e., 16 = 64 / 4, 16 = 16 / 1
  #  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
  imuFrequence: 400
  imuAccNoise: 0.0011501915187049582
  imuGyrNoise: 5.084312924828687e-05
  imuAccBiasN: 0.06080652138668933
#  imuAccBias_N: [ 0.06080652138668933, 0.08353074835853214, 0.057072968234636895 ]
  imuGyrBiasN: -0.0015351229643790084
#  imuGyrBias_N: [ -0.0015351229643790084, -0.0013449146576507546, 0.00030127855524786183 ]
  imuGravity: 9.804989173462031
#  imuGravity_N: [ 0.01165152782783894, 9.804989173462031, -0.008749296634685332 ]
  imuRPYWeight: 0.01

  #
  #  # Extrinsics (lidar -> IMU)
  extrinsicTrans: [ -0.001, -0.00855, 0.055 ]
    #  extrinsicRot: [ -0.0000000, -1.0000000,  0.0000000,
    #                  1.0000000, -0.0000000,  0.0000000,
    #                  0.0000000,  0.0000000, -1.0000000 ]
    #  extrinsicRPY: [ -0.0000000, -1.0000000,  0.0000000,
    #                  1.0000000, -0.0000000,  0.0000000,
    #                  0.0000000,  0.0000000, -1.0000000 ]

  extrinsicRot: [ 0, -1,  0,
                  -1, 0,  0,
                  0,  0, -1 ]
  extrinsicRPY: [ 0, -1,  0,
                  -1, 0,  0,
                  0,  0, -1 ]
#  extrinsicRot: [ 1, 0, 0,
#                  0, 1, 0,
#                  0, 0, 1 ]
#  extrinsicRPY: [ 1, 0, 0,
#                  0, 1, 0,
#                  0, 0, 1 ]

  # LOAM feature threshold
  edgeThreshold: 1.0
  surfThreshold: 0.1
  edgeFeatureMinValidNum: 10
  surfFeatureMinValidNum: 100

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

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

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

  # Surrounding map
  surroundingkeyframeAddingDistThreshold: 0.1   # meters, regulate keyframe adding threshold
  surroundingkeyframeAddingAngleThreshold: 0.1  # radians, regulate keyframe adding threshold
  surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses
  surroundingKeyframeSearchRadius: 30.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)

  # Loop closure
  loopClosureEnableFlag: true
  loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
  surroundingKeyframeSize: 30                   # submap size (when loop closure enabled)
  historyKeyframeSearchRadius: 30.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
  historyKeyframeFitnessScore: 0.9              # 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 ]
oskarmue commented 2 years ago

Hey, how did you fix that issue?

JasonWang9805 commented 1 year ago

why you use 6-axis,but output rpy?

harisbinyousaf24 commented 1 year ago

@JokerJohn Can you tell me how did you resolve that issue. I am currently working on LIO-SAM and i am facing the same issue. Your help would be appreciated.

yuma116 commented 1 year ago

Also, I want to information or code for fix. Could you share it?