Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Large velocity nosie form imuPreintegration node? #104

Closed
shibowing opened this issue Sep 8, 2020 · 9 comments
Closed

Large velocity nosie form imuPreintegration node? #104

shibowing opened this issue Sep 8, 2020 · 9 comments
Labels

Comments

@shibowing
Copy link

shibowing commented Sep 8, 2020

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.

image

image

Thank you very much for your suggestions!

@TixiaoShan
Copy link
Owner

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

@rcabg
Copy link

rcabg commented Sep 18, 2020

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:

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:
    saw_pos_sign
    (red: '/odometry/imu/twist/linear/y'; orange: '/odometry/imu/pose/position/y'; green: vicon ground truth)

  • Negative:
    saw_neg_sign
    (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!

@TixiaoShan
Copy link
Owner

@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.

@rcabg
Copy link

rcabg commented Sep 22, 2020

Hello @TixiaoShan

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

Here some examples:

ex1
ex2
(pink: last commit; red: old commit)

ex3
(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!

@TixiaoShan
Copy link
Owner

@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.

@shibowing
Copy link
Author

shibowing commented Sep 22, 2020

@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!

image

image

image

@shibowing
Copy link
Author

shibowing commented Sep 22, 2020

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!

@TixiaoShan
Copy link
Owner

@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.

@stale
Copy link

stale bot commented Sep 30, 2020

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

No branches or pull requests

3 participants