[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

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

gen/cpp/sym/factors/internal/imu_manifold_preintegration_update.h

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

gen/cpp/sym/factors/internal/imu_manifold_preintegration_update_auto_derivative.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/internal_imu_factor.h

@@ -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/geo/matrix.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)