Large velocity, reset IMU-preintegration! error
liamm545 opened this issue · comments
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.
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.
Hello I have the same problem with liosam
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.
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 # #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_correct to 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
the following photos, the path of odom/gps is going far from the liosam and odom/imu path.
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:
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?
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.
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.)
Although there are differences between IMU odometry and GPS odometry, during the map generation process, optimization is performed, which helps the odometries stay similar.
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.