IMU factor with Gravity Estimation

[nkhedekar]

As discussed here (https://groups.google.com/g/gtsam-users/c/BMlDTdKvjRg) the intended use of the IMU factor is with a constant gravity vector (that may be pointing either up or down based on the user's convention) in the navigation frame and an initial attitude that should be estimated. Inaccuracy in this initial attitude will have a large detrimental effect with larger translations in the XY plane. This problem becomes even more evident when creating a large monolithic map as in LiDAR-Inertial Odometry. Estimating the initial orientation is additionally challenging if the motion of the robot does not involve rotations (until much further into the trajectory) making the problem unobservable.

A possible solution to this was presented in Lupton and Sukkarieh's paper [1](Section V). Particularly, they proposed fixing the initial body frame to be the navigation frame and estimated the direction of gravity in it. While [2] theoretically improved on [1], this solution from [1] seems to not have been implemented.

This PR adds a new IMUFactorWithGravity so that this solution can be implemented. The IMUFactorWithGravity is a 6-way factor that is essentially a modification of the IMUFactor to connect it to a variable for the gravity while trying to not duplicate too much code.

Note: This first version implements directly a Vector3 for the gravity. Since the gravity magnitude is known to a high degree of accuracy around the globe, its probably better to have the gravity as a Unit3 with the magnitude being specified by norm of the vector in the preintegration params (to maintain compatibility)?

References

[1] T. Lupton and S. Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions," in IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61-76, Feb. 2012, doi: 10.1109/TRO.2011.2170332.
[2] Christian Forster, Luca Carlone, Frank Dellaert, Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation" in Robotics: Science and Systems XI

[nkhedekar]

5f18305 sets the gravity up as a Unit3 for the direction and utilizes the p().n_gravity.norm() for the magnitude.

[dellaert]

Hi @nkhedekar

Thanks for the PR - we always welcome contributions. But if I'm honest - I don't fully understand the reason for this.

Our IMU factor is based on Lupton & Sukkarieh. We are not fixing any initial orientation of the body frame: that is part of the optimization process. That was also my answer in that discussion thread.

The assumption we make is that the gravity is constant and aligned with either Z-up or Z-down in the navigation frame. I don't really see a good reason to not assume that: an earth-fixed tangent frame is what most of our users assume. In particular, making the original body frame the navigation frame is suboptimal as it would require - as you implement here - tying the gravity direction to every IMU factor. I see the cost, but not really the benefit :-)

Can you explain why optimizing over the whole trajectory (which will recover gravity as well, by changing the entire trajectory to align) is not the solution? If done incrementally, because every non-accelerating IMUY measurements points to gravity, the correct orientation should be stable very quickly.

Best
FD

[dellaert]

@nkhedekar maybe you want to chat with me about it separately? Send me an email at [email protected] and we can reserve some time.

[nkhedekar]

@nkhedekar maybe you want to chat with me about it separately? Send me an email at [email protected] and we can reserve some time.

Sure! I have sent an email :)

[dellaert]

@nkhedekar nice chatting! Let me know via email when I should look at this again.