Merge pull request #1769 from JuliaRobotics/23Q3/fix/derel

wip fixing DERelative

NEWS.md

@@ -1,4 +1,4 @@
-# Additional NEWS on IncrementalInference.jl Releases
+# NEWS on IncrementalInference.jl Releases
 
 Currently general maintenance and bug fix changes are mostly tracked via Github Integrations.  E.g. see Milestones along with Label filters to quickly find specific issues.
 - https://github.com/JuliaRobotics/IncrementalInference.jl/milestones?state=closed
@@ -13,8 +13,11 @@ The list below highlights breaking changes according to normal semver workflow -
 
 # Changes in v0.34
 
-- Start transition to Manopt.jl via Riemannian Levenberg Marquart.
+- Start transition to Manopt.jl via Riemannian Levenberg-Marquart.
 - Deprecate `AbstractRelativeRoots`.
+- Standardization improvements surrounding weakdeps code extensions. 
+- Code quality improvements along wiht refactoring and reorganizing of file names and locations.
+- Restoring `DERelative` factors, although further fixes necessary beyond anticipated patch release v0.34.1.
 
 # Changes in v0.33
 

Project.toml

@@ -65,7 +65,7 @@ IncrInfrInteractiveUtilsExt = "InteractiveUtils"
 IncrInfrInterpolationsExt = "Interpolations"
 
 [compat]
-ApproxManifoldProducts = "0.7"
+ApproxManifoldProducts = "0.7, 0.8"
 BSON = "0.2, 0.3"
 BlockArrays = "0.16"
 Combinatorics = "1.0"

ext/IncrInfrDiffEqFactorExt.jl

@@ -9,6 +9,7 @@ using Dates
 
 using IncrementalInference
 import IncrementalInference: getSample, getManifold, DERelative
+import IncrementalInference: sampleFactor
 
 using DocStringExtensions
 
@@ -94,12 +95,12 @@ end
 #
 #
 
-# Xtra splat are variable points (X3::Matrix, X4::Matrix,...)
+# n-ary factor: Xtra splat are variable points (X3::Matrix, X4::Matrix,...)
 function _solveFactorODE!(measArr, prob, u0pts, Xtra...)
-  # should more variables be included in calculation
+  # happens when more variables (n-ary) must be included in DE solve
   for (xid, xtra) in enumerate(Xtra)
     # update the data register before ODE solver calls the function
-    prob.p[xid + 1][:] = Xtra[xid][:]
+    prob.p[xid + 1][:] = xtra[:]
   end
 
   # set the initial condition
@@ -111,47 +112,47 @@ function _solveFactorODE!(measArr, prob, u0pts, Xtra...)
   return sol
 end
 
-getSample(cf::CalcFactor{<:DERelative}) = error("getSample(::CalcFactor{<:DERelative}) must still be implemented in new IIF design")
+# # # output for AbstractRelative is tangents (but currently we working in coordinates for integration with DiffEqs)
+# # # FIXME, how to consolidate DERelative with parametric solve which currently only goes through getMeasurementParametric
+# function getSample(cf::CalcFactor{<:DERelative})
+#   #
+#   oder = cf.factor
+
+#   # how many trajectories to propagate?
+#   # @show getLabel(cf.fullvariables[2]), getDimension(cf.fullvariables[2])
+#   meas = zeros(getDimension(cf.fullvariables[2])) 
+
+#   # pick forward or backward direction
+#   # set boundary condition
+#   u0pts = if cf.solvefor == 1
+#     # backward direction
+#     prob = oder.backwardProblem
+#     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
+#       convert(Tuple, getManifold(getVariableType(cf.fullvariables[1]))),
+#     )
+#     # FIXME use ccw.varValsAll containter?
+#     (getBelief(cf.fullvariables[2]) |> getPoints)[cf._sampleIdx]
+#   else
+#     # forward backward
+#     prob = oder.forwardProblem
+#     # buffer manifold operations for use during factor evaluation
+#     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
+#       convert(Tuple, getManifold(getVariableType(cf.fullvariables[2]))),
+#     )
+#     # FIXME use ccw.varValsAll containter?
+#     (getBelief(cf.fullvariables[1]) |> getPoints)[cf._sampleIdx]
+#   end
+
+#   # solve likely elements
+#   # TODO, does this respect hyporecipe ???
+#   # TBD check if cf._legacyParams == ccw.varValsAll???
+#   idxArr = (k -> cf._legacyParams[k][cf._sampleIdx]).(1:length(cf._legacyParams))
+#   _solveFactorODE!(meas, prob, u0pts, _maketuplebeyond2args(idxArr...)...)
+#   # _solveFactorODE!(meas, prob, u0pts, i, _maketuplebeyond2args(cf._legacyParams...)...)
+
+#   return meas, diffOp
+# end
 
