IMU factor with Gravity Estimation

doc/ImuFactor.lyx

@@ -324,6 +324,200 @@ Since the Combined IMU Factor has a larger state variable due to the inclusion
 , similarly defined on the tangent space of the NavState manifold.
 \end_layout
 
+\begin_layout Subsubsection*
+Gravity as an Optimized Variable
+\end_layout
+
+\begin_layout Standard
+Both factors above treat the gravity vector 
+\begin_inset Formula $g\in\Rthree$
+\end_inset
+
+, expressed in the navigation frame, as a known constant taken from the
+ preintegration parameters.
+ Because both preintegration variants accumulate the IMU measurements 
+\emph on
+without
+\emph default
+ gravity
+\begin_inset space ~
+\end_inset
+
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Lupton12tro"
+literal "false"
+
+\end_inset
+
+, gravity enters only when the preintegrated measurements are corrected
+ to predict 
+\begin_inset Formula $\widehat{X_{j}}$
+\end_inset
+
+, and can therefore be changed, or estimated, at correction time without
+ re-integrating.
+ The ImuFactorWithGravity and CombinedImuFactorWithGravity families exploit
+ this to make gravity an additional optimized variable, for navigation frames
+ that are not gravity-aligned (e.g.
+ LiDAR-odometry map frames), for initialization-free operation, or for online
+ gravity refinement.
+\end_layout
+
+\begin_layout Standard
+With 
+\begin_inset Formula $R_{i}$
+\end_inset
+
+ the attitude at 
+\begin_inset Formula $t_{i}$
+\end_inset
+
+ and 
+\begin_inset Formula $\Delta t_{ij}$
+\end_inset
+
+ the preintegration interval, the gravity-dependent part of the correction
+ adds 
+\begin_inset Formula $\frac{1}{2}\Delta t_{ij}^{2}R_{i}^{T}g$
+\end_inset
+
+ to the position rows and 
+\begin_inset Formula $\Delta t_{ij}R_{i}^{T}g$
+\end_inset
+
+ to the velocity rows of the corrected tangent vector 
+\begin_inset Formula $\xi\in\Rnine$
+\end_inset
+
+, so the Jacobian of 
+\begin_inset Formula $\xi$
+\end_inset
+
+ with respect to the gravity vector is simply
+\begin_inset Formula 
+\begin{equation}
+\frac{\partial\xi}{\partial g}=\begin{bmatrix}0_{3\times3}\\
+\frac{1}{2}\Delta t_{ij}^{2}R_{i}^{T}\\
+\Delta t_{ij}R_{i}^{T}
+\end{bmatrix}\in\Rninethree\label{eq:gravity-jacobian}
+\end{equation}
+
+\end_inset
+
+where the three block rows correspond to the rotation, position, and velocity
+ components of the tangent vector.
+ Two parametrizations of the gravity variable chain onto this matrix:
+\end_layout
+
+\begin_layout Enumerate
+ImuFactorWithGravityDirection optimizes a direction 
+\begin_inset Formula $d\in S^{2}$
+\end_inset
+
+ (a Unit3, 2
+\begin_inset space ~
+\end_inset
+
+DOF) with a fixed magnitude 
+\begin_inset Formula $m$
+\end_inset
+
+ given at construction, i.e.
+
+\begin_inset Formula $g=m\,d$
+\end_inset
+
+ and 
+\begin_inset Formula $\partial g/\partial\delta d=m\,B$
+\end_inset
+
+ with 
+\begin_inset Formula $B\in\mathbb{R}^{3\times2}$
+\end_inset
+
+ the tangent basis of 
+\begin_inset Formula $d$
+\end_inset
+
+.
+ On Earth, standard gravity is accurate to about 
+\begin_inset Formula $0.3\%$
+\end_inset
+
+ everywhere
+\begin_inset space ~
+\end_inset
+
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Nemiroff23arxiv"
+literal "false"
+
+\end_inset
+
+, so fixing the magnitude removes a weakly observable degree of freedom
+ by construction.
+\end_layout
+
+\begin_layout Enumerate
+ImuFactorWithGravityVector optimizes the free vector 
+\begin_inset Formula $g\in\Rthree$
+\end_inset
+
+, following 
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Lupton12tro"
+literal "false"
+
+\end_inset
+
+.
+ When the magnitude is approximately known it should be paired with a norm
+ pseudo-observation 
+\begin_inset Formula $e=\left\Vert g\right\Vert -g_{0}$
+\end_inset
+
+, 
+\begin_inset Formula $\partial e/\partial g=g^{T}/\left\Vert g\right\Vert $
+\end_inset
+
+ (a VectorNormFactor), added 
+\emph on
+once
+\emph default
+ per gravity variable (never per IMU factor, which would count the same
+ prior information multiple times).
+\end_layout
+
+\begin_layout Standard
+Since only the combination 
+\begin_inset Formula $R_{i}^{T}g$
+\end_inset
+
+ is observed by the accelerometer, gravity and initial attitude are entangled:
+ a graph should anchor exactly one of the two, either prior knowledge of
+ roll and pitch or a prior on the gravity variable, but not both tightly.
+ Gravity magnitude and the accelerometer bias component along gravity are
+ only jointly observable under rotation excitation whose axis changes over
+ time
+\begin_inset space ~
+\end_inset
+
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Nemiroff23arxiv"
+literal "false"
+
+\end_inset
+
+.
+ In the CombinedImuFactorWithGravity variants only the nine preintegration
+ rows of the error depend on gravity; the bias random-walk rows have a zero
+ gravity Jacobian.
+\end_layout
+
 \begin_layout Subsubsection*
 Covariance Matrices
 \end_layout

