Merge pull request #720 from JuliaRobotics/23Q4/enh/consolimutests

better consolidate IMU testing

src/canonical/GenerateCommon.jl

@@ -207,7 +207,7 @@ end
 
 Simulates IMU measurements from body frame rates and desired world frame accelerations.
 """
-function generateField_InertialMeasurement_RateZ(;
+function generateField_InertialMeasurement(;
   dt = 0.01,
   N = 401,
   rate = [0.0, 0.0, pi/2],
@@ -223,6 +223,8 @@ function generateField_InertialMeasurement_RateZ(;
   gn = norm(σ_ω) < 1e-14 ? ()->[0, 0, 0] : ()->rand(MvNormal(diagm(ones(3)*σ_ω^2 * 1/dt)))
   an = norm(σ_a) < 1e-14 ? ()->[0, 0, 0] : ()->rand(MvNormal(diagm(ones(3)*σ_a^2 * 1/dt)))
 
+  Σy  = diagm([ones(3)*σ_a^2; ones(3)*σ_ω^2])
+
   gyros = [rate + gn() for _ = 1:N]
 
   accels = Vector{typeof(accel0)}()
@@ -237,20 +239,26 @@ function generateField_InertialMeasurement_RateZ(;
     push!(accels, (b_a .+ an()) + w_R_b' * accel0)
   end
 
-  (;tspan,gyros,accels)
+  (;tspan,gyros,accels,Σy)
 end
 
+generateField_InertialMeasurement_RateZ(;
+  dt = 0.01,
+  N = 401,
+  rate = [0.0, 0.0, pi/2],
+  kw...
+) = generateField_InertialMeasurement(;dt,N,rate,kw...)
 
 
-generateField_InertialMeasurement_RateZ_noise(;
+generateField_InertialMeasurement_noise(;
   dt = 0.1,
   N = 11,
   rate = [0, 0, 0.001],
   gravity = SA[0, 0, 9.81],
   accel0 = [0, 0, -1.0] + gravity,
   σ_a = 1e-4,             # 0.16e-3*9.81  # noise density m/s²/√Hz
   σ_ω = deg2rad(0.0001),  # noise density rad/√Hz
-) = generateField_InertialMeasurement_RateZ(;
+) = generateField_InertialMeasurement(;
   dt, 
   N, 
   rate, 

test/inertial/testIMUDeltaFactor.jl

@@ -149,32 +149,30 @@ p_SE3 = exp_lie(M_SE3, X_SE3)
 ## test factor with rotation around z axis and initial velocity up
 # DUPLICATED IN testInertialDynamic.jl
 dt = 0.1
-σ_a = 1e-4#0.16e-3*9.81  # noise density m/s²/√Hz
+N = 11
+
+σ_a = 1e-4 #0.16e-3*9.81  # noise density m/s²/√Hz
 σ_ω = deg2rad(0.0001)  # noise density rad/√Hz
-gn = MvNormal(diagm(ones(3)*σ_ω^2 * 1/dt))
-an = MvNormal(diagm(ones(3)*σ_a^2 * 1/dt))
+imu = RoME.generateField_InertialMeasurement_noise(; dt, N, rate=SA[0, 0, 0.001], accel0=SA[0, 0, 9.81-1], σ_a, σ_ω)
 
-Σy  = diagm([ones(3)*σ_a^2; ones(3)*σ_ω^2])
-gyros = [SA[0, 0, 0.001] + rand(gn) for _ = 1:11]
-accels = [SA[0, 0, 9.81 - 1] + rand(an) for _ = 1:11]
-timestamps = collect(range(0; step=dt, length=11))
+timestamps = collect(range(0; step=dt, length=N))
 
 a_b = SA[0.,0,0]
 ω_b = SA[0.,0,0]
 
 fac = RoME.IMUDeltaFactor(
-    accels,
-    gyros,
+    imu.accels,
+    imu.gyros,
     timestamps,
-    Σy,
+    imu.Σy,
     a_b,
     ω_b
 )
 
 # Rotation part
 M_SO3 = SpecialOrthogonal(3)
 ΔR = identity_element(M_SO3)
-for g in gyros[1:end-1]
+for g in imu.gyros[1:end-1]
     exp!(M_SO3, ΔR, ΔR, hat(M_SO3, Identity(M_SO3), g*dt))
 end
 #TODO I would have expected these 2 to be exactly the same
@@ -213,7 +211,13 @@ addFactor!(
     [:x0],
     ManifoldPrior(
         getManifold(RotVelPos),
-        ArrayPartition(SA[1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], SA[10.0, 0.0, 0.0], SA[0.0, 0.0, 0.0]),
+        ArrayPartition(
+            SA[ 1.0 0.0 0.0; 
+                0.0 1.0 0.0; 
+                0.0 0.0 1.0], 
+            SA[10.0, 0.0, 0.0], 
+            SA[0.0, 0.0, 0.0]
+        ),
         MvNormal(diagm(ones(9)*1e-3))
     )
 )
@@ -233,11 +237,12 @@ x1 = getVariableSolverData(fg, :x1, :parametric).val[1]
 dt = 0.01
 N = 11
 dT = (N-1)*dt
-gyros = [SA[0, 0, 0.1] for _ = 1:N]
-accels = [SA[0, 0, 9.81] for _ = 1:N]
+imu = RoME.generateField_InertialMeasurement(;dt,N,accel0=SA[0, 0, 9.81],rate=SA[0, 0, 0.1])
+# gyros = [SA[0, 0, 0.1] for _ = 1:N]
+# accels = [SA[0, 0, 9.81] for _ = 1:N]
 timestamps = collect(range(0; step=dt, length=N))
 
-Δ, Σ, J_b = RoME.preintegrateIMU(accels, gyros, timestamps, Σy, a_b, ω_b)
+Δ, Σ, J_b = RoME.preintegrateIMU(imu.accels, imu.gyros, timestamps, Σy, a_b, ω_b)
 Σ = Σ[SOneTo(9),SOneTo(9)]
 
 @test Δ.x[1] ≈ RotZ(0.1*dT)
@@ -246,6 +251,7 @@ timestamps = collect(range(0; step=dt, length=N))
 @test Δ.x[4] == dT
 
 ##
+# imu = RoME.generateField_InertialMeasurement(;dt,N,accel0=SA[0, 0, 9.81],rate=SA[0.01, 0, 0])
 gyros = [SA[0.01, 0, 0] for _ = 1:N]
 accels = [SA[0, 0, 9.81] for _ = 1:N]
 timestamps = collect(range(0; step=dt, length=N))
@@ -258,6 +264,7 @@ timestamps = collect(range(0; step=dt, length=N))
 @test Δ.x[3][2] < 0
 
 ##
+# imu = RoME.generateField_InertialMeasurement(;dt,N,accel0=SA[0, 0, 9.81],rate=SA[0, 0.01, 0])
 gyros = [SA[0, 0.01, 0] for _ = 1:N]
 accels = [SA[0, 0, 9.81] for _ = 1:N]
 timestamps = collect(range(0; step=dt, length=N))

test/inertial/testInertialDynamic.jl

@@ -19,7 +19,7 @@ N = 11
 
 σ_a = 1e-4 #0.16e-3*9.81  # noise density m/s²/√Hz
 σ_ω = deg2rad(0.0001)  # noise density rad/√Hz
-imu = RoME.generateField_InertialMeasurement_RateZ_noise(; dt, N, σ_a, σ_ω)
+imu = RoME.generateField_InertialMeasurement_noise(; dt, N, rate=SA[0, 0, 0.001], accel0=SA[0, 0, 9.81-1], σ_a, σ_ω)
 
 tst = now(localzone())
 tsp = tst + Second(imu.tspan[2]-imu.tspan[1])

test/runtests.jl

@@ -18,7 +18,11 @@ testfiles = [
   "testG2oParser.jl";  # deferred
 
   # dev test first, for faster issues.
+  # Inertial
   "inertial/testODE_INS.jl";
+  "inertial/testIMUDeltaFactor.jl";
+  "inertial/testInertialDynamic.jl";
+
   # ...
   # "testFluxModelsPose2.jl";
   "testPartialRangeCrossCorrelations.jl";
@@ -41,10 +45,6 @@ testfiles = [
   "testBearing2D.jl";
   "testMultimodalRangeBearing.jl"; # restore after Bearing factors are fixed
 
-  # Inertial
-  "inertial/testIMUDeltaFactor.jl";
-  "inertial/testInertialDynamic.jl";
-
   # regular tests expected to pass
   "testpackingconverters.jl";
   "testInflation380.jl";