-# FIXME see #1025, `multihypo=` will not work properly yet
-function sampleFactor(cf::CalcFactor{<:DERelative}, N::Int = 1)
-  #
-  oder = cf.factor
-
-  # how many trajectories to propagate?
-  # @show getLabel(cf.fullvariables[2]), getDimension(cf.fullvariables[2])
-  meas = [zeros(getDimension(cf.fullvariables[2])) for _ = 1:N]
-
-  # pick forward or backward direction
-  # set boundary condition
-  u0pts = if cf.solvefor == 1
-    # backward direction
-    prob = oder.backwardProblem
-    addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
-      convert(Tuple, getManifold(getVariableType(cf.fullvariables[1]))),
-    )
-    getBelief(cf.fullvariables[2]) |> getPoints
-  else
-    # forward backward
-    prob = oder.forwardProblem
-    # buffer manifold operations for use during factor evaluation
-    addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
-      convert(Tuple, getManifold(getVariableType(cf.fullvariables[2]))),
-    )
-    getBelief(cf.fullvariables[1]) |> getPoints
-  end
-
-  # solve likely elements
-  for i = 1:N
-    # TODO, does this respect hyporecipe ???
-    idxArr = (k -> cf._legacyParams[k][i]).(1:length(cf._legacyParams))
-    _solveFactorODE!(meas[i], prob, u0pts[i], _maketuplebeyond2args(idxArr...)...)
-    # _solveFactorODE!(meas, prob, u0pts, i, _maketuplebeyond2args(cf._legacyParams...)...)
-  end
-
-  return map(x -> (x, diffOp), meas)
-end
-# getDimension(oderel.domain)
 
 # NOTE see #1025, CalcFactor should fix `multihypo=` in `cf.__` fields; OBSOLETE
 function (cf::CalcFactor{<:DERelative})(measurement, X...)
@@ -197,6 +198,56 @@ function (cf::CalcFactor{<:DERelative})(measurement, X...)
   return res
 end
 
+
+
+
+## =========================================================================
+## MAYBE legacy
+
+# FIXME see #1025, `multihypo=` will not work properly yet
+function IncrementalInference.sampleFactor(cf::CalcFactor{<:DERelative}, N::Int = 1)
+  #
+  oder = cf.factor
+
+  # how many trajectories to propagate?
+  # @show getLabel(cf.fullvariables[2]), getDimension(cf.fullvariables[2])
+  meas = [zeros(getDimension(cf.fullvariables[2])) for _ = 1:N]
+
+  # pick forward or backward direction
+  # set boundary condition
+  u0pts = if cf.solvefor == 1
+    # backward direction
+    prob = oder.backwardProblem
+    addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
+      convert(Tuple, getManifold(getVariableType(cf.fullvariables[1]))),
+    )
+    getBelief(cf.fullvariables[2]) |> getPoints
+  else
+    # forward backward
+    prob = oder.forwardProblem
+    # buffer manifold operations for use during factor evaluation
+    addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
+      convert(Tuple, getManifold(getVariableType(cf.fullvariables[2]))),
+    )
+    getBelief(cf.fullvariables[1]) |> getPoints
+  end
+
+  # solve likely elements
+  for i = 1:N
+    # TODO, does this respect hyporecipe ???
+    idxArr = (k -> cf._legacyParams[k][i]).(1:length(cf._legacyParams))
+    _solveFactorODE!(meas[i], prob, u0pts[i], _maketuplebeyond2args(idxArr...)...)
+    # _solveFactorODE!(meas, prob, u0pts, i, _maketuplebeyond2args(cf._legacyParams...)...)
+  end
+
+  return map(x -> (x, diffOp), meas)
+end
+# getDimension(oderel.domain)
+
+
+
+
+
 ## the function
 # ode.problem.f.f
 

src/IncrementalInference.jl

@@ -130,7 +130,8 @@ include("entities/FactorOperationalMemory.jl")
 include("Factors/GenericMarginal.jl")
 # Special belief types for sampling as a distribution
 include("entities/AliasScalarSampling.jl")
-include("entities/OptionalDensities.jl") # used in BeliefTypes.jl::SamplableBeliefs
+include("entities/ExtDensities.jl") # used in BeliefTypes.jl::SamplableBeliefs
+include("entities/ExtFactors.jl")
 include("entities/BeliefTypes.jl")
 
 include("services/HypoRecipe.jl")
@@ -234,7 +235,7 @@ include("services/SolverAPI.jl")
 # Symbolic tree analysis files.
 include("services/AnalysisTools.jl")
 
