diff --git a/IncrementalInference/Project.toml b/IncrementalInference/Project.toml index 38137d39..981e24be 100644 --- a/IncrementalInference/Project.toml +++ b/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" diff --git a/IncrementalInference/src/entities/BeliefTypes.jl b/IncrementalInference/src/entities/BeliefTypes.jl index f5ee8aba..61677667 100644 --- a/IncrementalInference/src/entities/BeliefTypes.jl +++ b/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, diff --git a/IncrementalInference/src/services/FactorGraph.jl b/IncrementalInference/src/services/FactorGraph.jl index 630ee14f..1aa90615 100644 --- a/IncrementalInference/src/services/FactorGraph.jl +++ b/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,17 +128,19 @@ 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!( @@ -136,8 +148,9 @@ function setVal!( 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!( @@ -145,8 +158,9 @@ function setVal!( 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 diff --git a/IncrementalInferenceTypes/Project.toml b/IncrementalInferenceTypes/Project.toml index 843fce4d..ad613cf8 100644 --- a/IncrementalInferenceTypes/Project.toml +++ b/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"