Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

compat DFG 25 AMP 9 #759

Merged
merged 10 commits into from
Dec 1, 2024
Merged

compat DFG 25 AMP 9 #759

merged 10 commits into from
Dec 1, 2024

Conversation

dehann
Copy link
Member

@dehann dehann commented Nov 26, 2024 •
edited
Loading

@dehann dehann added the compat label Nov 26, 2024
@dehann dehann added this to the v0.24.6 milestone Nov 26, 2024
@dehann dehann self-assigned this Nov 27, 2024
@dehann
Copy link
Member Author

dehann commented Nov 27, 2024

oops, unexpected test failure in IMU delta time calculation:

TEST 3] inertial/testIMUDeltaFactor.jl =============================================================
┌ Warning: IMU Covar check
│   ch =
│    DomainError with [3.02229e-13, 3.02229e-13, 3.04617e-13, 6.50936e-11, 6.5306e-11, 6.5306e-11, 1.26741e-9, 1.27623e-9, 1.27623e-9]:
│    The point [3.3367561117710476e-10 -2.7609649944671247e-22 1.0262056611332025e-17 5.029455534169307e-10 8.35749406018414e-16 4.063291736400337e-17 -1.117067588023346e-18 4.472807055334006e-13 1.2011010579525087e-17; -2.760964900056146e-22 3.336756111765377e-10 2.695113171909391e-17 -8.35751454292895e-16 5.029455534148217e-10 9.634212488678988e-17 -4.472807055333133e-13 -1.1170672993474503e-18 -4.839304950412257e-18; 1.0262056611332176e-17 2.695113171909391e-17 3.3250000000078545e-10 2.6444736964138582e-17 7.314228987205475e-17 5.000000000029254e-10 -1.2013935132806313e-17 4.837716764346104e-18 1.255019869403438e-25; 5.029455534169303e-10 -8.357514542928227e-16 2.6444736964138495e-17 1.0078613465442785e-9 -3.899658713026748e-21 1.0818140445092925e-16 -2.234132632011363e-18 1.3418364483810585e-12 4.986238966596496e-17; 8.357494060185379e-16 5.029455534148219e-10 7.314228987205473e-17 -3.899658712587481e-21 1.0078613465361172e-9 2.720263935782622e-16 -1.3418364483810262e-12 -2.2341320692213882e-18 -1.9754586841461696e-17; 4.063291736400347e-17 9.634212488678971e-17 5.000000000029253e-10 1.0818140445092915e-16 2.720263935782619e-16 1.0000000000112698e-9 -4.986469218521429e-17 1.974888176689332e-17 1.190522328871386e-25; -1.1170675880233842e-18 -4.4728070553331347e-13 -1.2013935132806316e-17 -2.2341326320113564e-18 -1.3418364483810258e-12 -4.9864692185214304e-17 3.046174195333595e-13 -4.624059969910203e-27 -2.5790158038474624e-25; 4.472807055334007e-13 -1.1170672993473838e-18 4.8377167643461035e-18 1.3418364483810591e-12 -2.2341320692213967e-18 1.9748881766893314e-17 -4.624058207392764e-27 3.0461741953336023e-13 -6.418747739924718e-25; 1.2011010579525088e-17 -4.839304950412256e-18 1.255019869403413e-25 4.9862389665964966e-17 -1.9754586841461696e-17 1.1905223288714064e-25 -2.579015804289007e-25 -6.418747739921987e-25 3.046174197866861e-13] does not lie on SymmetricPositiveDefinite(9) since its not a positive definite matrix.
└ @ RoME ~/.julia/dev/RoME/src/factors/Inertial/IMUDeltaFactor.jl:478
IMUDeltaFactor spot checks: Error During Test at /home/dehann/.julia/dev/RoME/test/inertial/testIMUDeltaFactor.jl:39
  Got exception outside of a @test
  AssertionError: Time descrepancy in IMUDeltaFactor: Δt = 1.0000000002508003e-6
  Stacktrace:
    [1] (::IncrementalInference.CalcFactorResidual{IMUDeltaFactor{FullNormal}, 2, 9, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}, Float64}}, 81, @NamedTuple{timestams::Tuple{Nanosecond, Nanosecond}}})(Δmeas::ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}, Float64}}, p::ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}, Float64}}, q::ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}, Float64}}, b::SVector{6, Float64})
      @ RoME ~/.julia/dev/RoME/src/factors/Inertial/IMUDeltaFactor.jl:358
    [2] CalcFactor
      @ ~/.julia/dev/RoME/src/factors/Inertial/IMUDeltaFactor.jl:373 [inlined]
    [3] CalcFactor
      @ ~/.julia/dev/RoME/src/factors/Inertial/IMUDeltaFactor.jl:369 [inlined]
    [4] (::IncrementalInference.CalcFactorResidual{IMUDeltaFactor{FullNormal}, 2, 9, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}, Float64}}, 81, @NamedTuple{timestams::Tuple{Nanosecond, Nanosecond}}})(p::ArrayPartition{Float64, Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}})
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:78
    [5] (::IncrementalInference.var"#550#551"{ArrayPartition{Float64, Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}})(f::IncrementalInference.CalcFactorResidual{IMUDeltaFactor{FullNormal}, 2, 9, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}, Float64}}, 81, @NamedTuple{timestams::Tuple{Nanosecond, Nanosecond}}})
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:169
    [6] iterate
      @ ./generator.jl:48 [inlined]
    [7] collect_to!(dest::Vector{SVector{9, Float64}}, itr::Base.Generator{Vector{IncrementalInference.CalcFactorResidual}, IncrementalInference.var"#550#551"{ArrayPartition{Float64, Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}}}, offs::Int64, st::Int64)
      @ Base ./array.jl:838
    [8] collect_to_with_first!(dest::Vector{SVector{9, Float64}}, v1::SVector{9, Float64}, itr::Base.Generator{Vector{IncrementalInference.CalcFactorResidual}, IncrementalInference.var"#550#551"{ArrayPartition{Float64, Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}}}, st::Int64)
      @ Base ./array.jl:816
    [9] _collect(c::Vector{IncrementalInference.CalcFactorResidual}, itr::Base.Generator{Vector{IncrementalInference.CalcFactorResidual}, IncrementalInference.var"#550#551"{ArrayPartition{Float64, Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}}}, ::Base.EltypeUnknown, isz::Base.HasShape{1})
      @ Base ./array.jl:810
   [10] collect_similar
      @ ./array.jl:709 [inlined]
   [11] map
      @ ./abstractarray.jl:3371 [inlined]
   [12] IncrementalInference.JacF_RLM!(M::ProductManifold{ℝ, Tuple{IncrementalInference.NPowerManifold{ℝ, ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, LeftInvariantRepresentation}}}}, costF!::IncrementalInference.CostFres!{Tuple{Vector{IncrementalInference.CalcFactorResidual{ManifoldPrior{ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, LeftInvariantRepresentation}, ZeroMeanFullNormal{Tuple{Base.OneTo{Int64}}}, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, ManifoldsBase.VeeOrthogonalBasis{ℝ}}, 1, 9, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 81, Nothing}}, Vector{IncrementalInference.CalcFactorResidual{IMUDeltaFactor{FullNormal}, 2, 9, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}, Float64}}, 81, @NamedTuple{timestams::Tuple{Nanosecond, Nanosecond}}}}}}, p::ArrayPartition{Float64, Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, fg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}; all_points::ArrayPartition{Float64, Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, basis_domain::DefaultOrthogonalBasis{ℝ, ManifoldsBase.TangentSpaceType}, is_sparse::Bool)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:169
   [13] solve_RLM(fg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, varlabels::Vector{Symbol}, faclabels::Vector{Symbol}; is_sparse::Bool, finiteDiffCovariance::Bool, solveKey::Symbol, kwargs::@Kwargs{damping_term_min::Float64, expect_zero_residual::Bool})
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:337
   [14] solve_RLM (repeats 2 times)
      @ ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:307 [inlined]
   [15] solveGraphParametric!(::GraphsDFG{SolverParams, DFGVariable, DFGFactor}; init::Bool, solveKey::Symbol, is_sparse::Bool, kwargs::@Kwargs{damping_term_min::Float64, expect_zero_residual::Bool})
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/parametric/services/ParametricManopt.jl:608
   [16] macro expansion
      @ ./timing.jl:581 [inlined]
   [17] macro expansion
      @ ~/.julia/dev/RoME/test/inertial/testIMUDeltaFactor.jl:255 [inlined]
   [18] macro expansion
      @ ~/.julia/juliaup/julia-1.11.1+0.x64.linux.gnu/share/julia/stdlib/v1.11/Test/src/Test.jl:1700 [inlined]
   [19] top-level scope
      @ ~/.julia/dev/RoME/test/inertial/testIMUDeltaFactor.jl:42
   [20] include(fname::String)
      @ Main ./sysimg.jl:38
   [21] top-level scope
      @ ~/.julia/dev/RoME/test/runtests.jl:88
   [22] include(fname::String)
      @ Main ./sysimg.jl:38
   [23] top-level scope
      @ none:6
   [24] eval
      @ ./boot.jl:430 [inlined]
   [25] exec_options(opts::Base.JLOptions)
      @ Base ./client.jl:296
   [26] _start()
      @ Base ./client.jl:531
