[ImuFactor] Add an on manifold Imu Factor

Unit testing is left to a follow up commit.

The core use-facing api is found in
- symforce/slam/imu_preintegration/preintegrated_imu_measurements.h
- symforce/slam/imu_preintegration/imu_factor.h

Unlike most other factors (/residual functions) in symforce, the
ImuFactor is a functor so as to store the pre-integrated values
with the function rather than in the `Values`. (There are a lot of
parameters that are constant and unique to each factor).

A python version of the `ImuFactor` class will come in a subsequent
commit.

Topic: on_manifold_imu_factor

Author: bradley-solliday-skydio

CMakeLists.txt

@@ -286,6 +286,8 @@ if(SYMFORCE_BUILD_CC_SYM)
   add_subdirectory(symforce/pybind)
 endif()
 
+add_subdirectory(symforce/slam)
+
 
 # ==============================================================================
 # Examples, Benchmarks, and Tests

gen/cpp/sym/factors/imu_manifold_preintegration_update.h

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

gen/cpp/sym/factors/internal_imu_factor.h

@@ -0,0 +1,23 @@
+# ----------------------------------------------------------------------------
+# 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_EIGEN_TARGET}
+)

gen/python/sym/factors/__init__.py

@@ -0,0 +1,4 @@
+# ----------------------------------------------------------------------------
+# SymForce - Copyright 2022, Skydio, Inc.
+# This source code is under the Apache 2.0 license found in the LICENSE file.
+# ----------------------------------------------------------------------------

symforce/pybind/CMakeLists.txt

@@ -0,0 +1,4 @@
+# ----------------------------------------------------------------------------
+# SymForce - Copyright 2022, Skydio, Inc.
+# This source code is under the Apache 2.0 license found in the LICENSE file.
+# ----------------------------------------------------------------------------

symforce/slam/CMakeLists.txt

@@ -0,0 +1,48 @@
+# ----------------------------------------------------------------------------
+# SymForce - Copyright 2022, Skydio, Inc.
+# This source code is under the Apache 2.0 license found in the LICENSE file.
+# ----------------------------------------------------------------------------
+
+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.
+    """
+    codegen_update = codegen.Codegen.function(
+        imu_manifold_preintegration_update,
+        config=config,
+        output_names=[
+            "new_DR",
+            "new_Dv",
+            "new_Dp",
+            "new_covariance",
+            "new_DR_D_gyro_bias",
+            "new_Dp_D_gyro_bias",
+            "new_Dp_D_accel_bias",
+            "new_Dv_D_gyro_bias",
+            "new_Dv_D_accel_bias",
+        ],
+    )
+    codegen_update.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/__init__.py

@@ -0,0 +1,45 @@
+/* ----------------------------------------------------------------------------
+ * 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_imu_factor.h>
+
+#include "preintegrated_imu_measurements.h"
+
+namespace sym {
+
+template <typename Scalar>
+ImuFactor<Scalar>::ImuFactor(const PreintegratedImuMeasurements<Scalar>& preintegrated_measurements,
+                             const Eigen::Matrix<Scalar, 9, 9>& covariance)
+    : preintegrated_measurements_{preintegrated_measurements},
+      sqrt_info_{covariance.llt().matrixL().solve(Eigen::Matrix<Scalar, 9, 9>::Identity())} {}
+
+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_.Dp_D_accel_bias,
+      preintegrated_measurements_.Dp_D_gyro_bias, preintegrated_measurements_.Dv_D_accel_bias,
+      preintegrated_measurements_.Dv_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>;