towards partial IPC on relative

Author: dehann

src/ApproxConv.jl

@@ -399,9 +399,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
   mani = getManifold(getVariableType(Xi[sfidx]))
 
   # perform the numeric solutions on the indicated elements
-  # error("ccwl.xDim=$(ccwl.xDim)")
   # FIXME consider repeat solve as workaround for inflation off-zero 
-  # @info "EVALSPEC END" ccwl.varidx string(ccwl.params) string(allelements)
   computeAcrossHypothesis!( ccwl, allelements, activehypo, certainidx, 
                             sfidx, maxlen, mani, spreadNH=spreadNH, 
                             inflateCycles=inflateCycles, skipSolve=skipSolve,

src/CalcFactor.jl

@@ -114,11 +114,12 @@ function calcFactorResidualTemporary( fct::AbstractRelative,
                                       measurement::Tuple,
                                       varTypes::Tuple,
                                       pts::Tuple;
-                                      tfg::AbstractDFG = initfg() )
+                                      tfg::AbstractDFG = initfg(),
+                                      _blockRecursion::Bool=false )
   #
 
   # build a new temporary graph
-  _, _dfgfct = _buildGraphByFactorAndTypes!(fct, varTypes, pts, dfg=tfg)
+  _, _dfgfct = _buildGraphByFactorAndTypes!(fct, varTypes, pts, dfg=tfg, _blockRecursion=_blockRecursion)
 
   # get a fresh measurement if needed
   _measurement = if length(measurement) != 0

src/Deprecated.jl

@@ -25,6 +25,8 @@ end
 ## Deprecate code below before v0.27
 ##==============================================================================
 
+@deprecate calcPerturbationFromVariable( fgc::FactorGradientsCached!, fromVar::Int, infoPerCoord::AbstractVector;tol::Real=0.02*fgc._h ) calcPerturbationFromVariable(fgc, [fromVar => infoPerCoord;], tol=tol )
+
 @deprecate findRelatedFromPotential(w...;kw...) (calcProposalBelief(w...;kw...),)
 
 # function generateNullhypoEntropy( val::AbstractMatrix{<:Real},

src/FactorGraph.jl

@@ -605,7 +605,7 @@ function calcZDim(cf::CalcFactor{T}) where {T <: AbstractFactor}
       M = getManifold(cf.factor)
       return manifold_dimension(M)
     catch
-      @warn "no method getManifold(::$T), calcZDim will attempt legacy length(sample) method instead"
+      @warn "no method getManifold(::$(string(T))), calcZDim will attempt legacy length(sample) method instead"
     end
   end
 
@@ -626,22 +626,23 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
                                 multihypo::Union{Nothing, Distributions.Categorical}=nothing,
                                 nullhypo::Real=0.0,
                                 threadmodel=MultiThreaded,
-                                inflation::Real=0.0  ) where {T <: FunctorInferenceType}
+                                inflation::Real=0.0,
+                                _blockRecursion::Bool=false  ) where {T <: AbstractFactor}
   #
   pttypes = getVariableType.(Xi) .|> getPointType
   PointType = 0 < length(pttypes) ? pttypes[1] : Vector{Float64}
   # FIXME stop using Any, see #1321
-  varParams = Vector{Vector{Any}}()
-  maxlen, sfidx, mani = prepareparamsarray!(varParams, Xi, nothing, 0) # Nothing for init.
+  varParamsAll = Vector{Vector{Any}}()
+  maxlen, sfidx, mani = prepareparamsarray!(varParamsAll, Xi, nothing, 0) # Nothing for init.
 
   # standard factor metadata
   sflbl = 0==length(Xi) ? :null : getLabel(Xi[end])
-  fmd = FactorMetadata(Xi, getLabel.(Xi), varParams, sflbl, nothing)
+  fmd = FactorMetadata(Xi, getLabel.(Xi), varParamsAll, sflbl, nothing)
 
   # create a temporary CalcFactor object for extracting the first sample
   # TODO, deprecate this:  guess measurement points type
   # MeasType = Vector{Float64} # FIXME use `usrfnc` to get this information instead
