Large drift with staircase_crazy_rotation dataset

[xiaobrnbrn]

Thanks for sharing your excellent work. I would like to test the project with the staircase_crazy_rotation dataset, but the poses suffer from large drift at the very beginning, just like the figure as below. What would be the reason? Is the parameter setting needed to be adjusted?

YAML: 1.0

preprocess:
  point_filter_num: 1
  lidar_type: 4 #2-velodyne 3-ouster  4-robosense

max_iteration: 10
common:
  imu_topic: /imu/data #/imu_raw   /gps/gtimu  /imu/data
  lid_topic: /rslidar_points  #/points_raw  /rslidar_points
  time_sync_en: false
mapping:
  extrinsic_est_en: true
  extrinsic_T: [ 0.112, 0.176, -0.247]
  extrinsic_R: [ 1, 0, 0,  0, 1, 0,  0, 0, 1]

delay_time: 0.15

odometry:
  surf_res: 0.5
  log_print: false
  # ct_icp
  icpmodel: CT_POINT_TO_PLANE                    # Options: [CT_POINT_TO_PLANE, POINT_TO_PLANE]
  size_voxel_map: 1.0                         # The voxel size of in the voxel map
  min_distance_points: 0.15
  max_num_points_in_voxel: 20                 # The maximum number of points per voxel of the map
  max_distance: 500.0                        # The threshold of the distance to suppress voxels from the map
  weight_alpha: 0.9
  weight_neighborhood: 0.1
  max_dist_to_plane_icp: 0.3
  init_num_frames: 20
  voxel_neighborhood: 1
  max_number_neighbors: 20
  threshold_voxel_occupancy: 1
  estimate_normal_from_neighborhood: true
  min_number_neighbors: 20                    # The minimum number of neighbor points to define a valid neighborhood
  power_planarity: 2.0
  num_closest_neighbors: 1

  sampling_rate: 1.5
  ratio_of_nonground: 2
  max_num_residuals: 1000
  min_num_residuals: 300
  motion_compensation: CONSTANT_VELOCITY #NONE, CONSTANT_VELOCITY, ITERATIVE, CONTINUOUS
  beta_location_consistency: 1.0
  beta_orientation_consistency: 1.0
  beta_constant_velocity: 0.0
  beta_small_velocity: 0.0

  thres_translation_norm: 0.01            # m
  thres_orientation_norm: 0.1             # deg

[xiaobrnbrn]

In addition, since this work involves a loosely coupled integration of LiDAR and IMU, does it support a pure LiDAR working mode?

[chengwei0427]

Hi, @xiaobrnbrn Thanks for your interesting in our project. The system needs to init in static with a few seconds. And you can test the robosese dataset "2022-08-30-20-33-52_0.bag" or play the bags with play *.bag, then the data will be played in sequence;

It doesnot support a pure LiDAR working mode currently.

[chengwei0427]

Hi, @xiaobrnbrn For staircase_crazy_rotation dataset, you need adjust the param sample_size in the main function, and the surf_res in mapping.yaml for narrow scene. In addition, you may need to adjust the the num_iter_icp in the optimize function in lidarodom.cpp; And make sure the extrinsic params are correct.

YAML: 1.0

preprocess:
  point_filter_num: 1
  lidar_type: 4  # 1-AVIA 2-velodyne 3-ouster  4-robosense 5-pandar
  blind: 0.01

common:
  imu_topic: /imu/data #/os_cloud_node/imu  /imu_raw   /gps/gtimu
  lid_topic: /rslidar_points #/os_cloud_node/points /points_raw  /rslidar_points /velodyne_points

mapping:
  extrinsic_est_en: true
  extrinsic_T: [ 0.065, 0.0, -0.05]
  extrinsic_R: [ 1, 0, 0, 0, -1, 0, 0, 0, -1]

delay_time: 0.3

odometry:
  surf_res: 0.2
  log_print: false
  max_num_iteration: 15
  # ct_icp
  icpmodel: CT_POINT_TO_PLANE                    # Options: [CT_POINT_TO_PLANE, POINT_TO_PLANE]
  size_voxel_map: 0.2                         # The voxel size of in the voxel map
  min_distance_points: 0.05
  max_num_points_in_voxel: 20                 # The maximum number of points per voxel of the map
  max_distance: 500.0                        # The threshold of the distance to suppress voxels from the map
  weight_alpha: 0.9
  weight_neighborhood: 0.1
  max_dist_to_plane_icp: 0.1   # This is important
  init_num_frames: 20
  voxel_neighborhood: 1
  max_number_neighbors: 20
  threshold_voxel_occupancy: 1
  estimate_normal_from_neighborhood: true
  min_number_neighbors: 20                    # The minimum number of neighbor points to define a valid neighborhood
  power_planarity: 2.0
  num_closest_neighbors: 1

  sampling_rate: 1.0
  ratio_of_nonground: 2
  max_num_residuals: 3000
  min_num_residuals: 300
  motion_compensation: CONSTANT_VELOCITY #NONE, CONSTANT_VELOCITY, ITERATIVE, CONTINUOUS
  beta_location_consistency: 1.0
  beta_orientation_consistency: 1.0
  beta_constant_velocity: 0.0
  beta_small_velocity: 0.0

  thres_translation_norm: 0.01            # m
  thres_orientation_norm: 0.1             # deg

[CharlieV5]

Besides the parameters the author has mentioned, I think you should set the correct extrinsic parameters in yaml file. This is very important. I have set the extrinsic_R as [1,0,0, 0,-1,0, 0,0,-1] and get a good map for this dataset.

[xiaobrnbrn]

Got it, thanks a lot!

[chengwei0427]

Hi, @CharlieV5 @xiaobrnbrn

A mapping.yaml for staircase_crazy_rotation dataset is update here.

[YYangyuan]

Hi, @CharlieV5 @xiaobrnbrn

A mapping.yaml for staircase_crazy_rotation dataset is update here.

When I set "motion_compensation" to "CONSTANT_VELOCITY", the "IcpModel" will be "POINT_TO_PLANE". Therefore, it is unrelated to ct-icp. Could you please tell me how to run staircase_crazy_rotation dataset with "CT_POINT_TO_PLANE"?

[chengwei0427]

Hi, @CharlieV5 @xiaobrnbrn A mapping.yaml for staircase_crazy_rotation dataset is update here.

When I set "motion_compensation" to "CONSTANT_VELOCITY", the "IcpModel" will be "POINT_TO_PLANE". Therefore, it is unrelated to ct-icp. Could you please tell me how to run staircase_crazy_rotation dataset with "CT_POINT_TO_PLANE"?

Tests have shown that rs lidar does not support CT mode due to timestamp issues. It is recommended that you use a velodyne/ouster/livox lidar for (CT mode) testing.

[YYangyuan]

Hi, @CharlieV5 @xiaobrnbrn A mapping.yaml for staircase_crazy_rotation dataset is update here.

When I set "motion_compensation" to "CONSTANT_VELOCITY", the "IcpModel" will be "POINT_TO_PLANE". Therefore, it is unrelated to ct-icp. Could you please tell me how to run staircase_crazy_rotation dataset with "CT_POINT_TO_PLANE"?

Tests have shown that rs lidar does not support CT mode due to timestamp issues. It is recommended that you use a velodyne/ouster/livox lidar for (CT mode) testing.

Thank you very much for your reply!

[chengwei0427]

Set "motion_compensation: CONTINUOUS" for the CT mode. @YYangyuan