deserialize and multihypo fixes for CCW's FGC

Author: dehann

NEWS.md

@@ -19,6 +19,7 @@ The list below highlights major breaking changes, and please note that significa
 - Deprecating use of `ensureAllInitialized!`, use `initAll!` instead.
 - New helper function `randToPoints(::SamplableBelief, N=1)::Vector{P}` to help with `getSample` for cases with new `ManifoldKernelDensity` beliefs for manifolds containing points of type `P`.
 - Upstream `calcHelix_T` canonical generator utility from RoME.jl.
+- Deserialization of factors with DFG needs new API and change of solverData and CCW type in factor.
 
 # Major changes in v0.24
 

src/DispatchPackedConversions.jl

@@ -63,14 +63,17 @@ end
 After deserializing a factor using decodePackedType, use this to
 completely rebuild the factor's CCW and user data.
 """
-function rebuildFactorMetadata!(dfg::AbstractDFG{SolverParams}, factor::DFGFactor)
+function rebuildFactorMetadata!(dfg::AbstractDFG{SolverParams}, 
+                                factor::DFGFactor,
+                                neighbors = map(vId->getVariable(dfg, vId), getNeighbors(dfg, factor)) )
+  #
   # Set up the neighbor data
-  neighbors = map(vId->getVariable(dfg, vId), getNeighbors(dfg, factor))
-  neighborUserData = map(v->getVariableType(v), neighbors)
 
   # Rebuilding the CCW
   fsd = getSolverData(factor)
-  ccw_new = getDefaultFactorData( dfg, neighbors, getFactorType(factor), 
+  fnd_new = getDefaultFactorData( dfg, 
+                                  neighbors, 
+                                  getFactorType(factor), 
                                   multihypo=fsd.multihypo,
                                   nullhypo=fsd.nullhypo,
                                   # special inflation override 
@@ -79,7 +82,29 @@ function rebuildFactorMetadata!(dfg::AbstractDFG{SolverParams}, factor::DFGFacto
                                   potentialused=fsd.potentialused,
                                   edgeIDs=fsd.edgeIDs,
                                   solveInProgress=fsd.solveInProgress)
-  setSolverData!(factor, ccw_new)
+  #
+  
+  factor_ = if typeof(fnd_new) != typeof(getSolverData(factor))
+    # must change the type of factor solver data FND{CCW{...}}
+    # create a new factor
+    factor__ = DFGFactor(getLabel(factor),
+                        getTimestamp(factor),
+                        factor.nstime,
+                        getTags(factor),
+                        fnd_new,
+                        getSolvable(factor),
+                        Tuple(getVariableOrder(factor)))
+    #
+
+    # replace old factor in dfg with a new one
+    deleteFactor!(dfg, factor)
+    addFactor!(dfg, factor__)
+
+    factor__
+  else
+    setSolverData!(factor, fnd_new)
+    factor
+  end
 
   #... Copying neighbor data into the factor?
   # JT TODO it looks like this is already updated in getDefaultFactorData -> prepgenericconvolution
@@ -88,7 +113,7 @@ function rebuildFactorMetadata!(dfg::AbstractDFG{SolverParams}, factor::DFGFacto
   #   ccw_new.fnc.cpt[i].factormetadata.variableuserdata = deepcopy(neighborUserData)
   # end
 
-  return factor
+  return factor_
 end
 
 

src/FactorGraph.jl

@@ -620,6 +620,21 @@ calcZDim(cf::CalcFactor{<:GenericMarginal}) = 0
 
 calcZDim(cf::CalcFactor{<:ManifoldPrior}) = manifold_dimension(cf.factor.M)
 
+# return a BitVector masking the fractional portion, assuming converted 0's on 100% confident variables 
+_getFractionalVars(varList::Union{<:Tuple, <:AbstractVector}, mh::Nothing) = zeros(length(varList)) .== 1
+_getFractionalVars(varList::Union{<:Tuple, <:AbstractVector}, mh::Categorical) = 0 .< mh.p
+
+function _selectHypoVariables(allVars::Union{<:Tuple, <:AbstractVector}, 
+                              mh::Categorical,
+                              sel::Integer = rand(mh) )
+  #
+  mask = mh.p .≈ 0.0
+  mask[sel] = true
+  (1:length(allVars))[mask]
+end
+
+_selectHypoVariables(allVars::Union{<:Tuple, <:AbstractVector},mh::Nothing,sel::Integer=0 ) = collect(1:length(allVars))
+
 
 function prepgenericconvolution(Xi::Vector{<:DFGVariable},
                                 usrfnc::T;
@@ -647,8 +662,9 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
   # get a measurement sample
   meas_single = sampleFactor(_cf, 1)
 
+  # get the measurement dimension
   zdim = calcZDim(_cf)
-  # zdim = T != GenericMarginal ? size(getSample(usrfnc, 2)[1],1) : 0
+  # some hypo resolution
   certainhypo = multihypo !== nothing ? collect(1:length(multihypo.p))[multihypo.p .== 0.0] : collect(1:length(Xi))
 
   # sort out partialDims here
@@ -664,17 +680,24 @@ function prepgenericconvolution(Xi::Vector{<:DFGVariable},
   gradients = nothing
   # prepare new cached gradient lambdas (attempt)
   try
+    # this try block definitely fails on deserialization, due to empty DFGVariable[] vector here:
+    # https://github.com/JuliaRobotics/IncrementalInference.jl/blob/db7ff84225cc848c325e57b5fb9d0d85cb6c79b8/src/DispatchPackedConversions.jl#L46
+    # also https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/590#issuecomment-891450762
     if (!_blockRecursion) && usrfnc isa AbstractRelative
       # take first value from each measurement-tuple-element
       measurement_ = map(x->x[1], meas_single)
+      # compensate if no info available during deserialization
       # 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);
+      # assume for now fractional-var in multihypo have same varType
+      hypoidxs = _selectHypoVariables(pts_, multihypo)
+      gradients = FactorGradientsCached!(usrfnc, tuple(varTypes[hypoidxs]...), measurement_, tuple(pts_[hypoidxs]...), _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."
+    @warn "Unable to create measurements and gradients for $usrfnc during prep of CCW, falling back on no-partial information assumption.  Enable @debug printing to see the error."
+    @debug(e)
   end
 
   ccw = CommonConvWrapper(
@@ -1185,7 +1208,7 @@ Experimental
 - `inflation`, to better disperse kernels before convolution solve, see IIF #1051.
 """
 function DFG.addFactor!(dfg::AbstractDFG,
-                        Xi::Vector{<:DFGVariable},
+                        Xi::AbstractVector{<:DFGVariable},
                         usrfnc::AbstractFactor;
                         multihypo::Vector{Float64}=Float64[],
                         nullhypo::Float64=0.0,
@@ -1218,16 +1241,23 @@ function DFG.addFactor!(dfg::AbstractDFG,
                         timestamp=timestamp)
   #
 
-  success = DFG.addFactor!(dfg, newFactor)
+  success = addFactor!(dfg, newFactor)
 
   # TODO: change this operation to update a conditioning variable
   graphinit && doautoinit!(dfg, Xi, singles=false)
 
   return newFactor
 end
 
+function _checkFactorAdd(usrfnc, xisyms)
+  if length(xisyms) == 1 && !(usrfnc isa AbstractPrior) && !(usrfnc isa Mixture)
+    @warn("Listing only one variable $xisyms for non-unary factor type $(typeof(usrfnc))")
+  end
+  nothing
+end
+
 function DFG.addFactor!(dfg::AbstractDFG,
-                        xisyms::Vector{Symbol},
+                        xisyms::AbstractVector{Symbol},
                         usrfnc::AbstractFactor;
                         suppressChecks::Bool=false,
                         kw...  )
@@ -1243,12 +1273,12 @@ function DFG.addFactor!(dfg::AbstractDFG,
   # depcrecation
 
   # basic sanity check for unary vs n-ary
-  if !suppressChecks && length(xisyms) == 1 && !(usrfnc isa AbstractPrior) && !(usrfnc isa Mixture)
-    @warn("Listing only one variable $xisyms for non-unary factor type $(typeof(usrfnc))")
+  if !suppressChecks
+    _checkFactorAdd(usrfnc, xisyms)
   end
 
-  variables = getVariable.(dfg, xisyms)
-  # verts = map(vid -> DFG.getVariable(dfg, vid), xisyms)
+  # variables = getVariable.(dfg, xisyms)
+  variables = map(vid -> getVariable(dfg, vid), xisyms)
   addFactor!(dfg, variables, usrfnc; suppressChecks=suppressChecks, kw... ) # multihypo=multihypo, nullhypo=nullhypo, solvable=solvable, tags=tags, graphinit=graphinit, threadmodel=threadmodel, timestamp=timestamp, inflation=inflation )
 end
 

test/testSaveLoadDFG.jl

@@ -2,73 +2,80 @@
 using IncrementalInference
 using Test
 
+##
+
 @testset "saving to and loading from FileDFG" begin
+##
+
+fg = generateCanonicalFG_Kaess()
+addVariable!(fg, :x4, ContinuousScalar)
+addFactor!(fg, [:x2;:x3;:x4], LinearRelative(Normal()), multihypo=[1.0;0.6;0.4])
 
-  fg = generateCanonicalFG_Kaess()
-  addVariable!(fg, :x4, ContinuousScalar)
-  addFactor!(fg, [:x2;:x3;:x4], LinearRelative(Normal()), multihypo=[1.0;0.6;0.4])
+saveFolder = "/tmp/dfg_test"
+saveDFG(fg, saveFolder)
+# VERSION above 1.0.x hack required since Julia 1.0 does not seem to havfunction `splitpath`
 
-  saveFolder = "/tmp/dfg_test"
-  saveDFG(fg, saveFolder)
-  # VERSION above 1.0.x hack required since Julia 1.0 does not seem to havfunction `splitpath`
-  if v"1.1" <= VERSION
-    retDFG = initfg()
-    retDFG = loadDFG!(retDFG, saveFolder)
-    Base.rm(saveFolder*".tar.gz")
+retDFG = initfg()
+retDFG = loadDFG!(retDFG, saveFolder)
+Base.rm(saveFolder*".tar.gz")
 
-    @test symdiff(ls(fg), ls(retDFG)) == []
-    @test symdiff(lsf(fg), lsf(retDFG)) == []
+@test symdiff(ls(fg), ls(retDFG)) == []
+@test symdiff(lsf(fg), lsf(retDFG)) == []
 
-    @show getFactor(fg, :x2x3x4f1).solverData.multihypo
-    @show getFactor(retDFG, :x2x3x4f1).solverData.multihypo
+@show getFactor(fg, :x2x3x4f1).solverData.multihypo
+@show getFactor(retDFG, :x2x3x4f1).solverData.multihypo
 
-    # check for match
-    @test getFactor(fg, :x2x3x4f1).solverData.multihypo - getFactor(retDFG, :x2x3x4f1).solverData.multihypo |> norm < 1e-10
-    @test getFactor(fg, :x2x3x4f1).solverData.certainhypo - getFactor(retDFG, :x2x3x4f1).solverData.certainhypo |> norm < 1e-10
-  end
+# check for match
+@test getFactor(fg, :x2x3x4f1).solverData.multihypo - getFactor(retDFG, :x2x3x4f1).solverData.multihypo |> norm < 1e-10
+@test getFactor(fg, :x2x3x4f1).solverData.certainhypo - getFactor(retDFG, :x2x3x4f1).solverData.certainhypo |> norm < 1e-10
 
+##
 end
 
 
 @testset "saving to and loading from FileDFG with nullhypo, eliminated, solveInProgress" begin
-  fg = generateCanonicalFG_Kaess()
-  addVariable!(fg, :x4, ContinuousScalar)
-  addFactor!(fg, [:x2;:x3;:x4], LinearRelative(Normal()), multihypo=[1.0;0.6;0.4])
-  addFactor!(fg, [:x1;], Prior(Normal(10,1)), nullhypo=0.5)
+##
+
+fg = generateCanonicalFG_Kaess()
+addVariable!(fg, :x4, ContinuousScalar)
+addFactor!(fg, [:x2;:x3;:x4], LinearRelative(Normal()), multihypo=[1.0;0.6;0.4])
+addFactor!(fg, [:x1;], Prior(Normal(10,1)), nullhypo=0.5)
+
+solveTree!(fg)
 
-  solveTree!(fg)
+#manually change a few fields to test if they are preserved
+fa = getFactor(fg, :x2x3x4f1)
+fa.solverData.eliminated = true
+fa.solverData.solveInProgress = 1
+fa.solverData.nullhypo = 0.5
 
-  #manually change a few fields to test if they are preserved
-  fa = getFactor(fg, :x2x3x4f1)
-  fa.solverData.eliminated = true
-  fa.solverData.solveInProgress = 1
-  fa.solverData.nullhypo = 0.5
 
+saveFolder = "/tmp/dfg_test"
+saveDFG(fg, saveFolder)
 
-  saveFolder = "/tmp/dfg_test"
-  saveDFG(fg, saveFolder)
+retDFG = initfg()
+loadDFG!(retDFG, saveFolder)
+Base.rm(saveFolder*".tar.gz")
 
-  retDFG = initfg()
-  loadDFG!(retDFG, saveFolder)
-  Base.rm(saveFolder*".tar.gz")
+@test issetequal(ls(fg), ls(retDFG))
+@test issetequal(lsf(fg), lsf(retDFG))
 
-  @test issetequal(ls(fg), ls(retDFG))
-  @test issetequal(lsf(fg), lsf(retDFG))
+@show getFactor(fg, :x2x3x4f1).solverData.multihypo
+@show getFactor(retDFG, :x2x3x4f1).solverData.multihypo
 
-  @show getFactor(fg, :x2x3x4f1).solverData.multihypo
-  @show getFactor(retDFG, :x2x3x4f1).solverData.multihypo
+# check for match
+@test isapprox(getFactor(fg, :x2x3x4f1).solverData.multihypo, getFactor(retDFG, :x2x3x4f1).solverData.multihypo)
+@test isapprox(getFactor(fg, :x2x3x4f1).solverData.certainhypo, getFactor(retDFG, :x2x3x4f1).solverData.certainhypo)
 
-  # check for match
-  @test isapprox(getFactor(fg, :x2x3x4f1).solverData.multihypo, getFactor(retDFG, :x2x3x4f1).solverData.multihypo)
-  @test isapprox(getFactor(fg, :x2x3x4f1).solverData.certainhypo, getFactor(retDFG, :x2x3x4f1).solverData.certainhypo)
 
+fb = getFactor(retDFG, :x2x3x4f1)
+@test fa == fb
 
-  fb = getFactor(retDFG, :x2x3x4f1)
-  @test fa == fb
+fa = getFactor(fg, :x1f2)
+fb = getFactor(retDFG, :x1f2)
 
-  fa = getFactor(fg, :x1f2)
-  fb = getFactor(retDFG, :x1f2)
+@test fa == fb
 
-  @test fa == fb
+##
 end