Merge pull request #1653 from JuliaRobotics/master

release v0.31.1-rc1

.github/workflows/ci.yml

@@ -17,7 +17,6 @@ jobs:
       fail-fast: false
       matrix:
         version:
-          - '1.6'
           - '1.8'
         os:
           - ubuntu-latest
@@ -57,6 +56,7 @@ jobs:
           - x64
         version:
           - '1.8'
+          - '~1.9.0-0'
         group:
           - 'basic_functional_group'
           - 'test_cases_group'

Project.toml

@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.31.0"
+version = "0.31.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

src/ExportAPI.jl

@@ -359,5 +359,6 @@ export setCliqueDrawColor!, getCliqueDrawColor
 export appendSeparatorToClique!
 
 export buildTreeFromOrdering! # TODO make internal and deprecate external use to only `buildTreeReset!``
+export makeSolverData!
 
 #

src/IncrementalInference.jl

@@ -101,6 +101,7 @@ const BeliefArray{T} = Union{<:AbstractMatrix{<:T}, <:Adjoint{<:T, AbstractMatri
 # API Exports
 
 # Package aliases
+# FIXME, remove this and let the user do either import or const definitions
 export KDE, AMP, DFG, FSM, IIF
 
 # TODO temporary for initial version of on-manifold products

src/ODE/DERelative.jl

@@ -172,7 +172,7 @@ function sampleFactor(cf::CalcFactor{<:DERelative}, N::Int = 1)
 end
 # getDimension(oderel.domain)
 
-# NOTE see #1025, CalcFactor should fix `multihypo=` in `cf.metadata` fields
+# NOTE see #1025, CalcFactor should fix `multihypo=` in `cf.__` fields; OBSOLETE
 function (cf::CalcFactor{<:DERelative})(measurement, X...)
   #
   meas1 = measurement[1]

src/ParametricUtils.jl

@@ -141,18 +141,19 @@ function CalcFactorMahalanobis(fg, fct::DFGFactor)
   fac_func = getFactorType(fct)
   varOrder = getVariableOrder(fct)
 
-  _meas, _iΣ = getMeasurementParametric(fac_func)
+  # NOTE, use getMeasurementParametric on DFGFactor{<:CCW} to allow special cases like OAS factors
+  _meas, _iΣ = getMeasurementParametric(fct) # fac_func
   M = getManifold(getFactorType(fct))
   dims = manifold_dimension(M)
   ϵ = getPointIdentity(M)
 
-  if typeof(_meas) <: Tuple
-    _measX = map(m -> hat(M, ϵ, m), _meas)
+  _measX = if typeof(_meas) <: Tuple
     # TODO perhaps better consolidate manifold prior 
+    map(m -> hat(M, ϵ, m), _meas)
   elseif fac_func isa ManifoldPrior
-    _measX = (_meas,)
+    (_meas,)
   else
-    _measX = (convert(typeof(ϵ), get_vector(M, ϵ, _meas, DefaultOrthogonalBasis())),)
+    (convert(typeof(ϵ), get_vector(M, ϵ, _meas, DefaultOrthogonalBasis())),)
   end
 
   meas = fac_func isa AbstractPrior ? map(X -> exp(M, ϵ, X), _measX) : _measX
@@ -366,24 +367,32 @@ end
 function cost_cfp(
   @nospecialize(cfp::CalcFactorMahalanobis),
   @nospecialize(p::AbstractArray),
-  vi::NTuple{1, Int},
-)
-  return cfp(p[vi[1]])
-end
-function cost_cfp(
-  @nospecialize(cfp::CalcFactorMahalanobis),
-  @nospecialize(p::AbstractArray),
-  vi::NTuple{2, Int},
-)
-  return cfp(p[vi[1]], p[vi[2]])
-end
-function cost_cfp(
-  @nospecialize(cfp::CalcFactorMahalanobis),
-  @nospecialize(p::AbstractArray),
-  vi::NTuple{3, Int},
-)
-  return cfp(p[vi[1]], p[vi[2]], p[vi[3]])
-end
+  vi::NTuple{N, Int},
+) where N
+  cfp(map(v->p[v],vi)...)
+end
+# function cost_cfp(
+#   @nospecialize(cfp::CalcFactorMahalanobis),
+#   @nospecialize(p::AbstractArray),
+#   vi::NTuple{1, Int},
+# )
+#   return cfp(p[vi[1]])
+# end
+# function cost_cfp(
+#   @nospecialize(cfp::CalcFactorMahalanobis),
+#   @nospecialize(p::AbstractArray),
+#   vi::NTuple{2, Int},
+# )
+#   return cfp(p[vi[1]], p[vi[2]])
+# end
+# function cost_cfp(
+#   @nospecialize(cfp::CalcFactorMahalanobis),
+#   @nospecialize(p::AbstractArray),
+#   vi::NTuple{3, Int},
+# )
+#   return cfp(p[vi[1]], p[vi[2]], p[vi[3]])
+# end
+
 
 # function (gsc::GraphSolveContainer)(f::Vector{T}, Xc::Vector{T}, ::Val{true}) where T <: Real
 #   #
@@ -542,6 +551,22 @@ function solveGraphParametric(
 
   #optim setup and solve
   alg = algorithm(; algorithmkwargs...)
+  # alg = NewtonTrustRegion(;
+  #   initial_delta = 1.0,
+  #   delta_hat = 100.0,
+  #   eta = 0.1,
+  #   rho_lower = 0.25,
+  #   rho_upper = 0.75
+  # )
+  # alg = LBFGS(; 
+  #   m = 10,
+  #   alphaguess = LineSearches.InitialStatic(),
+  #   linesearch = LineSearches.HagerZhang(),
+  #   P = nothing,
+  #   precondprep = (P, x) -> nothing,
+  #   manifold = Flat(),
+  #   scaleinvH0::Bool = true && (typeof(P) <: Nothing)
+  # )
   tdtotalCost = Optim.TwiceDifferentiable(gsc, initValues; autodiff = autodiff)
 
   result = Optim.optimize(tdtotalCost, initValues, alg, options)
@@ -791,11 +816,19 @@ end
     $SIGNATURES
 Add parametric solver to fg, batch solve using [`solveGraphParametric`](@ref) and update fg.
 """
-function solveGraphParametric!(fg::AbstractDFG; init::Bool = true, kwargs...)
+function solveGraphParametric!(
+  fg::AbstractDFG; 
+  init::Bool = true, 
+  solveKey::Symbol = :parametric, # FIXME, moot since only :parametric used for parametric solves
+  initSolveKey::Symbol = :default, 
+  kwargs...
+)
+  # make sure variables has solverData, see #1637
+  makeSolverData!(fg; solveKey)
   if !(:parametric in fg.solverParams.algorithms)
     addParametricSolver!(fg; init = init)
   elseif init
-    initParametricFrom!(fg)
+    initParametricFrom!(fg, initSolveKey; parkey=solveKey)
   end
 
   vardict, result, varIds, Σ = solveGraphParametric(fg; kwargs...)

src/Serialization/services/DispatchPackedConversions.jl

@@ -72,16 +72,23 @@ end
 ##
 
 """
-  $(SIGNATURES)
+    $(SIGNATURES)
+
 After deserializing a factor using decodePackedType, use this to
 completely rebuild the factor's CCW and user data.
+
+Notes:
+- This function is likely to be used for cache heavy factors, e.g. `ObjectAffordanceSubcloud`.
+
 Dev Notes:
 - TODO: We should only really do this in-memory if we can by without it (review this).
+- TODO: needs testing
 """
 function rebuildFactorMetadata!(
   dfg::AbstractDFG{SolverParams},
   factor::DFGFactor,
-  neighbors = map(vId -> getVariable(dfg, vId), getNeighbors(dfg, factor)),
+  neighbors = map(vId -> getVariable(dfg, vId), getNeighbors(dfg, factor));
+  _blockRecursionGradients::Bool=false
 )
   #
   # Set up the neighbor data
@@ -100,6 +107,7 @@ function rebuildFactorMetadata!(
     potentialused = fsd.potentialused,
     edgeIDs = fsd.edgeIDs,
     solveInProgress = fsd.solveInProgress,
+    _blockRecursion=_blockRecursionGradients
   )
   #
 

src/SolverAPI.jl

@@ -364,7 +364,7 @@ function solveTree!(
     @info "Setting `.multiproc=false` since `Distributed.nprocs() == 1`"
     opt.multiproc = false
   end
-
+  
   if opt.graphinit
     @info "Ensure variables are all initialized (graphinit)"
     if algorithm == :parametric

src/VariableStatistics.jl

@@ -18,7 +18,6 @@ function Statistics.cov(
   return cov(getManifold(vartype), ptsArr; basis, kwargs...)
 end
 
-# To replace calcCovarianceBasic
 function calcStdBasicSpread(vartype::InferenceVariable, ptsArr::Vector{P}) where {P}
   σ = std(vartype, ptsArr)
 

src/entities/FactorOperationalMemory.jl

@@ -9,8 +9,7 @@ Notes
 
 ```julia
 function (cf::CalcFactor{<:LinearRelative})(res::AbstractVector{<:Real}, z, xi, xj)
-  cf.metadata.variablelist
-  cf.metadata.targetvariable
+  cf.variablelist
   cf.cache
   # generic on-manifold residual function 
   return distance(z, distance(xj, xi))
@@ -42,6 +41,7 @@ struct CalcFactor{T <: AbstractFactor, P <: Union{<:Tuple, Nothing, AbstractVect
 
   ## TODO Consolidation WIP with FactorMetadata
   # full list of variables connected to the factor
+  # TODO make sure this list is of the active hypo only
   fullvariables::Vector{<:DFGVariable}
   # which index is being solved for?
   solvefor::Int
@@ -114,7 +114,7 @@ mutable struct CommonConvWrapper{T <: AbstractFactor, NTP <: Tuple, G, MT, CT} <
   dummyCache::CT
 
   #Consolidation from FMD
-  fullvariables::Vector{DFGVariable}
+  fullvariables::Vector{<:DFGVariable}
 
   #Consolidation from CPT
   # the actual particle being solved at this moment

src/services/CalcFactor.jl

@@ -282,7 +282,7 @@ function _prepParamVec(
   solvefor::Union{Nothing, Symbol},
   N::Int = 0;
   solveKey::Symbol = :default,
-) where {P}
+)
   #
   # FIXME refactor to new NamedTuple instead
   varParamsAll = getVal.(Xi; solveKey)

src/services/EvalFactor.jl

@@ -72,7 +72,6 @@ function calcVariableDistanceExpectedFractional(
 )
   #
   if sfidx in certainidx
-    # msst_ = sqrt(calcCovarianceBasic(getManifold(ccwl.vartypes[sfidx]), ccwl.params[sfidx]))
     msst_ = calcStdBasicSpread(ccwl.vartypes[sfidx](), ccwl.params[sfidx])
     return kappa * msst_
   end
@@ -473,7 +472,6 @@ function evalPotentialSpecific(
 
   # view on elements marked for nullhypo
   addEntrNH = view(addEntr, nhmask)
-  # spreadDist = spreadNH*sqrt(calcCovarianceBasic(mani, addEntr))
   spreadDist = spreadNH * calcStdBasicSpread(getVariableType(Xi[sfidx]), addEntr)
   # partials are treated differently
   ipc = if !isPartial(ccwl) #ccwl.partial

src/services/FactorGraph.jl

@@ -362,14 +362,15 @@ function resetVariable!(
   return resetVariable!(getVariable(dfg, sym); solveKey = solveKey)
 end
 
+# return VariableNodeData
 function DefaultNodeDataParametric(
   dodims::Int,
   dims::Int,
   variableType::InferenceVariable;
   initialized::Bool = true,
   dontmargin::Bool = false,
-)::VariableNodeData
-
+  solveKey::Symbol = :parametric
+)
   # this should be the only function allocating memory for the node points
   if false && initialized
     error("not implemented yet")
@@ -408,14 +409,23 @@ function DefaultNodeDataParametric(
   end
 end
 
+"""
+    $SIGNATURES
+
+Makes and sets a parametric `VariableNodeData` object (`.solverData`).
+
+DevNotes
+- TODO assumes parametric solves will always just be under the `solveKey=:parametric`, should be generalized.
+"""
 function setDefaultNodeDataParametric!(
   v::DFGVariable,
   variableType::InferenceVariable;
+  solveKey::Symbol = :parametric,
   kwargs...,
 )
-  vnd = DefaultNodeDataParametric(0, variableType |> getDimension, variableType; kwargs...)
-  setSolverData!(v, vnd, :parametric)
-  return nothing
+  vnd = DefaultNodeDataParametric(0, variableType |> getDimension, variableType; solveKey, kwargs...)
+  setSolverData!(v, vnd, solveKey)
+  nothing
 end
 
 """
@@ -707,11 +717,11 @@ function getDefaultFactorData(
   _blockRecursion::Bool = false,
 ) where {T <: AbstractFactor}
   #
-
+  
   # prepare multihypo particulars
   # storeMH::Vector{Float64} = multihypo == nothing ? Float64[] : [multihypo...]
   mhcat, nh = parseusermultihypo(multihypo, nullhypo)
-
+  
   # allocate temporary state for convolutional operations (not stored)
   userCache = preambleCache(dfg, Xi, usrfnc)
   ccw = _prepCCW(
@@ -724,7 +734,7 @@ function getDefaultFactorData(
     _blockRecursion,
     userCache,
   )
-
+  
   # and the factor data itself
   return FunctionNodeData{typeof(ccw)}(
     eliminated,

src/services/GraphInit.jl

@@ -1,4 +1,47 @@
 
+
+"""
+    $SIGNATURES
+
+For variables in `varList` check and if necessary make solverData objects for both `:default` and `:parametric` solveKeys. 
+
+Example
+```julia
+num_made = makeSolverData(fg; solveKey=:parametric)
+```
+
+Notes
+- Part of solving JuliaRobotics/IncrementalInference.jl issue 1637
+
+DevNotes
+- TODO, assumes parametric solves will always just be in solveKey `:parametric`.
+
+See also: [`doautoinit!`](@ref), [`initAll!`](@ref)
+"""
+function makeSolverData!(
+  dfg::AbstractDFG;
+  solvable = 1,
+  varList::AbstractVector{Symbol} = ls(dfg; solvable),
+  solveKey::Symbol=:default
+)
+  count = 0
+  for vl in varList
+    v = getVariable(dfg,vl)
+    varType = getVariableType(v) |> IIF._variableType
+    vsolveKeys = listSolveKeys(dfg,vl)
+    if solveKey != :parametric && !(solveKey in vsolveKeys)
+        IIF.setDefaultNodeData!(v, 0, getSolverParams(dfg).N, getDimension(varType); initialized=false, varType, solveKey) # dodims
+        count += 1
+    elseif solveKey == :parametric && !(:parametric in vsolveKeys)
+        # global doinit = true
+        IIF.setDefaultNodeDataParametric!(v, varType; initialized=false, solveKey)
+        count += 1
+    end
+  end
+
+  return count
+end
+
 """
     $SIGNATURES
 
@@ -464,7 +507,8 @@ function initAll!(
     varType = getVariableType(vari) |> _variableType
     # does SolverData exist for this solveKey?
     vsolveKeys = listSolveKeys(vari)
-    if !_parametricInit && !(solveKey in vsolveKeys)
+    # FIXME, likely some consolidation needed with #1637
+    if !_parametricInit && !(solveKey in vsolveKeys)  
       # accept complete defaults for a novel solveKey
       setDefaultNodeData!(
         vari,