[ImuFactor] Unit test the on manifold ImuFactor

Adds three tests meant to jointly test the
`ImuManifoldPreintegrationUpdate` function.

The calculated covariance is tested by sampling the values it is a
covariance of and comparing it to the numircally calculated covariance
from the samples. Warning, there is a tradeoff between time (number of
samples) and accuracy for the calculated covariance. To speed things up,
the sampling process is multi-threaded. Note, with my current parameters,
it takes about 2^23 samples to be pretty robust to changes in the random
seed. But that takes a minute to run on my desktop with 16 cores. So in
the interest of testing time, I've cut the sample size by a bit. Of
course, this means if you change the seed, the test might end up
failing. To know if it's really a problem, bump up the sample size and
see if it persists.

The derivatives wrt to bias are calculated by perturbing the input in
each direction to numerically calculate the columns of the derivative.

The actual change in state (DR, Dv, Dp) isn't tested in a very
intelligent manner. They didn't have any simple and obvious properties
that could be easily tested. Still, their implementations are pretty
simple, so I figured comparing to a C++ implementation would at the very
least protect against someone accidentally deleting a line in the
symbolic code or whatever.

Topic: on_manifold_imu_factor_test
Relative: on_manifold_imu_factor
Labels: draft

Author: bradley-solliday-skydio

test/CMakeLists.txt

@@ -58,6 +58,7 @@ foreach(SOURCE_FILE ${SYMFORCE_TEST_CC_SOURCES})
     symforce_opt
     symforce_examples
     symforce_test
+    symforce_slam
   )
 
   list(APPEND SYMFORCE_CC_TEST_TARGETS ${TARGET_NAME})

test/slam_manifold_preintegration_update_test.cc

