Merge branch '23Q1/enh/dfg19update' of github.com:JuliaRobotics/IncrementalInference.jl into 23Q1/api/dfg19solve

Author: GearsAD

src/ParametricUtils.jl

@@ -790,20 +790,10 @@ Update the parametric solver data value and covariance.
 """
 function updateSolverDataParametric! end
 
-function updateSolverDataParametric!(
-  v::DFGVariable,
-  val::AbstractArray{<:Real},
-  cov::Matrix;
-  solveKey::Symbol = :parametric,
-)
-  vnd = getSolverData(v, solveKey)
-  return updateSolverDataParametric!(vnd, val, cov)
-end
-
 function updateSolverDataParametric!(
   vnd::VariableNodeData,
-  val::AbstractArray{<:Real},
-  cov::Matrix,
+  val::AbstractArray,
+  cov::AbstractMatrix,
 )
   # fill in the variable node data value
   vnd.val[1] = val
@@ -812,6 +802,17 @@ function updateSolverDataParametric!(
   return vnd
 end
 
+function updateSolverDataParametric!(
+  v::DFGVariable,
+  val::AbstractArray,
+  cov::AbstractMatrix;
+  solveKey::Symbol = :parametric,
+)
+  vnd = getSolverData(v, solveKey)
+  return updateSolverDataParametric!(vnd, val, cov)
+end
+
+
 """
     $SIGNATURES
 Add parametric solver to fg, batch solve using [`solveGraphParametric`](@ref) and update fg.

src/services/FGOSUtils.jl

@@ -252,9 +252,23 @@ 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_)
+  return ppeType(;
+    id, 
+    solveKey=ppeKey, 
+    suggested=Pme_, 
+    max=Pma, 
+    mean=Pme_, 
+    createdTimestamp=ZonedDateTime(timestamp, localzone())
+  )
 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,
@@ -862,7 +864,7 @@ end
 
 function DFG.addFactor!(
   dfg::AbstractDFG,
-  xisyms::AbstractVector{Symbol},
+  vlbs::AbstractVector{Symbol},
   usrfnc::AbstractFactor;
   suppressChecks::Bool = false,
   kw...,
@@ -871,11 +873,12 @@ function DFG.addFactor!(
 
   # basic sanity check for unary vs n-ary
   if !suppressChecks
-    _checkFactorAdd(usrfnc, xisyms)
+    _checkFactorAdd(usrfnc, vlbs)
+    @assert length(vlbs) == length(unique(vlbs)) "List of variables should be unique and ordered."
   end
 
-  # variables = getVariable.(dfg, xisyms)
-  variables = map(vid -> getVariable(dfg, vid), xisyms)
+  # variables = getVariable.(dfg, vlbs)
+  variables = map(vid -> getVariable(dfg, vid), vlbs)
   return addFactor!(dfg, variables, usrfnc; suppressChecks = suppressChecks, kw...)
 end