Test Summary:              | Pass  Error  Total     Time
IMUDeltaFactor spot checks |   39      1     40  2m14.0s
ERROR: LoadError: Some tests did not pass: 39 passed, 0 failed, 1 errored, 0 broken.
in expression starting at /home/dehann/.julia/dev/RoME/test/inertial/testIMUDeltaFactor.jl:39
in expression starting at /home/dehann/.julia/dev/RoME/test/runtests.jl:86
ERROR: Package RoME errored during testing

@Affie
Copy link
Member

Affie commented Nov 28, 2024

I didn't see this so merged the compat PRs because my environment was broken. I'll resolve the conflicts.

@Affie
Copy link
Member

Affie commented Nov 28, 2024

oops, unexpected test failure in IMU delta time calculation:

This is unexpected, I thought I fixed the IMU timing issues. I don't think the fix is to increase the tolerance, the check is there to show that something is wrong. It's fine for now, but we should look at why the time is out.

@Affie
Copy link
Member

Affie commented Nov 28, 2024

IMU factor tests should be fixed now. Debugging test slipped through the cracks.

@dehann
Copy link
Member Author

dehann commented Nov 29, 2024

Thanks!

@dehann
Copy link
Member Author

dehann commented Nov 29, 2024 •
edited
Loading

So there is a stochastic issue with testInertialDynamic (reduced stack trace below). I've been struggling to figure out what's causing it and been trying a few things. Here are the things I can say with moderate certainty:

  • Sometimes the tests pass, sometimes they fail.
  • I tried looking at the extrapolate value of the offending gyro line and *think the error correlates when timestamp is ~1e9, but seems to work when timestamp is ~1e8 (unconfirmed). See value in stack trace below.
  • Sometimes the failures seem to come in batches or be related to how much time elapsed since the last test, but there is barely something distinctive. This lead me to think it has something to do with the random number generator.
  • If you approxConv many points (i.e. getSolverParams(fg).N = 1000) the tests fails every time at a different sample number. Converse, if you reduce to say N=40 then the approxConv tends to succeed more often. Why some points are bad is still unknown.
