Large drift and rotation upon initializing the mapping processs in ROS2

[wxyLuna2024]

Hello,
Thank you for creating this repo. I am working on mapping KITTI360 with LIO-SAM in ROS2 humble.
For the extrinsic, I am using the KITTI extrinsic posted on readme.

The green trajectory is the GPS trajectory(gt). The pointcloud reconstruction failed at the beginning when pc are projected then it started to spin and fly around, similar to #433 . I also noticed that the pc loading speed is much slower than GPS. I have already set the loading speed to -r 0.1.
I get error messages like "Lookup would require extrapolation into the past." "Large velocity (38.120705), reset IMU-preintegration!"

Interestingly, same thing also happened with another dataset I have, which was tested on another PC with same ROS2 yaml setup and produced perfect mapping results.

So I wonder if my problem is related to #437 . Help will be much appreciated.

[ZaynabEM]

Hello,

I am having similar issue applying the code for a dataset that I recorded on a ROS2 instrumented car having VLP16 and hesai pandar, and used the Hexagon Novatel pwkpak7 for the imu, odom and gps topics.

The params.yaml is the following :

/**:
  ros__parameters:

    # Topics
    pointCloudTopic: "/velodyne/velodyne_points"                   # Point cloud data
    imuTopic: "/novatel/oem7/imu/data"                        # IMU data
    odomTopic: "/novatel/oem7/odom"                    # IMU pre-preintegration odometry, same frequency as IMU
    gpsTopic: "/novatel/oem7/fix"                    # GPS odometry topic from navsat, see module_navsat.launch file

    # Frames
    lidarFrame: "velodyne_frame"
    baselinkFrame: "base_link"
    odometryFrame: "novatel_frame"
    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: false                               # https://github.com/TixiaoShan/LIO-SAM/issues/3
    savePCDDirectory: "/Downloads/LOAM/"         # 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, either 'velodyne', 'ouster' or 'livox'
    N_SCAN: 16 #64                                   # number of lidar channels (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
...

But I am getting a very weird spiral accumulation of the data more than the creation of the map.

@wxyLuna2024 were you able to resolve the issue?
@TixiaoShan can this be related to a calibration issue, or is it another problem?

Thank you in advance for your input.

[wxyLuna2024]

Hello,

I am having similar issue applying the code for a dataset that I recorded on a ROS2 instrumented car having VLP16 and hesai pandar, and used the Hexagon Novatel pwkpak7 for the imu, odom and gps topics.

The params.yaml is the following :

/**:
  ros__parameters:

    # Topics
    pointCloudTopic: "/velodyne/velodyne_points"                   # Point cloud data
    imuTopic: "/novatel/oem7/imu/data"                        # IMU data
    odomTopic: "/novatel/oem7/odom"                    # IMU pre-preintegration odometry, same frequency as IMU
    gpsTopic: "/novatel/oem7/fix"                    # GPS odometry topic from navsat, see module_navsat.launch file

    # Frames
    lidarFrame: "velodyne_frame"
    baselinkFrame: "base_link"
    odometryFrame: "novatel_frame"
    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: false                               # https://github.com/TixiaoShan/LIO-SAM/issues/3
    savePCDDirectory: "/Downloads/LOAM/"         # 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, either 'velodyne', 'ouster' or 'livox'
    N_SCAN: 16 #64                                   # number of lidar channels (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
...

But I am getting a very weird spiral accumulation of the data more than the creation of the map. @wxyLuna2024 were you able to resolve the issue? @TixiaoShan can this be related to a calibration issue, or is it another problem?

Thank you in advance for your input.

Hi, I did what was mentioned in #437 , didn't really work out. I would say try to debug your extrinsic matrix (ROT and RPY specifically) first, and then check your IMU publishing rate to see if it is matching the pc. However me personally is still struggling in getting the first few frames of the mapping to work correctly. If you found the solution towards this please don't hesitate to post here

[LYouC]

Hi, I have encountered this issue as well. I use lio sam in ros2-humble, and there is persistent positional deviation during the localization process, but this error does not occur every time. Are there any suitable solutions available now?
Thank you for any response.

[LYouC]

@wxyLuna2024 I have found that there might be instances where pointcloud messages cannot be successfully received during the operation, leading to trajectory drift. By modifying the QoS settings, I was able to resolve this issue, and you might want to give it a try.

 // in utility.hpp
rmw_qos_profile_t qos_profile_lidar{
    RMW_QOS_POLICY_HISTORY_KEEP_LAST,
    5,
    RMW_QOS_POLICY_RELIABILITY_RELIABLE,   // pre is BEST_EFFORD
    RMW_QOS_POLICY_DURABILITY_VOLATILE,
    RMW_QOS_DEADLINE_DEFAULT,
    RMW_QOS_LIFESPAN_DEFAULT,
    RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
    RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
    false
};