-# optional densities on weakdeps
+# extension densities on weakdeps
 include("Serialization/entities/SerializingOptionalDensities.jl")
 include("Serialization/services/SerializingOptionalDensities.jl")
 

src/entities/OptionalDensities.jl → src/entities/ExtDensities.jl

@@ -72,36 +72,6 @@ struct LevelSetGridNormal{T <: Real, H <: HeatmapGridDensity}
   heatmap::H
 end
 
-##
-
-"""
-    $TYPEDEF
-
-Build a full ODE solution into a relative factor to condense possible sensor data into a relative transformation,
-but keeping the parameter estimation process fluid.  Assumes first and second variable in order 
-are of same dimension and compatible manifolds, such that ODE runs from Xi to Xi+1 on all
-dimensions.  Internal state vector can be decoupled onto different domain as needed.
-
-Notes
-- Based on DifferentialEquations.jl
-- `getSample` step does the `solve(ODEProblem)` step.
-- `tspan` is taken from variables only once at object construction -- i.e. won't detect changed timestamps.
-- Regular factor evaluation is done as full dimension `AbstractRelativeRoots`, and is basic linear difference.
-
-DevNotes
-- FIXME see 1025, `multihypo=` will not yet work. 
-- FIXME Lots of consolidation and standardization to do, see RoME.jl #244 regarding Manifolds.jl.
-- TODO does not yet handle case where a factor spans across two timezones.
-"""
-struct DERelative{T <: InferenceVariable, P, D} <: AbstractRelativeMinimize
-  domain::Type{T}
-  forwardProblem::P
-  backwardProblem::P
-  """ second element of this data tuple is additional variables that will be passed down as a parameter """
-  data::D
-  specialSampler::Function
-end
-
 
 ##
 

src/entities/ExtFactors.jl

@@ -0,0 +1,29 @@
+
+
+"""
+    $TYPEDEF
+
+Build a full ODE solution into a relative factor to condense possible sensor data into a relative transformation,
+but keeping the parameter estimation process fluid.  Assumes first and second variable in order 
+are of same dimension and compatible manifolds, such that ODE runs from Xi to Xi+1 on all
+dimensions.  Internal state vector can be decoupled onto different domain as needed.
+
+Notes
+- Based on DifferentialEquations.jl
+- `getSample` step does the `solve(ODEProblem)` step.
+- `tspan` is taken from variables only once at object construction -- i.e. won't detect changed timestamps.
+- Regular factor evaluation is done as full dimension `AbstractRelativeRoots`, and is basic linear difference.
+
+DevNotes
+- FIXME see 1025, `multihypo=` will not yet work. 
+- FIXME Lots of consolidation and standardization to do, see RoME.jl #244 regarding Manifolds.jl.
+- TODO does not yet handle case where a factor spans across two timezones.
+"""
+struct DERelative{T <: InferenceVariable, P, D} <: AbstractRelativeMinimize
+  domain::Type{T}
+  forwardProblem::P
+  backwardProblem::P
+  """ second element of this data tuple is additional variables that will be passed down as a parameter """
+  data::D
+  specialSampler::Function
+end

test/runtests.jl

@@ -5,20 +5,21 @@ TEST_GROUP = get(ENV, "IIF_TEST_GROUP", "all")
 # temporarily moved to start (for debugging)
 #...
 if TEST_GROUP in ["all", "tmp_debug_group"]
+include("testSpecialOrthogonalMani.jl")
+include("testDERelative.jl")
 include("testMultiHypo3Door.jl")
 include("priorusetest.jl")
 end
 
 if TEST_GROUP in ["all", "basic_functional_group"]
-# more frequent stochasic failures from numerics
+  # more frequent stochasic failures from numerics
 include("manifolds/manifolddiff.jl")
 include("manifolds/factordiff.jl")
 include("testSpecialEuclidean2Mani.jl")
 include("testEuclidDistance.jl")
 
 # regular testing
 include("testSphereMani.jl")
-include("testSpecialOrthogonalMani.jl")
 include("testBasicManifolds.jl")
 
 # start as basic as possible and build from there
@@ -96,11 +97,6 @@ include("testCircular.jl")
 include("testMixtureLinearConditional.jl")
 include("testFluxModelsDistribution.jl")
 include("testAnalysisTools.jl")
-try
-  include("testDERelative.jl")
-catch
-  @error "[FAILED]  Fix testDERelative.jl, likely just requires implementing DiffEqFactorExt.getSample(::CalcFactor{<:DERelative})."
-end
 
 include("testBasicParametric.jl")
 include("testMixtureParametric.jl")