more fixes toward DFG v0.29.0 release

IncrementalInference/Project.toml

@@ -77,7 +77,7 @@ Combinatorics = "1.0"
 DataStructures = "0.18, 0.19"
 DelimitedFiles = "1"
 DifferentialEquations = "7"
-DistributedFactorGraphs = "0.27, 0.28"
+DistributedFactorGraphs = "0.29"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
 FileIO = "1"

IncrementalInference/src/entities/BeliefTypes.jl

@@ -110,7 +110,7 @@ function TreeBelief(::RootsOnlyTopology, vnd::State, solvDim::Real = 0)
   return TreeBelief(
     DFG.refMeans(vnd),
     DFG.refCovariances(vnd)[1],
-    vnd.observability,
+    DFG.refObservability(vnd),
     getStateKind(vnd),
     getManifold(vnd),
     solvDim,
@@ -121,7 +121,7 @@ function TreeBelief(::LeavesOnlyTopology, vnd::State, solvDim::Real = 0)
   return TreeBelief(
     DFG.refPoints(vnd),
     DFG.refBandwidth(vnd),
-    vnd.observability,
+    DFG.refObservability(vnd),
     getStateKind(vnd),
     getManifold(vnd),
     solvDim,

IncrementalInference/src/services/FactorGraph.jl

@@ -90,26 +90,36 @@ function setBW!(v::VariableCompute, bw::Array{Float64, 2}; solveKey::Symbol = :d
   return nothing
 end
 
-function setVal!(vd::State, val::AbstractVector{P}) where {P}
+function setVal!(
+  vd::State, 
+  val::AbstractVector{P}; 
+  observability::AbstractVector{<:Real} = [0.0;]
+) where {P}
   points = DFG.refPoints(vd)
   resize!(points, length(val))
   points .= val
+
+  observability = DFG.refObservability(vd)
+  resize!(observability, length(observability))
+  observability .= observability
   return nothing
 end
 function setVal!(
   v::VariableCompute,
   val::AbstractVector{P};
   solveKey::Symbol = :default,
+  observability::AbstractVector{<:Real} = [0.0;],
 ) where {P}
-  setVal!(getState(v, solveKey), val)
+  setVal!(getState(v, solveKey), val; observability)
   return nothing
 end
 function setVal!(
   vd::State,
   val::AbstractVector{P},
-  bw::AbstractMatrix{Float64},
+  bw::AbstractMatrix{Float64};
+  observability::AbstractVector{<:Real} = [0.0;],
 ) where {P}
-  setVal!(vd, val)
+  setVal!(vd, val; observability)
   setBW!(vd, bw)
   return nothing
 end
@@ -118,35 +128,39 @@ function setVal!(
   val::AbstractVector{P},
   bw::AbstractMatrix{Float64};
   solveKey::Symbol = :default,
+  observability::AbstractVector{<:Real} = [0.0;],
 ) where {P}
-  setVal!(v, val; solveKey = solveKey)
-  setBW!(v, bw; solveKey = solveKey)
+  setVal!(v, val; solveKey, observability)
+  setBW!(v, bw; solveKey)
   return nothing
 end
 function setVal!(
   vd::State,
   val::AbstractVector{P},
-  bw::AbstractVector{Float64},
+  bw::AbstractVector{Float64};
+  observability::AbstractVector{<:Real} = [0.0;],
 ) where {P}
-  setVal!(vd, val, reshape(bw, length(bw), 1))
+  setVal!(vd, val, reshape(bw, length(bw), 1); observability)
   return nothing
 end
 function setVal!(
   v::VariableCompute,
   val::AbstractVector{P},
   bw::AbstractVector{Float64};
   solveKey::Symbol = :default,
+  observability::AbstractVector{<:Real} = [0.0;],
 ) where {P}
-  setVal!(getState(v, solveKey), val, bw)
+  setVal!(getState(v, solveKey), val, bw; observability)
   return nothing
 end
 function setVal!(
   dfg::AbstractDFG,
   sym::Symbol,
   val::AbstractVector{P};
   solveKey::Symbol = :default,
+  observability::AbstractVector{<:Real} = [0.0;],
 ) where {P}
-  return setVal!(getVariable(dfg, sym), val; solveKey = solveKey)
+  return setVal!(getVariable(dfg, sym), val; solveKey, observability)
 end
 
 """
@@ -167,9 +181,9 @@ function setValKDE!(
 ) where {P}
   #
 
-  setVal!(vd, pts, bws) # BUG ...al!(., val, . ) ## TODO -- this can be a little faster
+  setVal!(vd, pts, bws; observability = ipc) # BUG ...al!(., val, . ) ## TODO -- this can be a little faster
   setinit ? (vd.initialized = true) : nothing
-  vd.observability = ipc
+  # vd.observability = ipc # TODO, state.belief.observability = ipc instead
   return nothing
 end
 

IncrementalInferenceTypes/Project.toml

@@ -19,7 +19,7 @@ StructTypes = "856f2bd8-1eba-4b0a-8007-ebc267875bd4"
 [compat]
 Dates = "1.10.0"
 Distributed = "1.10.0"
-DistributedFactorGraphs = "0.27, 0.28.0"
+DistributedFactorGraphs = "0.29"
 Distributions = "0.25.120"
 DocStringExtensions = "0.9.5"
 LieGroups = "0.1.2"