@@ -0,0 +1,312 @@
+/* ----------------------------------------------------------------------------
+ * SymForce - Copyright 2022, Skydio, Inc.
+ * This source code is under the Apache 2.0 license found in the LICENSE file.
+ * ---------------------------------------------------------------------------- */
+
+#include <numeric>
+#include <random>
+#include <thread>
+
+#include <Eigen/Dense>
+#include <catch2/catch_test_macros.hpp>
+
+#include <sym/factors/internal/imu_manifold_preintegration_update.h>
+#include <sym/rot3.h>
+#include <sym/util/epsilon.h>
+#include <symforce/slam/imu_preintegration/imu_preintegrator.h>
+
+using Eigen::Ref;
+using Eigen::Vector3d;
+using sym::Rot3d;
+
+/**
+ * Calculates what ImuManifoldPreintegrationUpdate should for the changes in orientation,
+ * velocity, and position.
+ */
+void UpdateState(const Rot3d& DR, const Ref<const Vector3d>& Dv, const Ref<const Vector3d>& Dp,
+                 const Ref<const Vector3d>& gyro, const Ref<const Vector3d>& accel,
+                 const Ref<const Vector3d>& gyro_bias, const Ref<const Vector3d>& accel_bias,
+                 const double dt, const double epsilon, Rot3d& new_DR, Ref<Vector3d> new_Dv,
+                 Ref<Vector3d> new_Dp) {
+  const auto corrected_accel = accel - accel_bias;
+  new_DR = DR * Rot3d::FromTangent((gyro - gyro_bias) * dt, epsilon);
+  new_Dv = Dv + DR * corrected_accel * dt;
+  new_Dp = Dp + Dv * dt + DR * corrected_accel * (0.5 * dt * dt);
+}
+
+TEST_CASE("Test ImuManifoldPreintegrationUpdate has correct DR, Dv, & Dp", "[slam]") {
+  std::mt19937 gen(1804);
+  std::normal_distribution<double> dist{0.0, 10.0};
+  const auto randomM3 = Eigen::Matrix3d::NullaryExpr([&]() { return dist(gen); });
+  const auto randomV3 = Eigen::Vector3d::NullaryExpr([&]() { return dist(gen); });
+  const auto randomM9 = Eigen::Matrix<double, 9, 9>::NullaryExpr([&]() { return dist(gen); });
+
+  for (int i_ = 0; i_ < 10; i_++) {
+    const sym::Rot3d DR = sym::Rot3d::Random(gen);
+    const Eigen::Vector3d Dv = randomV3;
+    const Eigen::Vector3d Dp = randomV3;
+    const Eigen::Matrix<double, 9, 9> covariance = randomM9;
+    const Eigen::Matrix3d DR_D_gyro_bias = randomM3;
+    const Eigen::Matrix3d Dv_D_gyro_bias = randomM3;
+    const Eigen::Matrix3d Dv_D_accel_bias = randomM3;
+    const Eigen::Matrix3d Dp_D_gyro_bias = randomM3;
+    const Eigen::Matrix3d Dp_D_accel_bias = randomM3;
+
+    const Eigen::Vector3d gyro = randomV3;
+    const Eigen::Vector3d accel = randomV3;
+
+    const double dt = 1.24;
+
+    const Eigen::Vector3d gyro_bias = randomV3;
+    const Eigen::Vector3d accel_bias = randomV3;
+    const Eigen::Vector3d gyro_cov = randomV3;
+    const Eigen::Vector3d accel_cov = randomV3;
+
+    sym::Rot3d new_DR;
+    Eigen::Vector3d new_Dv;
+    Eigen::Vector3d new_Dp;
+    Eigen::Matrix<double, 9, 9> new_covarinace;
+    Eigen::Matrix3d new_DR_D_gyro_bias;
+    Eigen::Matrix3d new_Dp_D_gyro_bias;
+    Eigen::Matrix3d new_Dp_D_accel_bias;
+    Eigen::Matrix3d new_Dv_D_gyro_bias;
+    Eigen::Matrix3d new_Dv_D_accel_bias;
+
+    sym::ImuManifoldPreintegrationUpdate<double>(
+        DR, Dv, Dp, covariance, DR_D_gyro_bias, Dv_D_accel_bias, Dv_D_gyro_bias, Dp_D_accel_bias,
+        Dp_D_gyro_bias, accel_bias, gyro_bias, accel_cov, gyro_cov, accel, gyro, dt,
+        sym::kDefaultEpsilond, &new_DR, &new_Dv, &new_Dp, nullptr, nullptr, nullptr, nullptr,
+        nullptr, nullptr);
+
+    sym::Rot3d expected_DR;
+    Eigen::Vector3d expected_Dv;
+    Eigen::Vector3d expected_Dp;
+
+    UpdateState(DR, Dv, Dp, gyro, accel, gyro_bias, accel_bias, dt, sym::kDefaultEpsilond,
+                expected_DR, expected_Dv, expected_Dp);
+
+    CHECK(sym::IsClose(new_DR, expected_DR, 1e-14));
+    CHECK(sym::IsClose(new_Dv, expected_Dv, 1e-14));
+    CHECK(sym::IsClose(new_Dp, expected_Dp, 1e-14));
+  }
+}
+
+/**
+ * Helper class to generate samples from a multi-variate normal distribution with
+ * the same covariance as that passed into the constructor.
+ */
+class MultiVarNormalDist {
+ private:
+  const Eigen::MatrixXd L;
+  std::normal_distribution<double> dist;
+
+ public:
+  /**
+   * covar is the covariance of the desired normal distribution.
+   * Precondition: covar is a symmetric, positive definite matrix.
+   */
+  MultiVarNormalDist(const Eigen::MatrixXd covar) : L{covar.llt().matrixL()}, dist{} {}
+
+  /**
+   * Returns a matrix whose (count) colums are samples from the distribution.
+   */
+  template <typename Generator>
+  Eigen::MatrixXd Sample(Generator& gen, const int count) {
+    Eigen::MatrixXd rand =
+        Eigen::MatrixXd::NullaryExpr(L.cols(), count, [&]() { return dist(gen); });
+    return L * rand;
+  }
+};
+
+/**
+ * Calculates the covariance of the columns of samples.
+ */
+Eigen::MatrixXd Covariance(const Eigen::MatrixXd& samples) {
+  const long int sample_size = samples.cols();
+  const Eigen::VectorXd avg_col = samples.rowwise().sum() / sample_size;
+  const Eigen::MatrixXd meanless_samples = samples.colwise() - avg_col;
+  return (meanless_samples * meanless_samples.transpose()) / (sample_size - 1);
+}
+
+TEST_CASE("Test MultiVarNormalDist generates samples with correct covariance", "[slam]") {
+  std::mt19937 gen(170);
+
+  // Create covar, a symmetric positive definite matrix
+  // NOTE(brad): This matrix is positive semi-definite because the diagonal entries all
+  // have values of at least 2.0, and the max value of an off-diagonal entry is .2. Thus
+  // within a row, the sum of the non-diagonal entries is capped by 0.2 * 8 = 1.6. Thus by the
+  // Gershgorin circle theorem, any eigen value of covar is at least 0.4 (i.e., positive).
+  std::uniform_real_distribution<double> uniform_dist(0.01, 0.1);
+  Eigen::MatrixXd covar = Eigen::MatrixXd::NullaryExpr(9, 9, [&]() { return uniform_dist(gen); });
+  covar += covar.transpose().eval();
+  covar.diagonal().array() += 2.0;
+
+  MultiVarNormalDist dist(covar);
+
+  const Eigen::MatrixXd samples = dist.Sample(gen, 1 << 23);
+  const Eigen::Matrix<double, 9, 9> calculated_covar = Covariance(samples);
+
+  for (int col = 0; col < 9; col++) {
+    for (int row = 0; row < 9; row++) {
+      const double covar_rc = covar(row, col);
+      const double difference = covar_rc - calculated_covar(row, col);
+      // NOTE(brad): trade-off between sample count (time) and accuracy of calculated_covar
+      CHECK(difference * difference < 9e-3 * covar_rc * covar_rc);
+    }
+  }
+}
+
+namespace example {
+static const Eigen::Vector3d accel_bias = {3.4, 1.6, -5.9};
+static const Eigen::Vector3d gyro_bias = {1.2, -2.4, 0.5};
+static const Eigen::Vector3d accel_cov(7e-5, 7e-5, 7e-5);
+static const Eigen::Vector3d gyro_cov(1e-3, 1e-3, 1e-3);
+static const Eigen::Vector3d true_gyro = Eigen::Vector3d::Constant(10.2);
+static const Eigen::Vector3d true_accel = Eigen::Vector3d::Constant(4.3);
+}  // namespace example
+
+TEST_CASE("Test ImuPreintegator.covariance", "[slam]") {
+  // In order to test that the computed covariance is correct, we sample the values many
+  // time to numerically calculate the covariance to compare.
+  using M99 = Eigen::Matrix<double, 9, 9>;
+  // Parameters
+  const double dt = 1e-4;
+  const int measurements_per_sample = 130;
+  const int sample_count = 1 << 19;
+
+  // Multithreading
+  const int thread_count = 16;
+  const int steps_per_thread = sample_count / thread_count;
+  // NOTE(brad): I assume I can equally distribute samples among the threads
+  CHECK(sample_count % thread_count == 0);
+  std::vector<std::thread> threads;
+  std::array<Eigen::Matrix<double, 9, 9>, thread_count> covariance_sums;
+  covariance_sums.fill(M99::Zero());
+
+  Eigen::MatrixXd samples(9, sample_count);
+
+  sym::ImuPreintegrator<double> noiseless_integrator(example::accel_bias, example::gyro_bias);
+  for (int k = 0; k < measurements_per_sample; k++) {
+    noiseless_integrator.IntegrateMeasurement(
+        example::accel_bias + example::true_accel, example::gyro_bias + example::true_gyro,
+        example::accel_cov, example::gyro_cov, dt, sym::kDefaultEpsilond);
+  }
+
+  for (int i = 0; i < thread_count; i++) {
+    threads.emplace_back([&samples, &covariance_sums, &noiseless_integrator, dt, i]() {
+      std::mt19937 gen(1216 + i);
+      // Each thread needs its own distribution as they are not thread safe
+      MultiVarNormalDist accel_dist(example::accel_cov.asDiagonal() * (1 / dt));
+      MultiVarNormalDist gyro_dist(example::gyro_cov.asDiagonal() * (1 / dt));
+
+      Eigen::MatrixXd accel_noise;
+      Eigen::MatrixXd gyro_noise;
+
+      for (int j = steps_per_thread * i; j < steps_per_thread * (i + 1); j++) {
+        sym::ImuPreintegrator<double> integrator(example::accel_bias, example::gyro_bias);
+
+        gyro_noise = gyro_dist.Sample(gen, measurements_per_sample);
+        accel_noise = accel_dist.Sample(gen, measurements_per_sample);
+
+        for (int k = 0; k < measurements_per_sample; k++) {
+          integrator.IntegrateMeasurement(
+              example::accel_bias + example::true_accel + accel_noise.col(k),
+              example::gyro_bias + example::true_gyro + gyro_noise.col(k), example::accel_cov,
+              example::gyro_cov, dt, sym::kDefaultEpsilond);
+        }
+
+        samples.col(j).segment(0, 3) =
+            noiseless_integrator.PreintegratedMeasurements().DR.LocalCoordinates(
+                integrator.PreintegratedMeasurements().DR);
+        samples.col(j).segment(3, 3) = integrator.PreintegratedMeasurements().Dv;
+        samples.col(j).segment(6, 3) = integrator.PreintegratedMeasurements().Dp;
+
+        covariance_sums[i] += integrator.Covariance();
+      }
+    });
+  }
+  for (std::thread& t : threads) {
+    t.join();
+  }
+
+  const M99 calculated_covariance =
+      (1.0 / sample_count) *
+      std::accumulate(covariance_sums.begin(), covariance_sums.end(), M99::Zero().eval());
+
+  const Eigen::MatrixXd sampled_covariance = Covariance(samples);
+  const double sampled_covariance_max = sampled_covariance.array().abs().maxCoeff();
+
+  const Eigen::MatrixXd weighted_relative_error = sampled_covariance.binaryExpr(
+      calculated_covariance, [=](const double x, const double y) -> double {
+        // NOTE(brad): 3e-2 is partially arbitrary. Seemed like a reasonable value
+        return std::abs((x - y) / (std::abs(x) + 3e-2 * sampled_covariance_max));
+      });
+
+  // NOTE(brad): 0.05 is also somewhat arbitrary. Seemed reasonable.
+  CHECK(weighted_relative_error.maxCoeff() < 0.05);
+}
+
+TEST_CASE("Test preintegrated derivatives wrt IMU biases", "[slam]") {
+  using M33 = Eigen::Matrix<double, 3, 3>;
+  using M96 = Eigen::Matrix<double, 9, 6>;
+
+  const double dt = 1e-3;
+  const int iterations = 100;
+
+  sym::ImuPreintegrator<double> integrator(example::accel_bias, example::gyro_bias);
+
+  for (int k_ = 0; k_ < iterations; k_++) {
+    integrator.IntegrateMeasurement(example::accel_bias + example::true_accel,
+                                    example::gyro_bias + example::true_gyro, example::accel_cov,
+                                    example::gyro_cov, dt, sym::kDefaultEpsilond);
+  }
+
+  Eigen::Matrix<double, 9, 6> state_D_bias =
+      (Eigen::Matrix<double, 9, 6>() << integrator.PreintegratedMeasurements().DR_D_gyro_bias,
+       M33::Zero(), integrator.PreintegratedMeasurements().Dv_D_gyro_bias,
+       integrator.PreintegratedMeasurements().Dv_D_accel_bias,
+       integrator.PreintegratedMeasurements().Dp_D_gyro_bias,
+       integrator.PreintegratedMeasurements().Dp_D_accel_bias)
+          .finished();
+
+  // Perturb inputs and calculate derivatives numerically from them
+  const double perturbation = 1e-5;
+  M96 numerical_state_D_bias;
+  for (int i = 0; i < 6; i++) {
+    // Create biases and perturb one of their coefficients
+    Eigen::Vector3d perturbed_gyro_bias = example::gyro_bias;
+    Eigen::Vector3d perturbed_accel_bias = example::accel_bias;
+    if (i < 3) {
+      perturbed_gyro_bias[i] += perturbation;
+    } else {
+      perturbed_accel_bias[i - 3] += perturbation;
+    }
+
+    sym::ImuPreintegrator<double> perturbed_integrator(perturbed_accel_bias, perturbed_gyro_bias);
+    for (int k_ = 0; k_ < iterations; k_++) {
+      perturbed_integrator.IntegrateMeasurement(
+          example::accel_bias + example::true_accel, example::gyro_bias + example::true_gyro,
+          example::accel_cov, example::gyro_cov, dt, sym::kDefaultEpsilond);
+    }
+
+    numerical_state_D_bias.col(i).segment(0, 3) =
+        integrator.PreintegratedMeasurements().DR.LocalCoordinates(
+            perturbed_integrator.PreintegratedMeasurements().DR) /
+        perturbation;
+    numerical_state_D_bias.col(i).segment(3, 3) =
+        (perturbed_integrator.PreintegratedMeasurements().Dv -
+         integrator.PreintegratedMeasurements().Dv) /
+        perturbation;
+    numerical_state_D_bias.col(i).segment(6, 3) =
+        (perturbed_integrator.PreintegratedMeasurements().Dp -
+         integrator.PreintegratedMeasurements().Dp) /
+        perturbation;
+  }
+
+  const Eigen::MatrixXd relative_error =
+      numerical_state_D_bias.binaryExpr(state_D_bias, [](const double x, const double y) -> double {
+        return std::abs((x - y) / (std::abs(x) + 1e-10));
+      });
+
+  CHECK(relative_error.maxCoeff() < 0.05);
+}