Merge pull request #1331 from JuliaRobotics/21Q3/enh/relpartl

towards partial IPC on relative via gradients

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
 

Project.toml

@@ -47,7 +47,7 @@ ApproxManifoldProducts = "0.4.11"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
-DistributedFactorGraphs = "0.15"
+DistributedFactorGraphs = "0.15.1"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8"
 FileIO = "1"

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/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

@@ -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
 
@@ -620,34 +620,51 @@ 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;
                                 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)
 
+  # 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
@@ -662,19 +679,32 @@ 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
+    # 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.
+      # 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.  Enable @debug printing to see the error."
+    @debug(e)
+  end
 
   ccw = CommonConvWrapper(
           usrfnc,
           PointType[],
           zdim,
-          varParams,
+          varParamsAll,
           fmd,
           specialzDim = hasfield(T, :zDim),
           partial = ispartl,
@@ -707,15 +737,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)
@@ -1177,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,
@@ -1188,7 +1219,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 +1231,8 @@ function DFG.addFactor!(dfg::AbstractDFG,
                                     multihypo=multihypo, 
                                     nullhypo=nullhypo, 
                                     threadmodel=threadmodel,
-                                    inflation=inflation)
+                                    inflation=inflation,
+                                    _blockRecursion=_blockRecursion)
   newFactor = DFGFactor(Symbol(namestring),
                         varOrderLabels,
                         solverData;
@@ -1208,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...  )
@@ -1233,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
 

src/Factors/GenericFunctions.jl

@@ -71,7 +71,7 @@ export ManifoldFactor
 # For now, `Z` is on the tangent space in coordinates at the point used in the factor.
 # For groups just the lie algebra
 # As transition it will be easier this way, we can reevaluate
-struct ManifoldFactor{M <: AbstractManifold, T <: SamplableBelief} <: AbstractManifoldMinimize#AbstractFactor
+struct ManifoldFactor{M <: AbstractManifold, T <: SamplableBelief} <: AbstractManifoldMinimize #AbstractFactor
     M::M
     Z::T
 end

src/NumericalCalculations.jl

@@ -199,6 +199,7 @@ function _buildCalcFactorLambdaSample(ccwl::CommonConvWrapper,
     # slack is used to shift the residual away from the natural "zero" tension position of a factor, 
     # this is useful when calculating factor gradients at a variety of param locations resulting in "non-zero slack" of the residual.
     # see `IIF.calcFactorResidualTemporary`
+    # NOTE this minus operation assumes _slack is either coordinate or tangent vector element (not a manifold or group element)
     () -> cf( (_getindextuple(measurement_, smpid))..., (getindex.(varParams, smpid))... ) .- _slack
   end
 

src/SolverUtilities.jl

@@ -165,20 +165,21 @@ 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
   len = length(varTypes)
-  vars = [Symbol(destPrefix, s_) for s_ in (currNumber .+ (1:len))]
+  vars = Symbol[Symbol(destPrefix, s_) for s_ in (currNumber .+ (1:len))]
   for (s_, vTyp) in enumerate(varTypes)
     # add the necessary variables
     exists(dfg, vars[s_]) ? nothing : addVariable!(dfg, vars[s_],  vTyp)
     # set the numerical values if available
     ((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