doc/refs.bib

@@ -70,3 +70,9 @@ @inproceedings{Trawny05report_IndirectKF
   author={Nikolas Trawny and Stergios I. Roumeliotis},
   year={2005}
 }
+
+@misc{Nemiroff23arxiv,
+	author = {Nemiroff, Ryan and Chen, Kenny and Lopez, Brett T.},
+	title = {Joint On-Manifold Gravity and Accelerometer Intrinsics Estimation for Inertially Aligned Mapping},
+	howpublished = {arXiv:2303.03505},
+	year = {2023}}

gtsam/geometry/Unit3.cpp

@@ -154,6 +154,16 @@ Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const {
   return p_;
 }
 
+/* ************************************************************************* */
+Vector3 Unit3::scaled(double magnitude, OptionalJacobian<3, 2> H_this,
+                      OptionalJacobian<3, 1> H_magnitude) const {
+  if (H_this)
+    *H_this = magnitude * basis();
+  if (H_magnitude)
+    *H_magnitude = p_;
+  return magnitude * p_;
+}
+
 /* ************************************************************************* */
 std::ostream& operator<<(std::ostream& os, const Unit3& pair) {
   os << pair.p_ << endl;

gtsam/geometry/Unit3.h

@@ -136,7 +136,20 @@ class GTSAM_EXPORT Unit3 {
   /// Return unit-norm Vector
   Vector3 unitVector(OptionalJacobian<3, 2> H = {}) const;
 
-  /// Return scaled direction as Point3
+  /**
+   * Return this direction scaled by a magnitude, i.e. magnitude * unitVector().
+   * Useful to reconstruct a physical vector from a direction and a known (or
+   * separately estimated) magnitude, e.g. a gravity or magnetic field vector.
+   * @param magnitude scale applied to the unit vector
+   * @param H_this optional 3x2 Jacobian wrt this direction
+   * @param H_magnitude optional 3x1 Jacobian wrt the magnitude
+   * @sa operator*(double, const Unit3&), which returns the same value
+   *     without Jacobians
+   */
+  Vector3 scaled(double magnitude, OptionalJacobian<3, 2> H_this = {},
+                 OptionalJacobian<3, 1> H_magnitude = {}) const;
+
+  /// Return scaled direction as Point3 (no Jacobians; see scaled())
   friend Point3 operator*(double s, const Unit3& d) {
     return Point3(s * d.p_);
   }

gtsam/geometry/tests/testUnit3.cpp

@@ -56,6 +56,26 @@ TEST(Unit3, point3) {
   }
 }
 
+//*******************************************************************************
+static Vector3 scaled_(const Unit3& p, const double& magnitude) {
+  return p.scaled(magnitude);
+}
+
+TEST(Unit3, scaled) {
+  const Unit3 d(1, 2, 3);
+  const double magnitude = 9.81;
+  Matrix32 actualH_d;
+  Matrix31 actualH_m;
+  const Vector3 actual = d.scaled(magnitude, actualH_d, actualH_m);
+  EXPECT(assert_equal(Vector3(magnitude * d.unitVector()), actual, 1e-9));
+  const Matrix expectedH_d =
+      numericalDerivative21<Vector3, Unit3, double>(scaled_, d, magnitude);
+  const Matrix expectedH_m =
+      numericalDerivative22<Vector3, Unit3, double>(scaled_, d, magnitude);
+  EXPECT(assert_equal(expectedH_d, Matrix(actualH_d), 1e-5));
+  EXPECT(assert_equal(expectedH_m, Matrix(actualH_m), 1e-5));
+}
+
 //*******************************************************************************
 static Unit3 rotate_(const Rot3& R, const Unit3& p) { return R * p; }