reduced stack trace of error 
┌ Info: gyro
└   t = 1.7327661938228242e9
test InertialDynamic factor: Error During Test at /home/dehann/.julia/dev/RoME/test/inertial/testInertialDynamic.jl:10
  Got exception outside of a @test
  BoundsError: attempt to access 11-element extrapolate(interpolate((::Vector{Float64},), ::Vector{SVector{3, Float64}}, Gridded(Linear())), Throw()) with element type SVector{3, Float64} at index [1.7327661938228242e9]
  Stacktrace:
    [1] throw_boundserror(A::Interpolations.Extrapolation{SVector{3, Float64}, 1, Interpolations.GriddedInterpolation{SVector{3, Float64}, 1, Vector{SVector{3, Float64}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Tuple{Vector{Float64}}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Interpolations.Throw{Nothing}}, I::Tuple{Float64})
      @ Base ./essentials.jl:14
    [2] inbounds_index
      @ ~/.julia/packages/Interpolations/91PhN/src/extrapolation/extrapolation.jl:111 [inlined]
    [3] inbounds_position
      @ ~/.julia/packages/Interpolations/91PhN/src/extrapolation/extrapolation.jl:102 [inlined]
    [4] Extrapolation
      @ ~/.julia/packages/Interpolations/91PhN/src/extrapolation/extrapolation.jl:48 [inlined]
    [5] imuKinematic!(du::ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, u::ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, p::Base.RefValue{@NamedTuple{gyro::Interpolations.Extrapolation{SVector{3, Float64}, 1, Interpolations.GriddedInterpolation{SVector{3, Float64}, 1, Vector{SVector{3, Float64}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Tuple{Vector{Float64}}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Interpolations.Throw{Nothing}}, accel::Interpolations.Extrapolation{SVector{3, Float64}, 1, Interpolations.GriddedInterpolation{SVector{3, Float64}, 1, Vector{SVector{3, Float64}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Tuple{Vector{Float64}}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Interpolations.Throw{Nothing}}}}, t::Float64; g::SVector{3, Float64})
      @ RoME ~/.julia/dev/RoME/ext/factors/InertialDynamic.jl:25
    [6] imuKinematic!
      @ ~/.julia/dev/RoME/ext/factors/InertialDynamic.jl:14 [inlined]
    [7] ODEFunction
      @ ~/.julia/packages/SciMLBase/VAClc/src/scimlfunctions.jl:2358 [inlined]
    [8] TimeGradientWrapper
      @ ~/.julia/packages/SciMLBase/VAClc/src/function_wrappers.jl:17 [inlined]
    [9] finite_difference_gradient!(df::ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, f::SciMLBase.TimeGradientWrapper{true, ODEFunction{true, SciMLBase.AutoSpecialize, typeof(RoME.imuKinematic!), UniformScaling{Bool}, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, typeof(SciMLBase.DEFAULT_OBSERVED), Nothing, Nothing, Nothing, Nothing}, ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Base.RefValue{@NamedTuple{gyro::Interpolations.Extrapolation{SVector{3, Float64}, 1, Interpolations.GriddedInterpolation{SVector{3, Float64}, 1, Vector{SVector{3, Float64}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Tuple{Vector{Float64}}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Interpolations.Throw{Nothing}}, accel::Interpolations.Extrapolation{SVector{3, Float64}, 1, Interpolations.GriddedInterpolation{SVector{3, Float64}, 1, Vector{SVector{3, Float64}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Tuple{Vector{Float64}}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Interpolations.Throw{Nothing}}}}}, x::Float64, cache::FiniteDiff.GradientCache{Nothing, ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Float64, Val{:forward}(), Float64, Val{true}()}; relstep::Float64, absstep::Float64, dir::Int64)
      @ FiniteDiff ~/.julia/packages/FiniteDiff/uMUWB/src/gradients.jl:433

      [10] finite_difference_gradient!
      @ ~/.julia/packages/FiniteDiff/uMUWB/src/gradients.jl:413 [inlined]

   [34] #solve#51
      @ ~/.julia/packages/DiffEqBase/HW4ge/src/solve.jl:1036 [inlined]
   [35] solve
      @ ~/.julia/packages/DiffEqBase/HW4ge/src/solve.jl:1026 [inlined]
   [36] _solveFactorODE!(::ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, ::ODEProblem{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Tuple{Float64, Float64}, true, Base.RefValue{@NamedTuple{gyro::Interpolations.Extrapolation{SVector{3, Float64}, 1, Interpolations.GriddedInterpolation{SVector{3, Float64}, 1, Vector{SVector{3, Float64}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Tuple{Vector{Float64}}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Interpolations.Throw{Nothing}}, accel::Interpolations.Extrapolation{SVector{3, Float64}, 1, Interpolations.GriddedInterpolation{SVector{3, Float64}, 1, Vector{SVector{3, Float64}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Tuple{Vector{Float64}}}, Interpolations.Gridded{Interpolations.Linear{Interpolations.Throw{Interpolations.OnGrid}}}, Interpolations.Throw{Nothing}}}}, ODEFunction{true, SciMLBase.AutoSpecialize, typeof(RoME.imuKinematic!), UniformScaling{Bool}, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, Nothing, typeof(SciMLBase.DEFAULT_OBSERVED), Nothing, Nothing, Nothing, Nothing}, @Kwargs{dt::Float64}, SciMLBase.StandardODEProblem}, ::ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}})
      @ IncrInfrDiffEqFactorExt ~/.julia/dev/IncrementalInference/ext/IncrInfrDiffEqFactorExt.jl:138
   [37] sampleFactor(cf::IncrementalInference.CalcFactorNormSq{  DERelative{
      RotVelPos
      ODEProblem
    }
  , Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}, Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}, Nothing, Tuple{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}, DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}, Vector{Tuple{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}}}, Nothing}, N::Int64)
      @ IncrInfrDiffEqFactorExt ~/.julia/dev/IncrementalInference/ext/IncrInfrDiffEqFactorExt.jl:308
   [38] #sampleFactor#296
      @ ~/.julia/dev/IncrementalInference/src/services/SolverUtilities.jl:75 [inlined]
   [39] sampleFactor
      @ ~/.julia/dev/IncrementalInference/src/services/SolverUtilities.jl:68 [inlined]
   [40] sampleFactor!(ccwl::CommonConvWrapper{  DERelative{
      RotVelPos
      ODEProblem
    }
  , Tuple{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}, DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, Base.RefValue{Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}, Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, Nothing, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}, IncrementalInference.HypoRecipeCompute{Nothing, Vector{Int64}}, Tuple{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}}, Nothing}, N::Int64; _allowThreads::Bool)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/SolverUtilities.jl:63
   [41] sampleFactor!
      @ ~/.julia/dev/IncrementalInference/src/services/SolverUtilities.jl:50 [inlined]
   [42] updateMeasurement!(ccwl::CommonConvWrapper{  DERelative{
      RotVelPos
      ODEProblem
    }
  , Tuple{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}, DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, Base.RefValue{Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}, Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, Nothing, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}, IncrementalInference.HypoRecipeCompute{Nothing, Vector{Int64}}, Tuple{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}}, Nothing}, N::Int64; measurement::Vector{Tuple}, needFreshMeasurements::Bool, _allowThreads::Bool)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/CalcFactor.jl:500
   [43] updateMeasurement!
      @ ~/.julia/dev/IncrementalInference/src/services/CalcFactor.jl:488 [inlined]
   [44] _beforeSolveCCW!(F_::Type{  DERelative{
      RotVelPos
      ODEProblem
    }
  }, ccwl::CommonConvWrapper{  DERelative{
      RotVelPos
      ODEProblem
    }
  , Tuple{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}, DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, Base.RefValue{Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}, Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, Nothing, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}, IncrementalInference.HypoRecipeCompute{Nothing, Vector{Int64}}, Tuple{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}}, Nothing}, variables::Vector{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, sfidx::Int64, N::Int64; measurement::Vector{Tuple}, needFreshMeasurements::Bool, solveKey::Symbol)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/CalcFactor.jl:578
   [45] _beforeSolveCCW!
      @ ~/.julia/dev/IncrementalInference/src/services/CalcFactor.jl:519 [inlined]
   [46] #_beforeSolveCCW!#258
      @ ~/.julia/dev/IncrementalInference/src/services/CalcFactor.jl:629 [inlined]
   [47] _beforeSolveCCW!
      @ ~/.julia/dev/IncrementalInference/src/services/CalcFactor.jl:620 [inlined]
   [48] evalPotentialSpecific(variables::Vector{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, ccwl::CommonConvWrapper{  DERelative{
      RotVelPos
      ODEProblem
    }
  , Tuple{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}, DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, Base.RefValue{Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}, Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, Nothing, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}, IncrementalInference.HypoRecipeCompute{Nothing, Vector{Int64}}, Tuple{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}}, Nothing}, solvefor::Symbol, T_::Type{  DERelative{
      RotVelPos
      ODEProblem
    }
  }, measurement::Vector{Tuple}; needFreshMeasurements::Bool, solveKey::Symbol, sfidx::Int64, N::Int64, spreadNH::Float64, inflateCycles::Int64, nullSurplus::Int64, dbg::Bool, skipSolve::Bool, _slack::Nothing)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:345
   [49] #evalPotentialSpecific#376
      @ ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:563 [inlined]
   [50] evalPotentialSpecific
      @ ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:555 [inlined]
   [51] evalFactor(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, fct::DFGFactor{CommonConvWrapper{  DERelative{
      RotVelPos
      ODEProblem
    }
  , Tuple{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}, DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, Base.RefValue{Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}, Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, Nothing, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}, IncrementalInference.HypoRecipeCompute{Nothing, Vector{Int64}}, Tuple{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}}, Nothing}, 2}, solvefor::Symbol, measurement::Vector{Tuple}; needFreshMeasurements::Bool, solveKey::Symbol, variables::Vector{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, N::Int64, inflateCycles::Int64, nullSurplus::Int64, dbg::Bool, skipSolve::Bool, _slack::Nothing)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/EvalFactor.jl:587
   [52] approxConvBelief(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, fc::DFGFactor{CommonConvWrapper{  DERelative{
      RotVelPos
      ODEProblem
    }
  , Tuple{DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}, DFGVariable{RotVelPos, ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}, 9}}, Base.RefValue{Tuple{Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}, Vector{ArrayPartition{Float64, Tuple{SMatrix{3, 3, Float64, 9}, SVector{3, Float64}, SVector{3, Float64}}}}}}, Nothing, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}, IncrementalInference.HypoRecipeCompute{Nothing, Vector{Int64}}, Tuple{ArrayPartition{Float64, Tuple{MMatrix{3, 3, Float64, 9}, MVector{3, Float64}, MVector{3, Float64}}}, Manifolds.ProductGroup{ℝ, Tuple{SpecialOrthogonal{ManifoldsBase.TypeParameter{Tuple{3}}}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}, TranslationGroup{ManifoldsBase.TypeParameter{Tuple{3}}, ℝ}}, Manifolds.LeftInvariantRepresentation}}, Nothing}, 2}, target::Symbol, measurement::Vector{Tuple}; solveKey::Symbol, N::Int64, nullSurplus::Int64, skipSolve::Bool)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/ApproxConv.jl:20
   [53] approxConvBelief(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, from::Symbol, target::Symbol, measurement::Vector{Tuple}; solveKey::Symbol, N::Int64, tfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, setPPEmethod::Nothing, setPPE::Bool, path::Vector{Symbol}, skipSolve::Bool, nullSurplus::Int64)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/ApproxConv.jl:131
   [54] approxConvBelief
      @ ~/.julia/dev/IncrementalInference/src/services/ApproxConv.jl:75 [inlined]
   [55] approxConvBelief(dfg::GraphsDFG{SolverParams, DFGVariable, DFGFactor}, from::Symbol, target::Symbol)
      @ IncrementalInference ~/.julia/dev/IncrementalInference/src/services/ApproxConv.jl:75
   [56] macro expansion
      @ ~/.julia/dev/RoME/test/inertial/testInertialDynamic.jl:74 [inlined]
   [57] macro expansion
      @ ~/.julia/juliaup/julia-1.11.1+0.x64.linux.gnu/share/julia/stdlib/v1.11/Test/src/Test.jl:1700 [inlined]
   [58] top-level scope
      @ ~/.julia/dev/RoME/test/inertial/testInertialDynamic.jl:17
   [59] include(fname::String)
      @ Main ./sysimg.jl:38
   [60] top-level scope
      @ ~/.julia/dev/RoME/test/runtests.jl:89
   [61] include(fname::String)
      @ Main ./sysimg.jl:38
   [62] top-level scope
      @ none:6
   [63] eval
      @ ./boot.jl:430 [inlined]
   [64] exec_options(opts::Base.JLOptions)
      @ Base ./client.jl:296
