From 3be09c4da317783ea464c492a3fbbd2ae8fb18d4 Mon Sep 17 00:00:00 2001 From: bradley-solliday-skydio Date: Mon, 19 Dec 2022 14:57:42 -0800 Subject: [PATCH] [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 --- test/CMakeLists.txt | 1 + ...lam_manifold_preintegration_update_test.cc | 383 ++++++++++++++++++ 2 files changed, 384 insertions(+) create mode 100644 test/slam_manifold_preintegration_update_test.cc diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 39c9d6772..c0179f041 100644 --- a/test/CMakeLists.txt +++ b/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}) diff --git a/test/slam_manifold_preintegration_update_test.cc b/test/slam_manifold_preintegration_update_test.cc new file mode 100644 index 000000000..2c0d4532a --- /dev/null +++ b/test/slam_manifold_preintegration_update_test.cc @@ -0,0 +1,383 @@ +/* ---------------------------------------------------------------------------- + * SymForce - Copyright 2022, Skydio, Inc. + * This source code is under the Apache 2.0 license found in the LICENSE file. + * ---------------------------------------------------------------------------- */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +using Eigen::Ref; +using Eigen::Vector3d; +using sym::Rot3d; + +/** + * Calculates what ImuManifoldPreintegrationUpdate should be for the changes in orientation, + * velocity, and position. + */ +void UpdateState(const Rot3d& DR, const Ref& Dv, const Ref& Dp, + const Ref& accel, const Ref& gyro, + const Ref& accel_bias, const Ref& gyro_bias, + const double dt, const double epsilon, Rot3d& new_DR, Ref new_Dv, + Ref 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); + const double nan = std::numeric_limits::quiet_NaN(); + const Eigen::Matrix nanM99 = Eigen::Matrix::Constant(nan); + const Eigen::Matrix3d nanM33 = Eigen::Matrix3d::Constant(nan); + + for (int i_ = 0; i_ < 10; i_++) { + const sym::Rot3d DR = sym::Rot3d::Random(gen); + const Eigen::Vector3d Dv = sym::Random(gen); + const Eigen::Vector3d Dp = sym::Random(gen); + + // Set to NaN because output should not depend on them + const Eigen::Matrix covariance = nanM99; + const Eigen::Matrix3d DR_D_gyro_bias = nanM33; + const Eigen::Matrix3d Dv_D_accel_bias = nanM33; + const Eigen::Matrix3d Dv_D_gyro_bias = nanM33; + const Eigen::Matrix3d Dp_D_accel_bias = nanM33; + const Eigen::Matrix3d Dp_D_gyro_bias = nanM33; + + const Eigen::Vector3d accel = sym::Random(gen); + const Eigen::Vector3d gyro = sym::Random(gen); + + const double dt = 1.24; + + const Eigen::Vector3d accel_bias = sym::Random(gen); + const Eigen::Vector3d gyro_bias = sym::Random(gen); + const Eigen::Vector3d accel_cov = sym::Random(gen); + const Eigen::Vector3d gyro_cov = sym::Random(gen); + + sym::Rot3d new_DR; + Eigen::Vector3d new_Dv; + Eigen::Vector3d new_Dp; + + sym::ImuManifoldPreintegrationUpdate( + 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, accel, gyro, accel_bias, gyro_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 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 + 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 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 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 kAccelBias = {3.4, 1.6, -5.9}; +static const Eigen::Vector3d kGyroBias = {1.2, -2.4, 0.5}; +static const Eigen::Vector3d kAccelCov(7e-5, 7e-5, 7e-5); +static const Eigen::Vector3d kGyroCov(1e-3, 1e-3, 1e-3); +static const Eigen::Vector3d kTrueAccel = Eigen::Vector3d::Constant(4.3); +static const Eigen::Vector3d kTrueGyro = Eigen::Vector3d::Constant(10.2); +} // namespace example + +TEST_CASE("Test ImuPreintegator.covariance", "[slam]") { + // In order to test that the computed covariance is correct, we sample the values many + // times to numerically calculate the covariance to compare. + using M99 = Eigen::Matrix; + // Parameters + const double dt = 1e-3; + const int kMeasurementsPerSample = 70; + const int kSampleCount = 1 << 18; + + // Multithreading + const int kThreadCount = 1; + const int kStepsPerThread = kSampleCount / kThreadCount; + // NOTE(brad): I assume I can equally distribute samples among the threads + CHECK(kSampleCount % kThreadCount == 0); + std::vector threads; + std::array, kThreadCount> covariance_sums; + covariance_sums.fill(M99::Zero()); + + Eigen::MatrixXd samples(9, kSampleCount); + + sym::ImuPreintegrator noiseless_integrator(example::kAccelBias, example::kGyroBias); + for (int k = 0; k < kMeasurementsPerSample; k++) { + noiseless_integrator.IntegrateMeasurement( + example::kAccelBias + example::kTrueAccel, example::kGyroBias + example::kTrueGyro, + example::kAccelCov, example::kGyroCov, dt, sym::kDefaultEpsilond); + } + + for (int i = 0; i < kThreadCount; i++) { + threads.emplace_back([&samples, &covariance_sums, &noiseless_integrator, dt, i]() { + std::mt19937 gen(45816 + i); + // Each thread needs its own distribution as they are not thread safe + MultiVarNormalDist accel_dist(example::kAccelCov.asDiagonal() * (1 / dt)); + MultiVarNormalDist gyro_dist(example::kGyroCov.asDiagonal() * (1 / dt)); + + Eigen::MatrixXd accel_noise; + Eigen::MatrixXd gyro_noise; + + for (int j = kStepsPerThread * i; j < kStepsPerThread * (i + 1); j++) { + sym::ImuPreintegrator integrator(example::kAccelBias, example::kGyroBias); + + accel_noise = accel_dist.Sample(gen, kMeasurementsPerSample); + gyro_noise = gyro_dist.Sample(gen, kMeasurementsPerSample); + + for (int k = 0; k < kMeasurementsPerSample; k++) { + integrator.IntegrateMeasurement( + example::kAccelBias + example::kTrueAccel + accel_noise.col(k), + example::kGyroBias + example::kTrueGyro + gyro_noise.col(k), example::kAccelCov, + example::kGyroCov, 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 / kSampleCount) * + 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): 6e-2 is partially arbitrary. Seemed like a reasonable value + return std::abs((x - y) / (std::abs(x) + 6e-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; + using M96 = Eigen::Matrix; + + const double dt = 1e-3; + const int iterations = 100; + + sym::ImuPreintegrator integrator(example::kAccelBias, example::kGyroBias); + + for (int k_ = 0; k_ < iterations; k_++) { + integrator.IntegrateMeasurement(example::kAccelBias + example::kTrueAccel, + example::kGyroBias + example::kTrueGyro, example::kAccelCov, + example::kGyroCov, dt, sym::kDefaultEpsilond); + } + + Eigen::Matrix state_D_bias = + (Eigen::Matrix() << 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_accel_bias = example::kAccelBias; + Eigen::Vector3d perturbed_gyro_bias = example::kGyroBias; + if (i < 3) { + perturbed_gyro_bias[i] += perturbation; + } else { + perturbed_accel_bias[i - 3] += perturbation; + } + + sym::ImuPreintegrator perturbed_integrator(perturbed_accel_bias, perturbed_gyro_bias); + for (int k_ = 0; k_ < iterations; k_++) { + perturbed_integrator.IntegrateMeasurement( + example::kAccelBias + example::kTrueAccel, example::kGyroBias + example::kTrueGyro, + example::kAccelCov, example::kGyroCov, 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); +} + +TEST_CASE("Verify handwritten and auto-derivative impls are equivalent", "[slam]") { + // NOTE(Brad): To explain, some derivatives are needed in order to calculate the expressions + // for the new covariance and the new derivatives of the state w.r.t. the IMU biases in the + // symbolic formulation of ImuManifoldPreintegrationUpdate. These derivatives can be + // be automatically computed using tangent_jacobians, but the expressions can also be obtained + // by hand. For whatever reason (likely having to do with CSE) the handwritten expressions use + // fewer ops. To verify that the handwritten implementations are correct, we compare them to + // the version of ImuManifoldPreintegrationUpdate using the automatic derivatives. + using M99 = Eigen::Matrix; + std::mt19937 gen(1104); + + for (int i_ = 0; i_ < 10; i_++) { + // Generate sample inputs to preintegration update functions + const sym::Rot3d DR = sym::Rot3d::Random(gen); + const Eigen::Vector3d Dv = sym::Random(gen); + const Eigen::Vector3d Dp = sym::Random(gen); + + const M99 covariance = + (sym::Random(gen) + 10 * M99::Identity()).selfadjointView(); + const Eigen::Matrix3d DR_D_gyro_bias = sym::Random(gen); + const Eigen::Matrix3d Dv_D_accel_bias = sym::Random(gen); + const Eigen::Matrix3d Dv_D_gyro_bias = sym::Random(gen); + const Eigen::Matrix3d Dp_D_accel_bias = sym::Random(gen); + const Eigen::Matrix3d Dp_D_gyro_bias = sym::Random(gen); + + const Eigen::Vector3d accel = sym::Random(gen); + const Eigen::Vector3d gyro = sym::Random(gen); + + const double dt = 1.24; + + const Eigen::Vector3d accel_bias = sym::Random(gen); + const Eigen::Vector3d gyro_bias = sym::Random(gen); + const Eigen::Vector3d accel_cov = sym::Random(gen); + const Eigen::Vector3d gyro_cov = sym::Random(gen); + + // Calculate outputs of handwritten derivative version + Eigen::Matrix new_covariance; + Eigen::Matrix3d new_DR_D_gyro_bias; + Eigen::Matrix3d new_Dv_D_accel_bias; + Eigen::Matrix3d new_Dv_D_gyro_bias; + Eigen::Matrix3d new_Dp_D_accel_bias; + Eigen::Matrix3d new_Dp_D_gyro_bias; + + sym::ImuManifoldPreintegrationUpdate( + 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, nullptr, nullptr, nullptr, &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); + + // Calculate outputs of auto derivative version + Eigen::Matrix new_covariance_auto; + Eigen::Matrix3d new_DR_D_gyro_bias_auto; + Eigen::Matrix3d new_Dv_D_accel_bias_auto; + Eigen::Matrix3d new_Dv_D_gyro_bias_auto; + Eigen::Matrix3d new_Dp_D_accel_bias_auto; + Eigen::Matrix3d new_Dp_D_gyro_bias_auto; + + sym::ImuManifoldPreintegrationUpdateAutoDerivative( + 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, nullptr, nullptr, nullptr, &new_covariance_auto, + &new_DR_D_gyro_bias_auto, &new_Dv_D_accel_bias_auto, &new_Dv_D_gyro_bias_auto, + &new_Dp_D_accel_bias_auto, &new_Dp_D_gyro_bias_auto); + + // Verify they're equivalent + CHECK(sym::IsClose(new_covariance, new_covariance_auto, 1e-8)); + CHECK(sym::IsClose(new_DR_D_gyro_bias, new_DR_D_gyro_bias_auto, 1e-8)); + CHECK(sym::IsClose(new_Dv_D_accel_bias, new_Dv_D_accel_bias_auto, 1e-8)); + CHECK(sym::IsClose(new_Dv_D_gyro_bias, new_Dv_D_gyro_bias_auto, 1e-8)); + CHECK(sym::IsClose(new_Dp_D_accel_bias, new_Dp_D_accel_bias_auto, 1e-8)); + CHECK(sym::IsClose(new_Dp_D_gyro_bias, new_Dp_D_gyro_bias_auto, 1e-8)); + } +}