ROS2 and Solid-state LiDAR

[yw9803]

@Taeyoung96 Great work！ I tried to migrate your code to ROS2 and adapt it for solid-state LiDAR ([https://github.com/url-kaist/patchwork-plusplus], patchworkpp has eliminated the impact of rings, so solid-state LiDAR can also work). The LiDAR I am using is [https://www.robosense.ai/en/rslidar/E1], but I did not get good results. The configuration I used is like this. `/**: ros__parameters: common: lid_topic: "/rslidar_points" imu_topic: "/imu_data"

    preprocess:
        lidar_type: 8                # Velodyne LiDAR
        scan_line: 1                # LiDAR scan line number
        blind: 0.5                     # blind area in the beginning of each scan line   
        feature_extract_en: false    

    calibration:
        cut_frame: true              # false: do not cut input pointcloud
        cut_frame_num: 3             # must be positive integer
        orig_odom_freq: 10           # original Lidar frequency (for velodyne, it is 10Hz)
        mean_acc_norm: 9.81          # 1: for livox built-in IMU

        imu_sensor_height : 0.4      # Height from ground to IMU sensor (meter)
        verbose : true               # For debug, Rotation matrix related ground, lidar sensor height

        # For Data Accumulation
        data_accum_length: 200.0      # To determine how much data is used
        x_accumulate : 0.001         # Parameter that determines how much the x-axis rotates (0.0 ~ 0.999)
        y_accumulate : 0.001         # Parameter that determines how much the y-axis rotates (0.0 ~ 0.999)
        z_accumulate : 0.1         # Parameter that determines how much the z-axis rotates (0.0 ~ 0.999)

        # Good initial guess for translation vector (from IMU frame I to LiDAR frame L) - unit : m
        trans_IL_x : 0.05 
        trans_IL_y : -0.0 
        trans_IL_z : -0.1 
        set_boundary : false
        bound_th : 0.3

        # Weight for residual
        gyro_factor : 10.0
        acc_factor : 1.0
        ground_factor : 5.0

    mapping:
        filter_size_surf: 0.2 
        filter_size_map: 0.3 
        gyr_cov: 0.1 
        acc_cov: 0.1 
        det_range: 30.0              # LiDAR range
        ground_cov : 120.0

    publish:
        path_en:  true                # true: output the path of the robot
        scan_publish_en: true         # false: close all the point cloud output
        dense_publish_en: false       # false: low down the points number in a global-frame point clouds scan.
        scan_bodyframe_pub_en: true   # true: output the point cloud scans in IMU-body-frame

    # For LiDAR Map Saving
    pcd_save:
        pcd_save_en: false
        interval: -1                 # how many LiDAR frames saved in each pcd file; 
                                    # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

    # For LiDAR odometry Saving
    trajectory_save:
        traj_save_en: false
        traj_file_path: "/root/catkin_ws/src/result/traj.txt" # When you use docker, you should enter docker absolute path.

    # For use patchwork++, more info : https://github.com/url-kaist/patchwork-plusplus 
    patchworkpp:
        sensor_height: 0.4        # For M2DGR dataset : https://github.com/SJTU-ViSYS/M2DGR

        verbose: false   # To check effect of uprightness/elevation/flatness

        # Ground Plane Fitting parameters
        num_iter: 3             # Number of iterations for ground plane estimation using PCA.
        num_lpr: 20             # Maximum number of points to be selected as lowest points representative.
        num_min_pts: 0         # Minimum number of points to be estimated as ground plane in each patch.
        th_seeds: 0.15           # threshold for lowest point representatives using in initial seeds selection of ground points.
        th_dist: 0.07          # threshold for thickenss of ground.
        th_seeds_v: 0.12        # threshold for lowest point representatives using in initial seeds selection of vertical structural points.
        th_dist_v: 0.4          # threshold for thickenss of vertical structure.
        max_range: 4.0 # 4.0             # max_range of ground estimation area
        min_range: 0.2              # min_range of ground estimation area
        uprightness_thr: 0.05  # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE.

` Is this method not very suitable for solid-state LiDAR with a small FOV?. If you want to give it a try, here is the dataset I used https://drive.google.com/file/d/1uDVxiupJaw84qUKLMOk1S91RZ3LQOtE2/view?usp=drive_link. Thank you again for contributing such a wonderful method, and I wish you a happy life!

[Taeyoung96]

@yw9803
Thanks for testing our work!
Now I've created a humble branch! :smile:
Feel free to test it out! I haven't tested ROS2 version properly, so there may be bugs.

And I have a few questions for you.
While you're experimenting, can you share the Rviz screen and what you see in the terminal when you set verbose to true?

If the ground segmentation works well, does it look like the height of the LiDAR from the ground was also estimated well?

[yw9803]

@Taeyoung96 Thanks very much for your reply! I tried using your Humble version. During this process, I found two issues:

1. For ceres-2.2.0, in [Gril_Calib.cpp], on lines 369 and 424, maybe use [ceres::Manifold *quatManifold = new ceres::QuaternionManifold();]
2. I used rosbags-convert to convert hall04.bag to ROS2. During usage, I often encounter this error (sometimes it works normally, but most of the time it crashes at the start). I can't find the source of the error, and I hope you can help me troubleshoot it.

Thank you so much for contributing such wonderful work and have a nice day!

[Taeyoung96]

@yw9803
Thanks for the detailed explanation.

1. Thanks for letting me know, is it possible to proceed with ceres 2.2.0 and still get similar calibration results?
2. This appears to be a bug that was found in LI-Init, which is the baseline I'm following.
   Could this be related to the issues below?
   • https://github.com/hku-mars/LiDAR_IMU_Init/issues/81
   • https://github.com/hku-mars/LiDAR_IMU_Init/issues/6

It also looks like you should try to utilize gdb to find out where the error is coming from.
For example, mapping_velodyne.launch.py,

gril_calib_node = Node(
        package='gril_calib',
        executable='gril_calib_exec',
        parameters=[config_path,
                    {'use_sim_time': use_sim_time,
                    'point_filter_num': point_filter_num_param,
                    'max_iteration': max_iteration_param,
                    'cube_side_length': cube_side_length_param}],
         prefix='gdb -ex=run --args',   # Add this
         output='screen'
    )