From 664cc63214fffcfcc31c3426d7910d3d68149823 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 4 Mar 2026 16:22:16 +0200 Subject: [PATCH 1/9] Refactor State and State Serialization --- src/Deprecated.jl | 4 +- src/DistributedFactorGraphs.jl | 2 - src/entities/DFGVariable.jl | 262 ++++++++++++++++------- src/serialization/DFGStructStyles.jl | 32 +++ src/serialization/PackedSerialization.jl | 2 +- src/serialization/StateSerialization.jl | 147 ++++++------- src/services/CompareUtils.jl | 9 +- src/services/CustomPrinting.jl | 8 +- test/GraphsDFGSummaryTypes.jl | 81 ++----- test/compareTests.jl | 12 +- test/consol_DataEntryBlobTests.jl | 2 +- test/runtests.jl | 4 + test/testBlocks.jl | 127 ++++++----- test/testSerializingVariables.jl | 104 +++++++++ 14 files changed, 491 insertions(+), 305 deletions(-) create mode 100644 test/testSerializingVariables.jl diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 3c468586..72a4ff82 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -84,7 +84,7 @@ end function getTypeFromSerializationModule(::AbstractString) return error( - "getTypeFromSerializationModule is obsolete, use DFG.parseVariableType or IIF.getTypeFromSerializationModule.", + "getTypeFromSerializationModule is obsolete, use DFG.parseStateKind or IIF.getTypeFromSerializationModule.", ) end @@ -129,7 +129,7 @@ end function getVariableTypeName(v::VariableSummary) Base.depwarn("getVariableTypeName is deprecated.", :getVariableTypeName) - return v.statetype + return v.statekindsymbol end function getMetadata(dfg::AbstractDFG, label::Symbol, key::Symbol) diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index 4bca4ed6..162bcbca 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -474,10 +474,8 @@ const unstable_functions::Vector{Symbol} = [ :printNode, :plotDFG, :packBlob, - :packState, :hasTags, :unpackBlob, - :unpackState, :emptyTags!, :ls, :lsf, diff --git a/src/entities/DFGVariable.jl b/src/entities/DFGVariable.jl index 25165fad..a7af827b 100644 --- a/src/entities/DFGVariable.jl +++ b/src/entities/DFGVariable.jl @@ -5,6 +5,104 @@ abstract type AbstractStateType{N} end const StateType = AbstractStateType +##============================================================================== +## BeliefRepresentation +##============================================================================== +abstract type AbstractDensityKind end + +"""Single Gaussian (mean + covariance).""" +struct GaussianDensityKind <: AbstractDensityKind end + +"""Kernel density / particle-based (points + shared bandwidth).""" +struct NonparametricDensityKind <: AbstractDensityKind end + +"""Homotopy between particles and Gaussian.""" +struct HomotopyDensityKind <: AbstractDensityKind end + +function StructUtils.lower(::StructUtils.StructStyle, p::AbstractDensityKind) + return StructUtils.lower(Packed(p)) +end +@choosetype AbstractDensityKind resolvePackedType + +# TODO naming? Density, DensityRepresentation, BeliefRepresentation, BeliefState, etc? +# TODO flatten in State? likeley not for easier serialization of points. +@kwdef struct BeliefRepresentation{T <: StateType, P} + statekind::T = T()# NOTE duplication for serialization, TODO maybe only in State and therefore belief cannot deserialize seperately. + """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`." + weights::Vector{Float64} = Float64[] + + #--- Non-parametric / Homotopy leaves --- + """On-manifold sample points. For KDE/HomotopyDensity, these are the leaf kernel means.""" + 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 + +# # we can also do somthing like this: +# getComponent(state::State, i) = ( +# mean = refMeans(state)[i], +# cov = refCovariances(state)[i], +# weight = refWeights(state)[i], +# ) + +JSON.omit_empty(::Type{<:BeliefRepresentation}) = true + +function BeliefRepresentation(T::AbstractStateType) + return BeliefRepresentation{typeof(T), getPointType(T)}(; statekind = T) +end + +function BeliefRepresentation(::NonparametricDensityKind, T::AbstractStateType; kwargs...) + return BeliefRepresentation{typeof(T), getPointType(T)}(; + statekind = T, + densitykind = NonparametricDensityKind(), + bandwidth = zeros(getDimension(T), getDimension(T)), + kwargs..., + ) +end + +function BeliefRepresentation(::GaussianDensityKind, T::AbstractStateType; kwargs...) + return BeliefRepresentation{typeof(T), getPointType(T)}(; + statekind = T, + densitykind = GaussianDensityKind(), + bandwidth = nothing, + kwargs..., + ) +end + +function StructUtils.fielddefaults( + ::StructUtils.StructStyle, + ::Type{BeliefRepresentation{T, P}}, +) where {T, P} + return ( + statekind = T(), + densitykind = NonparametricDensityKind(), + means = P[], + covariances = Matrix{Float64}[], + weights = Float64[], + points = P[], + bandwidth = nothing, + ) +end + +function resolveBeliefRepresentationType(lazyobj) + statekind = liftStateKind(lazyobj.statekind[]) + return BeliefRepresentation{typeof(statekind), getPointType(statekind)} +end + +@choosetype BeliefRepresentation resolveBeliefRepresentationType + ##============================================================================== ## State ##============================================================================== @@ -16,67 +114,51 @@ Data container for solver-specific data. --- T: Variable type, such as Position1, or RoME.Pose2, etc. P: Variable point type, the type of the manifold point. -N: Manifold dimension. Fields: $(TYPEDFIELDS) """ -@kwdef mutable struct State{T <: StateType, P, N} - """ - Identifier associated with this State object. - """ +@kwdef mutable struct State{T <: StateType, P} + """Identifier associated with this State object.""" label::Symbol # TODO renamed from solveKey + """Singleton type for the state, eg. Pose{3}(), Position{2}(), etc. Used for dispatch and serialization.""" + statekind::T = T() """ - Vector of on-manifold points used to represent a ManifoldKernelDensity (or parametric) belief. - """ - val::Vector{P} = Vector{P}() + Generic Belief representation for this state, including the discriminator for which representation is active + and the associated fields for each representation kind. """ - Common kernel bandwith parameter used with ManifoldKernelDensity, see field `covar` for the parametric covariance. - """ - bw::Matrix{Float64} = zeros(0, 0) - "Parametric (Gaussian) covariance." - covar::Vector{SMatrix{N, N, Float64}} = - SMatrix{getDimension(T), getDimension(T), Float64}[] - # BayesNetOutVertIDs::Vector{Symbol} = Symbol[] #TODO looks unused? - - # dims::Int = getDimension(T) #TODO should we deprecate in favor of N - # """ - # Flag used by junction (Bayes) tree construction algorithm to know whether this variable has yet been included in the tree construction. - # """ - # eliminated::Bool = false - # BayesNetVertID::Symbol = :NOTHING # Union{Nothing, } #TODO deprecate + belief::BeliefRepresentation{T, P} = BeliefRepresentation{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. - """ + """False if initial numerical values are not yet available or stored values are not ready for further processing yet.""" initialized::Bool = false - """ - Stores the amount information (per measurement dimension) captured in each coordinate dimension. - """ - observability::Vector{Float64} = Float64[]#zeros(getDimension(T)) #TODO renamed from infoPerCoord - """ - Should this state be treated as marginalized in inference computations. - """ - marginalized::Bool = false #TODO renamed from ismargin - # """ - # Should this variable solveKey always be kept fluid and not be automatically marginalized. - # """ - # dontmargin::Bool = false - """ - How many times has a solver updated this state estimate. - """ - solves::Int = 0 # TODO renamed from solvedCount - # """ - # Future proofing field for when more multithreading operations on graph nodes are implemented, these conditions are meant to be used for atomic write transactions to this VND. - # """ - # events::Dict{Symbol, Threads.Condition} = Dict{Symbol, Threads.Condition}() - # - statetype::Symbol = Symbol(stringVariableType(T())) + """Stores the amount information (per measurement dimension) captured in each coordinate dimension.""" + observability::Vector{Float64} = Float64[]#zeros(getDimension(T)) #TODO renamed from infoPerCoord in v0.29 + """Should this state be treated as marginalized in inference computations.""" + 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 cache that can be used for caching HomotopyDensity (StateCache or BeliefCache) + # abstract type AbstractStateCache end + # const StateCache = AbstractStateCache + # solvercache::Base.RefValue{<:StateCache} = Ref{StateCache}() & (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) +# covar::Vector{Matrix{Float64}} = Matrix{Float64}[] +# BayesNetOutVertIDs::Vector{Symbol} = Symbol[] +# dims::Int = getDimension(T) +# eliminated::Bool = false +# BayesNetVertID::Symbol = :NOTHING # Union{Nothing, } +# events::Dict{Symbol, Threads.Condition} = Dict{Symbol, Threads.Condition}() +# dontmargin::Bool = false + ##------------------------------------------------------------------------------ ## Constructors function State{T}(; kwargs...) where {T <: StateType} - return State{T, getPointType(T), getDimension(T)}(; kwargs...) + return State{T, getPointType(T)}(; kwargs...) end function State(label::Symbol, variableType::StateType; kwargs...) return State{typeof(variableType)}(; label, kwargs...) @@ -89,13 +171,34 @@ function State(state::State; kwargs...) ) end -StructUtils.structlike(::Type{<:State}) = false -StructUtils.lower(state::State) = DFG.packState(state) -StructUtils.lift(::Type{<:State}, obj) = DFG.unpackState(obj) +# TODO consider omitting empty fields in State, needs constructor that can take nothing. +# JSON.omit_empty(::Type{<:State}) = true + +# Field defaults and tags for State, not through @kwarg macro due to error with State{T, P, N} +function StructUtils.fielddefaults( + ::StructUtils.StructStyle, + ::Type{State{T, P}}, +) where {T, P} + return ( + belief = BeliefRepresentation{T, P}(; statekind = T()), + separator = Symbol[], + initialized = false, + observability = Float64[], + marginalized = false, + solves = 0, + statekind = T(), + ) +end + +refMeans(state::State) = state.belief.means +refCovariances(state::State) = state.belief.covariances +refWeights(state::State) = state.belief.weights +refPoints(state::State) = state.belief.points +refBandwidth(state::State) = state.belief.bandwidth ##------------------------------------------------------------------------------ ## States - OrderedDict{Symbol, State} -const States = OrderedDict{Symbol, State{T, P, N}} where {T <: AbstractStateType, P, N} +const States = OrderedDict{Symbol, State{T, P}} where {T <: AbstractStateType, P} StructUtils.dictlike(::Type{<:States}) = false StructUtils.structlike(::Type{<:States}) = false @@ -103,18 +206,26 @@ StructUtils.arraylike(::Type{<:States}) = false function StructUtils.lower(states::States) return map(collect(values(states))) do (state) - return StructUtils.lower(state) + return state end end +# Lazy lift: receives the LazyValue directly and parses each element lazily. +# States is lowered as a JSON array, so on deserialization StructUtils sees a +# Vector and needs lift to reconstruct the OrderedDict. Dispatching on +# JSON.LazyValue keeps every element lazy so nested matrices parse correctly +# (avoids StructUtils.MultiDimClosure receiving String keys from eager objects). function StructUtils.lift( - ::StructUtils.StructStyle, + style::StructUtils.StructStyle, S::Type{<:States{T}}, - json_vector::Vector, + lazystates::JSON.LazyValue, + tags::NamedTuple = (;), ) where {T} + StateT = State{T, getPointType(T)} states = S() - foreach(json_vector) do obj - return push!(states, Symbol(obj.label) => StructUtils.make(State{T}, obj)) + StructUtils.applyeach(lazystates) do i, lazy_element + state = JSON.parse(lazy_element, StateT; style = style) + return push!(states, state.label => state) end return states, nothing end @@ -145,7 +256,7 @@ Complete variable structure for a DistributedFactorGraph variable. Fields: $(TYPEDFIELDS) """ -@kwdef struct VariableDFG{T <: StateType, P, N} <: AbstractGraphVariable +@kwdef struct VariableDFG{T <: StateType, P} <: AbstractGraphVariable """Variable label, e.g. :x1. Accessor: [`getLabel`](@ref)""" label::Symbol @@ -160,7 +271,7 @@ $(TYPEDFIELDS) tags::Set{Symbol} = Set{Symbol}() """Dictionary of state data. May be a subset of all solutions if a solver label was specified in the get call. Accessors: [`addState!`](@ref), [`mergeState!`](@ref), and [`deleteState!`](@ref)""" - states::OrderedDict{Symbol, State{T, P, N}} = OrderedDict{Symbol, State{T, P, N}}() #NOTE field renamed from solverDataDict in v0.29 + states::OrderedDict{Symbol, State{T, P}} = OrderedDict{Symbol, State{T, P}}() #NOTE field renamed from solverDataDict in v0.29 """Dictionary of small data associated with this variable. Accessors: [`getBloblet`](@ref), [`addBloblet!`](@ref)""" bloblets::Bloblets = Bloblets() #NOTE changed from smallData in v0.29 @@ -170,8 +281,8 @@ $(TYPEDFIELDS) """Solvable flag for the variable. Accessors: [`getSolvable`](@ref), [`setSolvable!`](@ref)""" solvable::Base.RefValue{Int} = Ref{Int}(1) #& (lower = getindex,) - statetype::Symbol = Symbol(stringVariableType(T())) - # TODO autotype or version and statetype + statekind::T = T() + # TODO autotype or version and statekind _autotype::Nothing = nothing #& (name = :type, lower = _ -> TypeMetadata(VariableDFG)) end version(::Type{<:VariableDFG}) = v"0.29" @@ -180,12 +291,12 @@ refStates(v::VariableDFG) = v.states #NOTE fielddefaults and fieldtags not through @kwarg macro due to error with State{T, P, N} function StructUtils.fielddefaults( ::StructUtils.StructStyle, - ::Type{VariableDFG{T, P, N}}, -) where {T, P, N} + ::Type{VariableDFG{T, P}}, +) where {T, P} return ( timestamp = now_tdz(), tags = Set{Symbol}(), - # states = OrderedDict{Symbol, State{T, P, N}}(), + states = OrderedDict{Symbol, State{T, P}}(), bloblets = Bloblets(), blobentries = Blobentries(), solvable = Ref(1), @@ -200,12 +311,12 @@ function StructUtils.fieldtags(::StructUtils.StructStyle, ::Type{<:VariableDFG}) ) end -function resolveVariableType(lazyobj) - T = parseVariableType(lazyobj.statetype[]) - return VariableDFG{T, getPointType(T), getDimension(T)} +function resolveVariableDFGType(lazyobj) + statekind = liftStateKind(lazyobj.statekind[]) + return VariableDFG{typeof(statekind), getPointType(statekind)} end -@choosetype VariableDFG resolveVariableType +@choosetype VariableDFG resolveVariableDFGType # JSON.omit_empty(::DistributedFactorGraphs.DFGJSONStyle, ::Type{<:VariableDFG}) = true @@ -220,7 +331,7 @@ The default VariableDFG constructor. #IIF like contruction helper for VariableDFG function VariableDFG( label::Symbol, - statetype::Union{T, Type{T}}; + statekind::Union{T, Type{T}}; tags::Union{Set{Symbol}, Vector{Symbol}} = Set{Symbol}(), timestamp::Union{TimeDateZone, ZonedDateTime} = now_tdz(), solvable::Union{Int, Base.RefValue{Int}} = Ref{Int}(1), @@ -247,9 +358,8 @@ function VariableDFG( end union!(tags, [:VARIABLE]) - N = getDimension(T) P = getPointType(T) - return VariableDFG{T, P, N}(; label, solvable, tags, timestamp, kwargs...) + return VariableDFG{T, P}(; label, solvable, tags, timestamp, kwargs...) end function VariableDFG(label::Symbol, state::State; kwargs...) @@ -301,7 +411,7 @@ $(TYPEDFIELDS) Accessors: [`listTags`](@ref), [`mergeTags!`](@ref), and [`deleteTags!`](@ref)""" tags::Set{Symbol} """Symbol for the state type for the underlying variable.""" - statetype::Symbol + statekind::AbstractStateType """Dictionary of large data associated with this variable. Accessors: [`addBlobentry!`](@ref), [`getBlobentry`](@ref), [`mergeBlobentry!`](@ref), and [`deleteBlobentry!`](@ref)""" blobentries::Blobentries @@ -337,13 +447,7 @@ end ##============================================================================== function VariableSummary(v::VariableDFG{T}) where {T} - return VariableSummary( - v.label, - v.timestamp, - copy(v.tags), - Symbol(stringVariableType(T())), - copy(v.blobentries), - ) + return VariableSummary(v.label, v.timestamp, copy(v.tags), T(), copy(v.blobentries)) end function VariableSkeleton(v::AbstractGraphVariable) diff --git a/src/serialization/DFGStructStyles.jl b/src/serialization/DFGStructStyles.jl index 4959ba79..4e061041 100644 --- a/src/serialization/DFGStructStyles.jl +++ b/src/serialization/DFGStructStyles.jl @@ -1,13 +1,45 @@ struct DFGJSONStyle <: JSON.JSONStyle end +# Base.RefValue{Int} serialization StructUtils.structlike(::DFGJSONStyle, ::Type{Base.RefValue{Int}}) = false StructUtils.lower(::DFGJSONStyle, x::Base.RefValue{Int}) = x[] function StructUtils.lift(::DFGJSONStyle, ::Type{Base.RefValue{Int}}, x::Integer) return Ref{Int}(x), nothing end +# TimeDateZone serialization StructUtils.structlike(::DFGJSONStyle, ::Type{TimeDateZone}) = false StructUtils.lower(::DFGJSONStyle, x::TimeDateZone) = string(x) function StructUtils.lift(::DFGJSONStyle, ::Type{TimeDateZone}, x::AbstractString) return TimeDateZone(x), nothing end + +# SArray serialization overloads +StructUtils.lower(::DFGJSONStyle, x::SArray) = x + +function StructUtils.makearray( + style::DFGJSONStyle, + ::Type{<:SArray{S, T}}, + source, +) where {S, T} + v, st = StructUtils.makearray(style, Vector{T}, source) + return SArray{S, T}(v), st +end + +# ArrayPartition serialization overloads +StructUtils.arraylike(::DFGJSONStyle, ::Type{<:ArrayPartition}) = false +StructUtils.structlike(::DFGJSONStyle, ::Type{<:ArrayPartition}) = false + +StructUtils.lower(::DFGJSONStyle, ap::ArrayPartition) = ap.x + +function StructUtils.lift( + ::DFGJSONStyle, + ::Type{AP}, + source::Vector, +) where {T, S, AP <: ArrayPartition{T, S}} + ptypes = fieldtypes(S) + parts = ntuple(length(ptypes)) do i + return ptypes[i](source[i]) + end + return AP(parts), nothing +end diff --git a/src/serialization/PackedSerialization.jl b/src/serialization/PackedSerialization.jl index 8ccd8010..3abdee34 100644 --- a/src/serialization/PackedSerialization.jl +++ b/src/serialization/PackedSerialization.jl @@ -67,7 +67,7 @@ function resolvePackedType(obj::JSON.Object) return Packed{getfield(pkg, Symbol(type.name))} end -function resolveType(obj::DFG.JSON.Object) +function resolveType(obj::JSON.Object) type = obj.type pkg = Base.require(Main, Symbol(type.pkg)) if !isdefined(Main, Symbol(type.pkg)) diff --git a/src/serialization/StateSerialization.jl b/src/serialization/StateSerialization.jl index 4067c530..e018289f 100644 --- a/src/serialization/StateSerialization.jl +++ b/src/serialization/StateSerialization.jl @@ -1,5 +1,53 @@ -function stringVariableType(varT::AbstractStateType{N}) where {N} +function lowerStateKind(varT::AbstractStateType{N}) where {N} + typemeta = TypeMetadata(typeof(varT)) + if N == Any + return typemeta + # return string(parentmodule(T), ".", nameof(T)) + elseif N isa Integer + return TypeMetadata( + typemeta.pkg, + Symbol(typemeta.name, "{", N, "}"), + typemeta.version, + ) + # return string(parentmodule(T), ".", nameof(T), "{", join(N, ","), "}") + else + throw( + SerializationError( + "Serializing Variable State type only supports an integer parameter, got '$(T)'.", + ), + ) + end +end + +#NOTE Cannot resolve with `resolveType` because of the N parameter +# tried resolveType(JSON.Object(:type=>obj)) +function liftStateKind(type::DFG.JSON.Object) + pkg = Base.require(Main, Symbol(type.pkg)) + if !isdefined(Main, Symbol(type.pkg)) + throw(SerializationError("Module $(pkg) is available, but not loaded in `Main`.")) + end + m = match(r"{(\d+)}", type.name) + if !isnothing(m) #parameters in type + param = parse(Int, m[1]) + typeString = type.name[1:(m.offset - 1)] + return getfield(pkg, Symbol(typeString)){param}() + else + typeString = type.name + return getfield(pkg, Symbol(typeString))() + end +end + +StructUtils.structlike(::Type{<:AbstractStateType}) = false +StructUtils.lower(T::AbstractStateType) = lowerStateKind(T) +StructUtils.lift(::Type{AbstractStateType}, s) = liftStateKind(s) + +##============================================================================== +## OLD State Packing and unpacking +##============================================================================== + +# State Kind is handled seperately because it includes the N parameter. +function stringStateKind(varT::AbstractStateType{N}) where {N} T = typeof(varT) if N == Any return string(parentmodule(T), ".", nameof(T)) @@ -14,7 +62,7 @@ function stringVariableType(varT::AbstractStateType{N}) where {N} end end -function parseVariableType(_typeString::AbstractString) +function parseStateKind(_typeString::AbstractString) m = match(r"{(\d+)}", _typeString) if !isnothing(m) #parameters in type param = parse(Int, m[1]) @@ -46,16 +94,13 @@ function parseVariableType(_typeString::AbstractString) if isnothing(param) # no parameters, just return the type - return subtype + return subtype() else # return the type with parameters - return subtype{param} + return subtype{param}() end end -##============================================================================== -## State Packing and unpacking -##============================================================================== # Old PackedState struct fields # id::Union{UUID, Nothing} # vecval::Vector{Float64} @@ -78,73 +123,10 @@ end # covar::Vector{Float64} # _version::VersionNumber = _getDFGVersion() -# returns a named tuple until State serialization is fully consolidated -function packState(state::State{T}) where {T <: StateType} - castval = if 0 < length(state.val) - precast = getCoordinates.(T, state.val) - @cast castval[i, j] := precast[j][i] - castval - else - zeros(1, 0) - end - - length(state.covar) > 1 && @warn( - "Packing of more than one parametric covariance is NOT supported yet, only packing first." - ) - - return ( - label = state.label, - vecval = castval[:], - dimval = size(castval, 1), - vecbw = state.bw[:], - dimbw = size(state.bw, 1), - separator = state.separator, - statetype = stringVariableType(getStateKind(state)), - initialized = state.initialized, - observability = state.observability, - marginalized = state.marginalized, - solves = state.solves, - covar = isempty(state.covar) ? Float64[] : vec(state.covar[1]), - version = version(State), - ) -end - -function unpackState(obj) - T = parseVariableType(obj.statetype) - r3 = obj.dimval - c3 = r3 > 0 ? floor(Int, length(obj.vecval) / r3) : 0 - M3 = reshape(obj.vecval, r3, c3) - @cast val_[j][i] := M3[i, j] - vals = Vector{getPointType(T)}(undef, length(val_)) - # vals = getPoint.(T, val_) - for (i, v) in enumerate(val_) - vals[i] = getPoint(T, v) - end - - r4 = obj.dimbw - c4 = r4 > 0 ? floor(Int, length(obj.vecbw) / r4) : 0 - BW = reshape(obj.vecbw, r4, c4) - - # - N = getDimension(T) - return State{T, getPointType(T), N}(; - label = Symbol(obj.label), - val = vals, - bw = BW, - #TODO only one covar is currently supported in packed VND - covar = isempty(obj.covar) ? SMatrix{N, N, Float64}[] : [obj.covar], - separator = Symbol.(obj.separator), - initialized = obj.initialized, - observability = obj.observability, - marginalized = obj.marginalized, - solves = obj.solves, - ) -end - function unpackOldState(d) @debug "Dispatching conversion packed variable -> variable for type $(string(d.variableType))" # Figuring out the variableType - T = parseVariableType(d.variableType) + T = parseStateKind(d.variableType) r3 = d.dimval c3 = r3 > 0 ? floor(Int, length(d.vecval) / r3) : 0 @@ -161,13 +143,22 @@ function unpackOldState(d) BW = reshape(d.vecbw, r4, c4) # - N = getDimension(T) - return State{T, getPointType(T), N}(; - label = Symbol(d.solveKey), - val = vals, - bw = BW, - #TODO only one covar is currently supported in packed VND - covar = isempty(d.covar) ? SMatrix{N, N, Float64}[] : [d.covar], + label = Symbol(d.solveKey) + !isempty(d.covar) && error("covar field is not suppoted") + if label == :parametric + belief = + BeliefRepresentation(GaussianDensityKind(), T; means = vals, covariances = [BW]) + else + belief = BeliefRepresentation( + NonparametricDensityKind(), + T; + points = vals, + bandwidth = BW, + ) + end + return State{T, getPointType(T)}(; + label, + belief, separator = Symbol.(d.separator), initialized = d.initialized, observability = d.infoPerCoord, diff --git a/src/services/CompareUtils.jl b/src/services/CompareUtils.jl index adfaf1d0..1732a784 100644 --- a/src/services/CompareUtils.jl +++ b/src/services/CompareUtils.jl @@ -19,6 +19,7 @@ implement compare if needed. # Generate compares automatically for all in this union const GeneratedCompareUnion = Union{ + BeliefRepresentation, State, Blobentry, Bloblet, @@ -208,8 +209,10 @@ end #Compare State function compare(a::State, b::State) - a.val != b.val && @debug("val is not equal") === nothing && return false - a.bw != b.bw && @debug("bw is not equal") === nothing && return false + refPoints(a) != refPoints(b) && @debug("val is not equal") === nothing && return false + refBandwidth(a) != refBandwidth(b) && + @debug("bw is not equal") === nothing && + return false # a.BayesNetOutVertIDs != b.BayesNetOutVertIDs && # @debug("BayesNetOutVertIDs is not equal") === nothing && # return false @@ -255,7 +258,7 @@ function compareVariable( # skiplist = union([:states, :atzone, :inzone, :blobentries, :bloblets], skip) TP = compareAll(A, B; skip = skiplist, show = show) - varskiplist = skipsamples ? [:val; :bw] : Symbol[] + varskiplist = skipsamples ? [:belief] : Symbol[] skiplist = union([:variableType;], varskiplist) union!(skiplist, skip) # TP = TP && compareAll(A.states, B.states; skip = skiplist, show = show) diff --git a/src/services/CustomPrinting.jl b/src/services/CustomPrinting.jl index efd3ec9e..91bb7dc8 100644 --- a/src/services/CustomPrinting.jl +++ b/src/services/CustomPrinting.jl @@ -57,15 +57,15 @@ function printVariable( println(ioc, " :default <-- State") println(ioc, " initialized: ", isInitialized(vert, :default)) println(ioc, " marginalized: ", isMarginalized(vert, :default)) - println(ioc, " size bel. samples: ", size(vnd.val)) + println(ioc, " size bel. samples: ", size(vnd.belief.points)) print(ioc, " kde bandwidths: ") - 0 < length(vnd.bw) ? println(ioc, round.(vnd.bw[1]; digits = 4)) : nothing + if 0 < length(vnd.belief.bandwidth) + println(ioc, round.(vnd.belief.bandwidth; digits = 4)) + end printstyled(ioc, " VNDs: "; bold = true) println(ioc, solk[smsk], 4 < lsolk ? "..." : "") end println(ioc, " # Blobentries: (", length(listBlobentries(vert)), ")") - printstyled(ioc, " VariableType: "; color = :blue, bold = true) - println(ioc, vari) # println(ioc, "kde max: $(round.(getKDEMax(getBelief(vnd)),digits=4))") # println(ioc, "kde max: $(round.(getKDEMax(getBelief(vnd)),digits=4))") else diff --git a/test/GraphsDFGSummaryTypes.jl b/test/GraphsDFGSummaryTypes.jl index f4c7efcc..23f3474f 100644 --- a/test/GraphsDFGSummaryTypes.jl +++ b/test/GraphsDFGSummaryTypes.jl @@ -2,46 +2,17 @@ # VARTYPE = VariableSummary # FACTYPE = FactorSummary -dfg = GraphsDFG{NoSolverParams, VARTYPE, FACTYPE}() -function DistributedFactorGraphs.VariableSummary(label::Symbol) - return VariableSummary( - label, - TimeDateZone("2025-11-13T15:21:57.474125421+01:00"), - Set{Symbol}(), - :Pose2, - DFG.Blobentries(), - ) -end - -function DistributedFactorGraphs.VariableSummary(label::Symbol, ::State{T}) where {T} - return VariableSummary( - label, - TimeDateZone("2025-11-13T15:21:57.474125421+01:00"), - Set{Symbol}(), - Symbol(T), - DFG.Blobentries(), - ) -end - -function DistributedFactorGraphs.VariableSkeleton(label::Symbol, args...) - return VariableSkeleton(label) -end - -function DistributedFactorGraphs.VariableSkeleton(label::Symbol, ::State{T}) where {T} - return VariableSkeleton(label, Set{Symbol}()) -end +# generate variables and factors +var1, var2, var3, vorphan, v1_tags = DFGVariableSCA() +fac0, fac1, fac2 = DFGFactorSCA() dfg = GraphsDFG{NoSolverParams, VARTYPE, FACTYPE}() -v1 = VARTYPE(:a) -v2 = VARTYPE(:b) -v3 = VARTYPE(:c) -f0 = FACTYPE(:af1, [:a]) -f1 = FACTYPE(:abf1, [:a, :b]) -f2 = FACTYPE(:bcf1, [:b, :c]) - -union!(v1.tags, [:VARIABLE, :POSE]) -union!(v2.tags, [:VARIABLE, :LANDMARK]) -union!(f1.tags, [:FACTOR]) +v1 = VARTYPE(var1) +v2 = VARTYPE(var2) +v3 = VARTYPE(var3) +f0 = FACTYPE(fac0) +f1 = FACTYPE(fac1) +f2 = FACTYPE(fac2) if false #TODO add to tests @@ -105,42 +76,22 @@ end @testset "Adjacency Matrices" begin fg = GraphsDFG{NoSolverParams, VARTYPE, FACTYPE}() - addVariable!(fg, VARTYPE(:a)) - addVariable!(fg, VARTYPE(:b)) - addFactor!(fg, FACTYPE(:abf1, [:a, :b])) - addVariable!(fg, VARTYPE(:orphan)) + addVariable!(fg, VARTYPE(var1)) + addVariable!(fg, VARTYPE(var2)) + addFactor!(fg, FACTYPE(fac1)) + addVariable!(fg, VARTYPE(vorphan)) AdjacencyMatricesTestBlock(fg) end @testset "Getting Neighbors" begin - GettingNeighbors( - GraphsDFG{NoSolverParams, VARTYPE, FACTYPE}; - VARTYPE = VARTYPE, - FACTYPE = FACTYPE, - ) + GettingNeighbors(GraphsDFG; VARTYPE = VARTYPE, FACTYPE = FACTYPE) end @testset "Building Subgraphs" begin - BuildingSubgraphs( - GraphsDFG{NoSolverParams, VARTYPE, FACTYPE}; - VARTYPE = VARTYPE, - FACTYPE = FACTYPE, - ) -end - -@testset "Producing Dot Files" begin - ProducingDotFiles( - GraphsDFG{NoSolverParams, VARTYPE, FACTYPE}; - VARTYPE = VARTYPE, - FACTYPE = FACTYPE, - ) + BuildingSubgraphs(GraphsDFG; VARTYPE = VARTYPE, FACTYPE = FACTYPE) end @testset "Connectivity Test" begin - ConnectivityTest( - GraphsDFG{NoSolverParams, VARTYPE, FACTYPE}; - VARTYPE = VARTYPE, - FACTYPE = FACTYPE, - ) + ConnectivityTest(GraphsDFG; VARTYPE = VARTYPE, FACTYPE = FACTYPE) end diff --git a/test/compareTests.jl b/test/compareTests.jl index 165f405e..b9698b2c 100644 --- a/test/compareTests.jl +++ b/test/compareTests.jl @@ -13,10 +13,10 @@ vnd2 = deepcopy(vnd1) vnd3 = State(:default, TestVariableType2()) @test vnd1 == vnd2 -push!(vnd1.val, [1.0;]) -push!(vnd2.val, [1.0;]) +push!(DFG.refPoints(vnd1), [1.0;]) +push!(DFG.refPoints(vnd2), [1.0;]) @test vnd1 == vnd2 -vnd2.val[1] = [0.1;] +DFG.refPoints(vnd2)[1] = [0.1;] @test !(vnd1 == vnd2) @test !(vnd1 == vnd3) @@ -54,9 +54,9 @@ vnd3 = State(:default, TestVariableType2()) @test !compare(vnd1, vnd3) @test compare(vnd1, vnd2) -push!(vnd1.val, [1.0;]) -push!(vnd2.val, [1.0;]) +push!(DFG.refPoints(vnd1), [1.0;]) +push!(DFG.refPoints(vnd2), [1.0;]) @test compare(vnd1, vnd2) -vnd2.val[1][1] = 0.1 +DFG.refPoints(vnd2)[1] = [0.1;] @test !compare(vnd1, vnd2) @test !compare(vnd1, vnd3) diff --git a/test/consol_DataEntryBlobTests.jl b/test/consol_DataEntryBlobTests.jl index 8b065756..748b02f6 100644 --- a/test/consol_DataEntryBlobTests.jl +++ b/test/consol_DataEntryBlobTests.jl @@ -19,7 +19,7 @@ testDFGAPI = GraphsDFG ## DataEntry Blobs ##============================================================================== -dfg, verts, facs = connectivityTestGraph(testDFGAPI) +dfg, verts, facs = connectivityTestGraph(testDFGAPI, VariableDFG, FactorDFG) dataset1 = rand(UInt8, 1000) dataset2 = rand(UInt8, 1000) diff --git a/test/runtests.jl b/test/runtests.jl index 9a6c84b6..775c1cdc 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -26,6 +26,10 @@ DFG.@usingDFG true include("compareTests.jl") end + @testset "Testing Serialization of Variables" begin + include("testSerializingVariables.jl") + end + @testset "Testing GraphsDFG.FactorGraphs functions" begin include("FactorGraphsTests.jl") end diff --git a/test/testBlocks.jl b/test/testBlocks.jl index b8de27d7..42121a51 100644 --- a/test/testBlocks.jl +++ b/test/testBlocks.jl @@ -343,9 +343,9 @@ function DFGFactorSCA() obs = TestFunctorInferenceType1() - f1 = FactorCompute(f1_lbl, [:a, :b], obs; tags = f1_tags, solvable = 0) + f1 = FactorDFG(f1_lbl, [:a, :b], obs; tags = f1_tags, solvable = 0) - f2 = FactorCompute( + f2 = FactorDFG( :bcf1, [:b, :c], TestFunctorInferenceType1(); @@ -388,7 +388,7 @@ function DFGFactorSCA() @test getSolvable(f1) == 1 # create f0 here for a later timestamp - f0 = FactorCompute(:af1, [:a], obs_prior; tags = Set([:PRIOR])) + f0 = FactorDFG(:af1, [:a], obs_prior; tags = Set([:PRIOR])) @test DFG.calcDeltatime(f1, f2) isa Real #fill in undefined fields @@ -431,8 +431,8 @@ function VariablesandFactorsCRUD_SET!(fg, v1, v2, v3, f0, f1, f2) @test_throws LabelExistsError addFactor!(fg, f2) #TODO Graphs.jl, but look at refactoring absract @test_throws LabelExistsError addFactor!(fg, f2) - if f2 isa FactorCompute - f2_mod = FactorCompute( + if f2 isa FactorDFG + f2_mod = FactorDFG( f2.label, (:a,), f2.observation, @@ -506,7 +506,7 @@ function VariablesandFactorsCRUD_SET!(fg, v1, v2, v3, f0, f1, f2) @test isFactor(fg, :abf1) @test !isFactor(fg, :a) - if f0 isa FactorCompute + if f0 isa FactorDFG @test isPrior(fg, :af1) @test !isPrior(fg, :abf1) end @@ -528,7 +528,7 @@ function VariablesandFactorsCRUD_SET!(fg, v1, v2, v3, f0, f1, f2) end # simple broadcast test - if f0 isa FactorCompute + if f0 isa FactorDFG @test issetequal( getObservation.(fg, lsf(fg)), [TestFunctorInferenceType1(), TestAbstractPrior()], @@ -1228,9 +1228,9 @@ end # Now make a complex graph for connectivity tests function connectivityTestGraph( - ::Type{T}; - VARTYPE = VariableDFG, - FACTYPE = FactorCompute, + ::Type{T}, + ::Type{<:VariableDFG}, + ::Type{<:FactorDFG}, ) where {T <: AbstractDFG}#InMemoryDFGTypes #settings numNodesType1 = 5 @@ -1240,11 +1240,11 @@ function connectivityTestGraph( vars = vcat( map( - n -> VARTYPE(Symbol("x$n"), State{TestVariableType1}(; label = :default)), + n -> VariableDFG(Symbol("x$n"), State{TestVariableType1}(; label = :default)), 1:numNodesType1, ), map( - n -> VARTYPE( + n -> VariableDFG( Symbol("x$(numNodesType1+n)"), State{TestVariableType2}(; label = :default), ), @@ -1254,51 +1254,51 @@ function connectivityTestGraph( addVariables!(dfg, vars) - if FACTYPE == FactorCompute - #change ready and solveInProgress for x7,x8 for improved tests on x7x8f1 - #NOTE because defaults changed - setSolvable!(dfg, :x8, 0) - setSolvable!(dfg, :x9, 0) - - state = DFG.Recipestate(; eliminated = true, potentialused = true) - hyper = DFG.Recipehyper(; multihypo = Float64[], inflation = 1.0) - f_tags = Set([:FACTOR]) - - facs = map( - n -> addFactor!( - dfg, - FactorCompute( - Symbol("x$(n)x$(n+1)f1"), - [vars[n].label, vars[n + 1].label], - TestFunctorInferenceType1(), - deepcopy(hyper), - deepcopy(state); - tags = copy(f_tags), - ), - ), - 1:(length(vars) - 1), - ) - setSolvable!(dfg, :x7x8f1, 0) - - else - facs = map( - n -> addFactor!( - dfg, - FACTYPE(Symbol("x$(n)x$(n+1)f1"), [vars[n].label, vars[n + 1].label]), + #change ready and solveInProgress for x7,x8 for improved tests on x7x8f1 + #NOTE because defaults changed + setSolvable!(dfg, :x8, 0) + setSolvable!(dfg, :x9, 0) + + state = DFG.Recipestate(; eliminated = true, potentialused = true) + hyper = DFG.Recipehyper(; multihypo = Float64[], inflation = 1.0) + f_tags = Set([:FACTOR]) + + facs = map( + n -> addFactor!( + dfg, + FactorDFG( + Symbol("x$(n)x$(n+1)f1"), + [vars[n].label, vars[n + 1].label], + TestFunctorInferenceType1(), + deepcopy(hyper), + deepcopy(state); + tags = copy(f_tags), ), - 1:(length(vars) - 1), - ) - end + ), + 1:(length(vars) - 1), + ) + setSolvable!(dfg, :x7x8f1, 0) return (dfg = dfg, variables = vars, factors = facs) end +function connectivityTestGraph( + ::Type{T}, + ::Type{<:GraphVariable}, + ::Type{<:GraphFactor}, +) where {T <: AbstractDFG}#InMemoryDFGTypes + (; dfg, variables, factors) = connectivityTestGraph(T, VariableDFG, FactorDFG) + sfg = T() + addVariables!(sfg, variables) + addFactors!(sfg, factors) + return (dfg = dfg, variables = getVariables(sfg), factors = getFactors(sfg)) +end + # dfg, verts, facs = connectivityTestGraph(testDFGAPI) -function GettingNeighbors(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorCompute) +function GettingNeighbors(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDFG) # "Getting Neighbors" - dfg, verts, facs = - connectivityTestGraph(testDFGAPI; VARTYPE = VARTYPE, FACTYPE = FACTYPE) + dfg, verts, facs = connectivityTestGraph(testDFGAPI, VARTYPE, FACTYPE) # Trivial test to validate that intersect([], []) returns order of first parameter @test intersect([:x3, :x2, :x1], [:x1, :x2]) == [:x2, :x1] # Get neighbors tests @@ -1327,7 +1327,7 @@ function GettingNeighbors(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorCom end #TODO confirm these tests are covered somewhere then delete -# function GettingSubgraphs(testDFGAPI; VARTYPE=VariableDFG, FACTYPE=FactorCompute) +# function GettingSubgraphs(testDFGAPI; VARTYPE=VariableDFG, FACTYPE=FactorDFG) # # # "Getting Subgraphs" # dfg, verts, facs = connectivityTestGraph(testDFGAPI, VARTYPE=VARTYPE, FACTYPE=FACTYPE) @@ -1377,11 +1377,10 @@ end # # end -function BuildingSubgraphs(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorCompute) +function BuildingSubgraphs(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDFG) # "Getting Subgraphs" - dfg, verts, facs = - connectivityTestGraph(testDFGAPI; VARTYPE = VARTYPE, FACTYPE = FACTYPE) + dfg, verts, facs = connectivityTestGraph(testDFGAPI, VARTYPE, FACTYPE) # Subgraphs dfgSubgraph = buildSubgraph(testDFGAPI, dfg, [verts[1].label], 2) # Only returns x1 and x2 @@ -1431,7 +1430,7 @@ end #TODO Summaries and Summary Graphs function Summaries(testDFGAPI) # "Summaries and Summary Graphs" - dfg, verts, facs = connectivityTestGraph(testDFGAPI) + dfg, verts, facs = connectivityTestGraph(testDFGAPI, VariableDFG, FactorDFG) #TODO for summary # if VARTYPE == VariableSummary # factorFields = fieldnames(FACTYPE) @@ -1463,7 +1462,7 @@ function ProducingDotFiles( v2 = nothing, f1 = nothing; VARTYPE = VariableDFG, - FACTYPE = FactorCompute, + FACTYPE = FactorDFG, ) # "Producing Dot Files" # create a simpler graph for dot testing @@ -1476,8 +1475,8 @@ function ProducingDotFiles( v2 = VARTYPE(:b, State{TestVariableType1}(; label = :default)) end if f1 === nothing - if (FACTYPE == FactorCompute) - f1 = FactorCompute(:abf1, [:a, :b], TestFunctorInferenceType1()) + if (FACTYPE == FactorDFG) + f1 = FactorDFG(:abf1, [:a, :b], TestFunctorInferenceType1()) else f1 = FACTYPE(:abf1, [:a, :b]) end @@ -1487,7 +1486,7 @@ function ProducingDotFiles( addVariable!(dotdfg, v2) # FIXME, fix deprecation # ┌ Warning: addFactor!(dfg, variables, factor) is deprecated, use addFactor!(dfg, factor) - # │ caller = ProducingDotFiles(testDFGAPI::Type{GraphsDFG}, v1::Nothing, v2::Nothing, f1::Nothing; VARTYPE::Type{VariableDFG}, FACTYPE::Type{FactorCompute}) at testBlocks.jl:1440 + # │ caller = ProducingDotFiles(testDFGAPI::Type{GraphsDFG}, v1::Nothing, v2::Nothing, f1::Nothing; VARTYPE::Type{VariableDFG}, FACTYPE::Type{FactorDFG}) at testBlocks.jl:1440 # └ @ Main ~/.julia/dev/DistributedFactorGraphs/test/testBlocks.jl:1440 addFactor!(dotdfg, f1) #NOTE hardcoded toDot will have different results so test Graphs seperately @@ -1508,8 +1507,8 @@ function ProducingDotFiles( return Base.rm("something.dot") end -function ConnectivityTest(testDFGAPI; kwargs...) - dfg, verts, facs = connectivityTestGraph(testDFGAPI; kwargs...) +function ConnectivityTest(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDFG) + dfg, verts, facs = connectivityTestGraph(testDFGAPI, VARTYPE, FACTYPE) @test isConnected(dfg) == true # @test @test_deprecated isFullyConnected(dfg) == true # @test @test_deprecated hasOrphans(dfg) == false @@ -1521,12 +1520,12 @@ function ConnectivityTest(testDFGAPI; kwargs...) @test isConnected(dfg) == false end -function CopyFunctionsTest(testDFGAPI; kwargs...) +function CopyFunctionsTest(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDFG) # testDFGAPI = GraphsDFG # kwargs = () - dfg, verts, facs = connectivityTestGraph(testDFGAPI; kwargs...) + dfg, verts, facs = connectivityTestGraph(testDFGAPI, VARTYPE, FACTYPE) varlbls = ls(dfg) faclbls = lsf(dfg) @@ -1615,12 +1614,12 @@ function CopyFunctionsTest(testDFGAPI; kwargs...) end -function FileDFGTestBlock(testDFGAPI; kwargs...) +function FileDFGTestBlock(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDFG) # testDFGAPI = GraphsDFG # kwargs = () # filename = "/tmp/fileDFG" - dfg, verts, facs = connectivityTestGraph(testDFGAPI; kwargs...) + dfg, verts, facs = connectivityTestGraph(testDFGAPI, VARTYPE, FACTYPE) v4 = getVariable(dfg, :x4) vnd = getState(v4, :default) # set everything diff --git a/test/testSerializingVariables.jl b/test/testSerializingVariables.jl new file mode 100644 index 00000000..fa1ce8d3 --- /dev/null +++ b/test/testSerializingVariables.jl @@ -0,0 +1,104 @@ +## ================================================================================ +## Test serializing and deserializing variables, states, and beliefs +## ================================================================================ + +using Test +using JSON +using DistributedFactorGraphs +using StaticArrays +using LinearAlgebra +using LieGroups + +DFG.@defStateTypeN Pose{N} SpecialEuclideanGroup(N; variant = :right) ArrayPartition( + zeros(SVector{N, Float64}), + SMatrix{N, N, Float64}(I), +) + +## Build a test variable with states, bloblets, and blobentries +function make_test_variable() + v = DFG.VariableDFG(:x1, Pose{3}()) + + state = addState!(v, State(:default, Pose{3}())) + addState!(v, State(:parametric, Pose{3}())) + + # Add sample points to the default state's belief + push!(state.belief.points, DFG.getPointIdentity(Pose{3}())) + G = DFG.getManifold(Pose{3}()) + push!(state.belief.points, rand(G, ArrayPartition)) + + # Add bloblets and a blobentry + DFG.addBloblet!(v, DFG.Bloblet(:a, "1")) + DFG.addBloblet!(v, DFG.Bloblet(:b, "2")) + addBlobentry!(v, DFG.Blobentry(:bel; metadata = (start = 54, N = 20, s = :test))) + + return v +end + +@testset "Serializing Variables" begin + @testset "BeliefRepresentation round-trip" begin + bel = DFG.BeliefRepresentation(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.BeliefRepresentation; style = DFG.DFGJSONStyle()) + @test bel == parsed + end + + @testset "GaussianDensityKind round-trip" begin + dim = DFG.getDimension(Pose{3}()) + bel = DFG.BeliefRepresentation( + DFG.GaussianDensityKind(), + Pose{3}(); + means = [DFG.getPointIdentity(Pose{3}())], + covariances = [diagm(ones(dim))], + ) + jstr = JSON.json(bel; pretty = true, style = DFG.DFGJSONStyle()) + parsed = JSON.parse(jstr, DFG.BeliefRepresentation; style = DFG.DFGJSONStyle()) + @test bel == parsed + end + + @testset "State round-trip" begin + v = make_test_variable() + state = v.states[:default] + + jstr = JSON.json(state; pretty = true, style = DFG.DFGJSONStyle()) + parsed = JSON.parse(jstr, State; style = DFG.DFGJSONStyle()) + @test state == parsed + end + + @testset "VariableDFG round-trip" begin + v = make_test_variable() + + jstr = JSON.json(v; pretty = true, style = DFG.DFGJSONStyle()) + parsed = JSON.parse(jstr, DFG.VariableDFG; style = DFG.DFGJSONStyle()) + @test v == parsed + end + + @testset "VariableDFG parsefile round-trip" begin + v = make_test_variable() + jstr = JSON.json(v; pretty = true, style = DFG.DFGJSONStyle()) + + tmpfile = tempname() * ".json" + write(tmpfile, jstr) + parsed = JSON.parsefile(tmpfile, DFG.VariableDFG; style = DFG.DFGJSONStyle()) + @test v == parsed + rm(tmpfile; force = true) + end + + @testset "VariableSummary from VariableDFG" begin + v = make_test_variable() + vs = VariableSummary(v) + @test vs.label == :x1 + @test vs.tags == v.tags + @test vs.blobentries == v.blobentries + end + + @testset "VariableSkeleton from VariableDFG" begin + v = make_test_variable() + vsk = VariableSkeleton(v) + @test vsk.label == :x1 + @test vsk.tags == v.tags + end +end From 7ab4ea317e9aac7e0680b828889525636f828d94 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 5 Mar 2026 12:05:04 +0200 Subject: [PATCH 2/9] deprecate getPoint, getCoordinates --- src/Deprecated.jl | 55 +++++++++++++++++++++++++++++++++++++ src/entities/DFGVariable.jl | 18 +++++++----- src/services/DFGVariable.jl | 49 --------------------------------- 3 files changed, 66 insertions(+), 56 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 72a4ff82..4c4832da 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -369,3 +369,58 @@ function getVariable(dfg::AbstractDFG, label::Symbol, stateLabel::Symbol) return getVariable(dfg, label) # return getVariable(dfg, label; stateLabelFilter = ==(stateLabel)) end + +# Deprecated with new State serialization. +# """ +# $SIGNATURES + +# Default escalzation from coordinates to a group representation point. Override if defaults are not correct. +# E.g. coords -> se(2) -> SE(2). + +# DevNotes +# - TODO Likely remove as part of serialization updates, see #590 +# - Used in transition period for Serialization. This function will likely be changed or deprecated entirely. + +# Related + +# [`getCoordinates`](@ref) +# """ +function getPoint( + ::Type{T}, + v::AbstractVector, + basis = ManifoldsBase.DefaultOrthogonalBasis(), +) where {T <: StateType} + Base.depwarn("getPoint is deprecated. Use get_vector and exp directly.", :getPoint) + M = getManifold(T) + p0 = getPointIdentity(T) + X = ManifoldsBase.get_vector(M, p0, v, basis) + return ManifoldsBase.exp(M, p0, X) +end + +# """ +# $SIGNATURES + +# Default reduction of a variable point value (a group element) into coordinates as `Vector`. Override if defaults are not correct. + +# DevNotes +# - TODO Likely remove as part of serialization updates, see #590 +# - Used in transition period for Serialization. This function will likely be changed or deprecated entirely. + +# Related + +# [`getPoint`](@ref) +# """ +function getCoordinates( + ::Type{T}, + p, + basis = ManifoldsBase.DefaultOrthogonalBasis(), +) where {T <: StateType} + Base.depwarn( + "getCoordinates is deprecated. Use log and get_coordinates directly.", + :getCoordinates, + ) + M = getManifold(T) + p0 = getPointIdentity(T) + X = ManifoldsBase.log(M, p0, p) + return ManifoldsBase.get_coordinates(M, p0, X, basis) +end diff --git a/src/entities/DFGVariable.jl b/src/entities/DFGVariable.jl index a7af827b..8185130f 100644 --- a/src/entities/DFGVariable.jl +++ b/src/entities/DFGVariable.jl @@ -50,13 +50,6 @@ end # JSON.parse(JSON.json(zeros(0, 0)), Matrix{Float64}) errors, so trying with nothing union end -# # we can also do somthing like this: -# getComponent(state::State, i) = ( -# mean = refMeans(state)[i], -# cov = refCovariances(state)[i], -# weight = refWeights(state)[i], -# ) - JSON.omit_empty(::Type{<:BeliefRepresentation}) = true function BeliefRepresentation(T::AbstractStateType) @@ -196,6 +189,17 @@ refWeights(state::State) = state.belief.weights refPoints(state::State) = state.belief.points refBandwidth(state::State) = state.belief.bandwidth +getDensityKind(state::State) = state.belief.densitykind + +# we can also do somthing like this: +function getComponent(state::State, i) + return ( + mean = refMeans(state)[i], + cov = refCovariances(state)[i], + weight = refWeights(state)[i], + ) +end + ##------------------------------------------------------------------------------ ## States - OrderedDict{Symbol, State} const States = OrderedDict{Symbol, State{T, P}} where {T <: AbstractStateType, P} diff --git a/src/services/DFGVariable.jl b/src/services/DFGVariable.jl index 4292b527..564d501b 100644 --- a/src/services/DFGVariable.jl +++ b/src/services/DFGVariable.jl @@ -156,55 +156,6 @@ Notes function getPointIdentity end getPointIdentity(::T) where {T <: StateType} = getPointIdentity(T) -""" - $SIGNATURES - -Default escalzation from coordinates to a group representation point. Override if defaults are not correct. -E.g. coords -> se(2) -> SE(2). - -DevNotes -- TODO Likely remove as part of serialization updates, see #590 -- Used in transition period for Serialization. This function will likely be changed or deprecated entirely. - -Related - -[`getCoordinates`](@ref) -""" -function getPoint( - ::Type{T}, - v::AbstractVector, - basis = ManifoldsBase.DefaultOrthogonalBasis(), -) where {T <: StateType} - M = getManifold(T) - p0 = getPointIdentity(T) - X = ManifoldsBase.get_vector(M, p0, v, basis) - return ManifoldsBase.exp(M, p0, X) -end - -""" - $SIGNATURES - -Default reduction of a variable point value (a group element) into coordinates as `Vector`. Override if defaults are not correct. - -DevNotes -- TODO Likely remove as part of serialization updates, see #590 -- Used in transition period for Serialization. This function will likely be changed or deprecated entirely. - -Related - -[`getPoint`](@ref) -""" -function getCoordinates( - ::Type{T}, - p, - basis = ManifoldsBase.DefaultOrthogonalBasis(), -) where {T <: StateType} - M = getManifold(T) - p0 = getPointIdentity(T) - X = ManifoldsBase.log(M, p0, p) - return ManifoldsBase.get_coordinates(M, p0, X, basis) -end - ##------------------------------------------------------------------------------ ## solvedCount ##------------------------------------------------------------------------------ From 98f650298349b05c020cf2e7b56ca578570943f7 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 5 Mar 2026 14:57:48 +0200 Subject: [PATCH 3/9] Test unpackOldState, lower/lift Complex --- src/serialization/DFGStructStyles.jl | 20 ++++++++++++++++++++ src/serialization/StateSerialization.jl | 20 ++------------------ test/testSerializingVariables.jl | 9 +++++++++ 3 files changed, 31 insertions(+), 18 deletions(-) diff --git a/src/serialization/DFGStructStyles.jl b/src/serialization/DFGStructStyles.jl index 4e061041..7958a62c 100644 --- a/src/serialization/DFGStructStyles.jl +++ b/src/serialization/DFGStructStyles.jl @@ -43,3 +43,23 @@ function StructUtils.lift( end return AP(parts), nothing end + +# Complex Serialization +StructUtils.structlike(::DFGJSONStyle, ::Type{<:Complex}) = false +StructUtils.lower(::DFGJSONStyle, x::Complex) = (real(x), imag(x)) +function StructUtils.lift(::DFGJSONStyle, ::Type{T}, x::JSON.LazyValue) where T <: Complex + return T(x[1][], x[2][]), nothing +end + +# Above does not work for Arrya{ComplexF64, 0} +StructUtils.lower(::DFGJSONStyle, x::AbstractArray{<:Complex, 0}) = (real(x), imag(x)) +function StructUtils.lift(::DFGJSONStyle, ::Type{A}, x::JSON.LazyValue) where A <: AbstractArray{T,0} where T <: Complex + m = A(undef) + m[] = (T(x[1][], x[2][])) + return m, nothing +end + +# if serialized as an Struct +# function StructUtils.lift(::DFGJSONStyle, ::Type{T}, x::JSON.LazyValue) where T <: Complex +# return T(x.re[], x.im[]), nothing +# end diff --git a/src/serialization/StateSerialization.jl b/src/serialization/StateSerialization.jl index e018289f..061a7bd0 100644 --- a/src/serialization/StateSerialization.jl +++ b/src/serialization/StateSerialization.jl @@ -43,25 +43,9 @@ StructUtils.lower(T::AbstractStateType) = lowerStateKind(T) StructUtils.lift(::Type{AbstractStateType}, s) = liftStateKind(s) ##============================================================================== -## OLD State Packing and unpacking +## OLD State Packing and unpacking - Deprecated v0.29 - kept for migration ##============================================================================== -# State Kind is handled seperately because it includes the N parameter. -function stringStateKind(varT::AbstractStateType{N}) where {N} - T = typeof(varT) - if N == Any - return string(parentmodule(T), ".", nameof(T)) - elseif N isa Integer - return string(parentmodule(T), ".", nameof(T), "{", join(N, ","), "}") - else - throw( - SerializationError( - "Serializing Variable State type only supports an integer parameter, got '$(T)'.", - ), - ) - end -end - function parseStateKind(_typeString::AbstractString) m = match(r"{(\d+)}", _typeString) if !isnothing(m) #parameters in type @@ -156,7 +140,7 @@ function unpackOldState(d) bandwidth = BW, ) end - return State{T, getPointType(T)}(; + return State{typeof(T), getPointType(T)}(; label, belief, separator = Symbol.(d.separator), diff --git a/test/testSerializingVariables.jl b/test/testSerializingVariables.jl index fa1ce8d3..fdda75b0 100644 --- a/test/testSerializingVariables.jl +++ b/test/testSerializingVariables.jl @@ -102,3 +102,12 @@ end @test vsk.tags == v.tags end end + +# TODO deprecated v0.29, remove this test and unpackOldState +@testset "Serializing Old State" begin + Pose2 = Pose{2} + oldstates = JSON.parsefile(@__DIR__() * "/data/oldstate.json") + states = DFG.unpackOldState.(oldstates) + # just a spot check + @test issetequal(getLabel.(states), [:default, :parametric, :graphinit]) +end \ No newline at end of file From a8603e2a4cd2bbb11f4fbc9da20eb1704db55358 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Thu, 5 Mar 2026 15:03:37 +0200 Subject: [PATCH 4/9] Apply suggestions from code review Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/Deprecated.jl | 2 +- src/serialization/StateSerialization.jl | 4 ++-- src/services/CustomPrinting.jl | 5 +++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 4c4832da..f8344c21 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -129,7 +129,7 @@ end function getVariableTypeName(v::VariableSummary) Base.depwarn("getVariableTypeName is deprecated.", :getVariableTypeName) - return v.statekindsymbol + return v.statekind end function getMetadata(dfg::AbstractDFG, label::Symbol, key::Symbol) diff --git a/src/serialization/StateSerialization.jl b/src/serialization/StateSerialization.jl index 061a7bd0..0034eafd 100644 --- a/src/serialization/StateSerialization.jl +++ b/src/serialization/StateSerialization.jl @@ -14,7 +14,7 @@ function lowerStateKind(varT::AbstractStateType{N}) where {N} else throw( SerializationError( - "Serializing Variable State type only supports an integer parameter, got '$(T)'.", + "Serializing Variable State type only supports an integer parameter, got '$(N)'.", ), ) end @@ -128,7 +128,7 @@ function unpackOldState(d) # label = Symbol(d.solveKey) - !isempty(d.covar) && error("covar field is not suppoted") + !isempty(d.covar) && error("covar field is not supported") if label == :parametric belief = BeliefRepresentation(GaussianDensityKind(), T; means = vals, covariances = [BW]) diff --git a/src/services/CustomPrinting.jl b/src/services/CustomPrinting.jl index 91bb7dc8..f1203654 100644 --- a/src/services/CustomPrinting.jl +++ b/src/services/CustomPrinting.jl @@ -59,8 +59,9 @@ function printVariable( println(ioc, " marginalized: ", isMarginalized(vert, :default)) println(ioc, " size bel. samples: ", size(vnd.belief.points)) print(ioc, " kde bandwidths: ") - if 0 < length(vnd.belief.bandwidth) - println(ioc, round.(vnd.belief.bandwidth; digits = 4)) + bandwidth = vnd.belief.bandwidth + if !isnothing(bandwidth) && 0 < length(bandwidth) + println(ioc, round.(bandwidth; digits = 4)) end printstyled(ioc, " VNDs: "; bold = true) println(ioc, solk[smsk], 4 < lsolk ? "..." : "") From 396aee0d9108e2091d719a5a353674efce224119 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 5 Mar 2026 15:05:06 +0200 Subject: [PATCH 5/9] fix formatting --- src/serialization/DFGStructStyles.jl | 8 ++++++-- test/testSerializingVariables.jl | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/serialization/DFGStructStyles.jl b/src/serialization/DFGStructStyles.jl index 7958a62c..3921cef7 100644 --- a/src/serialization/DFGStructStyles.jl +++ b/src/serialization/DFGStructStyles.jl @@ -47,13 +47,17 @@ end # Complex Serialization StructUtils.structlike(::DFGJSONStyle, ::Type{<:Complex}) = false StructUtils.lower(::DFGJSONStyle, x::Complex) = (real(x), imag(x)) -function StructUtils.lift(::DFGJSONStyle, ::Type{T}, x::JSON.LazyValue) where T <: Complex +function StructUtils.lift(::DFGJSONStyle, ::Type{T}, x::JSON.LazyValue) where {T <: Complex} return T(x[1][], x[2][]), nothing end # Above does not work for Arrya{ComplexF64, 0} StructUtils.lower(::DFGJSONStyle, x::AbstractArray{<:Complex, 0}) = (real(x), imag(x)) -function StructUtils.lift(::DFGJSONStyle, ::Type{A}, x::JSON.LazyValue) where A <: AbstractArray{T,0} where T <: Complex +function StructUtils.lift( + ::DFGJSONStyle, + ::Type{A}, + x::JSON.LazyValue, +) where {A <: AbstractArray{T, 0}} where {T <: Complex} m = A(undef) m[] = (T(x[1][], x[2][])) return m, nothing diff --git a/test/testSerializingVariables.jl b/test/testSerializingVariables.jl index fdda75b0..98ddd13f 100644 --- a/test/testSerializingVariables.jl +++ b/test/testSerializingVariables.jl @@ -110,4 +110,4 @@ end states = DFG.unpackOldState.(oldstates) # just a spot check @test issetequal(getLabel.(states), [:default, :parametric, :graphinit]) -end \ No newline at end of file +end From c1ab79024fe97af42ad777c82386a78f1a41264b Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 5 Mar 2026 15:20:55 +0200 Subject: [PATCH 6/9] missed oldstate test json --- test/data/oldstate.json | 704 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 704 insertions(+) create mode 100644 test/data/oldstate.json diff --git a/test/data/oldstate.json b/test/data/oldstate.json new file mode 100644 index 00000000..780f61c0 --- /dev/null +++ b/test/data/oldstate.json @@ -0,0 +1,704 @@ +[ + { + "vecval": [ + 0.0, + 0.0, + 0.0 + ], + "dimval": 3, + "vecbw": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "dimbw": 3, + "BayesNetOutVertIDs": [], + "dimIDs": [], + "dims": 3, + "eliminated": false, + "BayesNetVertID": "NOTHING", + "separator": [], + "variableType": "RoME.Pose2", + "initialized": false, + "infoPerCoord": [ + 0.0, + 0.0, + 0.0 + ], + "ismargin": false, + "dontmargin": false, + "solveInProgress": 0, + "solvedCount": 0, + "solveKey": "parametric", + "covar": [], + "_version": "0.24.1" + }, + { + "vecval": [ + -0.06350623199881132, + -0.06994866614529621, + 0.13462391551883568, + 0.05114044037540477, + -0.00036885747810888185, + -0.0704079333582879, + 0.027464994438425313, + 0.23401645843595872, + -0.018177649902973117, + 0.07335220448106351, + -0.08094750348836972, + -0.12868744972057775, + -0.002370434533940942, + 0.056719017598590486, + 0.10935861564591808, + -0.15724151374653547, + -0.10123160314722118, + 0.012422393326874481, + -0.007567048894279677, + -0.026764504637994225, + -0.0839029877288076, + 0.02935722122804739, + -0.014527083335731937, + -0.14740506948476756, + -0.0770730472977878, + -0.021695146196778238, + -0.08252505479500659, + 0.04054583831750875, + -0.03978601241277072, + 0.08938968724750238, + -0.030880106464528108, + 0.015673457154789672, + -0.014704214966034626, + 0.08029501276828234, + 0.0638116586411302, + -0.0661419412783124, + -0.08301750153170895, + -0.006076364397674576, + 0.08160720234546748, + -0.09687296877337649, + -0.0988642015664761, + 0.03730147496931432, + 0.0897509052603585, + -0.034597928773113656, + -0.0713418360216755, + -0.002902147178807328, + -0.027000418659287984, + -0.00848160054263391, + -0.007052493556196172, + 0.04195519267267101, + -0.10930532363308715, + 0.03541676309538558, + -0.23623166085499073, + 0.14173902727715465, + -0.2015343253005613, + -0.11259938507801966, + 0.06990641116345953, + 0.02889278210344722, + 0.04152216007961288, + -0.06922405156971144, + 0.1057850023202521, + -0.02851960839735357, + 0.01979931525380505, + 0.062172988529748556, + -0.002797110267199377, + -0.08939073458788523, + -0.14246028353735607, + 0.12457263972644528, + 0.05363831410934026, + 0.05301609695698219, + 0.1746564771201361, + 0.03662947149210064, + 0.000762091897366461, + 0.14351731084336944, + 0.06805389905474894, + 0.034248370538960946, + -0.012161754667197634, + 0.06447788687148989, + 0.04383815042782877, + -0.031139762091513326, + -0.03528863592996781, + -0.02030474103449828, + -0.020913251351893133, + -0.05882934313036943, + -0.10697414075023455, + 0.038382637657778246, + 0.11937434681440118, + -0.01902969089971558, + 0.04922457038987383, + -0.006691790676812773, + -0.005703701706772378, + -0.07793975763345644, + -0.03316907798498037, + -0.10862238010137165, + -0.07892206630767405, + -0.07732181506067375, + -0.032174833767324815, + 0.026609732839087442, + 0.07155710767478789, + -0.02922572997995932, + -0.09539310625331102, + -0.09163233372906254, + -0.02904424756611775, + 0.13683634547345783, + -0.11863735555491109, + 0.05635101787755418, + 0.010547953739237021, + 0.14658803327302297, + 0.01723650297585835, + -0.08590630293668826, + 0.04149398151946856, + -0.054290191513532406, + -0.10240423194204876, + 0.024113542438610778, + -0.19699674509051576, + 0.11776812911253841, + 0.3349324040301085, + -0.17370359122552403, + 0.1185848507109792, + -0.06970100172111858, + 0.00498361889662998, + -0.0018987119641630111, + -0.10633204901724234, + -0.07205497204230725, + 0.12376506082287485, + 0.011725030611618314, + 0.03106325524627552, + 0.13333583459832757, + 0.11421665597144398, + 0.013473670880349585, + -0.017321394578548074, + -0.05299658581692425, + 0.042046202825952984, + 0.0750859002119944, + -0.1300256503106831, + -0.1886101743157876, + 0.09856463451255497, + -0.09752091698484493, + 0.12614014626713607, + -0.0652139820530923, + 0.06416353161887019, + -0.04221565508434549, + 0.03438440646458904, + 0.06982867390476676, + -0.09610221147979779, + -0.15990277029148936, + -0.09150535941090938, + 0.06279522658885628, + -0.13708442245312125, + -0.04213520990583408, + 0.02868227380233833, + 0.06351659260758351, + -0.10536202688047039, + -0.13357123989329847, + 0.011719820824241604, + 0.05106851309846682, + -0.20313209378417488, + -0.009460366442580671, + 0.005660418475987267, + -0.010745125056951875, + -0.11464167402964594, + 0.014061751586909796, + -0.08047924694173031, + 0.0777387696850376, + 0.041473891963304636, + -0.04084280750490851, + 0.16814051368361016, + -0.10839070207969351, + -0.03870343482613581, + -0.04759361501379056, + -0.1578834162142546, + -0.029363727849326216, + -0.08444899437239241, + -0.003925753843783211, + 0.022787878131206075, + -0.05475219608001003, + 0.007874965381571262, + 0.08905733180073072, + 0.12756585265826256, + -0.06283327735372769, + 0.0072863382717188886, + 0.15410859408526187, + 0.08782243635632198, + -0.018807668247750365, + 0.11453692022087425, + 0.05734511349715683, + 0.04807725292032841, + -0.010037498631642874, + -0.006693540302957155, + -0.20268942562908943, + -0.0062506375285383, + 0.09504356446971483, + -0.1379521887128084, + 0.03326448111561545, + -0.15944368283624422, + -0.11287054048913375, + -0.000778805885879414, + 0.013839330579299779, + 0.1352323964148995, + 0.1411042048516334, + -0.007263488469469445, + 0.005130091659566183, + -0.028799017745496036, + 0.004652619142072936, + 0.033982641696756584, + 0.15247446198445033, + -0.05608493765764856, + 0.1867374689827701, + -0.13218415302566566, + 0.04556152588272182, + 0.11043988326539661, + 0.02270937239677588, + 0.044858373720967806, + -0.08237749271986759, + -0.007144936698450674, + -0.07161949965217398, + 0.12542116321170657, + 0.1832648726030075, + -0.03491276989723166, + -0.019330933030789393, + -0.054160839250924166, + 0.0017301221476376855, + 0.05731539457395954, + -0.12227997514466915, + 0.14440323876387484, + 0.11064589534588119, + 0.0417378046407886, + 0.017597136226656648, + 0.052795127251840046, + 0.03356166695347729, + -0.012499447101632087, + -0.0723968732818172, + 0.007558734887359641, + 0.06786952142556255, + -0.011875354550934875, + -0.08958840575625567, + -0.10603741841405594, + 0.09065399020810547, + -0.08251029920677813, + -0.12096406709955856, + -0.0318215337423701, + 0.21907407053999772, + 0.07401800572776947, + -0.003992513964757902, + -0.08148721535381555, + -0.013174269487605673, + -0.14615438576740883, + -0.1514709964884989, + 0.06993523485619448, + 0.0018714046631806308, + 0.09688195504033915, + -0.13058278180571808, + -0.07217005365211442, + 0.036525634958203955, + 0.11969763153390725, + -0.028714797406635822, + -0.03719474895534939, + -0.030398122172502, + 0.2002106914559709, + 0.11665951382836455, + -0.11400039493433818, + 0.010465146828673262, + -0.15980185298835892, + -0.3477380263087144, + -0.05031830841835845, + -0.09259440061784044, + 0.045164843272334915, + -0.0029293804615131086, + -0.02062838906576838, + -0.07365569000872853, + -0.010364997844648395, + -0.09452276728406604, + 0.14824345214801202, + 0.12222208260394439, + 0.07921805362728075, + 0.026717812390227386, + 0.031062105392843566, + -0.028757185193583657, + 0.1522286926538874, + 0.12456800399343751, + -0.12110771503609329, + 0.15402434647069116, + 0.09307706666187242, + -0.17981842498666678, + -0.12285965764688178, + -0.11172841152622529, + -0.04419164943627086, + 0.02733135079465647, + 0.024841610051015255, + -0.03538220134157788, + -0.14259913474568386, + -0.0494135534800316, + 0.054381222842654814, + -0.08100463697517905, + 0.12173240608924898, + 0.13588687087475845, + -0.026223110906025013, + -0.05460487981746395, + 0.1253939456989095, + -0.0741838974735389 + ], + "dimval": 3, + "vecbw": [ + 0.017971488819107664, + 0.04058277023111419, + 0.06636467308026067 + ], + "dimbw": 3, + "BayesNetOutVertIDs": [], + "dimIDs": [], + "dims": 3, + "eliminated": false, + "BayesNetVertID": "NOTHING", + "separator": [], + "variableType": "RoME.Pose2", + "initialized": true, + "infoPerCoord": [ + 1.0, + 1.0, + 1.0 + ], + "ismargin": false, + "dontmargin": false, + "solveInProgress": 0, + "solvedCount": 0, + "solveKey": "default", + "covar": [], + "_version": "0.24.1" + }, + { + "vecval": [ + -0.06350623199881132, + -0.06994866614529621, + 0.13462391551883568, + 0.05114044037540477, + -0.00036885747810888185, + -0.0704079333582879, + 0.027464994438425313, + 0.23401645843595872, + -0.018177649902973117, + 0.07335220448106351, + -0.08094750348836972, + -0.12868744972057775, + -0.002370434533940942, + 0.056719017598590486, + 0.10935861564591808, + -0.15724151374653547, + -0.10123160314722118, + 0.012422393326874481, + -0.007567048894279677, + -0.026764504637994225, + -0.0839029877288076, + 0.02935722122804739, + -0.014527083335731937, + -0.14740506948476756, + -0.0770730472977878, + -0.021695146196778238, + -0.08252505479500659, + 0.04054583831750875, + -0.03978601241277072, + 0.08938968724750238, + -0.030880106464528108, + 0.015673457154789672, + -0.014704214966034626, + 0.08029501276828234, + 0.0638116586411302, + -0.0661419412783124, + -0.08301750153170895, + -0.006076364397674576, + 0.08160720234546748, + -0.09687296877337649, + -0.0988642015664761, + 0.03730147496931432, + 0.0897509052603585, + -0.034597928773113656, + -0.0713418360216755, + -0.002902147178807328, + -0.027000418659287984, + -0.00848160054263391, + -0.007052493556196172, + 0.04195519267267101, + -0.10930532363308715, + 0.03541676309538558, + -0.23623166085499073, + 0.14173902727715465, + -0.2015343253005613, + -0.11259938507801966, + 0.06990641116345953, + 0.02889278210344722, + 0.04152216007961288, + -0.06922405156971144, + 0.1057850023202521, + -0.02851960839735357, + 0.01979931525380505, + 0.062172988529748556, + -0.002797110267199377, + -0.08939073458788523, + -0.14246028353735607, + 0.12457263972644528, + 0.05363831410934026, + 0.05301609695698219, + 0.1746564771201361, + 0.03662947149210064, + 0.000762091897366461, + 0.14351731084336944, + 0.06805389905474894, + 0.034248370538960946, + -0.012161754667197634, + 0.06447788687148989, + 0.04383815042782877, + -0.031139762091513326, + -0.03528863592996781, + -0.02030474103449828, + -0.020913251351893133, + -0.05882934313036943, + -0.10697414075023455, + 0.038382637657778246, + 0.11937434681440118, + -0.01902969089971558, + 0.04922457038987383, + -0.006691790676812773, + -0.005703701706772378, + -0.07793975763345644, + -0.03316907798498037, + -0.10862238010137165, + -0.07892206630767405, + -0.07732181506067375, + -0.032174833767324815, + 0.026609732839087442, + 0.07155710767478789, + -0.02922572997995932, + -0.09539310625331102, + -0.09163233372906254, + -0.02904424756611775, + 0.13683634547345783, + -0.11863735555491109, + 0.05635101787755418, + 0.010547953739237021, + 0.14658803327302297, + 0.01723650297585835, + -0.08590630293668826, + 0.04149398151946856, + -0.054290191513532406, + -0.10240423194204876, + 0.024113542438610778, + -0.19699674509051576, + 0.11776812911253841, + 0.3349324040301085, + -0.17370359122552403, + 0.1185848507109792, + -0.06970100172111858, + 0.00498361889662998, + -0.0018987119641630111, + -0.10633204901724234, + -0.07205497204230725, + 0.12376506082287485, + 0.011725030611618314, + 0.03106325524627552, + 0.13333583459832757, + 0.11421665597144398, + 0.013473670880349585, + -0.017321394578548074, + -0.05299658581692425, + 0.042046202825952984, + 0.0750859002119944, + -0.1300256503106831, + -0.1886101743157876, + 0.09856463451255497, + -0.09752091698484493, + 0.12614014626713607, + -0.0652139820530923, + 0.06416353161887019, + -0.04221565508434549, + 0.03438440646458904, + 0.06982867390476676, + -0.09610221147979779, + -0.15990277029148936, + -0.09150535941090938, + 0.06279522658885628, + -0.13708442245312125, + -0.04213520990583408, + 0.02868227380233833, + 0.06351659260758351, + -0.10536202688047039, + -0.13357123989329847, + 0.011719820824241604, + 0.05106851309846682, + -0.20313209378417488, + -0.009460366442580671, + 0.005660418475987267, + -0.010745125056951875, + -0.11464167402964594, + 0.014061751586909796, + -0.08047924694173031, + 0.0777387696850376, + 0.041473891963304636, + -0.04084280750490851, + 0.16814051368361016, + -0.10839070207969351, + -0.03870343482613581, + -0.04759361501379056, + -0.1578834162142546, + -0.029363727849326216, + -0.08444899437239241, + -0.003925753843783211, + 0.022787878131206075, + -0.05475219608001003, + 0.007874965381571262, + 0.08905733180073072, + 0.12756585265826256, + -0.06283327735372769, + 0.0072863382717188886, + 0.15410859408526187, + 0.08782243635632198, + -0.018807668247750365, + 0.11453692022087425, + 0.05734511349715683, + 0.04807725292032841, + -0.010037498631642874, + -0.006693540302957155, + -0.20268942562908943, + -0.0062506375285383, + 0.09504356446971483, + -0.1379521887128084, + 0.03326448111561545, + -0.15944368283624422, + -0.11287054048913375, + -0.000778805885879414, + 0.013839330579299779, + 0.1352323964148995, + 0.1411042048516334, + -0.007263488469469445, + 0.005130091659566183, + -0.028799017745496036, + 0.004652619142072936, + 0.033982641696756584, + 0.15247446198445033, + -0.05608493765764856, + 0.1867374689827701, + -0.13218415302566566, + 0.04556152588272182, + 0.11043988326539661, + 0.02270937239677588, + 0.044858373720967806, + -0.08237749271986759, + -0.007144936698450674, + -0.07161949965217398, + 0.12542116321170657, + 0.1832648726030075, + -0.03491276989723166, + -0.019330933030789393, + -0.054160839250924166, + 0.0017301221476376855, + 0.05731539457395954, + -0.12227997514466915, + 0.14440323876387484, + 0.11064589534588119, + 0.0417378046407886, + 0.017597136226656648, + 0.052795127251840046, + 0.03356166695347729, + -0.012499447101632087, + -0.0723968732818172, + 0.007558734887359641, + 0.06786952142556255, + -0.011875354550934875, + -0.08958840575625567, + -0.10603741841405594, + 0.09065399020810547, + -0.08251029920677813, + -0.12096406709955856, + -0.0318215337423701, + 0.21907407053999772, + 0.07401800572776947, + -0.003992513964757902, + -0.08148721535381555, + -0.013174269487605673, + -0.14615438576740883, + -0.1514709964884989, + 0.06993523485619448, + 0.0018714046631806308, + 0.09688195504033915, + -0.13058278180571808, + -0.07217005365211442, + 0.036525634958203955, + 0.11969763153390725, + -0.028714797406635822, + -0.03719474895534939, + -0.030398122172502, + 0.2002106914559709, + 0.11665951382836455, + -0.11400039493433818, + 0.010465146828673262, + -0.15980185298835892, + -0.3477380263087144, + -0.05031830841835845, + -0.09259440061784044, + 0.045164843272334915, + -0.0029293804615131086, + -0.02062838906576838, + -0.07365569000872853, + -0.010364997844648395, + -0.09452276728406604, + 0.14824345214801202, + 0.12222208260394439, + 0.07921805362728075, + 0.026717812390227386, + 0.031062105392843566, + -0.028757185193583657, + 0.1522286926538874, + 0.12456800399343751, + -0.12110771503609329, + 0.15402434647069116, + 0.09307706666187242, + -0.17981842498666678, + -0.12285965764688178, + -0.11172841152622529, + -0.04419164943627086, + 0.02733135079465647, + 0.024841610051015255, + -0.03538220134157788, + -0.14259913474568386, + -0.0494135534800316, + 0.054381222842654814, + -0.08100463697517905, + 0.12173240608924898, + 0.13588687087475845, + -0.026223110906025013, + -0.05460487981746395, + 0.1253939456989095, + -0.0741838974735389 + ], + "dimval": 3, + "vecbw": [ + 0.017971488819107664, + 0.04058277023111419, + 0.06636467308026067 + ], + "dimbw": 3, + "BayesNetOutVertIDs": [], + "dimIDs": [], + "dims": 3, + "eliminated": false, + "BayesNetVertID": "NOTHING", + "separator": [], + "variableType": "RoME.Pose2", + "initialized": true, + "infoPerCoord": [ + 1.0, + 1.0, + 1.0 + ], + "ismargin": false, + "dontmargin": false, + "solveInProgress": 0, + "solvedCount": 0, + "solveKey": "graphinit", + "covar": [], + "_version": "0.24.1" + } +] \ No newline at end of file From 5b16804a89510379a6073e0758994c232047c945 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 5 Mar 2026 15:30:56 +0200 Subject: [PATCH 7/9] Move Pose2 definition in tests --- test/testSerializingVariables.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/testSerializingVariables.jl b/test/testSerializingVariables.jl index 98ddd13f..84847d82 100644 --- a/test/testSerializingVariables.jl +++ b/test/testSerializingVariables.jl @@ -13,6 +13,7 @@ DFG.@defStateTypeN Pose{N} SpecialEuclideanGroup(N; variant = :right) ArrayParti zeros(SVector{N, Float64}), SMatrix{N, N, Float64}(I), ) +Pose2 = Pose{2} ## Build a test variable with states, bloblets, and blobentries function make_test_variable() @@ -105,7 +106,6 @@ end # TODO deprecated v0.29, remove this test and unpackOldState @testset "Serializing Old State" begin - Pose2 = Pose{2} oldstates = JSON.parsefile(@__DIR__() * "/data/oldstate.json") states = DFG.unpackOldState.(oldstates) # just a spot check From 101ad78f4ce531d52e90697b954667d48ab09386 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 5 Mar 2026 15:51:28 +0200 Subject: [PATCH 8/9] Refactor unpackOldState to improve state type handling and clarity --- src/serialization/StateSerialization.jl | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/serialization/StateSerialization.jl b/src/serialization/StateSerialization.jl index 0034eafd..bf262f1e 100644 --- a/src/serialization/StateSerialization.jl +++ b/src/serialization/StateSerialization.jl @@ -110,8 +110,9 @@ end function unpackOldState(d) @debug "Dispatching conversion packed variable -> variable for type $(string(d.variableType))" # Figuring out the variableType - T = parseStateKind(d.variableType) - + statekind = parseStateKind(d.variableType) + T = typeof(statekind) + r3 = d.dimval c3 = r3 > 0 ? floor(Int, length(d.vecval) / r3) : 0 M3 = reshape(d.vecval, r3, c3) @@ -131,16 +132,16 @@ function unpackOldState(d) !isempty(d.covar) && error("covar field is not supported") if label == :parametric belief = - BeliefRepresentation(GaussianDensityKind(), T; means = vals, covariances = [BW]) + BeliefRepresentation(GaussianDensityKind(), statekind; means = vals, covariances = [BW]) else belief = BeliefRepresentation( NonparametricDensityKind(), - T; + statekind; points = vals, bandwidth = BW, ) end - return State{typeof(T), getPointType(T)}(; + return State{T, getPointType(T)}(; label, belief, separator = Symbol.(d.separator), From 4959003a9c83db62d4e064c95c81a87461df4092 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 5 Mar 2026 15:55:50 +0200 Subject: [PATCH 9/9] formatting --- src/serialization/StateSerialization.jl | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/serialization/StateSerialization.jl b/src/serialization/StateSerialization.jl index bf262f1e..f4527749 100644 --- a/src/serialization/StateSerialization.jl +++ b/src/serialization/StateSerialization.jl @@ -112,7 +112,7 @@ function unpackOldState(d) # Figuring out the variableType statekind = parseStateKind(d.variableType) T = typeof(statekind) - + r3 = d.dimval c3 = r3 > 0 ? floor(Int, length(d.vecval) / r3) : 0 M3 = reshape(d.vecval, r3, c3) @@ -131,8 +131,12 @@ function unpackOldState(d) label = Symbol(d.solveKey) !isempty(d.covar) && error("covar field is not supported") if label == :parametric - belief = - BeliefRepresentation(GaussianDensityKind(), statekind; means = vals, covariances = [BW]) + belief = BeliefRepresentation( + GaussianDensityKind(), + statekind; + means = vals, + covariances = [BW], + ) else belief = BeliefRepresentation( NonparametricDensityKind(),