Test Summary:               | Pass  Error  Total   Time
test InertialDynamic factor |    2      1      3  59.1s
ERROR: LoadError: Some tests did not pass: 2 passed, 0 failed, 1 errored, 0 broken.
in expression starting at /home/dehann/.julia/dev/RoME/test/inertial/testInertialDynamic.jl:10
in expression starting at /home/dehann/.julia/dev/RoME/test/runtests.jl:87
ERROR: Package RoME errored during testing

(@v1.11) pkg>

<\details>

@dehann
Copy link
Member Author

dehann commented Nov 29, 2024 •
edited
Loading

Okay, saveDFG did not work on testInertialDynamic's graph but I was able to capture at least the variable values from two contrasting graphs (good vs bad).

inerdyn_N30_vars.zip

Updates,

  • the bad graph's factor's CCW.measurement only has #undef elements. So inline with stacktrace, the ODE solve is failing before any compute is done in IIF/RoME land. That would indicate a plumbing bug
  • Able to repeat the error on the fourth sample of bad graph case, sampleFactor(fg,flbl,4), but repeatably works for the first 3 samples
  • Here is a "bad" value point for var X0 that was found and assigned to different points in vnd.val vector and resulted in the error. Converse, when a "good" value is overwritten to the same place in the vnd.val then the error goes away:
    • ([0.9994576714289023 0.022733376498928745 0.023823446746312264; -0.0246874872375102 0.9960557066432854 0.085226505485343; -0.021791993846690425 -0.08576842575390656 0.9960767471173506], [10.039738701812453, 0.027911152977118225, 0.026309818699960436], [-0.06316840027162819, 0.025351056009170495, -0.0284352361071324])
      • Furthermore, problem seems to occur using just this rotation. TBD if its only rotation alone, or in combo with other dims.

@dehann
Copy link
Member Author

dehann commented Nov 29, 2024

now starting to think the ODE solution of INS is wondering away from proper rotation matrix and this is inducing errors during propagation. Not sure how that is manifesting into the extrapolation issue yet, but suspect the solution is somehow wandering off the "INS manifold"

@dehann
Copy link
Member Author

dehann commented Dec 1, 2024

deferring since there seems to be a 40% success rate with the test and the issue seems to be something more intricate.

@dehann dehann marked this pull request as ready for review December 1, 2024 11:16
@dehann dehann merged commit 75b1520 into master Dec 1, 2024
2 of 3 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@Affie Affie Affie left review comments

Assignees

@dehann dehann

Labels
bug compat deferred
Projects
None yet
Milestone
v0.24.6
Development

Successfully merging this pull request may close these issues.
2 participants
@dehann @Affie