-  _cf = CalcFactor( usrfnc, fmd, 0, 1, nothing, varParams) # (Vector{MeasType}(),)
+  _cf = CalcFactor( usrfnc, fmd, 0, 1, nothing, varParamsAll) # (Vector{MeasType}(),)
 
   # get a measurement sample
   meas_single = sampleFactor(_cf, 1)
@@ -662,19 +663,25 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
   varTypes::Vector{DataType} = typeof.(getVariableType.(Xi))
   gradients = nothing
   # prepare new cached gradient lambdas (attempt)
-  # try
-  #   measurement = tuple(((x->x[1]).(meas_single))...)
-  #   pts = tuple(((x->x[1]).(varParams))...)
-  #   gradients = FactorGradientsCached!(usrfnc, varTypes, measurement, pts);
-  # catch e
-  #   @warn "Unable to create measurements and gradients for $usrfnc during prep of CCW, falling back on no-partial information assumption."
-  # end
+  try
+    if (!_blockRecursion) && usrfnc isa AbstractRelative
+      # take first value from each measurement-tuple-element
+      measurement_ = map(x->x[1], meas_single)
+      # take the first value from each variable param
+      pts_ = map(x->x[1], varParamsAll)
+      # FIXME, only using first meas and params values at this time...
+      # NOTE, must block recurions here, since FGC uses this function to calculate numerical gradients on a temp fg.
+      gradients = FactorGradientsCached!(usrfnc, tuple(varTypes...), measurement_, tuple(pts_...), _blockRecursion=true);
+    end
+  catch e
+    @warn "Unable to create measurements and gradients for $usrfnc during prep of CCW, falling back on no-partial information assumption."
+  end
 
   ccw = CommonConvWrapper(
           usrfnc,
           PointType[],
           zdim,
-          varParams,
+          varParamsAll,
           fmd,
           specialzDim = hasfield(T, :zDim),
           partial = ispartl,
@@ -707,15 +714,16 @@ function getDefaultFactorData(dfg::AbstractDFG,
                               potentialused::Bool = false,
                               edgeIDs = Int[],
                               solveInProgress = 0,
-                              inflation::Real=getSolverParams(dfg).inflation ) where T <: FunctorInferenceType
+                              inflation::Real=getSolverParams(dfg).inflation,
+                              _blockRecursion::Bool=false ) where T <: FunctorInferenceType
   #
 
   # prepare multihypo particulars
   # storeMH::Vector{Float64} = multihypo == nothing ? Float64[] : [multihypo...]
   mhcat, nh = parseusermultihypo(multihypo, nullhypo)
 
   # allocate temporary state for convolutional operations (not stored)
-  ccw = prepgenericconvolution(Xi, usrfnc, multihypo=mhcat, nullhypo=nh, threadmodel=threadmodel, inflation=inflation)
+  ccw = prepgenericconvolution(Xi, usrfnc, multihypo=mhcat, nullhypo=nh, threadmodel=threadmodel, inflation=inflation, _blockRecursion=_blockRecursion)
 
   # and the factor data itself
   return FunctionNodeData{typeof(ccw)}(eliminated, potentialused, edgeIDs, ccw, multihypo, ccw.certainhypo, nullhypo, solveInProgress, inflation)
@@ -1188,7 +1196,8 @@ function DFG.addFactor!(dfg::AbstractDFG,
                         threadmodel=SingleThreaded,
                         suppressChecks::Bool=false,
                         inflation::Real=getSolverParams(dfg).inflation,
-                        namestring::Symbol = assembleFactorName(dfg, Xi)  )
+                        namestring::Symbol = assembleFactorName(dfg, Xi),
+                        _blockRecursion::Bool=false  )
   #
   # depcrecation
 
