|
| 1 | +// Numerical regression tests for the on-manifold IMU preintegration core |
| 2 | +// (include/lidar_localization/imu_preintegration.hpp, Forster et al. 2017). |
| 3 | +// |
| 4 | +// The preintegration math is the heart of the IMU estimator that issues #36 |
| 5 | +// (imu preintegration) and #77 (IMU angular-velocity utilization + estimator) |
| 6 | +// ask for, yet only its *guard* policy had a test -- the integration, bias |
| 7 | +// Jacobians, residual and covariance propagation were unverified. These tests |
| 8 | +// pin them against closed-form answers and a finite-difference check, in the |
| 9 | +// repository's plain-assert style. They are pure Eigen (no ROS), so they build |
| 10 | +// and run standalone: |
| 11 | +// |
| 12 | +// g++ -std=c++17 -I include -I /usr/include/eigen3 \ |
| 13 | +// test/test_imu_preintegration.cpp -o /tmp/t && /tmp/t |
| 14 | + |
| 15 | +#include "lidar_localization/imu_preintegration.hpp" |
| 16 | + |
| 17 | +#include <Eigen/Eigenvalues> |
| 18 | + |
| 19 | +#include <cassert> |
| 20 | +#include <cmath> |
| 21 | +#include <vector> |
| 22 | + |
| 23 | +namespace { |
| 24 | + |
| 25 | +bool close(double a, double b, double tol) { return std::abs(a - b) < tol; } |
| 26 | + |
| 27 | +bool vclose(const Eigen::Vector3d & a, const Eigen::Vector3d & b, double tol) |
| 28 | +{ |
| 29 | + return (a - b).norm() < tol; |
| 30 | +} |
| 31 | + |
| 32 | +// Geodesic distance between two rotations, in radians. |
| 33 | +double rot_diff(const Eigen::Matrix3d & A, const Eigen::Matrix3d & B) |
| 34 | +{ |
| 35 | + return so3::Log(A.transpose() * B).norm(); |
| 36 | +} |
| 37 | + |
| 38 | +struct ImuTick |
| 39 | +{ |
| 40 | + Eigen::Vector3d gyro; |
| 41 | + Eigen::Vector3d accel; |
| 42 | + double dt; |
| 43 | +}; |
| 44 | + |
| 45 | +// Integrate a fixed measurement stream under a given bias, returning the |
| 46 | +// (jacobian-bearing) preintegration object. |
| 47 | +ImuPreintegration integrate_stream( |
| 48 | + const std::vector<ImuTick> & ticks, |
| 49 | + const Eigen::Vector3d & bg, const Eigen::Vector3d & ba) |
| 50 | +{ |
| 51 | + ImuPreintegration pre; |
| 52 | + pre.reset(); |
| 53 | + for (const auto & t : ticks) { |
| 54 | + pre.integrate(t.gyro, t.accel, bg, ba, t.dt); |
| 55 | + } |
| 56 | + return pre; |
| 57 | +} |
| 58 | + |
| 59 | +// --------------------------------------------------------------------------- |
| 60 | + |
| 61 | +void test_reset_is_identity() |
| 62 | +{ |
| 63 | + ImuPreintegration pre; |
| 64 | + pre.integrate({0.3, -0.1, 0.2}, {1.0, 0.0, 9.0}, {0, 0, 0}, {0, 0, 0}, 0.01); |
| 65 | + pre.reset(); |
| 66 | + assert(vclose(pre.delta_p, Eigen::Vector3d::Zero(), 1e-12)); |
| 67 | + assert(vclose(pre.delta_v, Eigen::Vector3d::Zero(), 1e-12)); |
| 68 | + assert(rot_diff(pre.delta_R, Eigen::Matrix3d::Identity()) < 1e-12); |
| 69 | + assert(close(pre.dt_sum, 0.0, 1e-12)); |
| 70 | + assert(pre.covariance.norm() < 1e-12); |
| 71 | +} |
| 72 | + |
| 73 | +void test_dt_outside_range_is_ignored() |
| 74 | +{ |
| 75 | + // The integrator rejects non-positive and >0.5 s steps (sensor dropouts). |
| 76 | + ImuPreintegration pre; |
| 77 | + pre.reset(); |
| 78 | + pre.integrate({0.3, 0.0, 0.0}, {1.0, 0.0, 0.0}, {0, 0, 0}, {0, 0, 0}, 0.0); |
| 79 | + pre.integrate({0.3, 0.0, 0.0}, {1.0, 0.0, 0.0}, {0, 0, 0}, {0, 0, 0}, -0.01); |
| 80 | + pre.integrate({0.3, 0.0, 0.0}, {1.0, 0.0, 0.0}, {0, 0, 0}, {0, 0, 0}, 0.6); |
| 81 | + assert(close(pre.dt_sum, 0.0, 1e-12)); |
| 82 | + assert(rot_diff(pre.delta_R, Eigen::Matrix3d::Identity()) < 1e-12); |
| 83 | +} |
| 84 | + |
| 85 | +void test_pure_rotation_matches_analytic() |
| 86 | +{ |
| 87 | + // Constant angular velocity, zero specific force -> delta_R = Exp(omega * t), |
| 88 | + // and position/velocity stay zero (no acceleration). |
| 89 | + const Eigen::Vector3d omega(0.2, -0.3, 0.15); |
| 90 | + const double dt = 0.005; |
| 91 | + const int n = 200; // 1.0 s |
| 92 | + ImuPreintegration pre; |
| 93 | + pre.reset(); |
| 94 | + for (int i = 0; i < n; ++i) { |
| 95 | + pre.integrate(omega, Eigen::Vector3d::Zero(), {0, 0, 0}, {0, 0, 0}, dt); |
| 96 | + } |
| 97 | + const Eigen::Matrix3d expected_R = so3::Exp(omega * (dt * n)); |
| 98 | + assert(rot_diff(pre.delta_R, expected_R) < 1e-9); |
| 99 | + assert(vclose(pre.delta_v, Eigen::Vector3d::Zero(), 1e-12)); |
| 100 | + assert(vclose(pre.delta_p, Eigen::Vector3d::Zero(), 1e-12)); |
| 101 | + assert(close(pre.dt_sum, dt * n, 1e-9)); |
| 102 | +} |
| 103 | + |
| 104 | +void test_pure_translation_matches_analytic() |
| 105 | +{ |
| 106 | + // Zero rotation, constant specific force a -> the discrete scheme is exact for |
| 107 | + // constant acceleration: delta_v = a*t, delta_p = 0.5*a*t^2. |
| 108 | + const Eigen::Vector3d a(0.5, -1.0, 2.0); |
| 109 | + const double dt = 0.01; |
| 110 | + const int n = 100; // 1.0 s |
| 111 | + ImuPreintegration pre; |
| 112 | + pre.reset(); |
| 113 | + for (int i = 0; i < n; ++i) { |
| 114 | + pre.integrate(Eigen::Vector3d::Zero(), a, {0, 0, 0}, {0, 0, 0}, dt); |
| 115 | + } |
| 116 | + const double t = dt * n; |
| 117 | + assert(rot_diff(pre.delta_R, Eigen::Matrix3d::Identity()) < 1e-12); |
| 118 | + assert(vclose(pre.delta_v, a * t, 1e-9)); |
| 119 | + assert(vclose(pre.delta_p, 0.5 * a * t * t, 1e-9)); |
| 120 | +} |
| 121 | + |
| 122 | +void test_residual_zero_for_consistent_trajectory() |
| 123 | +{ |
| 124 | + // Build a world trajectory consistent with constant world acceleration |
| 125 | + // a_world and identity attitude. The IMU measures specific force |
| 126 | + // accel_raw = a_world - g, so a preintegration of accel_raw plus the gravity |
| 127 | + // terms in computeResidual must reproduce the trajectory exactly (residual 0). |
| 128 | + const Eigen::Vector3d g(0, 0, -9.81); |
| 129 | + const Eigen::Vector3d a_world(0.3, -0.2, 0.1); |
| 130 | + const Eigen::Vector3d accel_raw = a_world - g; |
| 131 | + const double dt = 0.01; |
| 132 | + const int n = 100; |
| 133 | + ImuPreintegration pre; |
| 134 | + pre.params.gravity = g; |
| 135 | + pre.reset(); |
| 136 | + for (int i = 0; i < n; ++i) { |
| 137 | + pre.integrate(Eigen::Vector3d::Zero(), accel_raw, {0, 0, 0}, {0, 0, 0}, dt); |
| 138 | + } |
| 139 | + const double t = dt * n; |
| 140 | + const Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); |
| 141 | + const Eigen::Vector3d p_i(2.0, -3.0, 1.0); |
| 142 | + const Eigen::Vector3d v_i(1.0, 0.5, -0.2); |
| 143 | + const Eigen::Vector3d v_j = v_i + a_world * t; |
| 144 | + const Eigen::Vector3d p_j = p_i + v_i * t + 0.5 * a_world * t * t; |
| 145 | + const Eigen::Matrix<double, 9, 1> r = |
| 146 | + pre.computeResidual(p_i, v_i, R, p_j, v_j, R); |
| 147 | + assert(r.norm() < 1e-6); |
| 148 | +} |
| 149 | + |
| 150 | +void test_bias_jacobian_matches_finite_difference() |
| 151 | +{ |
| 152 | + // The decisive test: the first-order bias correction (correctByBias, using the |
| 153 | + // accumulated d_*/d_b Jacobians) must match a full re-integration at the |
| 154 | + // perturbed bias, to within the O(delta^2) truncation error. This validates |
| 155 | + // d_dp_d_bg, d_dp_d_ba, d_dv_d_bg, d_dv_d_ba and d_dR_d_bg together. |
| 156 | + std::vector<ImuTick> ticks; |
| 157 | + for (int i = 0; i < 50; ++i) { |
| 158 | + const double s = 0.02 * i; |
| 159 | + ticks.push_back({ |
| 160 | + Eigen::Vector3d(0.1 + 0.05 * std::sin(s), -0.2, 0.3 * std::cos(s)), |
| 161 | + Eigen::Vector3d(0.5, -1.0 + 0.2 * s, 9.0), |
| 162 | + 0.01}); |
| 163 | + } |
| 164 | + const Eigen::Vector3d bg0(0.01, -0.02, 0.015); |
| 165 | + const Eigen::Vector3d ba0(0.05, 0.03, -0.04); |
| 166 | + const Eigen::Vector3d dbg(1e-4, -2e-4, 1.5e-4); |
| 167 | + const Eigen::Vector3d dba(2e-4, 1e-4, -1e-4); |
| 168 | + |
| 169 | + const ImuPreintegration nominal = integrate_stream(ticks, bg0, ba0); |
| 170 | + const ImuPreintegration truth = integrate_stream(ticks, bg0 + dbg, ba0 + dba); |
| 171 | + |
| 172 | + ImuPreintegration corrected = nominal; |
| 173 | + corrected.correctByBias(dbg, dba); |
| 174 | + |
| 175 | + // First-order match: residual error must be far below the perturbation size |
| 176 | + // (which moved delta_p/delta_v by ~1e-3) -- i.e. the Jacobians explain it. |
| 177 | + assert(vclose(corrected.delta_p, truth.delta_p, 1e-6)); |
| 178 | + assert(vclose(corrected.delta_v, truth.delta_v, 1e-6)); |
| 179 | + assert(rot_diff(corrected.delta_R, truth.delta_R) < 1e-6); |
| 180 | + |
| 181 | + // Sanity: the perturbation actually moved the state well above the tolerance, |
| 182 | + // so the test is not trivially passing on a no-op. |
| 183 | + assert((nominal.delta_p - truth.delta_p).norm() > 1e-5); |
| 184 | +} |
| 185 | + |
| 186 | +void test_covariance_symmetric_and_grows() |
| 187 | +{ |
| 188 | + ImuPreintegration pre; |
| 189 | + pre.reset(); |
| 190 | + auto step = [&]() { |
| 191 | + pre.integrate({0.1, -0.2, 0.05}, {0.4, -0.3, 9.2}, {0, 0, 0}, {0, 0, 0}, 0.01); |
| 192 | + }; |
| 193 | + step(); |
| 194 | + const double cov_after_1 = pre.covariance.trace(); |
| 195 | + for (int i = 0; i < 49; ++i) step(); |
| 196 | + const double cov_after_50 = pre.covariance.trace(); |
| 197 | + |
| 198 | + // Symmetric. |
| 199 | + assert((pre.covariance - pre.covariance.transpose()).norm() < 1e-12); |
| 200 | + // Monotone growth of total uncertainty as more noisy steps accumulate. |
| 201 | + assert(cov_after_50 > cov_after_1); |
| 202 | + // Positive semidefinite: smallest eigenvalue is non-negative. |
| 203 | + Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 9, 9>> es(pre.covariance); |
| 204 | + assert(es.eigenvalues().minCoeff() > -1e-12); |
| 205 | +} |
| 206 | + |
| 207 | +} // namespace |
| 208 | + |
| 209 | +int main() |
| 210 | +{ |
| 211 | + test_reset_is_identity(); |
| 212 | + test_dt_outside_range_is_ignored(); |
| 213 | + test_pure_rotation_matches_analytic(); |
| 214 | + test_pure_translation_matches_analytic(); |
| 215 | + test_residual_zero_for_consistent_trajectory(); |
| 216 | + test_bias_jacobian_matches_finite_difference(); |
| 217 | + test_covariance_symmetric_and_grows(); |
| 218 | + return 0; |
| 219 | +} |
0 commit comments