Merge pull request #1677 from JuliaRobotics/23Q1/enh/dfg19update

update for DFG v0.19

Author: dehann

src/services/FGOSUtils.jl

@@ -235,8 +235,7 @@ function calcPPE(
   varType::InferenceVariable = getVariableType(var);
   ppeType::Type{<:MeanMaxPPE} = MeanMaxPPE,
   solveKey::Symbol = :default,
-  ppeKey::Symbol = solveKey,
-  timestamp = now(),
+  ppeKey::Symbol = solveKey
 )
   #
   P = getBelief(var, solveKey)
@@ -253,9 +252,22 @@ function calcPPE(
   Pme_ = getCoordinates(varType, Pme)
   # Pma_ = getCoordinates(M,Pme)
 
+  ppes = getPPEDict(var)
+  id = if haskey(ppes, ppeKey)
+    ppes[ppeKey].id
+  else
+    nothing
+  end
+
   # suggested, max, mean, current time
   # TODO, poor constructor argument assumptions on `ppeType`
-  return ppeType(ppeKey, Pme_, Pma, Pme_, timestamp)
+  return ppeType(;
+    id, 
+    solveKey=ppeKey, 
+    suggested=Pme_, 
+    max=Pma, 
+    mean=Pme_, 
+  )
 end
 
 # calcPPE(var::DFGVariable; method::Type{<:AbstractPointParametricEst}=MeanMaxPPE, solveKey::Symbol=:default) = calcPPE(var, getVariableType(var), method=method, solveKey=solveKey)

src/services/FactorGraph.jl

@@ -386,25 +386,26 @@ function DefaultNodeDataParametric(
     #                         gbw2, Symbol[], sp,
     #                         dims, false, :_null, Symbol[], variableType, true, 0.0, false, dontmargin)
   else
-    sp = round.(Int, range(dodims; stop = dodims + dims - 1, length = dims))
+    # dimIDs = round.(Int, range(dodims; stop = dodims + dims - 1, length = dims))
     ϵ = getPointIdentity(variableType)
-    return VariableNodeData(
-      [ϵ],
-      zeros(dims, dims),
-      Symbol[],
-      sp,
+    return VariableNodeData(;
+      id=nothing,
+      val=[ϵ],
+      bw=zeros(dims, dims),
+      # Symbol[],
+      # dimIDs,
       dims,
-      false,
-      :_null,
-      Symbol[],
+      # false,
+      # :_null,
+      # Symbol[],
       variableType,
-      false,
-      zeros(dims),
-      false,
+      initialized=false,
+      infoPerCoord=zeros(dims),
+      ismargin=false,
       dontmargin,
-      0,
-      0,
-      :parametric,
+      # 0,
+      # 0,
+      solveKey,
     )
   end
 end
@@ -440,7 +441,7 @@ function setDefaultNodeData!(
   v::DFGVariable,
   dodims::Int,
   N::Int,
-  dims::Int;
+  dims::Int=getDimension(v);
   solveKey::Symbol = :default,
   gt = Dict(),
   initialized::Bool = true,
@@ -453,42 +454,43 @@ function setDefaultNodeData!(
   data = nothing
   isinit = false
   sp = Int[0;]
-  (valpts, bws) = if initialized
+  (val, bw) = if initialized
     pN = resample(getBelief(v))
-    bws = getBW(pN)[:, 1:1]
+    bw = getBW(pN)[:, 1:1]
     pNpts = getPoints(pN)
     isinit = true
-    (pNpts, bws)
+    (pNpts, bw)
   else
     sp = round.(Int, range(dodims; stop = dodims + dims - 1, length = dims))
     @assert getPointType(varType) != DataType "cannot add manifold point type $(getPointType(varType)), make sure the identity element argument in @defVariable $varType arguments is correct"
-    valpts = Vector{getPointType(varType)}(undef, N)
-    for i = 1:length(valpts)
-      valpts[i] = getPointIdentity(varType)
+    val = Vector{getPointType(varType)}(undef, N)
+    for i = 1:length(val)
+      val[i] = getPointIdentity(varType)
     end
-    bws = zeros(dims, 1)
+    bw = zeros(dims, 1)
     #
-    (valpts, bws)
+    (val, bw)
   end
   # make and set the new solverData
   setSolverData!(
     v,
-    VariableNodeData(
-      valpts,
-      bws,
-      Symbol[],
-      sp,
+    VariableNodeData(;
+      id=nothing,
+      val,
+      bw,
+      # Symbol[],
+      # sp,
       dims,
-      false,
-      :_null,
-      Symbol[],
-      varType,
-      isinit,
-      zeros(getDimension(v)),
-      false,
+      # false,
+      # :_null,
+      # Symbol[],
+      variableType=varType,
+      initialized=isinit,
+      infoPerCoord=zeros(getDimension(v)),
+      ismargin=false,
       dontmargin,
-      0,
-      0,
+      # 0,
+      # 0,
       solveKey,
     ),
     solveKey,
@@ -587,14 +589,17 @@ function addVariable!(
   #
   varType = _variableType(varTypeU)
 
+  _zonedtime(s::DateTime) = ZonedDateTime(s, localzone())
+  _zonedtime(s::ZonedDateTime) = s 
+
   union!(tags, [:VARIABLE])
   v = DFGVariable(
     label,
     varType;
     tags = Set(tags),
     smallData = smalldata,
     solvable = solvable,
-    timestamp = timestamp,
+    timestamp = _zonedtime(timestamp),
     nstime = Nanosecond(nanosecondtime),
   )
 
@@ -611,9 +616,7 @@ function addVariable!(
   (:parametric in initsolvekeys) &&
     setDefaultNodeDataParametric!(v, varType; initialized = false, dontmargin = dontmargin)
 
-  DFG.addVariable!(dfg, v)
-
-  return v
+  return DFG.addVariable!(dfg, v)
 end
 
 function parseusermultihypo(multihypo::Nothing, nullhypo::Float64)
@@ -825,6 +828,9 @@ function DFG.addFactor!(
 
   @assert (suppressChecks || length(multihypo) === 0 || length(multihypo) == length(Xi)) "When using multihypo=[...], the number of variables and multihypo probabilies must match.  See documentation on how to include fractional data-association uncertainty."
 
+  _zonedtime(s::ZonedDateTime) = s
+  _zonedtime(s::DateTime) = ZonedDateTime(s, localzone())
+
   varOrderLabels = Symbol[v.label for v in Xi]
   solverData = getDefaultFactorData(
     dfg,
@@ -843,16 +849,16 @@ function DFG.addFactor!(
     solverData;
     tags = Set(union(tags, [:FACTOR])),
     solvable,
-    timestamp,
+    timestamp = _zonedtime(timestamp),
   )
   #
 
-  success = addFactor!(dfg, newFactor)
+  factor = addFactor!(dfg, newFactor)
 
   # TODO: change this operation to update a conditioning variable
   graphinit && doautoinit!(dfg, Xi; singles = false)
 
-  return newFactor
+  return factor
 end
 
 function _checkFactorAdd(usrfnc, xisyms)
@@ -879,7 +885,7 @@ function DFG.addFactor!(
 
   # variables = getVariable.(dfg, vlbs)
   variables = map(vid -> getVariable(dfg, vid), vlbs)
-  return addFactor!(dfg, variables, usrfnc; suppressChecks = suppressChecks, kw...)
+  return addFactor!(dfg, variables, usrfnc; suppressChecks, kw...)
 end
 
 #

src/services/JunctionTreeUtils.jl

@@ -847,7 +847,7 @@ function prepBatchTreeOLD!(
     end
   end
 
-  tree = buildTreeFromOrdering!(dfg, p; drawbayesnet = false) # drawbayesnet
+  tree = buildTreeFromOrdering!(dfg, Symbol.(p); drawbayesnet = false) # drawbayesnet
 
   @info "Bayes Tree Complete"
   if drawpdf

test/testgraphpackingconverters.jl

@@ -68,8 +68,8 @@ dat = getSolverData(getVariable(fg,:x1))
 
 # dat.BayesNetVertID
 
-pd = packVariableNodeData(fg, dat)
-unpckd = unpackVariableNodeData(fg, pd)
+pd = packVariableNodeData(dat)
+unpckd = unpackVariableNodeData(pd)
 
 @test compareFields(dat, unpckd, skip=[:variableType])
 @test compareFields(getVariableType(dat), getVariableType(unpckd))