fixes, debugging, wip on derel test

Author: dehann

ext/IncrInfrDiffEqFactorExt.jl

@@ -8,8 +8,8 @@ import DifferentialEquations: solve
 using Dates
 
 using IncrementalInference
-import IncrementalInference: getSample, getManifold, DERelative
-import IncrementalInference: sampleFactor
+import IncrementalInference: DERelative, _solveFactorODE!
+import IncrementalInference: getSample, sampleFactor, getManifold
 
 using DocStringExtensions
 
@@ -174,12 +174,12 @@ function (cf::CalcFactor{<:DERelative})(measurement, X...)
     # need to recalculate new ODE (forward) for change in parameters (solving for 3rd or higher variable)
     solveforIdx = 2
     # use forward solve for all solvefor not in [1;2]
-    u0pts = getBelief(cf.fullvariables[1]) |> getPoints
+    # u0pts = getBelief(cf.fullvariables[1]) |> getPoints
     # update parameters for additional variables
     _solveFactorODE!(
       meas1,
       oderel.forwardProblem,
-      u0pts[cf._sampleIdx],
+      X[1], # u0pts[cf._sampleIdx],
       _maketuplebeyond2args(X...)...,
     )
   end
@@ -192,13 +192,52 @@ function (cf::CalcFactor{<:DERelative})(measurement, X...)
   #FIXME 
   res = zeros(size(X[2], 1))
   for i = 1:size(X[2], 1)
-    # diffop( test, reference )   <===>   ΔX = test \ reference
+    # diffop( reference?, test? )   <===>   ΔX = test \ reference
     res[i] = diffOp[i](X[solveforIdx][i], meas1[i])
   end
   return res
 end
 
 
+# # FIXME see #1025, `multihypo=` will not work properly yet
+# 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]))),
+#     )
+#     cf._legacyParams[2]
+#   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]))),
+#     )
+#     cf._legacyParams[1]
+#   end
+
+#   i = cf._sampleIdx
+#   # solve likely elements
+#   # TODO, does this respect hyporecipe ???
+#   idxArr = (k -> cf._legacyParams[k][i]).(1:length(cf._legacyParams))
+#   _solveFactorODE!(meas, prob, u0pts[i], _maketuplebeyond2args(idxArr...)...)
+#   # _solveFactorODE!(meas, prob, u0pts, i, _maketuplebeyond2args(cf._legacyParams...)...)
+
+#   return meas, diffOp
+# end
+
+
 
 
 ## =========================================================================
@@ -221,15 +260,17 @@ function IncrementalInference.sampleFactor(cf::CalcFactor{<:DERelative}, N::Int
     addOp, diffOp, _, _ = AMP.buildHybridManifoldCallbacks(
       convert(Tuple, getManifold(getVariableType(cf.fullvariables[1]))),
     )
-    getBelief(cf.fullvariables[2]) |> getPoints
+    # getBelief(cf.fullvariables[2]) |> getPoints
+    cf._legacyParams[2]
   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
+    # getBelief(cf.fullvariables[1]) |> getPoints
+    cf._legacyParams[1]
   end
 
   # solve likely elements

ext/WeakDepsPrototypes.jl

@@ -3,6 +3,9 @@
 function _ccolamd! end
 function _ccolamd end
 
+# DiffEq
+function _solveFactorODE! end
+
 # Flux.jl
 function MixtureFluxModels end
 

src/services/CalcFactor.jl

@@ -25,7 +25,7 @@ end
 function CalcFactor(
   ccwl::CommonConvWrapper;
   factor = ccwl.usrfnc!,
-  _sampleIdx = 0,
+  _sampleIdx = ccwl.particleidx[],
   _legacyParams = ccwl.varValsAll[],
   _allowThreads = true,
   cache = ccwl.dummyCache,
@@ -399,7 +399,7 @@ function _createCCW(
   # MeasType = Vector{Float64} # FIXME use `usrfnc` to get this information instead
   _cf = CalcFactor(
     usrfnc,
-    0,
+    1,
     _varValsAll,
     false,
     userCache,

src/services/FactorGraph.jl

@@ -206,6 +206,7 @@ function setValKDE!(
 ) where {P}
   # recover variableType information
   setValKDE!(getSolverData(v, solveKey), val, setinit, ipc)
+  # TODO setPPE!
   return nothing
 end
 function setValKDE!(
@@ -246,7 +247,7 @@ end
 
 function setValKDE!(
   vnd::VariableNodeData,
-  mkd::ManifoldKernelDensity{M, B, Nothing},
+  mkd::ManifoldKernelDensity{M, B, Nothing}, # TBD dispatch without partial?
   setinit::Bool = true,
   ipc::AbstractVector{<:Real} = [0.0;],
 ) where {M, B}

src/services/GraphProductOperations.jl

@@ -29,7 +29,7 @@ function propagateBelief(
 
   # get proposal beliefs
   destlbl = getLabel(destvar)
-  ipc = proposalbeliefs!(dfg, destlbl, factors, dens; solveKey = solveKey, N = N, dbg = dbg)
+  ipc = proposalbeliefs!(dfg, destlbl, factors, dens; solveKey, N, dbg)
 
   # @show dens[1].manifold
 
@@ -52,7 +52,7 @@ function propagateBelief(
     M;
     Niter = 1,
     oldPoints = oldpts,
-    N = N,
+    N,
     u0 = getPointDefault(varType),
   )
 

src/services/SolveTree.jl

@@ -70,7 +70,7 @@ function doFMCIteration(
       logger,
     )
 
-    if 0 < length(getPoints(dens))
+    if 0 < Npts(dens)
       setBelief!(vert, dens, true, ipc)
       # setValKDE!(vert, densPts, true, ipc)
       # TODO perhaps more debugging inside `propagateBelief`?

test/testBasicGraphs.jl

@@ -306,15 +306,57 @@ pts_ = getPoints(getBelief(fg, :x4))
 TensorCast.@cast pts[i,j] := pts_[j][i]
 @test 0.2 < Statistics.cov( pts[1,:] ) < 3.2
 
+
+
 @testset "Test localProduct on solveKey" begin
 
 localProduct(fg,:x2)
-
 localProduct(fg,:x2, solveKey=:graphinit)
 
 end
 
+end
+
+
+##
+@testset "consistency check on more factors (origin is a DERelative fail case)" begin
+##
+
+fg = initfg()
+
+addVariable!(fg, :x0, Position{1})
+addFactor!(fg, [:x0], Prior(Normal(1.0, 0.01)))
 
+# force a basic setup
+initAll!(fg)
+@test isapprox( 1, getPPE(fg, :x0).suggested[1]; atol=0.1)
+
+##
+
+addVariable!(fg, :x1, Position{1})
+addFactor!(fg, [:x0;:x1], LinearRelative(Normal(1.0, 0.01)))
+
+addVariable!(fg, :x2, Position{1})
+addFactor!(fg, [:x1;:x2], LinearRelative(Normal(1.0, 0.01)))
+
+addVariable!(fg, :x3, Position{1})
+addFactor!(fg, [:x2;:x3], LinearRelative(Normal(1.0, 0.01)))
+
+##
+
+tree = solveGraph!(fg)
+
+##
+
+@test isapprox( 1, getPPE(fg, :x0).suggested[1]; atol=0.1)
+@test isapprox( 4, getPPE(fg, :x3).suggested[1]; atol=0.3)
+
+## check contents of tree messages
+
+tree[1]
+msg1 = IIF.getMessageBuffer(tree[1])
+
+##
 end