From d1a3356ee07dfcdb522b244a0e431f9eabc82cf6 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 24 Apr 2026 20:48:39 +0200 Subject: [PATCH 1/3] Improve State.belief semantics and add explicit topology; mark SolverParams as deprecated --- src/DistributedFactorGraphs.jl | 4 +- src/Serialization/StateSerialization.jl | 14 +- src/entities/State.jl | 206 ++++++++++++++++-------- src/entities/equality.jl | 2 +- src/services/AbstractDFG.jl | 13 +- src/services/compare.jl | 2 +- src/services/print.jl | 6 +- test/testSerializingVariables.jl | 16 +- 8 files changed, 173 insertions(+), 90 deletions(-) diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index 1fca15f1..14c93843 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -61,13 +61,13 @@ using StructUtils: @kwarg, @tags export StructUtils # export for use in macros export AbstractManifold export ArrayPartition -public @format_str # from FileIO +export @format_str # from FileIO # DFG exports const DFG = DistributedFactorGraphs export DFG # module alias for DistributedFactorGraphs export getLabel #TODO move -public @defStateType # macro to define custom variable types +export @defStateType # macro to define custom variable types # ------------------------------------------------------------------------------ # Types diff --git a/src/Serialization/StateSerialization.jl b/src/Serialization/StateSerialization.jl index 81554721..8001697b 100644 --- a/src/Serialization/StateSerialization.jl +++ b/src/Serialization/StateSerialization.jl @@ -131,14 +131,18 @@ function unpackOldState(d) label = Symbol(d.solveKey) !isempty(d.covar) && error("covar field is not supported") if label == :parametric - belief = - StoredBelief(GaussianDensityKind(), statekind; means = vals, covariances = [BW]) + belief = StoredHomotopyBelief( + RootsOnlyTopology(), + statekind; + means = vals, + shapes = [BW], + ) else - belief = StoredBelief( - NonparametricDensityKind(), + belief = StoredHomotopyBelief( + LeavesOnlyTopology(), statekind; points = vals, - bandwidth = BW, + bandwidths = [BW], ) end return State{T, getPointType(T)}(; diff --git a/src/entities/State.jl b/src/entities/State.jl index 86144b4d..7caccdad 100644 --- a/src/entities/State.jl +++ b/src/entities/State.jl @@ -5,99 +5,160 @@ abstract type AbstractStateType{N} end const StateType = AbstractStateType -##============================================================================== -## StoredBelief -##============================================================================== -abstract type AbstractDensityKind end +# ============================================================================== +# StoredHomotopyBelief +# ============================================================================== +""" + AbstractHomotopyTopology + +Describes the physical layout of the nodes within a `StoredHomotopyBelief`. + +Since all beliefs in the Caesar ecosystem are fundamentally Homotopy densities, +this trait acts as a lightweight dispatch hint (a "Lens Selector"). It indicates +which parts of the tree (Roots vs. Leaves) are currently populated and how they +are wired, without requiring downstream packages to inspect the underlying vectors. + +**Role of the Topology Trait:** +- **DFG:** Determines how to serialize and spatial-index the belief in the database. +- **Visualizers:** Decides how to render the data (e.g., drawing ellipses for roots vs. a point cloud for leaves). +- **IIF/AMP:** Selects the correct mathematical view to construct (e.g., `MvNormal` vs. `ManifoldKernelDensity` vs. a full `HomotopyDensity`). + +!!! note "State, not Strategy" + The trait purely describes the *current physical shape* of the data (e.g., "I currently only have Roots populated"). + It does *not* dictate the solver strategy (e.g., "You must use a parametric solver"). + The math engine is always free to convert or expand the data based on the graph's needs. + +**Extending:** +If the standard tree-based Homotopy model does not fit your specific data layout +or solver requirements, you are encouraged to extend this abstract type with your +own custom topology struct. Alternatively, if you believe your use case represents +a missing core layout, please open an issue to discuss adding it to the +foundational ecosystem. +""" +abstract type AbstractHomotopyTopology end + +# --- 1. The Roots --- +"L1 structural nodes only. No L2 samples. (Schema: `means`, `weights`, `shapes` populated. `points` empty.)" +struct RootsOnlyTopology <: AbstractHomotopyTopology end -"""Single Gaussian (mean + covariance).""" -struct GaussianDensityKind <: AbstractDensityKind end +# --- 2. The Leaves --- +"L2 raw samples only. No L1 structure. (Schema: `points`, `bandwidths` populated. `means` empty.)" +struct LeavesOnlyTopology <: AbstractHomotopyTopology end -"""Kernel density / particle-based (points + shared bandwidth).""" -struct NonparametricDensityKind <: AbstractDensityKind end +# --- 3. The Full Trees --- +"Tree packed in arrays using 2i, 2i+1 math.(Schema: L1 and L2 populated. Parent arrays empty.)" +struct ImplicitTreeTopology <: AbstractHomotopyTopology end -"""Homotopy between particles and Gaussian.""" -struct HomotopyDensityKind <: AbstractDensityKind end +"Full tree using adjacency lists. (Schema: L1, L2, and Parent arrays fully populated.)" +struct ExplicitTreeTopology <: AbstractHomotopyTopology end -function StructUtils.lower(::StructUtils.StructStyle, p::AbstractDensityKind) +function StructUtils.lower(::StructUtils.StructStyle, p::AbstractHomotopyTopology) return StructUtils.lower(Packed(p)) end -@choosetype AbstractDensityKind resolvePackedType - -# TODO naming? Density, DensityRepresentation, StoredBelief, BeliefState, etc? -# TODO flatten in State? likeley not for easier serialization of points. -@kwdef struct StoredBelief{T <: StateType, P} - statekind::T = T()# NOTE duplication for serialization, TODO maybe only in State and therefore belief cannot deserialize separately. - """Discriminator for which representation is active.""" - densitykind::AbstractDensityKind = NonparametricDensityKind() - - #--- Parametric fields (Gaussian / GMM / Homotopy leading modes) --- - """On-manifold component means. - Gaussian: length 1. Homotopy: leading (tree_kernel) means.""" - means::Vector{P} = P[] # previously `val[1]` for Gaussian - """Component covariances, matching `means`.""" - covariances::Vector{Matrix{Float64}} = Matrix{Float64}[] # previously `covar` existed but was stored in `bw` (hacky) - "Component weights, matching `means`." +@choosetype AbstractHomotopyTopology resolvePackedType + +""" + StoredHomotopyBelief{T <: StateType, P} + +A multi-resolution "Grove of Trees" representing a manifold belief. +Each tree can be as deep (ExplicitTreeTopology) or as shallow (RootsOnlyTopology) +as the evidence requires, but they all speak the same language of Nodes and Parents. + +These are the internal raw beliefs and need to be viewed through a lens such as +provided by AMP for features like pdf evaluation. Organized into structural +Tree/Branch layers (L1) and empirical Leaf layers (L2). + +!!! warning "Raw Data Container" + `StoredHomotopyBelief` is the raw data schema used for database storage and serialization. + Mutating this structure in-place is discouraged. Rather, construct a new `State` object + and call `addState!` or `mergeState!`. +""" +@kwdef struct StoredHomotopyBelief{T <: StateType, P} + statekind::T = T()# NOTE duplication for serialization and self description. + """A hint for downstream solvers on how to interpret this data (The 'How')""" + topologykind::AbstractHomotopyTopology = LeavesOnlyTopology() + + # L1 Nodes + """ + [Order 0] The relative importance or probability of each node in L1. + """ weights::Vector{Float64} = Float64[] + """ + [Order 1] The location/center of each node, stored directly on the manifold. + """ + means::Vector{P} = P[] # previously `val[1]` for Gaussian + """ + [Order 2] The spread/curvature of each node (e.g., Covariance or Precision matrix). + """ + shapes::Vector{Matrix{Float64}} = Matrix{Float64}[] # previously `covar` existed but was stored in `bw` (hacky) - #--- Non-parametric / Homotopy leaves --- - """On-manifold sample points. For KDE/HomotopyDensity, these are the leaf kernel means.""" + # L2 Nodes + """ + The raw empirical samples on the manifold. Used for KDE and particle representations. + """ points::Vector{P} = P[] # previously `val` - """Shared kernel bandwidth matrix used with ManifoldKernelDensity, see field `covar` for the parametric covariance""" - bandwidth::Union{Nothing, Matrix{Float64}} = zeros(getDimension(T), getDimension(T)) #previously `bw` --- - # bandwidth::Matrix{Float64} = zeros(getDimension(T), getDimension(T)) - # TODO is bandwidth[s] matrix or vector or ::Vector{Matrix{Float64} or ::Vector{Vector{Float64}? - # JSON.parse(JSON.json(zeros(0, 0)), Matrix{Float64}) errors, so trying with nothing union -end + """ + The second-order bandwidths for the non-parametric points, supports variable bandwidth kernels. + """ + bandwidths::Vector{Matrix{Float64}} = Matrix{Float64}[] #previously `bw` --- -#FIXME remove old name before v0.29 -const BeliefRepresentation = StoredBelief + # --- Topology (The Hierarchy) --- + """ + L1 Internal Topology: mean_parents[i] = j means means[i] is a child of means[j]. A value of 0 indicates a Root node. + """ + mean_parents::Vector{Int} = Int[] + """ + L2-to-L1 Bridge: point_parents[i] = j means points[i] is governed by means[j]. Points are leaves. + """ + point_parents::Vector{Int} = Int[] +end -JSON.omit_empty(::Type{<:StoredBelief}) = true +JSON.omit_empty(::Type{<:StoredHomotopyBelief}) = true -function StoredBelief(T::AbstractStateType) - return StoredBelief{typeof(T), getPointType(T)}(; statekind = T) +function StoredHomotopyBelief(T::AbstractStateType) + return StoredHomotopyBelief{typeof(T), getPointType(T)}(; statekind = T) end -function StoredBelief(::NonparametricDensityKind, T::AbstractStateType; kwargs...) - return StoredBelief{typeof(T), getPointType(T)}(; +function StoredHomotopyBelief(::LeavesOnlyTopology, T::AbstractStateType; kwargs...) + return StoredHomotopyBelief{typeof(T), getPointType(T)}(; statekind = T, - densitykind = NonparametricDensityKind(), - bandwidth = zeros(getDimension(T), getDimension(T)), + topologykind = LeavesOnlyTopology(), + bandwidths = [zeros(getDimension(T), getDimension(T))], kwargs..., ) end -function StoredBelief(::GaussianDensityKind, T::AbstractStateType; kwargs...) - return StoredBelief{typeof(T), getPointType(T)}(; +function StoredHomotopyBelief(::RootsOnlyTopology, T::AbstractStateType; kwargs...) + return StoredHomotopyBelief{typeof(T), getPointType(T)}(; statekind = T, - densitykind = GaussianDensityKind(), - bandwidth = nothing, + topologykind = RootsOnlyTopology(), kwargs..., ) end function StructUtils.fielddefaults( ::StructUtils.StructStyle, - ::Type{StoredBelief{T, P}}, + ::Type{StoredHomotopyBelief{T, P}}, ) where {T, P} return ( statekind = T(), - densitykind = NonparametricDensityKind(), + topologykind = LeavesOnlyTopology(), means = P[], - covariances = Matrix{Float64}[], + shapes = Matrix{Float64}[], weights = Float64[], points = P[], - bandwidth = nothing, + bandwidths = Matrix{Float64}[], + mean_parents = Int[], + point_parents = Int[], ) end function resolveStoredBeliefType(lazyobj) statekind = liftStateKind(lazyobj.statekind[]) - return StoredBelief{typeof(statekind), getPointType(statekind)} + return StoredHomotopyBelief{typeof(statekind), getPointType(statekind)} end -@choosetype StoredBelief resolveStoredBeliefType +@choosetype StoredHomotopyBelief resolveStoredBeliefType ##============================================================================== ## State @@ -121,7 +182,7 @@ $(TYPEDFIELDS) """ Generic stored belief for this state. """ - belief::StoredBelief{T, P} = StoredBelief{T, P}()#; statekind = T()) + belief::StoredHomotopyBelief{T, P} = StoredHomotopyBelief{T, P}()#; statekind = T()) """List of symbols for separator variables for this state, used in variable elimination and inference computations.""" separator::Vector{Symbol} = Symbol[] """False if initial numerical values are not yet available or stored values are not ready for further processing yet.""" @@ -132,16 +193,7 @@ $(TYPEDFIELDS) marginalized::Bool = false #TODO renamed from ismargin v0.29 """How many times has a solver updated this state estimate.""" solves::Int = 0 # TODO renamed from solvedCount v0.29 - - #TODO belief container that can be used for active solver beliefs such as a HomotopyDensity - # The type is defined by a trait saved in the StoredBelief and - # verbs such as `hydrate!(state)` `persist!(state)` can be used at data at checkpoints. - # Forcing an explicit `persist!` acts as a state checkpoint, - #ensuring the graph only ever stores fully committed solver results rather than half-computed intermediate math. - # abstract type AbstractActiveBelief end - # active_belief::Base.RefValue{<:AbstractActiveBelief} = Ref{AbstractActiveBelief}() & (ignore = true,) end - # OLD deprecated fields, removed in v0.29, kept here for reference during transition # val::Vector{P} = Vector{P}() # bw::Matrix{Float64} = zeros(0, 0) @@ -153,6 +205,22 @@ end # events::Dict{Symbol, Threads.Condition} = Dict{Symbol, Threads.Condition}() # dontmargin::Bool = false +# ============================================================================== +# FUTURE VIEW WRAPPER (Internal DFG Placeholder) +# ============================================================================== +# NOTE: The `StoredHomotopyBelief` is currently expressive and fast enough that +# DFG does not need to store a resolved view next to it in memory. +# +# If future profiling requires it, DFG will introduce a verbose View wrapper +# to hold the raw data alongside the instantiated read-only math object. +# +# abstract type AbstractHomotopyBeliefView end +# +# struct HomotopyBeliefView{T, P, M} <: AbstractHomotopyBeliefView +# stored::StoredHomotopyBelief{T, P} +# math_engine::M # Read-only instantiated solver object (e.g., AMP.HomotopyDensity) +# end + ##------------------------------------------------------------------------------ ## Constructors function State{T}(; kwargs...) where {T <: StateType} @@ -178,7 +246,7 @@ function StructUtils.fielddefaults( ::Type{State{T, P}}, ) where {T, P} return ( - belief = StoredBelief{T, P}(; statekind = T()), + belief = StoredHomotopyBelief{T, P}(; statekind = T()), separator = Symbol[], initialized = false, observability = Float64[], @@ -189,12 +257,12 @@ function StructUtils.fielddefaults( end refMeans(state::State) = state.belief.means -refCovariances(state::State) = state.belief.covariances +refCovariances(state::State) = state.belief.shapes refWeights(state::State) = state.belief.weights refPoints(state::State) = state.belief.points -refBandwidth(state::State) = state.belief.bandwidth - -getDensityKind(state::State) = state.belief.densitykind +refBandwidth(state::State) = state.belief.bandwidths[1] +refBandwidths(state::State) = state.belief.bandwidths +getTopologyKind(state::State) = state.belief.topologykind # we can also do somthing like this: function getComponent(state::State, i) diff --git a/src/entities/equality.jl b/src/entities/equality.jl index 3a5eeb14..72f38f31 100644 --- a/src/entities/equality.jl +++ b/src/entities/equality.jl @@ -20,7 +20,7 @@ implement compare if needed. const GeneratedCompareUnion = Union{ Agent, Graphroot, - StoredBelief, + StoredHomotopyBelief, State, Blobentry, Bloblet, diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index d72c4abf..a429d2ce 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -26,8 +26,19 @@ getGraphLabel(dfg::AbstractDFG) = getLabel(getGraph(dfg)) """ $(SIGNATURES) + +!!! warning "Deprecated" + `getSolverParams(dfg)` is deprecated in DFG v0.29 Pass `SolverParams` directly + to `solveTree!()` as a keyword argument instead. """ -getSolverParams(dfg::AbstractDFG) = dfg.solverParams +function getSolverParams(dfg::AbstractDFG) + Base.depwarn( + "getSolverParams(dfg) is deprecated. SolverParams will be removed from the DFG object. " * + "Pass SolverParams directly to solveTree!() as a keyword argument instead.", + :getSolverParams, + ) + return dfg.solverParams +end """ $(SIGNATURES) diff --git a/src/services/compare.jl b/src/services/compare.jl index aa6383bc..b8b2d3be 100644 --- a/src/services/compare.jl +++ b/src/services/compare.jl @@ -155,7 +155,7 @@ end #Compare State function compare(a::State, b::State) refPoints(a) != refPoints(b) && @debug("val is not equal") === nothing && return false - refBandwidth(a) != refBandwidth(b) && + refBandwidths(a) != refBandwidths(b) && @debug("bw is not equal") === nothing && return false # a.BayesNetOutVertIDs != b.BayesNetOutVertIDs && diff --git a/src/services/print.jl b/src/services/print.jl index 87e6553e..e66967ee 100644 --- a/src/services/print.jl +++ b/src/services/print.jl @@ -59,9 +59,9 @@ function printVariable( println(ioc, " marginalized: ", isMarginalized(vert, :default)) println(ioc, " size bel. samples: ", size(vnd.belief.points)) print(ioc, " kde bandwidths: ") - bandwidth = vnd.belief.bandwidth - if !isnothing(bandwidth) && 0 < length(bandwidth) - println(ioc, round.(bandwidth; digits = 4)) + bws = vnd.belief.bandwidths + if !isempty(bws) + println(ioc, round.(bws[1]; digits = 4)) end printstyled(ioc, " VNDs: "; bold = true) println(ioc, solk[smsk], 4 < lsolk ? "..." : "") diff --git a/test/testSerializingVariables.jl b/test/testSerializingVariables.jl index 6f4691e1..f4de87b5 100644 --- a/test/testSerializingVariables.jl +++ b/test/testSerializingVariables.jl @@ -46,27 +46,27 @@ function make_test_variable() end @testset "Serializing Variables" begin - @testset "StoredBelief round-trip" begin - bel = DFG.StoredBelief(Pose{3}()) + @testset "StoredHomotopyBelief round-trip" begin + bel = DFG.StoredHomotopyBelief(Pose{3}()) push!(bel.points, DFG.getPointIdentity(Pose{3}())) G = DFG.getManifold(Pose{3}()) push!(bel.points, rand(G, ArrayPartition)) jstr = JSON.json(bel; pretty = true, style = DFG.DFGJSONStyle()) - parsed = JSON.parse(jstr, DFG.StoredBelief; style = DFG.DFGJSONStyle()) + parsed = JSON.parse(jstr, DFG.StoredHomotopyBelief; style = DFG.DFGJSONStyle()) @test bel == parsed end - @testset "GaussianDensityKind round-trip" begin + @testset "RootsOnlyTopology round-trip" begin dim = DFG.getDimension(Pose{3}()) - bel = DFG.StoredBelief( - DFG.GaussianDensityKind(), + bel = DFG.StoredHomotopyBelief( + DFG.RootsOnlyTopology(), Pose{3}(); means = [DFG.getPointIdentity(Pose{3}())], - covariances = [diagm(ones(dim))], + shapes = [diagm(ones(dim))], ) jstr = JSON.json(bel; pretty = true, style = DFG.DFGJSONStyle()) - parsed = JSON.parse(jstr, DFG.StoredBelief; style = DFG.DFGJSONStyle()) + parsed = JSON.parse(jstr, DFG.StoredHomotopyBelief; style = DFG.DFGJSONStyle()) @test bel == parsed end From 965f582e4c81e39ade2b327380c7c95ce6819edd Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 28 Apr 2026 13:52:24 +0200 Subject: [PATCH 2/3] only print bw when not empty --- src/services/print.jl | 4 ++-- test/interfaceTests.jl | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/services/print.jl b/src/services/print.jl index e66967ee..9dc61a81 100644 --- a/src/services/print.jl +++ b/src/services/print.jl @@ -58,9 +58,9 @@ function printVariable( println(ioc, " initialized: ", isInitialized(vert, :default)) println(ioc, " marginalized: ", isMarginalized(vert, :default)) println(ioc, " size bel. samples: ", size(vnd.belief.points)) - print(ioc, " kde bandwidths: ") - bws = vnd.belief.bandwidths + bws = refBandwidths(vnd) if !isempty(bws) + print(ioc, " kde bandwidths: ") println(ioc, round.(bws[1]; digits = 4)) end printstyled(ioc, " VNDs: "; bold = true) diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index e7d771c2..4b241100 100644 --- a/test/interfaceTests.jl +++ b/test/interfaceTests.jl @@ -76,7 +76,7 @@ end @test occursin(r"VariableDFG", varstr) @test occursin(r"timestamp", varstr) @test occursin(r"label", varstr) - @test occursin(r"bandwidths", varstr) + # @test occursin(r"bandwidths", varstr) # == "VariableDFG{TestVariableType1}\nlabel: a\ntags: Set([:VARIABLE, :POSE])\nsize marginal samples: (1, 1)\nkde bandwidths: [0.0]\nNo PPEs\n" @test printFactor(iobuf, fac1; skipfields = [:timestamp]) === nothing From 190ea054c835784ba35a4a55fa3ff5ea9b117107 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 28 Apr 2026 16:12:28 +0200 Subject: [PATCH 3/3] Fix bugs in deprecated functions IIF still use --- src/Deprecated.jl | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index bfe272ec..375f279e 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -569,8 +569,10 @@ function deepcopyGraph( graphLabel::Symbol = Symbol(getGraphLabel(sourceDFG), "_cp_$(string(uuid4())[1:6])"), kwargs..., ) where {T <: AbstractDFG} + sp = getSolverParams(sourceDFG) + sp_kw = sp isa fieldtype(T, :solverParams) ? (; solverParams = sp) : (;) destDFG = T(; - solverParams = getSolverParams(sourceDFG), + sp_kw..., graph = sourceDFG.graph, agents = deepcopy(sourceDFG.agents), graphLabel, @@ -710,9 +712,15 @@ function findShortestPathDijkstra( dfg, restrict_labels, ) - return findPath(subdfg, from, to).path + result = try + findPath(subdfg, from, to) + catch ex + ex isa DFG.LabelNotFoundError ? nothing : rethrow() + end + return result === nothing ? Symbol[] : result.path else - return findPath(dfg, from, to).path + result = findPath(dfg, from, to) + return result === nothing ? Symbol[] : result.path end end