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/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/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..9750a826 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 @@ -49,6 +48,17 @@ 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 +91,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 +102,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 +117,92 @@ 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 +214,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 +222,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 +251,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 #TODO deprecate id """Factor label, e.g. :x1f1. Accessor: [`getLabel`](@ref)""" label::Symbol @@ -198,117 +266,120 @@ 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 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 + """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 -""" -$(SIGNATURES) - -Construct a DFG factor given a label. -""" +# TODO standardize new fields in kw constructors, .id function FactorCompute( label::Symbol, - timestamp::Union{DateTime, ZonedDateTime}, - nstime::Nanosecond, - tags::Set{Symbol}, - solverData::GenericFunctionNodeData{T}, - solvable::Int, - _variableOrderSymbols::Tuple; + variableOrder::Union{Vector{Symbol}, Tuple}, + observation::AbstractFactor, + state::FactorState = FactorState(), + 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}(), -) 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} + 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, + tags, + Tuple(variableOrder), timestamp, - Nanosecond(0), - Set{Symbol}(), - data, - 1, - Tuple(variableOrderSymbols); - kw..., + nstime, + refsolverData, + Ref(solvable), + smallData, + observation, + state, + workmem, ) 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}( +) + Base.depwarn( + "`FactorCompute` constructor with `GenericFunctionNodeData` is deprecated. observation, state, and workmem should be provided explicitly.", + :FactorCompute, + ) + observation = getFactorType(solverData) + state = FactorState( + solverData.eliminated, + solverData.potentialused, + solverData.multihypo, + solverData.certainhypo, + solverData.nullhypo, + solverData.solveInProgress, + solverData.inflation, + ) + + if solverData.fnc isa FactorOperationalMemory + workmem = Ref(solverData.fnc) + else + workmem = Ref{FactorOperationalMemory}() + end + + return FactorCompute( label, + Tuple(variableOrder), + observation, + state, + workmem; + id, timestamp, nstime, tags, - data, - solvable, - Tuple(variableOrderSymbols); - id, + solverData, smallData, + solvable, ) end diff --git a/src/services/CompareUtils.jl b/src/services/CompareUtils.jl index a3f3d4f9..26a39c91 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 = [:workmem] 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, + :workmem, + :_variableOrderSymbols, + :_gradients, + ], + skip, + ) TP = compareAll(A, B; skip = skip_, show = show) @debug "compareFactor 1/5" TP TP = 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..69af1038 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 ##============================================================================== @@ -28,11 +32,27 @@ 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)) +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 + packtype = DFG.getTypeFromSerializationModule("Packed" * f.fnctype) + return packtype(; JSON3.read(f.observJSON)...) +end + +getWorkmem(f::FactorCompute) = f.workmem[] +setWorkmem!(f::FactorCompute, workmem::FactorOperationalMemory) = f.workmem[] = workmem + """ $SIGNATURES @@ -52,6 +72,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 ##============================================================================== @@ -89,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 808bd567..6d86899c 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,17 +402,68 @@ function unpackFactor(dfg::AbstractDFG, factor::FactorDFG; skipVersionCheck::Boo metadata = JSON3.read(base64decode(factor.metadata), Dict{Symbol, DFG.SmallDataTypes}) + if fullFactorData.fnc isa FactorOperationalMemory + workmem = Ref(fullFactorData.fnc) + else + 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, ) 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 + # 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..245b11e5 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 @@ -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) @@ -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