Merge pull request #1700 from JuliaRobotics/23Q1/fix/accumfactorparam

fix multiple accumulateFactorMeans bugs

Author: dehann

src/ConsolidateParametricRelatives.jl

@@ -25,8 +25,8 @@ function solveFactorParameteric(
   fct::DFGFactor,
   # currval::P1,
   srcsym_vals::AbstractVector{Pair{Symbol, P}},
-  trgsym::Symbol,
-  solveKey::Symbol = :default;
+  trgsym::Symbol;
+  solveKey::Symbol = :default,
   evaltmpkw...,
 ) where {P}
   #
@@ -71,7 +71,7 @@ function solveFactorParameteric(
     sfidx,
     measT,
     pts;
-    solveKey = solveKey,
+    solveKey,
     evaltmpkw...,
   )[1]
 

src/services/CalcFactor.jl

@@ -264,6 +264,7 @@ function _prepParamVec(
   # TODO --rather define reusable memory for the proposal
   # we are generating a proposal distribution, not direct replacement for existing memory and hence the deepcopy.
   if sfidx > 0
+    # FIXME, POSSIBLE SOURCE OF HUGE MEMORY CONSUMPTION ALLOCATION
     varParamsAll[sfidx] = deepcopy(varParamsAll[sfidx])
   end
 

src/services/EvalFactor.jl

@@ -609,8 +609,8 @@ function _evalFactorTemporary!(
     varTypes,
     pts;
     dfg = tfg,
-    solveKey = solveKey,
-    newFactor = newFactor,
+    solveKey,
+    newFactor,
     buildgraphkw...,
   )
 
@@ -624,9 +624,9 @@ function _evalFactorTemporary!(
     solvefor,
     measurement;
     needFreshMeasurements = false,
-    solveKey = solveKey,
+    solveKey,
     inflateCycles = 1,
-    _slack = _slack,
+    _slack,
   )
 
   return sfPts

src/services/TetherUtils.jl

@@ -116,7 +116,11 @@ Related:
 
 [`approxConv`](@ref), [`solveFactorParameteric`](@ref), `RoME.MutablePose2Pose2Gaussian`
 """
-function accumulateFactorMeans(dfg::AbstractDFG, fctsyms::AbstractVector{Symbol})
+function accumulateFactorMeans(
+  dfg::AbstractDFG, 
+  fctsyms::AbstractVector{Symbol};
+  solveKey::Symbol=:default
+)
 
   ## get the starting estimate
   nextidx = 1
@@ -137,7 +141,7 @@ function accumulateFactorMeans(dfg::AbstractDFG, fctsyms::AbstractVector{Symbol}
     nextsym =
       1 < length(fctsyms) ? intersect(vars, ls(dfg, fctsyms[nextidx + 1])) : vars[end]
     currsym = 1 < length(fctsyms) ? setdiff(vars, nextsym)[1] : vars[1]
-    calcPPE(dfg, currsym).suggested
+    calcPPE(dfg, currsym; solveKey).suggested
   end
 
   srcsym = currsym
@@ -146,7 +150,7 @@ function accumulateFactorMeans(dfg::AbstractDFG, fctsyms::AbstractVector{Symbol}
     # first find direction of solve
     vars = getVariableOrder(fct)
     trgsym = setdiff(vars, [srcsym])[1]
-    val = solveFactorParameteric(dfg, fct, [srcsym => val;], trgsym)
+    val = solveFactorParameteric(dfg, fct, [srcsym => val;], trgsym; solveKey)
     srcsym = trgsym
   end