Large velocity nosie form imuPreintegration node?

Question

Large velocity nosie form imuPreintegration node?

shibowing opened this issue 4 years ago · 9 comments

Hi Tixiao,

Thank you very much for sharing! This is really great work!

I just tested your code and noticed that the velocity you estimated is larger than the one estimated by the original loam, which will be difficult for the drone controller to use. Could you please give me some suggestions on how to reduce the velocity noise?

The red line is LIO-SAM and the blue line is the LOAM algorithm. It can be seen that the red line is not very smooth and has a big noise compared with the blue one.

Thank you very much for your suggestions!

Answer 1 · 2020-09-12T13:56:05.000Z

How is the velocity calculated from the original loam? Is a filter used there?

Answer 2 · 2020-09-18T06:58:42.000Z

Hello @TixiaoShan @shibowin

I think that probably my problem is related. I'm using LIO-SAM with an UAV too and I'm getting a really choppy odometry, both position and velocity. For instance, here is the Y axis:

(red: '/odometry/imu/twist/linear/y'; orange: '/odometry/imu/pose/position/y'; green: vicon ground truth)

This is making the UAV really difficult to control, almost impossible. I am trying to filter it, but still I get still a choppy signal.

Also, I realized that the "saw shape" signal evolves in the direction of the signal sign, for instance:

Possitive:

(red: '/odometry/imu/twist/linear/y'; orange: '/odometry/imu/pose/position/y'; green: vicon ground truth)
Negative:

(red: '/odometry/imu/twist/linear/y'; orange: '/odometry/imu/pose/position/y'; green: vicon ground truth)

I'm not sure, but my feeling is that the speed estimation is higher than it actually is, causing this "saw" to appear. Maybe it is related to the IMU pre-integration?

Any ideas?

Thanks!

Answer 3 · 2020-09-18T12:46:28.000Z

@rcabg @shibowing
Can you try this version of LIO-SAM? I changed how imuPreintegration.cpp works in the latest commit. I am not sure whether I introduced some bugs or not.

Answer 4 · 2020-09-22T12:52:49.000Z

Hello @TixiaoShan

Thanks for the response. It definitely improves a lot the odometry reducing noise.

Here some examples:

(pink: last commit; red: old commit)

(green: last commit; blue: old commit)

I didn't have enough time for analizing the code and find out what could provoke this. Any idea?

Thanks!

Answer 5 · 2020-09-22T13:19:38.000Z

@rcabg
So my new code introduces some bugs. Is that possible for you to share your bag with ground truth so I can debug the code? It would be great if I could test the code with the reference you have shown above.

In the new code for imu preintegration, I introduced a new lidar odometry in mapOptimization that is only incremental with drift - it doesn't change even with loop closure. So the imuPreintegration won't reset when a loop-closure happens. Thus, the imu odometry is also increamental with drift. In the old code, imuPreintegration resets everytime when a loop-closure is detected.

Answer 6 · 2020-09-22T23:19:40.000Z

@TixiaoShan
Hi Tixiao, thank you very much for your reply!
I think the issues are caused by the following piece of code.
After commenting on the code relevant to imuPreintegrationResetId, the noise of velocity is much smaller than before. However, I am confused that why you set priorVelNoise=1e4, ba.norm()>1.0 and bg.norm()>1.0 . Would you mind explaining the reasons behind it? Thank you very much!

Answer 7 · 2020-09-22T23:32:48.000Z

From my perspespective, it seems like we don't need to reset imu preintergration frequently. Frequent reset imu-preintegration may cause the large noise of velocity. Please correct me if I am wrong. Thank you very much!

Answer 8 · 2020-09-23T00:31:30.000Z

@shibowing
After commenting on the code relevant to imuPreintegrationResetId, the noise of velocity is much smaller than before.
The imuPreintegrationResetId was used in the old code. In the old code, the odometry from imuPreintegration directly serves as the initial guess for lidar scan matching in mapOptimization. So if a loop closure happens, the robot pose will jump. We need to update the imuPreintegration with the new pose, so the scan matching won't fail due to bad initial guess (if imuPreintegration is not reset). If loop closure or GPS is disabled, imuPreintegrationResetId is not used.

priorVelNoise=1e4 with a large velocity prior, we may be able to initialize the system when the car is driving at 10m/s.
ba.norm()>1.0, bg.norm()>1.0 I find that for a good IMU, the bias should be pretty small. So I assume the system fails when there is a large bias.

From my perspespective, it seems like we don't need to reset imu preintergration frequently. Frequent reset imu-preintegration may cause the large noise of velocity. Please correct me if I am wrong.
You are right. I reset the whole process every 100 step. Because this will bound the computation of a graph. If I don't reset, imuPreintegration will become slower and slower.

Answer 9 · 2020-09-30T03:13:30.000Z

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.