search

TixiaoShan / LIO-SAM

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

Large velocity, reset IMU-preintegration! error #501

Open liamm545 opened 3 months ago

liamm545 commented 3 months ago

Hello. I'm currently using lio-sam and have a question about it.

I am using a Velodyne VLP16 LiDAR and a 9-axis IMU sensor, but I'm encountering a "Large velocity, reset IMU-preintegration!" error, which is causing the map to become distorted.

Screenshot from 2024-07-02 13-41-41

So i checked the value of the velocity in "imuPreintegration.cpp" file.

[ INFO] [1719903620.545044741]: Current velocity: [0.002522, 0.026950, -0.738660]
[ INFO] [1719903620.738969615]: Current velocity: [0.007577, 0.023546, -1.881389]
[ INFO] [1719903620.943298064]: Current velocity: [0.001869, 0.046400, -2.980822]
[ INFO] [1719903621.157010335]: Current velocity: [-0.118041, 0.058666, -4.721511]
[ INFO] [1719903621.362424426]: Current velocity: [-0.140791, 0.066850, -6.486963]
[ INFO] [1719903621.558399682]: Current velocity: [-0.101092, 0.076252, -8.518886]
[ INFO] [1719903621.761393458]: Current velocity: [0.281467, 0.217931, -10.953489]
[ INFO] [1719903621.952568893]: Current velocity: [1.122804, 0.120016, -12.950680]
[ INFO] [1719903622.158289919]: Current velocity: [1.680549, 0.042164, -14.219228]
[ INFO] [1719903622.372236656]: Current velocity: [3.331799, -0.166409, -15.497406]
[ INFO] [1719903622.554968747]: Current velocity: [3.902030, -0.279297, -16.066641]
[ INFO] [1719903622.773346387]: Current velocity: [4.908000, -0.509627, -16.566099]
[ INFO] [1719903622.968782143]: Current velocity: [5.554502, -0.687169, -17.043941]
[ INFO] [1719903623.163830851]: Current velocity: [6.021399, -0.821991, -17.636927]
[ INFO] [1719903623.373023411]: Current velocity: [6.214448, -0.916967, -18.270435]
[ INFO] [1719903623.583705434]: Current velocity: [6.334829, -1.178291, -18.548609]
[ INFO] [1719903623.775572965]: Current velocity: [6.279804, -1.355855, -19.073402]
[ INFO] [1719903623.967412059]: Current velocity: [6.397782, -1.531144, -19.617725]
[ INFO] [1719903624.168677612]: Current velocity: [6.314263, -1.626316, -20.175163]
[ INFO] [1719903624.391759498]: Current velocity: [6.021196, -1.745188, -20.933523]
[ INFO] [1719903624.583264917]: Current velocity: [5.880361, -1.908399, -21.742311]
[ INFO] [1719903624.780177006]: Current velocity: [5.683855, -2.025586, -22.464531]
[ INFO] [1719903624.993278509]: Current velocity: [5.238449, -2.003572, -23.489372]
[ INFO] [1719903625.196377777]: Current velocity: [5.091327, -2.009847, -24.491163]
[ INFO] [1719903625.386508596]: Current velocity: [5.210320, -2.105345, -25.452808]
[ INFO] [1719903625.591137373]: Current velocity: [5.523574, -2.261984, -26.401688]
[ INFO] [1719903625.788424844]: Current velocity: [5.387509, -2.299114, -27.190140]
[ INFO] [1719903625.989044756]: Current velocity: [5.658954, -2.202892, -28.250267]
[ INFO] [1719903626.193373597]: Current velocity: [5.735162, -2.201276, -28.921423]
[ INFO] [1719903626.403031391]: Current velocity: [5.670805, -2.154603, -29.934925]
[ WARN] [1719903626.403047604]: Large velocity, reset IMU-preintegration!
[ INFO] [1719903626.795157021]: Current velocity: [0.005990, -0.224255, -0.929813]
[ INFO] [1719903627.002490002]: Current velocity: [-0.974400, 0.665856, -2.595716]
[ INFO] [1719903627.215069760]: Current velocity: [-92.187363, -8.238041, -34.974682]
[ WARN] [1719903627.215083046]: Large velocity, reset IMU-preintegration!
[ INFO] [1719903627.598015582]: Current velocity: [0.090168, -0.061924, -1.036262]
[ INFO] [1719903627.814239665]: Current velocity: [0.464561, -0.358684, -2.591830]
[ INFO] [1719903628.004267369]: Current velocity: [71.447754, -26.926102, -63.163818]
[ WARN] [1719903628.004281328]: Large velocity, reset IMU-preintegration!

As observed, I noticed significant spikes in velocity even when the vehicle was stationary. However, it seems there are no issues with the IMU sensor readings (attached below).

IMU acc: 
x: 0.0909259, y: -0.136082, z: -0.985991
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03875, pitch: -0.0671442, yaw: -1.52948

IMU acc: 
x: 0.0910207, y: -0.136188, z: -0.986981
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03875, pitch: -0.0671442, yaw: -1.52948

IMU acc: 
x: 0.0910207, y: -0.136188, z: -0.986981
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03875, pitch: -0.0671442, yaw: -1.52948

IMU acc: 
x: 0.0910207, y: -0.136188, z: -0.986981
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03875, pitch: -0.0671442, yaw: -1.52948

^CIMU acc: 
x: 0.0911574, y: -0.137171, z: -0.986863
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03894, pitch: -0.0671263, yaw: -1.52946

