Improve parametric performance and Cleanup (#1788)

Author: Affie

src/entities/CalcFactor.jl

@@ -100,27 +100,24 @@ end
 
 struct CalcFactorResidual{
   FT <: AbstractFactor,
-  C,
+  N,
   D,
-  L,
-  P,
   MEAS <: AbstractArray,
-  N
+  L,
+  C,
 } <: CalcFactor{FT}
   faclbl::Symbol
   factor::FT
-  cache::C
   varOrder::NTuple{N, Symbol}
   varOrderIdxs::NTuple{N, Int}
-  points::P #TODO remove or not?
   meas::MEAS
-  iΣ::SMatrix{D, D, Float64, L} #TODO remove or not?
   sqrt_iΣ::SMatrix{D, D, Float64, L}
+  cache::C
 end
 
-_nvars(::CalcFactorResidual{FT, C, D, L, P, MEAS, N}) where {FT, C, D, L, P, MEAS, N} = N
+_nvars(::CalcFactorResidual{FT, N, D, MEAS, L, C}) where {FT, N, D, MEAS, L, C} = N
 # _typeof_meas(::CalcFactorManopt{FT, C, D, L, MEAS, N}) where {FT, C, D, L, MEAS, N} = MEAS
-DFG.getDimension(::CalcFactorResidual{FT, C, D, L, P, MEAS, N}) where {FT, C, D, L, P, MEAS, N} = D
+DFG.getDimension(::CalcFactorResidual{FT, N, D, MEAS, L, C}) where {FT, N, D, MEAS, L, C} = D
 
 # workaround for issue #1781
 import Base: getproperty

src/manifolds/services/ManifoldsExtentions.jl

@@ -47,7 +47,8 @@ function Manifolds.get_vector!(M::NPowerManifold, Y, p, c, B::AbstractBasis)
     Y[i] = get_vector(
       M.manifold,
       Manifolds._read(M, rep_size, p, i),
-      view(c, v_iter:(v_iter + dim - 1)),
+      # view(c, v_iter:(v_iter + dim - 1)),
+      SVector{dim}(view(c, v_iter:(v_iter + dim - 1))),
       B,
     )
     v_iter += dim

src/parametric/services/ParametricManopt.jl

@@ -14,7 +14,7 @@ function getVarIntLabelMap(vartypeslist::OrderedDict{DataType, Vector{Symbol}})
   return varIntLabel, varlabelsAP
 end
 
-function CalcFactorResidual(fg, fct::DFGFactor, varIntLabel, points::Union{Nothing,ArrayPartition}=nothing)
+function CalcFactorResidual(fg, fct::DFGFactor, varIntLabel)
   fac_func = getFactorType(fct)
   varOrder = getVariableOrder(fct)
 
@@ -29,22 +29,14 @@ function CalcFactorResidual(fg, fct::DFGFactor, varIntLabel, points::Union{Nothi
   sqrt_iΣ = convert(SMatrix{dims, dims}, sqrt(iΣ))
   cache = preambleCache(fg, getVariable.(fg, varOrder), getFactorType(fct))
 
-  if isnothing(points)
-    points_view = nothing
-  else
-    points_view = @view points[varOrderIdxs]
-  end
-
   return CalcFactorResidual(
     fct.label,
     getFactorMechanics(fac_func),
-    cache,
     tuple(varOrder...),
     tuple(varOrderIdxs...),
-    points_view,
     meas,
-    iΣ,
     sqrt_iΣ,
+    cache,
   )
 end
 
@@ -53,7 +45,7 @@ end
   CalcFactorResidualAP
 Create an `ArrayPartition` of `CalcFactorResidual`s.
 """
-function CalcFactorResidualAP(fg::GraphsDFG, factorLabels::Vector{Symbol}, varIntLabel::OrderedDict{Symbol, Int64}, points)
+function CalcFactorResidualAP(fg::GraphsDFG, factorLabels::Vector{Symbol}, varIntLabel::OrderedDict{Symbol, Int64})
   factypes, typedict, alltypes = getFactorTypesCount(getFactor.(fg, factorLabels))
 
   # skip non-numeric prior (MetaPrior)
@@ -63,7 +55,7 @@ function CalcFactorResidualAP(fg::GraphsDFG, factorLabels::Vector{Symbol}, varIn
 
   parts = map(values(alltypes)) do labels
     map(getFactor.(fg, labels)) do fct
-      CalcFactorResidual(fg, fct, varIntLabel, points)
+      CalcFactorResidual(fg, fct, varIntLabel)
     end
   end
   parts_tuple = (parts...,)
@@ -76,10 +68,6 @@ function (cfm::CalcFactorResidual)(p)
   return cfm.sqrt_iΣ * cfm(meas, points...)
 end
 
-function (cfm::CalcFactorResidual{T})() where T
-  return cfm.sqrt_iΣ * cfm(cfm.meas, cfm.points...)
-end
-
 # cost function f: M->ℝᵈ for Riemannian Levenberg-Marquardt 
 struct CostFres_cond!{PT, CFT}
   points::PT
@@ -112,14 +100,13 @@ end
 
 function calcFactorResVec!(
   x::Vector{T},
-  cfm_part::Vector{<:CalcFactorResidual{FT}},
+  cfm_part::Vector{<:CalcFactorResidual{FT, N, D}},
   p::AbstractArray{T},
   st::Int
-) where {T,FT}
-  l = getDimension(cfm_part[1]) # all should be the same 
+) where {T, FT, N, D}
   for cfm in cfm_part
-    x[st:st + l - 1] = cfm(p) #NOTE looks like do not broadcast here
-    st += l
+    x[st:st + D - 1] = cfm(p) #NOTE looks like do not broadcast here
+    st += D
   end
   return st
 end
@@ -329,7 +316,7 @@ function solve_RLM(
   end
 
   # create an ArrayPartition{CalcFactorResidual} for faclabels
-  calcfacs = CalcFactorResidualAP(fg, faclabels, varIntLabel, p0)
+  calcfacs = CalcFactorResidualAP(fg, faclabels, varIntLabel)
 
   #cost and jacobian functions
   # cost function f: M->ℝᵈ for Riemannian Levenberg-Marquardt 
@@ -446,7 +433,7 @@ function solve_RLM_conditional(
   # varIntLabel_frontals = filter(p->first(p) in frontals, varIntLabel)
   # varIntLabel_separators = filter(p->first(p) in separators, varIntLabel)
 
-  calcfacs = CalcFactorResidualAP(fg, faclabels, all_varIntLabel, all_points)
+  calcfacs = CalcFactorResidualAP(fg, faclabels, all_varIntLabel)
 
   # get the manifold and variable types
 

src/services/FactorGradients.jl

@@ -5,67 +5,27 @@
 function factorJacobian(
   fg,
   faclabel::Symbol,
-  fro_p = first.(getVal.(fg, getVariableOrder(fg, faclabel), solveKey = :parametric))
+  p0 = ArrayPartition(first.(getVal.(fg, getVariableOrder(fg, faclabel), solveKey = :parametric))...),
+  backend = ManifoldDiff.TangentDiffBackend(ManifoldDiff.FiniteDiffBackend()),
 )
-  faclabels = Symbol[faclabel;]
-  frontals = ls(fg, faclabel)
-  separators = Symbol[] # setdiff(ls(fg), frontals)
 
-  # get the subgraph formed by all frontals, separators and fully connected factors
-  varlabels = union(frontals, separators)
+  fac = getFactor(fg, faclabel)
+  varlabels = getVariableOrder(fac)
+  varIntLabel = OrderedDict(zip(varlabels, eachindex(varlabels)))
 
-  filter!(faclabels) do fl
-    return issubset(getVariableOrder(fg, fl), varlabels)
+  cfm =  CalcFactorResidual(fg, fac, varIntLabel)
+  
+  function costf(p)
+    points = map(idx->p.x[idx], cfm.varOrderIdxs)
+    return cfm.sqrt_iΣ * cfm(cfm.meas, points...)
   end
 
-  facs = getFactor.(fg, faclabels)
-
-  # so the subgraph consists of varlabels(frontals + separators) and faclabels
-
-  varIntLabel = OrderedDict(zip(varlabels, collect(1:length(varlabels))))
-
-  # varIntLabel_frontals = filter(p->first(p) in frontals, varIntLabel)
-  # varIntLabel_separators = filter(p->first(p) in separators, varIntLabel)
-
-  calcfacs = map(f->IIF.CalcFactorManopt(f, varIntLabel), facs)
-
-  # get the manifold and variable types
-  frontal_vars = getVariable.(fg, frontals)
-  vartypes, vartypecount, vartypeslist = IIF.getVariableTypesCount(frontal_vars)
-
-  PMs = map(vartypes) do vartype
-    N = vartypecount[vartype]
-    G = getManifold(vartype)
-    return IIF.NPowerManifold(G, N)
-  end
-  M = ProductManifold(PMs...)
-
-  #
-  #FIXME 
-  @assert length(M.manifolds) == 1 "#FIXME, this only works with 1 manifold type component"
-  MM = M.manifolds[1]
-
-  # inital values and separators from fg
-  # fro_p = first.(getVal.(frontal_vars, solveKey = :parametric))
-  # sep_p::Vector{eltype(fro_p)} = first.(getVal.(fg, separators, solveKey = :parametric))
-  sep_p = Vector{eltype(fro_p)}()
-
-  #cost and jacobian functions
-  # cost function f: M->ℝᵈ for Riemannian Levenberg-Marquardt 
-  costF! = IIF.CostF_RLM!(calcfacs, fro_p, sep_p)
-  # jacobian of function for Riemannian Levenberg-Marquardt
-  jacF! = IIF.JacF_RLM!(MM, costF!)
-
-  num_components = length(jacF!.res)
-
-  p0 = deepcopy(fro_p)
-
-  # initial_residual_values = zeros(num_components)
-  J = zeros(num_components, manifold_dimension(MM))
-
-  jacF!(MM, J, p0)
-
-  J
+  M_dom = ProductManifold(getManifold.(fg, varlabels)...)
+  #TODO verify M_codom
+  M_codom = Euclidean(manifold_dimension(getManifold(fac)))
+  # Jx(M, p) = ManifoldDiff.jacobian(M, M_codom, calcfac, p, backend)
+  
+  return ManifoldDiff.jacobian(M_dom, M_codom, costf, p0, backend)
 end
 
 

test/runtests.jl

@@ -13,7 +13,7 @@ end
 if TEST_GROUP in ["all", "basic_functional_group"]
 # more frequent stochasic failures from numerics
 include("manifolds/manifolddiff.jl")
-# include("manifolds/factordiff.jl") #FIXME restore
+include("manifolds/factordiff.jl")
 include("testSpecialEuclidean2Mani.jl")
 include("testEuclidDistance.jl")
 
@@ -99,7 +99,7 @@ include("testFluxModelsDistribution.jl")
 include("testAnalysisTools.jl")
 
 include("testBasicParametric.jl")
-# include("testMixtureParametric.jl") #FIXME parametric mixtures #[TODO open issue]
+# include("testMixtureParametric.jl") #FIXME parametric mixtures #1787
 
 # dont run test on ARM, as per issue #527
 if Base.Sys.ARCH in [:x86_64;]