@@ -1199,7 +1208,8 @@ function DFG.addFactor!(dfg::AbstractDFG,
                                     multihypo=multihypo, 
                                     nullhypo=nullhypo, 
                                     threadmodel=threadmodel,
-                                    inflation=inflation)
+                                    inflation=inflation,
+                                    _blockRecursion=_blockRecursion)
   newFactor = DFGFactor(Symbol(namestring),
                         varOrderLabels,
                         solverData;

src/SolverUtilities.jl

@@ -165,7 +165,8 @@ function _buildGraphByFactorAndTypes!(fct::AbstractFactor,
                                       _allVars::AbstractVector{Symbol} = sortDFG(ls(dfg, destPattern)),
                                       currLabel::Symbol = 0 < length(_allVars) ? _allVars[end] : Symbol(destPrefix, 0),
                                       currNumber::Integer = reverse(match(r"\d+", reverse(string(currLabel))).match) |> x->parse(Int,x),
-                                      graphinit::Bool = false  )
+                                      graphinit::Bool = false,
+                                      _blockRecursion::Bool=false  )
   #
 
   # TODO generalize beyond binary
@@ -178,7 +179,7 @@ function _buildGraphByFactorAndTypes!(fct::AbstractFactor,
     ((0 < length(pts)) && (pts[s_] isa Nothing)) ? nothing : initManual!(dfg,  vars[s_], [pts[s_],], solveKey, bw=ones(getDimension(vTyp)))
   end
   # if newFactor then add the factor on vars, else assume only one existing factor between vars
-  _dfgfct = newFactor ? addFactor!(dfg, vars, fct, graphinit=graphinit) : getFactor(dfg, intersect((ls.(dfg, vars))...)[1] )
+  _dfgfct = newFactor ? addFactor!(dfg, vars, fct, graphinit=graphinit, _blockRecursion=_blockRecursion) : getFactor(dfg, intersect((ls.(dfg, vars))...)[1] )
 
   return dfg, _dfgfct
 end

src/services/FactorGradients.jl

@@ -11,8 +11,9 @@ function _prepFactorGradientLambdas(fct::Union{<:AbstractRelativeMinimize,<:Abst
                                     varTypes::Tuple,
                                     pts::Tuple;
                                     tfg::AbstractDFG = initfg(),
+                                    _blockRecursion::Bool=true,
                                     # gradients relative to coords requires 
-                                    slack_resid = calcFactorResidualTemporary(fct, measurement, varTypes, pts, tfg=tfg),
+                                    slack_resid = calcFactorResidualTemporary(fct, measurement, varTypes, pts, tfg=tfg, _blockRecursion=_blockRecursion),
                                     # numerical diff perturbation size
                                     h::Real=1e-4  ) 
   #
@@ -76,14 +77,15 @@ function FactorGradientsCached!(fct::Union{<:AbstractRelativeMinimize, <:Abstrac
                                 varTypes::Tuple,
                                 meas_single::Tuple, 
                                 pts::Tuple; 
-                                h::Real=1e-4  )
+                                h::Real=1e-4,
+                                _blockRecursion::Bool=true  )
   #
   # working memory location for computations
   tfg = initfg()
 
   # permanent location for points and later reference
   # generate the necessary lambdas
-  J__, λ_fncs, λ_sizes, slack_resid = _prepFactorGradientLambdas(fct, meas_single, varTypes, pts; tfg=tfg, h=1e-4);
+  J__, λ_fncs, λ_sizes, slack_resid = _prepFactorGradientLambdas(fct, meas_single, varTypes, pts; tfg=tfg, _blockRecursion=_blockRecursion, h=1e-4);
 
   # get the one factor in tfg
   fctsyms = lsf(tfg)
@@ -106,6 +108,12 @@ end
 getCoordSizes(fgc::FactorGradientsCached!) = fgc._coord_sizes
 
 
+_setFGCSlack!(fgc::FactorGradientsCached!{F}, slack) where F = _setPointsMani!(fgc.slack_residual, slack)
+
+function _setFGCSlack!(fgc::FactorGradientsCached!{F,S}, slack::Number) where {F,S<:Number}
+  fgc.slack_residual = slack
+end
+
 function (fgc::FactorGradientsCached!)(meas_pts...)
   # separate the measurements (forst) from the variable points (rest)
   lenm = length(fgc.measurement)
@@ -123,7 +131,8 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
   varTypes = tuple(getVariableType.(getVariable.(fgc._tfg, getVariableOrder(fgc.dfgfct)))...)
   new_slack = calcFactorResidualTemporary(fct, measurement, varTypes, pts; tfg=fgc._tfg)
   # TODO make sure slack_residual is properly wired up with all the lambda functions as expected
-  _setPointsMani!(fgc.slack_residual, new_slack)
+  _setFGCSlack!(fgc, new_slack)
+  # _setPointsMani!(fgc.slack_residual, new_slack)
 
   # set new points in preparation for new gradient calculation
   for (s,pt) in enumerate(meas_pts[(lenm+1):end])
@@ -174,8 +183,8 @@ end
 """
     $SIGNATURES
 
-Return a tuple of infoPerCoord vectors that result from input `fromVar::Int`'s `infoPerCoord`.
-For example, a binary `LinearRelative` factor has a one to one influence from the input to the one other variable.
+Return a tuple of infoPerCoord vectors that result from input variables as vector of `::Pair`, i.e. `fromVar::Int => infoPerCoord`.
+For example, a binary `LinearRelative` factor has a one-to-one influence from the input to the one other variable.
 
 Notes
 
@@ -197,7 +206,7 @@ fgc = FactorGradientsCached!(fct, varTypes, measurement, pts);
 fgc(measurement..., pts...)
 
 # check the perturbation influence through gradients on factor
-ret = calcPerturbationFromVariable(fgc, 1, [1;1])
+ret = calcPerturbationFromVariable(fgc, [1=>[1;1]])
 
 @assert isapprox(ret[2], [1;1])
 ```
@@ -210,20 +219,23 @@ Related
 [`FactorGradientsCached!`](@ref), [`checkGradientsToleranceMask`](@ref)
 """
 function calcPerturbationFromVariable(fgc::FactorGradientsCached!, 
-                                      fromVar::Int, 
-                                      infoPerCoord::AbstractVector;
+                                      from_var_ipc::AbstractVector{<:Pair};
                                       tol::Real=0.02*fgc._h )
   #
   blkszs = getCoordSizes(fgc)
-  @assert blkszs[fromVar] == length(infoPerCoord) "Expecting incoming length(infoPerCoord) to equal the block size for variable $fromVar, as per factor used to construct the FactorGradientsCached!: $(getFactorType(fgc.dfgfct))"
   # assume projection through pp-factor from first to second variable 
   # ipc values from first variable belief, and zero for second
   ipcAll = zeros(sum(blkszs))
-
-  # nextVar = minimum([fromVar+1; length(blkszs)])
-  curr_b = sum(blkszs[1:(fromVar-1)]) + 1
-  curr_e = sum(blkszs[1:fromVar])
-  ipcAll[curr_b:curr_e] .= infoPerCoord
+  
+  # set any incoming infoPerCoord values
+  for (fromVar, infoPC) in from_var_ipc
+    # check on sizes with print warning
+    (blkszs[fromVar] == length(infoPC)) ? nothing : @warn("Expecting incoming length(infoPerCoord) to equal the block size for variable $fromVar, as per factor used to construct the FactorGradientsCached!: $(getFactorType(fgc.dfgfct))")
+    # get range of interest
+    curr_b = sum(blkszs[1:(fromVar-1)]) + 1
+    curr_e = sum(blkszs[1:fromVar])
+    ipcAll[curr_b:curr_e] .= infoPC
+  end
 
   # clamp gradients below numerical solver resolution
   mask = checkGradientsToleranceMask(fgc, tol=tol)
@@ -251,4 +263,5 @@ function calcPerturbationFromVariable(fgc::FactorGradientsCached!,
 end
 
 
+
 #

test/testpartialconstraint.jl

@@ -2,10 +2,11 @@
 using Test
 using IncrementalInference
 # going to introduce two new constraint mutable structs
-import IncrementalInference: getSample
 using Statistics
 using TensorCast
+using Manifolds
 
+import IncrementalInference: getManifold, getSample
 
 ##
 
@@ -27,6 +28,8 @@ mutable struct DevelopPartialPairwise <: AbstractRelativeMinimize
   partial::Tuple
   DevelopPartialPairwise(x::Distribution) = new(x, (2,))
 end
+getManifold(::IIF.InstanceType{DevelopPartialPairwise}) = TranslationGroup(1)
+
 getSample(cf::CalcFactor{<:DevelopPartialPairwise}, N::Int=1) = ([rand(cf.factor.x, 1)[:] for _ in 1:N], )
 
 function (dp::CalcFactor{<:DevelopPartialPairwise})(meas,
@@ -93,7 +96,7 @@ pts_ = approxConv(fg, getLabel(f2), :x1, N=N)
 end
 
 
-@testset "check that partials are received through convolutions" begin
+@testset "check that partials are received through convolutions of prior" begin
 ##
 
 # check that a partial belief is obtained
@@ -105,13 +108,6 @@ X1_ = approxConvBelief(fg, :x1f2, :x1)
 end
 
 
-@testset "test partial info per coord through relative convolution (conditional)" begin
-
-@test_broken false
-
-end
-
-
 @testset "test solving of factor graph" begin
 ##
 
@@ -128,30 +124,52 @@ end
 # plotKDE(getBelief(fg, :x1),levels=3)
 
 
-
 ## partial relative gradient and graph
 
+v2 = addVariable!(fg,:x2,ContinuousEuclid{2},N=N)
+
 dpp = DevelopPartialPairwise(Normal(10.0, 1.0))
+f3  = addFactor!(fg,[:x1;:x2],dpp)
+
+dp2 = DevelopPartial( Normal(-20.0, 1.0), (1,) )
+f4  = addFactor!(fg,[:x2;], dp2, graphinit=false)
 
-# measurement = ([10.0;], )
-# pts = ([0;0.0], [0;10.0])
-# gradients = FactorGradientsCached!(dpp, (ContinuousEuclid{2}, ContinuousEuclid{2}), measurement, pts);
+doautoinit!(fg, :x2)
 
 ##
 
+@testset "test partial info per coord through relative convolution (conditional)" begin
+##
 
-v2 = addVariable!(fg,:x2,ContinuousEuclid{2},N=N)
+one_meas = ([10.0;], )
+pts = ([0;0.0], [0;10.0])
+gradients = FactorGradientsCached!(dpp, (ContinuousEuclid{2}, ContinuousEuclid{2}), one_meas, pts);
 
+##
 
-f3  = addFactor!(fg,[:x1;:x2],dpp)
+# check that the gradients can be calculated
+J = gradients(one_meas..., pts...)
 
+@test size(J) == (4,4)
+@test norm(J - [0 0 0 0; 0 0 0 1; 0 0 0 0; 0 1 0 0] ) < 1e-4
 
-dp2 = DevelopPartial( Normal(-20.0, 1.0), (1,) )
-f4  = addFactor!(fg,[:x2;], dp2, graphinit=false)
-doautoinit!(fg, :x2)
+## check perturbation logic
+
+prtb = calcPerturbationFromVariable(gradients, [1=>[1;1]])
+
+# self variation is taken as 0 at this time
+@test isapprox( prtb[1], [0;0] )
+# variable 1 influences 2 only through partial dimension 2 (as per DevelopPartialPairwise)
+@test isapprox( prtb[2], [0;1] )
 
-# drawGraph(fg, show=true)
+##  test evaluation through the convolution operation withing a factor graph
 
+# add relative IPC calculation inside evalFactor
+bel = approxConvBelief(fg, getLabel(f3), :x2)
+@test_broken isPartial(bel)
+
+##
+end
 
 ##