From 830f5077ff048451ffb6b49686770596a04176c5 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 2 Apr 2025 10:45:59 +0200 Subject: [PATCH 1/5] Spike on refactoring Factors --- Project.toml | 2 + src/DistributedFactorGraphs.jl | 1 + src/entities/DFGFactor.jl | 240 +++++++++++++++++++++------------ src/services/CustomPrinting.jl | 18 +-- src/services/DFGFactor.jl | 12 ++ src/services/Serialization.jl | 60 ++++++++- 6 files changed, 233 insertions(+), 100 deletions(-) diff --git a/Project.toml b/Project.toml index 6586b9af..19b12273 100644 --- a/Project.toml +++ b/Project.toml @@ -19,6 +19,7 @@ ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb" OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca" +Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" Reexport = "189a3867-3050-52da-a836-e630ba90ab69" SHA = "ea8e919c-243c-51af-8825-aaa63cd721ce" @@ -61,6 +62,7 @@ ManifoldsBase = "0.14, 0.15, 1" OrderedCollections = "1.4" Pkg = "1.4, 1.5" ProgressMeter = "1" +Random = "1.10" RecursiveArrayTools = "2, 3" Reexport = "1" SHA = "0.7, 1" diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index 92a40252..3194d322 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -18,6 +18,7 @@ using Base using Base64 using DocStringExtensions using Dates +using Random using TimeZones using Distributions using Reexport diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl index 8b4b66f9..e3d9c011 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -49,6 +49,18 @@ Base.@kwdef mutable struct GenericFunctionNodeData{ inflation::Float64 = 0.0 end +#TODO is this mutable +@kwdef mutable struct FactorState + eliminated::Bool = false + potentialused::Bool = false + multihypo::Vector{Float64} = Float64[] # TODO re-evaluate after refactoring w #477 + certainhypo::Vector{Int} = Int[] + nullhypo::Float64 = 0.0 + solveInProgress::Int = 0 #TODO maybe deprecated or move to operational memory, also why Int? + inflation::Float64 = 0.0 +end + + # TODO should we move non FactorOperationalMemory to FactorCompute: # fnc, multihypo, nullhypo, inflation ? # that way we split solverData <: FactorOperationalMemory and constants @@ -81,10 +93,10 @@ FunctionNodeData(args...; kw...) = FunctionNodeData{typeof(args[4])}(args...; kw # # | | label | tags | timestamp | solvable | solverData | # |-------------------|:-----:|:----:|:---------:|:--------:|:----------:| -# | FactorSkeleton | X | x | | | | -# | FactorSummary | X | X | X | | | -# | FactorDFG | X | X | X | X | X* | -# | FactorCompute | X | X | X | X | X | +# | FactorSkeleton | X | x | | | | +# | FactorSummary | X | X | X | | | +# | FactorDFG | X | X | X | X | X* | +# | FactorCompute | X | X | X | X | X | # *not available without reconstruction """ @@ -92,10 +104,13 @@ FunctionNodeData(args...; kw...) = FunctionNodeData{typeof(args[4])}(args...; kw The Factor information packed in a way that accomdates multi-lang using json. """ + +#TODO do we have parameter for packed observation or is it already a string? +#TODO Same with metadata? Base.@kwdef struct FactorDFG <: AbstractDFGFactor id::Union{UUID, Nothing} = nothing label::Symbol - tags::Vector{Symbol} + tags::Set{Symbol} _variableOrderSymbols::Vector{Symbol} timestamp::ZonedDateTime nstime::String @@ -104,35 +119,93 @@ Base.@kwdef struct FactorDFG <: AbstractDFGFactor data::String metadata::String _version::String = string(_getDFGVersion()) + state::FactorState + observJSON::String # serialized opbservation # blobEntries::Vector{Blobentry}#TODO should factor have blob entries? end + #TODO type not in DFG FactorDFG, should it be? # _type::String # createdTimestamp::DateTime # lastUpdatedTimestamp::DateTime +StructTypes.StructType(::Type{FactorDFG}) = StructTypes.UnorderedStruct() +StructTypes.idproperty(::Type{FactorDFG}) = :id +StructTypes.omitempties(::Type{FactorDFG}) = (:id,) + +#TODO deprecate, added in v0.26 as a bridge to new serialization structure +function FactorDFG( + id::Union{UUID, Nothing}, + label::Symbol, + tags::Set{Symbol}, + _variableOrderSymbols::Vector{Symbol}, + timestamp::ZonedDateTime, + nstime::String, + fnctype::String, + solvable::Int, + data::String, + metadata::String, + _version::String, + state::Union{Nothing, FactorState}, + observJSON::Union{Nothing, String}, +) + if isnothing(state) || isnothing(observJSON) + fd = JSON3.read(data) + state = FactorState( + fd.eliminated, + fd.potentialused, + fd.multihypo, + fd.certainhypo, + fd.nullhypo, + fd.solveInProgress, + fd.inflation, + ) + observJSON = JSON3.write(fd.fnc) + end + return FactorDFG( + id, + label, + tags, + _variableOrderSymbols, + timestamp, + nstime, + fnctype, + solvable, + data, + metadata, + _version, + state, + observJSON, + ) +end + + FactorDFG(f::FactorDFG) = f # TODO consolidate to just one type """ $(TYPEDEF) Abstract parent type for all InferenceTypes, which are the -functions inside of factors. +observation functions inside of factors. """ -abstract type InferenceType <: DFG.AbstractPackedFactor end +abstract type InferenceType <: AbstractPackedFactor end + +#TODO deprecate InferenceType in favor of AbstractPackedFactor v0.26 + # this is the GenericFunctionNodeData for packed types -const FactorData = PackedFunctionNodeData{InferenceType} +#TODO deprecate FactorData in favor of FactorState (with no more distinction between packed and compute) +const FactorData = PackedFunctionNodeData{AbstractPackedFactor} # Packed Factor constructor function assembleFactorName(xisyms::Union{Vector{String}, Vector{Symbol}}) - return Symbol(xisyms..., "f_", string(uuid4())[1:4]) + return Symbol(xisyms..., "_f", randstring(4)) end -getFncTypeName(fnc::InferenceType) = split(string(typeof(fnc)), ".")[end] +getFncTypeName(fnc::AbstractPackedFactor) = split(string(typeof(fnc)), ".")[end] function FactorDFG( xisyms::Vector{Symbol}, - fnc::InferenceType; + fnc::AbstractPackedFactor; multihypo::Vector{Float64} = Float64[], nullhypo::Float64 = 0.0, solvable::Int = 1, @@ -144,7 +217,7 @@ function FactorDFG( metadata::Dict{Symbol, DFG.SmallDataTypes} = Dict{Symbol, DFG.SmallDataTypes}(), ) # create factor data - factordata = FactorData(; fnc, multihypo, nullhypo, inflation) + state = FactorState(; multihypo, nullhypo, inflation) fnctype = getFncTypeName(fnc) @@ -152,23 +225,21 @@ function FactorDFG( # create factor factor = FactorDFG(; label, - tags, + tags = Set(tags), _variableOrderSymbols = xisyms, timestamp, nstime = string(nstime), fnctype, solvable, - data = JSON3.write(factordata), metadata = base64encode(JSON3.write(metadata)), + state, + observJSON = JSON3.write(fnc), + data = "", #TODO deprecate data completely ) return factor end -StructTypes.StructType(::Type{FactorDFG}) = StructTypes.UnorderedStruct() -StructTypes.idproperty(::Type{FactorDFG}) = :id -StructTypes.omitempties(::Type{FactorDFG}) = (:id,) - ## FactorCompute lv2 """ @@ -183,9 +254,9 @@ DevNotes Fields: $(TYPEDFIELDS) """ -Base.@kwdef struct FactorCompute{T, N} <: AbstractDFGFactor +Base.@kwdef struct FactorCompute{FT <: AbstractFactor, N} <: AbstractDFGFactor """The ID for the factor""" - id::Union{UUID, Nothing} + id::Union{UUID, Nothing} = nothing """Factor label, e.g. :x1f1. Accessor: [`getLabel`](@ref)""" label::Symbol @@ -198,117 +269,112 @@ Base.@kwdef struct FactorCompute{T, N} <: AbstractDFGFactor """Variable timestamp. Accessors: [`getTimestamp`](@ref), [`setTimestamp`](@ref)""" timestamp::ZonedDateTime - """Nano second time, for more resolution on timestamp (only subsecond information)""" + """Nano second time""" nstime::Nanosecond """Solver data. Accessors: [`getSolverData`](@ref), [`setSolverData!`](@ref)""" - solverData::Base.RefValue{GenericFunctionNodeData{T}} + solverData::Base.RefValue{<:GenericFunctionNodeData} """Solvable flag for the factor. Accessors: [`getSolvable`](@ref), [`setSolvable!`](@ref)""" solvable::Base.RefValue{Int} """Dictionary of small data associated with this variable. Accessors: [`getMetadata`](@ref), [`setMetadata!`](@ref)""" - smallData::Dict{Symbol, SmallDataTypes} - # Inner constructor - function FactorCompute{T}( - label::Symbol, - timestamp::Union{DateTime, ZonedDateTime}, - nstime::Nanosecond, - tags::Set{Symbol}, - solverData::GenericFunctionNodeData{T}, - solvable::Int, - _variableOrderSymbols::NTuple{N, Symbol}; - id::Union{UUID, Nothing} = nothing, - smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}(), - ) where {T, N} - return new{T, N}( - id, - label, - tags, - _variableOrderSymbols, - timestamp, - nstime, - Ref(solverData), - Ref(solvable), - smallData, - ) - end + smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}() + + #refactor fields + observation::FT + state::FactorState + computeMem::Base.RefValue{<:FactorOperationalMemory} #TODO easy of use vs. performance as container is abstract in any case. + end ##------------------------------------------------------------------------------ ## Constructors -""" -$(SIGNATURES) +#TODO consolidate constructors, currently IIF calls only +# DFGFactor( +# Symbol(namestring), +# varOrderLabels, +# solverData; +# tags = Set(union(tags, [:FACTOR])), +# solvable, +# timestamp = _zonedtime(timestamp), +# ) -Construct a DFG factor given a label. -""" function FactorCompute( label::Symbol, timestamp::Union{DateTime, ZonedDateTime}, nstime::Nanosecond, tags::Set{Symbol}, - solverData::GenericFunctionNodeData{T}, + solverData::GenericFunctionNodeData, solvable::Int, - _variableOrderSymbols::Tuple; + variableOrder::Union{Vector{Symbol}, Tuple}; + observation = getFactorType(solverData), + state::FactorState = FactorState(), + computeMem::Base.RefValue{<:FactorOperationalMemory} = Ref{FactorOperationalMemory}(), id::Union{UUID, Nothing} = nothing, smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}(), -) where {T} - return FactorCompute{T}( - label, - timestamp, - nstime, - tags, - solverData, - solvable, - _variableOrderSymbols; - id = id, - smallData = smallData, - ) -end - -function FactorCompute{T}( - label::Symbol, - variableOrderSymbols::Vector{Symbol}, - timestamp::Union{DateTime, ZonedDateTime} = now(localzone()), - data::GenericFunctionNodeData{T} = GenericFunctionNodeData(; fnc = T()); - kw..., -) where {T} +) return FactorCompute( + id, label, + tags, + Tuple(variableOrder), timestamp, - Nanosecond(0), - Set{Symbol}(), - data, - 1, - Tuple(variableOrderSymbols); - kw..., + nstime, + Ref(solverData), + Ref(solvable), + smallData, + observation, + state, + computeMem, ) end -# # TODO standardize new fields in kw constructors, .id function FactorCompute( label::Symbol, - variableOrderSymbols::Vector{Symbol}, - data::GenericFunctionNodeData{T}; + variableOrder::Union{Vector{Symbol}, Tuple}, + solverData::GenericFunctionNodeData; tags::Set{Symbol} = Set{Symbol}(), timestamp::Union{DateTime, ZonedDateTime} = now(localzone()), solvable::Int = 1, nstime::Nanosecond = Nanosecond(0), id::Union{UUID, Nothing} = nothing, smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}(), -) where {T} - return FactorCompute{T}( +) + + observation = getFactorType(solverData) + + state = FactorState( + solverData.eliminated, + solverData.potentialused, + solverData.multihypo, + solverData.certainhypo, + solverData.nullhypo, + solverData.solveInProgress, + solverData.inflation, + ) + + if solverData.fnc isa FactorOperationalMemory + computeMem = Ref(solverData.fnc) + else + computeMem = Ref{FactorOperationalMemory}() + end + + return FactorCompute( label, timestamp, nstime, tags, - data, + solverData, solvable, - Tuple(variableOrderSymbols); + Tuple(variableOrder); + observation, + computeMem, id, smallData, + state, ) end diff --git a/src/services/CustomPrinting.jl b/src/services/CustomPrinting.jl index 793d17dd..c8d1b0a7 100644 --- a/src/services/CustomPrinting.jl +++ b/src/services/CustomPrinting.jl @@ -119,13 +119,10 @@ function printFactor( ioc = IOContext(io, :limit => limit, :compact => compact) if short - opmemt = (getSolverData(vert).fnc |> typeof).name.name - fct = getFactorType(vert) + fct = getObservation(vert) fctt = fct |> typeof - printstyled(ioc, typeof(vert).name.name, "{", opmemt, "{"; bold = true) - printstyled(ioc, fctt.name.name; bold = true, color = :blue) - printstyled(ioc, "...}}"; bold = true) - println(ioc) + printstyled(ioc, summary(vert); bold = true) + println() println(ioc, " ID: ", vert.id) println(ioc, " timestamp: ", vert.timestamp) println(ioc, " nstime: ", vert.nstime) @@ -134,16 +131,15 @@ function printFactor( println(ioc) println(ioc, " solvable: ", vert.solvable) println(ioc, " VariableOrder: ", vert._variableOrderSymbols) - println(ioc, " multihypo: ", getSolverData(vert).multihypo) # FIXME #477 - println(ioc, " nullhypo: ", getSolverData(vert).nullhypo) + println(ioc, " multihypo: ", getState(vert).multihypo) # FIXME #477 + println(ioc, " nullhypo: ", getState(vert).nullhypo) println(ioc, " tags: ", vert.tags) printstyled(ioc, " FactorType: "; bold = true, color = :blue) println(ioc, fctt) # show(ioc, fctt) for f in setdiff(fieldnames(fctt), skipfields) - printstyled(ioc, f, ":"; color = :magenta) - println(ioc) - show(ioc, typeof(getproperty(fct, f)).name.name) + printstyled(ioc, f, ": "; color = :magenta) + show(ioc, typeof(getproperty(fct, f))) println(ioc) end else diff --git a/src/services/DFGFactor.jl b/src/services/DFGFactor.jl index 9629720e..2588175a 100644 --- a/src/services/DFGFactor.jl +++ b/src/services/DFGFactor.jl @@ -2,6 +2,10 @@ ## Accessors ##============================================================================== +function getMetadata(f::FactorDFG) + return JSON3.read(base64decode(f.metadata), Dict{Symbol, SmallDataTypes}) +end + ##============================================================================== ## GenericFunctionNodeData ##============================================================================== @@ -33,6 +37,14 @@ getFactorType(fct::FactorCompute) = getFactorType(getSolverData(fct)) getFactorType(f::FactorDFG) = getTypeFromSerializationModule(f.fnctype)() # TODO find a better way to do this that does not rely on empty constructor getFactorType(dfg::AbstractDFG, lbl::Symbol) = getFactorType(getFactor(dfg, lbl)) +getState(f::AbstractDFGFactor) = f.state +getObservation(f::FactorCompute) = f.observation +function getObservation(f::FactorDFG) + #FIXME completely refactor to not need getTypeFromSerializationModule and just use StructTypes + packtype = DFG.getTypeFromSerializationModule("Packed" * f.fnctype) + return packtype(; JSON3.read(f.observJSON)...) +end + """ $SIGNATURES diff --git a/src/services/Serialization.jl b/src/services/Serialization.jl index 808bd567..9257e51f 100644 --- a/src/services/Serialization.jl +++ b/src/services/Serialization.jl @@ -273,7 +273,7 @@ function packFactor(f::FactorCompute) return FactorDFG(; id = f.id, label = f.label, - tags = collect(f.tags), + tags = f.tags, _variableOrderSymbols = f._variableOrderSymbols, timestamp = f.timestamp, nstime = string(f.nstime.value), @@ -283,6 +283,8 @@ function packFactor(f::FactorCompute) # Pack the node data data = JSON3.write(_packSolverData(f, fnctype)), _version = string(_getDFGVersion()), + state = f.state, + observJSON = JSON3.write(packObservation(f)), ) return props end @@ -378,10 +380,12 @@ function unpackFactor(dfg::AbstractDFG, factor::FactorDFG; skipVersionCheck::Boo @debug "DECODING factor type = '$(factor.fnctype)' for factor '$(factor.label)'" !skipVersionCheck && _versionCheck(factor) - fullFactorData = nothing + local fullFactorData + local observation try if factor.data[1] == '{' packedFnc = fncStringToData(factor.fnctype, factor.data) + observation = unpackObservation(factor) else packedFnc = fncStringToData(factor.fnctype, String(base64decode(factor.data))) end @@ -398,6 +402,12 @@ function unpackFactor(dfg::AbstractDFG, factor::FactorDFG; skipVersionCheck::Boo metadata = JSON3.read(base64decode(factor.metadata), Dict{Symbol, DFG.SmallDataTypes}) + if fullFactorData.fnc isa FactorOperationalMemory + computeMem = Ref(fullFactorData.fnc) + else + computeMem = Ref{FactorOperationalMemory}() + end + return FactorCompute( factor.label, factor.timestamp, @@ -408,6 +418,52 @@ function unpackFactor(dfg::AbstractDFG, factor::FactorDFG; skipVersionCheck::Boo Tuple(factor._variableOrderSymbols); id = factor.id, smallData = metadata, + state = factor.state, + observation, + computeMem, + ) +end + +function unpackObservation(factor::FactorDFG) + #FIXME completely refactor to not need getTypeFromSerializationModule and just use StructTypes + observpacked = getObservation(factor) + #TODO change to unpack: observ = unpack(observpacked) + packtype = DFG.getTypeFromSerializationModule("Packed" * factor.fnctype) + return convert(convertStructType(packtype), observpacked) +end + +packObservation(f::FactorCompute) = packObservation(getObservation(f)) +function packObservation(observ::AbstractFactor) + packtype = convertPackedType(observ) + return convert(packtype, observ) +end + +function unpackFactor(factor::FactorDFG; skipVersionCheck::Bool = false) + # + @debug "DECODING factor type = '$(factor.fnctype)' for factor '$(factor.label)'" + !skipVersionCheck && _versionCheck(factor) + + local observation + try + observation = unpackObservation(factor) + catch + @error "Error while unpacking '$(factor.label)' as '$(factor.fnctype)', please check the unpacking/packing converters for this factor" + rethrow() + end + + return FactorCompute( + factor.id, + factor.label, + factor.tags, + Tuple(factor._variableOrderSymbols), + factor.timestamp, + Nanosecond(factor.nstime), + Ref{GenericFunctionNodeData}(), + Ref(factor.solvable), + getMetadata(factor), + observation, + factor.state, + Ref{FactorOperationalMemory}(), ) end From 14edc098dc40fe639cfe014a48b03821c7b38d60 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Wed, 2 Apr 2025 10:50:01 +0200 Subject: [PATCH 2/5] Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --- src/entities/DFGFactor.jl | 5 ----- src/services/Serialization.jl | 3 +-- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl index e3d9c011..b058d231 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -60,7 +60,6 @@ end inflation::Float64 = 0.0 end - # TODO should we move non FactorOperationalMemory to FactorCompute: # fnc, multihypo, nullhypo, inflation ? # that way we split solverData <: FactorOperationalMemory and constants @@ -179,7 +178,6 @@ function FactorDFG( ) end - FactorDFG(f::FactorDFG) = f # TODO consolidate to just one type @@ -285,7 +283,6 @@ Base.@kwdef struct FactorCompute{FT <: AbstractFactor, N} <: AbstractDFGFactor observation::FT state::FactorState computeMem::Base.RefValue{<:FactorOperationalMemory} #TODO easy of use vs. performance as container is abstract in any case. - end ##------------------------------------------------------------------------------ @@ -343,9 +340,7 @@ function FactorCompute( id::Union{UUID, Nothing} = nothing, smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}(), ) - observation = getFactorType(solverData) - state = FactorState( solverData.eliminated, solverData.potentialused, diff --git a/src/services/Serialization.jl b/src/services/Serialization.jl index 9257e51f..bfa09243 100644 --- a/src/services/Serialization.jl +++ b/src/services/Serialization.jl @@ -407,7 +407,6 @@ function unpackFactor(dfg::AbstractDFG, factor::FactorDFG; skipVersionCheck::Boo else computeMem = Ref{FactorOperationalMemory}() end - return FactorCompute( factor.label, factor.timestamp, @@ -426,7 +425,7 @@ end function unpackObservation(factor::FactorDFG) #FIXME completely refactor to not need getTypeFromSerializationModule and just use StructTypes - observpacked = getObservation(factor) + observpacked = getObservation(factor) #TODO change to unpack: observ = unpack(observpacked) packtype = DFG.getTypeFromSerializationModule("Packed" * factor.fnctype) return convert(convertStructType(packtype), observpacked) From 3d6c96dd11c8699ece5494eb882482b9046de38a Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Thu, 12 Jun 2025 06:40:46 -0400 Subject: [PATCH 3/5] defFactorType macro (#1077) * defFactorFunction macro * Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> * update defFactorType * Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> * Update DFGFactor.jl --------- Co-authored-by: Johannes Terblanche Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --- src/entities/DFGFactor.jl | 1 - src/services/DFGFactor.jl | 53 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl index b058d231..6787ee97 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -2,7 +2,6 @@ ## Abstract Types ##============================================================================== -# TODO consider changing this to AbstractFactor abstract type AbstractFactor end abstract type AbstractPackedFactor end diff --git a/src/services/DFGFactor.jl b/src/services/DFGFactor.jl index 2588175a..86361fac 100644 --- a/src/services/DFGFactor.jl +++ b/src/services/DFGFactor.jl @@ -64,6 +64,59 @@ function _getPriorType(_type::Type{<:InferenceVariable}) return getfield(_type.name.module, Symbol(:Prior, _type.name.name)) end +##============================================================================== +## Default Factors Function Macro +##============================================================================== +export PackedSamplableBelief +# export pack, unpack, packDistribution, unpackDistribution + +function pack end +function unpack end +function packDistribution end +function unpackDistribution end + +abstract type PackedSamplableBelief end +StructTypes.StructType(::Type{<:PackedSamplableBelief}) = StructTypes.UnorderedStruct() + +""" + @defFactorType StructName factortype<:AbstractFactor manifolds<:ManifoldsBase.AbstractManifold + +A macro to create a new factor function with name `StructName` and manifolds. Note that +the `manifolds` is an object and *must* be a subtype of `ManifoldsBase.AbstractManifold`. +See documentation in [Manifolds.jl on making your own](https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html). + +Example: +``` +DFG.@defFactorType Pose2Pos2 AbstractManifoldMinimize SpecialEuclidean(2) +``` +""" +macro defFactorType(structname, factortype, manifold) + packedstructname = Symbol("Packed", structname) + return esc( + quote + Base.@__doc__ struct $structname{T} <: $factortype + Z::T + end + + Base.@__doc__ struct $packedstructname{T <: PackedSamplableBelief} <: + AbstractPackedFactor + Z::T + end + + # user manifold must be a <:Manifold + @assert ($manifold isa AbstractManifold) "@defFactorType of " * + string($structname) * + " requires that the " * + string($manifold) * + " be a subtype of `ManifoldsBase.AbstractManifold`" + + DFG.getManifold(::Type{$structname}) = $manifold + DFG.pack(d::$structname) = $packedstructname(DFG.packDistribution(d.Z)) + DFG.unpack(d::$packedstructname) = $structname(DFG.unpackDistribution(d.Z)) + end, + ) +end + ##============================================================================== ## Factors ##============================================================================== From c45ba31d58692d852d9cc3f093dc2f0fe9c39b7d Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 12 Jun 2025 19:22:22 +0200 Subject: [PATCH 4/5] Fix tests --- src/services/CompareUtils.jl | 15 +++++++++++++-- src/services/DFGFactor.jl | 3 ++- test/plottingTest.jl | 3 ++- test/testBlocks.jl | 10 +++++----- 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/services/CompareUtils.jl b/src/services/CompareUtils.jl index a3f3d4f9..99047266 100644 --- a/src/services/CompareUtils.jl +++ b/src/services/CompareUtils.jl @@ -30,10 +30,11 @@ const GeneratedCompareUnion = Union{ FactorDFG, FactorSummary, FactorSkeleton, + FactorState, } @generated function ==(x::T, y::T) where {T <: GeneratedCompareUnion} - ignored = [] + ignored = [:computeMem] return mapreduce( n -> :(x.$n == y.$n), (a, b) -> :($a && $b), @@ -315,7 +316,17 @@ function compareFactor( skipcompute::Bool = true, ) # - skip_ = union([:attributes; :solverData; :_variableOrderSymbols; :_gradients], skip) + skip_ = union( + [ + :attributes, + :solverData, + :observation, + :computeMem, + :_variableOrderSymbols, + :_gradients, + ], + skip, + ) TP = compareAll(A, B; skip = skip_, show = show) @debug "compareFactor 1/5" TP TP = diff --git a/src/services/DFGFactor.jl b/src/services/DFGFactor.jl index 86361fac..fd037140 100644 --- a/src/services/DFGFactor.jl +++ b/src/services/DFGFactor.jl @@ -32,7 +32,8 @@ Return user factor type from factor graph identified by label `::Symbol`. Notes - Replaces older `getfnctype`. """ -getFactorType(data::GenericFunctionNodeData) = data.fnc.usrfnc! +getFactorType(data::GenericFunctionNodeData{<:FactorOperationalMemory}) = data.fnc.usrfnc! +getFactorType(data::GenericFunctionNodeData{<:AbstractFactor}) = data.fnc getFactorType(fct::FactorCompute) = getFactorType(getSolverData(fct)) getFactorType(f::FactorDFG) = getTypeFromSerializationModule(f.fnctype)() # TODO find a better way to do this that does not rely on empty constructor getFactorType(dfg::AbstractDFG, lbl::Symbol) = getFactorType(getFactor(dfg, lbl)) diff --git a/test/plottingTest.jl b/test/plottingTest.jl index 0099defa..2ec2159c 100644 --- a/test/plottingTest.jl +++ b/test/plottingTest.jl @@ -17,9 +17,10 @@ map(v -> addVariable!(dfg, v), verts) map( n -> addFactor!( dfg, - FactorCompute{TestFunctorInferenceType1}( + FactorCompute( Symbol("x$(n)x$(n+1)f1"), [verts[n].label, verts[n + 1].label], + GenericFunctionNodeData(; fnc=TestFunctorInferenceType1()) ), ), 1:(numNodes - 1), diff --git a/test/testBlocks.jl b/test/testBlocks.jl index 5c9af8c1..a27cb525 100644 --- a/test/testBlocks.jl +++ b/test/testBlocks.jl @@ -395,13 +395,13 @@ function DFGFactorSCA() gfnd = GenericFunctionNodeData(; fnc = TestCCW(TestFunctorInferenceType1())) - f1 = FactorCompute{TestCCW{TestFunctorInferenceType1}}(f1_lbl, [:a, :b]) f1 = FactorCompute(f1_lbl, [:a, :b], gfnd; tags = f1_tags, solvable = 0) - f2 = FactorCompute{TestCCW{TestFunctorInferenceType1}}( + f2 = FactorCompute( :bcf1, [:b, :c], - ZonedDateTime("2020-08-11T00:12:03.000-05:00"), + GenericFunctionNodeData(; fnc=TestCCW{TestFunctorInferenceType1}()); + timestamp = ZonedDateTime("2020-08-11T00:12:03.000-05:00"), ) #TODO add tests for mutating vos in updateFactor and orphan related checks. # we should perhaps prevent an empty vos @@ -473,7 +473,7 @@ function VariablesandFactorsCRUD_SET!(fg, v1, v2, v3, f0, f1, f2) @test getLabel(fg[getLabel(v1)]) == getLabel(v1) #TODO standardize this error and res also for that matter - fnope = FactorCompute{TestCCW{TestFunctorInferenceType1}}(:broken, [:a, :nope]) + fnope = FactorCompute(:broken, [:a, :nope], GenericFunctionNodeData(; fnc=TestCCW{TestFunctorInferenceType1}())) @test_throws KeyError addFactor!(fg, fnope) @test addFactor!(fg, f1) == f1 @@ -1683,7 +1683,7 @@ function ProducingDotFiles( end if f1 === nothing if (FACTYPE == FactorCompute) - f1 = FactorCompute{TestFunctorInferenceType1}(:abf1, [:a, :b]) + f1 = FactorCompute(:abf1, [:a, :b], GenericFunctionNodeData(;fnc = TestFunctorInferenceType1())) else f1 = FACTYPE(:abf1, [:a, :b]) end From eba3b38f5beea685ac0ba602a5ae9b42b061ddc8 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 13 Jun 2025 16:23:48 +0200 Subject: [PATCH 5/5] towards consolidated FactorCompute constructor --- src/Deprecated.jl | 33 ++++++++++++++++ src/entities/DFGFactor.jl | 73 ++++++++++++++++++++--------------- src/services/CompareUtils.jl | 4 +- src/services/DFGFactor.jl | 19 ++++++--- src/services/Serialization.jl | 22 +++++------ test/testBlocks.jl | 10 ++--- 6 files changed, 106 insertions(+), 55 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index f6fdbbc6..f93966fb 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -190,6 +190,39 @@ function updateVariableSolverData!( end end +function FactorCompute( + label::Symbol, + timestamp::Union{DateTime, ZonedDateTime}, + nstime::Nanosecond, + tags::Set{Symbol}, + solverData::GenericFunctionNodeData, + solvable::Int, + variableOrder::Union{Vector{Symbol}, Tuple}; + observation = getFactorType(solverData), + state::FactorState = FactorState(), + workmem::Base.RefValue{<:FactorOperationalMemory} = Ref{FactorOperationalMemory}(), + id::Union{UUID, Nothing} = nothing, + smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}(), +) + error( + "This constructor is deprecated, use FactorCompute(label, variableOrder, solverData; ...) instead", + ) + return FactorCompute( + id, + label, + tags, + Tuple(variableOrder), + timestamp, + nstime, + Ref(solverData), + Ref(solvable), + smallData, + observation, + state, + workmem, + ) +end + ## ================================================================================ ## Deprecated in v0.25 ##================================================================================= diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl index 6787ee97..9750a826 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -253,7 +253,7 @@ $(TYPEDFIELDS) """ Base.@kwdef struct FactorCompute{FT <: AbstractFactor, N} <: AbstractDFGFactor """The ID for the factor""" - id::Union{UUID, Nothing} = nothing + id::Union{UUID, Nothing} = nothing #TODO deprecate id """Factor label, e.g. :x1f1. Accessor: [`getLabel`](@ref)""" label::Symbol @@ -279,38 +279,46 @@ Base.@kwdef struct FactorCompute{FT <: AbstractFactor, N} <: AbstractDFGFactor smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}() #refactor fields + """Observation function or measurement for this factor. + Accessors: [`getObservation`](@ref)(@ref)""" observation::FT + """Describes the current state of the factor. Persisted in serialization. + Accessors: [`getFactorState`](@ref)""" state::FactorState - computeMem::Base.RefValue{<:FactorOperationalMemory} #TODO easy of use vs. performance as container is abstract in any case. + """Temporary, non-persistent memory used internally by the solver for intermediate numerical computations and buffers. + `workmem` is lazily allocated and only used during factor operations; it is not serialized or retained after solving. + Accessors: [`getWorkmem`](@ref), [`setWorkmem!`](@ref)""" + workmem::Base.RefValue{<:FactorOperationalMemory} #TODO easy of use vs. performance as container is abstract in any case. end ##------------------------------------------------------------------------------ ## Constructors -#TODO consolidate constructors, currently IIF calls only -# DFGFactor( -# Symbol(namestring), -# varOrderLabels, -# solverData; -# tags = Set(union(tags, [:FACTOR])), -# solvable, -# timestamp = _zonedtime(timestamp), -# ) - +# TODO standardize new fields in kw constructors, .id function FactorCompute( label::Symbol, - timestamp::Union{DateTime, ZonedDateTime}, - nstime::Nanosecond, - tags::Set{Symbol}, - solverData::GenericFunctionNodeData, - solvable::Int, - variableOrder::Union{Vector{Symbol}, Tuple}; - observation = getFactorType(solverData), + variableOrder::Union{Vector{Symbol}, Tuple}, + observation::AbstractFactor, state::FactorState = FactorState(), - computeMem::Base.RefValue{<:FactorOperationalMemory} = Ref{FactorOperationalMemory}(), + workmem = Ref{FactorOperationalMemory}(); + tags::Set{Symbol} = Set{Symbol}(), + timestamp::Union{DateTime, ZonedDateTime} = now(localzone()), + solvable::Int = 1, + nstime::Nanosecond = Nanosecond(0), id::Union{UUID, Nothing} = nothing, smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}(), + solverData = nothing ) + if isnothing(solverData) + refsolverData = Ref{GenericFunctionNodeData}() #TODO solverData deprecated in v0.27 WIP + else + Base.depwarn( + "`FactorCompute` solverData is deprecated", + :FactorCompute, + ) + refsolverData = Ref(solverData) + end + return FactorCompute( id, label, @@ -318,16 +326,15 @@ function FactorCompute( Tuple(variableOrder), timestamp, nstime, - Ref(solverData), + refsolverData, Ref(solvable), smallData, observation, state, - computeMem, + workmem, ) end -# TODO standardize new fields in kw constructors, .id function FactorCompute( label::Symbol, variableOrder::Union{Vector{Symbol}, Tuple}, @@ -339,6 +346,10 @@ function FactorCompute( id::Union{UUID, Nothing} = nothing, smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}(), ) + Base.depwarn( + "`FactorCompute` constructor with `GenericFunctionNodeData` is deprecated. observation, state, and workmem should be provided explicitly.", + :FactorCompute, + ) observation = getFactorType(solverData) state = FactorState( solverData.eliminated, @@ -351,24 +362,24 @@ function FactorCompute( ) if solverData.fnc isa FactorOperationalMemory - computeMem = Ref(solverData.fnc) + workmem = Ref(solverData.fnc) else - computeMem = Ref{FactorOperationalMemory}() + workmem = Ref{FactorOperationalMemory}() end return FactorCompute( label, + Tuple(variableOrder), + observation, + state, + workmem; + id, timestamp, nstime, tags, solverData, - solvable, - Tuple(variableOrder); - observation, - computeMem, - id, smallData, - state, + solvable, ) end diff --git a/src/services/CompareUtils.jl b/src/services/CompareUtils.jl index 99047266..26a39c91 100644 --- a/src/services/CompareUtils.jl +++ b/src/services/CompareUtils.jl @@ -34,7 +34,7 @@ const GeneratedCompareUnion = Union{ } @generated function ==(x::T, y::T) where {T <: GeneratedCompareUnion} - ignored = [:computeMem] + ignored = [:workmem] return mapreduce( n -> :(x.$n == y.$n), (a, b) -> :($a && $b), @@ -321,7 +321,7 @@ function compareFactor( :attributes, :solverData, :observation, - :computeMem, + :workmem, :_variableOrderSymbols, :_gradients, ], diff --git a/src/services/DFGFactor.jl b/src/services/DFGFactor.jl index fd037140..69af1038 100644 --- a/src/services/DFGFactor.jl +++ b/src/services/DFGFactor.jl @@ -39,6 +39,10 @@ getFactorType(f::FactorDFG) = getTypeFromSerializationModule(f.fnctype)() # TODO getFactorType(dfg::AbstractDFG, lbl::Symbol) = getFactorType(getFactor(dfg, lbl)) getState(f::AbstractDFGFactor) = f.state + +getFactorState(f::AbstractDFGFactor) = f.state +getFactorState(dfg::AbstractDFG, lbl::Symbol) = getFactorState(getFactor(dfg, lbl)) + getObservation(f::FactorCompute) = f.observation function getObservation(f::FactorDFG) #FIXME completely refactor to not need getTypeFromSerializationModule and just use StructTypes @@ -46,6 +50,9 @@ function getObservation(f::FactorDFG) return packtype(; JSON3.read(f.observJSON)...) end +getWorkmem(f::FactorCompute) = f.workmem[] +setWorkmem!(f::FactorCompute, workmem::FactorOperationalMemory) = f.workmem[] = workmem + """ $SIGNATURES @@ -155,12 +162,12 @@ end function setTimestamp(f::FactorCompute, ts::ZonedDateTime) return FactorCompute( f.label, - ts, - f.nstime, - f.tags, - f.solverData, - f.solvable, - getfield(f, :_variableOrderSymbols); + getfield(f, :_variableOrderSymbols), + f.solverData; + timestamp = ts, + nstime = f.nstime, + tags = f.tags, + solvable = f.solvable, id = f.id, ) end diff --git a/src/services/Serialization.jl b/src/services/Serialization.jl index bfa09243..6d86899c 100644 --- a/src/services/Serialization.jl +++ b/src/services/Serialization.jl @@ -403,23 +403,23 @@ function unpackFactor(dfg::AbstractDFG, factor::FactorDFG; skipVersionCheck::Boo metadata = JSON3.read(base64decode(factor.metadata), Dict{Symbol, DFG.SmallDataTypes}) if fullFactorData.fnc isa FactorOperationalMemory - computeMem = Ref(fullFactorData.fnc) + workmem = Ref(fullFactorData.fnc) else - computeMem = Ref{FactorOperationalMemory}() + workmem = Ref{FactorOperationalMemory}() end return FactorCompute( factor.label, - factor.timestamp, - Nanosecond(factor.nstime), - Set(factor.tags), - fullFactorData, - factor.solvable, - Tuple(factor._variableOrderSymbols); + Tuple(factor._variableOrderSymbols), + observation, + factor.state, + workmem; + solverData = fullFactorData, + nstime = Nanosecond(factor.nstime), + tags = Set(factor.tags), + solvable = factor.solvable, + timestamp = factor.timestamp, id = factor.id, smallData = metadata, - state = factor.state, - observation, - computeMem, ) end diff --git a/test/testBlocks.jl b/test/testBlocks.jl index a27cb525..245b11e5 100644 --- a/test/testBlocks.jl +++ b/test/testBlocks.jl @@ -493,12 +493,12 @@ function VariablesandFactorsCRUD_SET!(fg, v1, v2, v3, f0, f1, f2) if f2 isa FactorCompute f2_mod = FactorCompute( f2.label, - f2.timestamp, - f2.nstime, - f2.tags, - f2.solverData, - f2.solvable, (:a,), + f2.solverData; + timestamp = f2.timestamp, + nstime = f2.nstime, + tags = f2.tags, + solvable = f2.solvable, ) else f2_mod = deepcopy(f2)