[ INFO] [1719905391.513121174]: Current velocity: [0.605741, 0.026891, -18.718386]
IMU acc: 
x: 0.0910207, y: -0.136188, z: -0.986981
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03894, pitch: -0.0671263, yaw: -1.52946

IMU acc: 
x: 0.0910207, y: -0.136188, z: -0.986981
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03894, pitch: -0.0671263, yaw: -1.52946

[lio_sam_rviz-8] killing on exit
IMU acc: 
x: 0.0910207, y: -0.136188, z: -0.986981
IMU gyro: 
x: 0, y: -0, z: 0
IMU roll pitch yaw: 
roll: -3.03894, pitch: -0.0671263, yaw: -1.52946`

What could be the problem? I would appreciate your assistance. Thank you.

Zahra-Farajzadeh commented 3 months ago

Hello I have the same problem with liosam Screenshot 2024-06-29 185545 Screenshot 2024-06-29 175759

liamm545 commented 3 months ago

Hi, @Zahra-Farajzadeh I think wrong imu-lidar calibration is the reason. When i change extrinsicRot by unit matirx, odometry looks a bit shaky, but the above issue has been resolved.

Zahra-Farajzadeh commented 2 months ago

Hi @liamm545 my extrinsic matrixes are identity matrix this is my config file `lio_sam:

Topics

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

Frames

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

GPS Settings

useImuHeadingInitialization: true # 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: 1.0 # m^2, threshold for using GPS data

Export settings

savePCD: true # https://github.com/TixiaoShan/LIO-SAM/issues/3 savePCDDirectory: "/home/slam/Zahra/output/" # use global path, and end with "/"

warning: if you have already data in the above savePCDDirectory, it will all remove and remake them. Thus, backup is recommended if pre-made data exist.

Sensor Settings

sensor: velodyne # lidar sensor type, either 'velodyne' or 'ouster' N_SCAN: 80 # number of lidar channel (i.e., 16, 32, 64, 128) Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) downsampleRate: 2 # 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

imuAccNoise: 0.009939570888238808e-03 imuGyrNoise: 0.005636343949698187e-03 imuAccBiasN: 0.64356659353532566e-03 imuGyrBiasN: 0.35640318696367613e-03 imuGravity: 9.80511 imuRPYWeight: 0.01

Extrinsics (lidar -> IMU)

extrinsicTrans: [0, 0.0, 0.0] extrinsicRot: [1, 0, 0, 0, 1, 0, 0, 0, 1] extrinsicRPY: [1, 0, 0, 0, 1, 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.4 # default: 0.4 - outdoor, 0.2 - indoor mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor mappingSurfLeafSize: 0.4 # 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: 20 # number of cores for mapping optimization mappingProcessInterval: 0.15 # 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)

Loop closure

loopClosureEnableFlag: true loopClosureFrequency: 0.5 # 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 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 = 0.2

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/data

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, # linear velocity false, false, true, # angular velocity true, true, true] # linear acc 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] `

I tried to use gps and I followed the repo. I changed the imu0 value from imu_correctto imu/data. this is navsat launch file:

`

<arg name="project" default="lio_sam"/>

<env name="ROSCONSOLE_CONFIG_FILE" value="$(find lio_sam)/launch/include/rosconsole/rosconsole_error.conf"/>

<!-- EKF GPS-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_gps" respawn="true">
    <remap from="odometry/filtered" to="odometry/navsat" />
</node>

<!-- Navsat -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat" respawn="true">
    <!-- <rosparam param="datum">[42.35893211, -71.09345588, 0.0, world, base_link]</rosparam> -->
    <remap from="imu/data" to="/imu/data" />
    <remap from="gps/fix" to="gps/fix" />
    <remap from="odometry/filtered" to="odometry/navsat" />
</node>

`

in following photo the path is odom/gps and the white path is liosam path and odom/imu path is exactly on the liosam path image

the following photos, the path ofodom/gps is going far from the liosam and odom/imu path. Screenshot 2024-07-08 232534 Screenshot 2024-07-08 232924 Screenshot 2024-07-08 234221 Screenshot 2024-07-08 235250 Screenshot 2024-07-09 002848 Screenshot 2024-07-09 002814

and finally at duration of ~600 the liosam path is going to jittering and not working. we checked the bag file using fastliosam and it is working correctly.

the output point cloud is this:

Screenshot 2024-07-09 012146

My data has been recorded in the centre of the city and somewhere the GPS signal would be disconnected. The 'odom/imu' and 'liosam' paths are exactly correlated and the distance between them and 'odom/gps' is too great.

Do you know what is the problem? How can I increase the weight of using gps data where the gps signal is not poor?

liamm545 commented 1 month ago

Hi, @Zahra-Farajzadeh. I'm sorry for the late reply.

I think you should modify gpsCovThreshold and poseCovThreshold params. Also you can check(debug) gps data in "mapOptimization.cpp" (Especially "addGPSFactor" function)

I'm sorry I couldn't be of any help.

liamm545 commented 1 month ago

This is my case.

If you look at this picture, you can see the difference between gps odometry and imu odometry. (The small axis is imu odometry, and the relatively large axis at the bottom is gps odometry.) Screenshot from 2024-08-29 17-35-33

Although there are differences between IMU odometry and GPS odometry, during the map generation process, optimization is performed, which helps the odometries stay similar.

Screenshot from 2024-08-29 17-38-22 And although there were differences between IMU odometry and GPS odometry at first, you can see that the map is well-constructed as optimization and loop closure are eventually performed.

I recommend that you run it with SC-LIO-SAM.

Thanks.