IMU factor with Gravity Estimation - #2020
Conversation
|
5f18305 sets the gravity up as a |
|
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 |
|
@nkhedekar maybe you want to chat with me about it separately? Send me an email at dellaert@gmail.com and we can reserve some time. |
Sure! I have sent an email :) |
|
@nkhedekar nice chatting! Let me know via email when I should look at this again. |
c952ef9 to
1b49a91
Compare
|
Hi @dellaert , The default path of the IMU factors remains unchanged, so if one is operating in a gravity-aligned nav frame nothing changes. The new factors introduced here are for the cases where this assumption doesn't hold:
The core change is now just an optional 9×3 Jacobian: gravity enters preintegration only in the Both parametrizations are provided, as thin chain-rule adapters over one Jacobian:
Naming somewhat follows SymForce, which added the equivalent Beyond the factors, this PR also includes analytic-vs-numerical Jacobian tests for both parametrizations × both preintegration backends, tilted-gravity recovery tests for both modes, serialization round-trips, Merge with gravity-key/magnitude checks, wrapper entries with a Python smoke test, a "Gravity as an Optimized Variable" section in [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 [3] R. Nemiroff, K. Chen and B. T. Lopez, "Joint On-Manifold Gravity and Accelerometer Intrinsics Estimation for Inertially Aligned Mapping," 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Detroit, MI, USA, 2023, pp. 1388-1394, doi: 10.1109/IROS55552.2023.10342424. |
Returns magnitude * unitVector() with analytic Jacobians wrt both the direction (3x2, through the tangent basis) and the magnitude (3x1). This is the shared chain rule needed whenever a physical vector is parametrized as a known or estimated scalar magnitude times a Unit3 direction, e.g. magnetic field (MagFactor3) or gravity factors.
Replaces the inline scale-times-basis Jacobian assembly with the shared Unit3::scaled helper. Covered by the unchanged testMagFactor assertions.
Unary factor with error = ||v|| - norm, generalized over the vector dimension. Typical use is a magnitude pseudo-observation on a physical vector whose direction and magnitude are entangled in one variable, eg. an estimated gravity vector [Lupton and Sukkarieh, TRO 2012] or a local magnetic field vector (MagFactor2). The Jacobian is zeroed below a documented threshold around the (non-differentiable) origin, so variables must not be initialized at exactly zero. The N=3 instantiation is exposed in the wrapper as VectorNormFactor3.
…reintegrationBase NavState::correctPIM gains an optional 9x3 Jacobian of the tangent update wrt the nav-frame gravity vector (the rotation rows are zero; the shared unrotate Jacobian is now computed once). PreintegrationBase gains predict/computeError/computeErrorAndJacobians overloads taking an explicit gravity vector with the corresponding 9x3 Jacobians, so that factors can treat gravity as an optimized variable. The existing signatures delegate to them with the gravity vector from params and are unchanged in behavior.
Two parametrizations of an ImuFactor with gravity as a 6th optimized variable, sharing one template and the internal::GravityParametrization adapter: - ImuFactorWithGravityDirection: Unit3 direction with a fixed, known magnitude (explicit constructor parameter, defaulting to the norm of the params' gravity vector), following the scale*direction decomposition used by MagFactor3 and Unit3::scaled. - ImuFactorWithGravityVector: free Point3 vector entangling direction and magnitude, following Lupton and Sukkarieh (TRO 2012); pair with a single VectorNormFactor<3> on the gravity variable when the magnitude is approximately known. Both factors are exposed in the wrapper with a python smoke test. Tests cover analytic-vs-numerical Jacobians for both parametrizations and both preintegration types, equality against mismatched factor types, Merge (including rejection of mismatched gravity keys and magnitudes), and recovery of a tilted gravity from stationary IMU data with both factors.
…avityVector CombinedImuFactor variants with gravity as a 7th optimized variable, using the same GravityParametrization adapter as the ImuFactor variants. The 15-dof error and block-Jacobian assembly is factored into a helper shared with the plain CombinedImuFactorT (no functional change there); only the 9 preintegration rows depend on gravity, and the bias random walk rows have a zero gravity Jacobian. Jointly estimating gravity and the evolving accelerometer bias requires sufficient rotation excitation (Nemiroff, Chen and Lopez, 2023). Both factors are exposed in the wrapper.
- doc/ImuFactor.lyx (+ regenerated ImuFactor.pdf, refs.bib entry): new "Gravity as an Optimized Variable" section with the correction-time gravity Jacobian, both parametrizations and their chain rules, the norm pseudo-observation, and the observability caveats, citing Lupton and Sukkarieh (2012) and Nemiroff, Chen and Lopez (2023). - gtsam/navigation/doc/ImuFactorWithGravity.ipynb: family notebook (overview with the three-mode decision rule, summary math, executed usage example recovering a tilted gravity with both parametrizations) following the per-class notebook conventions. - gtsam/nonlinear/doc/VectorNormFactor.ipynb: overview, the once-per-variable rule, the zero-initialization warning, and an executed usage example. - Cross-links from the ImuFactor, CombinedImuFactor and MagFactor notebooks (the latter documenting the new MagFactor2 + VectorNormFactor magnitude-prior capability). - python/gtsam/examples/ImuFactorWithGravityExample.py: runnable demonstration of both gravity parametrizations.
1b49a91 to
8549976
Compare
2 participants
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
Vector3for 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 aUnit3with 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