[ImuFactor] Add an on manifold Imu Factor

CMakeLists.txt

@@ -280,6 +280,7 @@ install(DIRECTORY gen/cpp/ DESTINATION include FILES_MATCHING PATTERN "*.h" PATT
 
 if(SYMFORCE_BUILD_OPT)
   add_subdirectory(symforce/opt)
+  add_subdirectory(symforce/slam)
 endif()
 
 if(SYMFORCE_BUILD_CC_SYM)

symforce/geo/matrix.py

@@ -487,6 +487,23 @@ def transpose(self) -> Matrix:
         """
         return Matrix(self.mat.transpose())
 
+    def lower_triangle(self: MatrixT) -> MatrixT:
+        """
+        Returns the lower triangle (including diagonal) of self
+
+        self must be square
+        """
+        rows, cols = self.shape
+        if rows != cols:
+            raise ValueError(
+                f"Attempted to take lower triangle of non-square matrix (found shape {self.shape})"
+            )
+
+        lt = self.__class__()
+        for k in range(rows):
+            lt[k, : k + 1] = self[k, : k + 1]
+        return lt
+
     def reshape(self, rows: int, cols: int) -> Matrix:
         return Matrix(self.mat.reshape(rows, cols))
 

symforce/pybind/CMakeLists.txt

@@ -3,6 +3,10 @@
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ----------------------------------------------------------------------------
 
+# ==============================================================================
+# Third Party Dependencies
+# ==============================================================================
+
 include(FetchContent)
 
 find_package(pybind11 QUIET)
@@ -21,6 +25,13 @@ else()
   message(STATUS "pybind11 found")
 endif()
 
+# ==============================================================================
+# SymForce Targets
+# ==============================================================================
+
+# ------------------------------------------------------------------------------
+# cc_sym
+
 pybind11_add_module(
   cc_sym
   SHARED

symforce/slam/CMakeLists.txt

@@ -0,0 +1,24 @@
+# ----------------------------------------------------------------------------
+# SymForce - Copyright 2022, Skydio, Inc.
+# This source code is under the Apache 2.0 license found in the LICENSE file.
+# ----------------------------------------------------------------------------
+
+# ==============================================================================
+# SymForce Targets
+# ==============================================================================
+
+file(GLOB_RECURSE SYMFORCE_SLAM_SOURCES CONFIGURE_DEPENDS *.cc **/*.cc)
+file(GLOB_RECURSE SYMFORCE_SLAM_HEADERS CONFIGURE_DEPENDS *.h **/*.h *.tcc **/*.tcc)
+
+add_library(
+  symforce_slam
+  ${SYMFORCE_LIBRARY_TYPE}
+  ${SYMFORCE_SLAM_SOURCES}
+  ${SYMFORCE_SLAM_HEADERS}
+)
+target_compile_options(symforce_slam PRIVATE ${SYMFORCE_COMPILE_OPTIONS})
+target_link_libraries(symforce_slam
+  symforce_gen
+  symforce_opt
+  ${SYMFORCE_EIGEN_TARGET}
+)

symforce/slam/imu_preintegration/generate.py

@@ -0,0 +1,73 @@
+# ----------------------------------------------------------------------------
+# SymForce - Copyright 2022, Skydio, Inc.
+# This source code is under the Apache 2.0 license found in the LICENSE file.
+# ----------------------------------------------------------------------------
+
+import functools
+
+from symforce import codegen
+from symforce import typing as T
+from symforce.slam.imu_preintegration.manifold_symbolic import imu_manifold_preintegration_update
+from symforce.slam.imu_preintegration.manifold_symbolic import internal_imu_residual
+
+
+def generate_manifold_imu_preintegration(
+    config: codegen.CodegenConfig, output_dir: T.Openable
+) -> None:
+    """
+    Generate the on-manifold IMU preintegration update and residual functions.
+    """
+
+    update_output_names = [
+        "new_DR",
+        "new_Dv",
+        "new_Dp",
+        "new_covariance",
+        "new_DR_D_gyro_bias",
+        "new_Dv_D_accel_bias",
+        "new_Dv_D_gyro_bias",
+        "new_Dp_D_accel_bias",
+        "new_Dp_D_gyro_bias",
+    ]
+
+    codegen_update = codegen.Codegen.function(
+        functools.partial(imu_manifold_preintegration_update, use_handwritten_derivatives=True),
+        config=config,
+        output_names=update_output_names,
+    )
+    codegen_update.generate_function(output_dir=output_dir, skip_directory_nesting=True)
+
+    codegen_update_auto_derivative = codegen.Codegen.function(
+        functools.partial(imu_manifold_preintegration_update, use_handwritten_derivatives=False),
+        name="imu_manifold_preintegration_update_auto_derivative",
+        docstring="""
+    Alternative to ImuManifoldPreintegrationUpdate that uses auto-derivatives. Exists only to
+    help verify correctness of ImuManifoldPreintegrationUpdate. Should have the same output.
+    Since this function is more expensive, there is no reason to use it instead.
+        """
+        + (
+            imu_manifold_preintegration_update.__doc__
+            if imu_manifold_preintegration_update.__doc__ is not None
+            else ""
+        ),
+        config=config,
+        output_names=update_output_names,
+    )
+    codegen_update_auto_derivative.generate_function(
+        output_dir=output_dir, skip_directory_nesting=True
+    )
+
+    codegen_residual = codegen.Codegen.function(
+        internal_imu_residual,
+        config=config,
+    ).with_linearization(
+        which_args=[
+            "pose_i",
+            "vel_i",
+            "pose_j",
+            "vel_j",
+            "accel_bias_i",
+            "gyro_bias_i",
+        ]
+    )
+    codegen_residual.generate_function(output_dir=output_dir, skip_directory_nesting=True)

symforce/slam/imu_preintegration/imu_factor.cc

@@ -0,0 +1,53 @@
+/* ----------------------------------------------------------------------------
+ * SymForce - Copyright 2022, Skydio, Inc.
+ * This source code is under the Apache 2.0 license found in the LICENSE file.
+ * ---------------------------------------------------------------------------- */
+
+#include "./imu_factor.h"
+
+#include <Eigen/Cholesky>
+
+#include <sym/factors/internal/internal_imu_factor.h>
+
+#include "preintegrated_imu_measurements.h"
+
+namespace sym {
+
+template <typename Scalar>
+ImuFactor<Scalar>::ImuFactor(const ImuPreintegrator<Scalar>& preintegrator)
+    : preintegrated_measurements_{preintegrator.PreintegratedMeasurements()},
+      // NOTE(brad, chao): llt then inverse is 2x faster than inverse then llt
+      sqrt_info_{preintegrator.Covariance().llt().matrixL().solve(
+          Eigen::Matrix<Scalar, 9, 9>::Identity())} {}
+
+template <typename Scalar>
+sym::Factor<Scalar> ImuFactor<Scalar>::Factor(const std::vector<Key>& keys_to_func) const {
+  const auto begin = keys_to_func.begin();
+  // NOTE(brad): *this is copied. Keys to optimize happen to be first 6 keys to func
+  return sym::Factor<Scalar>::Hessian(*this, keys_to_func, std::vector<Key>(begin, begin + 6));
+}
+
+template <typename Scalar>
+void ImuFactor<Scalar>::operator()(
+    const sym::Pose3<Scalar>& pose_i, const Eigen::Matrix<Scalar, 3, 1>& vel_i,
+    const sym::Pose3<Scalar>& pose_j, const Eigen::Matrix<Scalar, 3, 1>& vel_j,
+    const Eigen::Matrix<Scalar, 3, 1>& accel_bias_i, const Eigen::Matrix<Scalar, 3, 1>& gyro_bias_i,
+    const Eigen::Matrix<Scalar, 3, 1>& gravity, const Scalar epsilon,
+    Eigen::Matrix<Scalar, 9, 1>* const res, Eigen::Matrix<Scalar, 9, 24>* const jacobian,
+    Eigen::Matrix<Scalar, 24, 24>* const hessian, Eigen::Matrix<Scalar, 24, 1>* const rhs) const {
+  InternalImuFactor(
+      pose_i, vel_i, pose_j, vel_j, accel_bias_i, gyro_bias_i, preintegrated_measurements_.DR,
+      preintegrated_measurements_.Dv, preintegrated_measurements_.Dp, sqrt_info_,
+      preintegrated_measurements_.DR_D_gyro_bias, preintegrated_measurements_.Dv_D_accel_bias,
+      preintegrated_measurements_.Dv_D_gyro_bias, preintegrated_measurements_.Dp_D_accel_bias,
+      preintegrated_measurements_.Dp_D_gyro_bias, preintegrated_measurements_.accel_bias,
+      preintegrated_measurements_.gyro_bias, gravity, preintegrated_measurements_.integrated_dt,
+      epsilon,
+      // outputs
+      res, jacobian, hessian, rhs);
+}
+
+}  // namespace sym
+
+template class sym::ImuFactor<double>;
+template class sym::ImuFactor<float>;