From f551cc70e1fff50a83b183b26e1ce0c8190b39f9 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 8 Apr 2026 10:35:19 +0200 Subject: [PATCH 1/3] Refactor Structure (only) --- .../Blobstores.jl} | 153 +--- src/Common.jl | 223 ----- src/DataBlobs/services/BlobEntry.jl | 471 ---------- src/Deprecated.jl | 21 + src/DistributedFactorGraphs.jl | 89 +- src/GraphsDFG/GraphsDFG.jl | 72 +- src/GraphsDFG/entities/GraphsDFG.jl | 8 - src/GraphsDFG/services/GraphsDFG.jl | 655 -------------- src/GraphsDFG/services/agent_ops.jl | 77 ++ src/GraphsDFG/services/blobentry_ops.jl | 286 ++++++ src/GraphsDFG/services/bloblet_ops.jl | 196 +++++ src/GraphsDFG/services/blobstore_ops.jl | 72 ++ src/GraphsDFG/services/factor_ops.jl | 112 +++ src/GraphsDFG/services/graph_ops.jl | 247 ++++++ src/GraphsDFG/services/state_ops.jl | 141 +++ src/GraphsDFG/services/tag_ops.jl | 79 ++ src/GraphsDFG/services/variable_ops.jl | 108 +++ .../services => Serialization}/BlobPacking.jl | 0 .../DFGStructStyles.jl | 2 +- .../DistributionSerialization.jl | 0 .../PackedSerialization.jl | 0 .../StateSerialization.jl | 0 src/entities/AbstractDFG.jl | 75 +- .../BlobEntry.jl => entities/Blobentry.jl} | 138 ++- src/entities/Bloblet.jl | 29 +- .../BlobStores.jl => entities/Blobstore.jl} | 0 src/{errors.jl => entities/Error.jl} | 0 src/entities/{DFGFactor.jl => Factor.jl} | 4 + src/entities/State.jl | 306 +++++++ src/entities/Tags.jl | 44 + src/entities/Timestamp.jl | 45 + src/entities/{DFGVariable.jl => Variable.jl} | 278 +----- src/entities/equality.jl | 53 ++ ...kdeps_prototypes.jl => extension_stubs.jl} | 0 src/services/AbstractDFG.jl | 833 +++++------------- src/services/Bloblet.jl | 264 ------ src/services/CommonAccessors.jl | 96 -- src/services/DFGVariable.jl | 533 ----------- src/services/Tags.jl | 164 ---- src/services/agent_ops.jl | 62 ++ .../blob_save_load.jl} | 0 src/services/blobentry_ops.jl | 203 +++++ src/services/bloblet_ops.jl | 205 +++++ src/services/blobstore_ops.jl | 171 ++++ src/services/{CompareUtils.jl => compare.jl} | 57 +- src/services/discovery.jl | 296 +++++++ src/services/{DFGFactor.jl => factor_ops.jl} | 96 +- src/services/find.jl | 132 --- src/services/graph_ops.jl | 131 +++ src/services/list.jl | 27 - src/services/{CustomPrinting.jl => print.jl} | 20 - src/services/state_ops.jl | 68 ++ src/services/tag_ops.jl | 122 +++ src/services/variable_ops.jl | 322 +++++++ test/interfaceTests.jl | 2 +- test/testBlocks.jl | 47 +- 56 files changed, 4052 insertions(+), 3783 deletions(-) rename src/{DataBlobs/services/BlobStores.jl => Blobstores/Blobstores.jl} (67%) delete mode 100644 src/Common.jl delete mode 100644 src/DataBlobs/services/BlobEntry.jl delete mode 100644 src/GraphsDFG/services/GraphsDFG.jl create mode 100644 src/GraphsDFG/services/agent_ops.jl create mode 100644 src/GraphsDFG/services/blobentry_ops.jl create mode 100644 src/GraphsDFG/services/bloblet_ops.jl create mode 100644 src/GraphsDFG/services/blobstore_ops.jl create mode 100644 src/GraphsDFG/services/factor_ops.jl create mode 100644 src/GraphsDFG/services/graph_ops.jl create mode 100644 src/GraphsDFG/services/state_ops.jl create mode 100644 src/GraphsDFG/services/tag_ops.jl create mode 100644 src/GraphsDFG/services/variable_ops.jl rename src/{DataBlobs/services => Serialization}/BlobPacking.jl (100%) rename src/{serialization => Serialization}/DFGStructStyles.jl (98%) rename src/{serialization => Serialization}/DistributionSerialization.jl (100%) rename src/{serialization => Serialization}/PackedSerialization.jl (100%) rename src/{serialization => Serialization}/StateSerialization.jl (100%) rename src/{DataBlobs/entities/BlobEntry.jl => entities/Blobentry.jl} (55%) rename src/{DataBlobs/entities/BlobStores.jl => entities/Blobstore.jl} (100%) rename src/{errors.jl => entities/Error.jl} (100%) rename src/entities/{DFGFactor.jl => Factor.jl} (99%) create mode 100644 src/entities/State.jl create mode 100644 src/entities/Tags.jl create mode 100644 src/entities/Timestamp.jl rename src/entities/{DFGVariable.jl => Variable.jl} (52%) create mode 100644 src/entities/equality.jl rename src/{weakdeps_prototypes.jl => extension_stubs.jl} (100%) delete mode 100644 src/services/Bloblet.jl delete mode 100644 src/services/CommonAccessors.jl delete mode 100644 src/services/DFGVariable.jl delete mode 100644 src/services/Tags.jl create mode 100644 src/services/agent_ops.jl rename src/{DataBlobs/services/BlobWrappers.jl => services/blob_save_load.jl} (100%) create mode 100644 src/services/blobentry_ops.jl create mode 100644 src/services/bloblet_ops.jl create mode 100644 src/services/blobstore_ops.jl rename src/services/{CompareUtils.jl => compare.jl} (88%) create mode 100644 src/services/discovery.jl rename src/services/{DFGFactor.jl => factor_ops.jl} (75%) delete mode 100644 src/services/find.jl create mode 100644 src/services/graph_ops.jl rename src/services/{CustomPrinting.jl => print.jl} (89%) create mode 100644 src/services/state_ops.jl create mode 100644 src/services/tag_ops.jl create mode 100644 src/services/variable_ops.jl diff --git a/src/DataBlobs/services/BlobStores.jl b/src/Blobstores/Blobstores.jl similarity index 67% rename from src/DataBlobs/services/BlobStores.jl rename to src/Blobstores/Blobstores.jl index 5dbc12bd..543217a3 100644 --- a/src/DataBlobs/services/BlobStores.jl +++ b/src/Blobstores/Blobstores.jl @@ -1,121 +1,8 @@ -##============================================================================== -## Blob CRUD interface -##============================================================================== - -""" -Get the data blob for the specified Blobstore or DFG. - -Related -[`getBlobentry`](@ref) -Implement -`getBlob(store::AbstractBlobstore, blobid::UUID)` - -$(METHODLIST) -""" -function getBlob end - -""" -Adds a blob to the Blobstore with the blobid. - -Related -[`addBlobentry!`](@ref) -Implement -`addBlob!(store::AbstractBlobstore, blobid::UUID, data)` -$(METHODLIST) -""" -function addBlob! end - -""" -Delete a blob from the blob store or dfg with the given entry. - -Related -[`deleteBlobentry!`](@ref) -Implement -`deleteBlob!(store::AbstractBlobstore, blobid::UUID)` -$(METHODLIST) -""" -function deleteBlob! end - -""" - $(SIGNATURES) -List all `blobid`s in the blob store. -Implement -`listBlobs(store::AbstractBlobstore)` -""" -function listBlobs end - -""" - $(SIGNATURES) -Check if the blob store has a blob with the given `blobid`. -""" -function hasBlob end - -##============================================================================== -## AbstractBlobstore derived CRUD for Blob -##============================================================================== -#TODO maybe we should generalize and move the cached Blobstore to DFG. -function getBlob(dfg::AbstractDFG, entry::Blobentry) - storeLabel = entry.storelabel - store = getBlobstore(dfg, storeLabel) - return getBlob(store, entry.blobid) -end - -function getBlob(store::AbstractBlobstore, entry::Blobentry) - return getBlob(store, entry.blobid) -end - -#add -function addBlob!(dfg::AbstractDFG, entry::Blobentry, data) - return addBlob!(getBlobstore(dfg, entry.storelabel), entry, data) -end - -function addBlob!(store::AbstractBlobstore{T}, entry::Blobentry, data::T) where {T} - return addBlob!(store, entry.blobid, data) -end - -# also creates an blobid as uuid4 -addBlob!(store::AbstractBlobstore, data) = addBlob!(store, uuid4(), data) -#delete -function deleteBlob!(dfg::AbstractDFG, entry::Blobentry) - return deleteBlob!(getBlobstore(dfg, entry.storelabel), entry) -end - -function deleteBlob!(store::AbstractBlobstore, entry::Blobentry) - return deleteBlob!(store, entry.blobid) -end - -#has -function hasBlob(store::AbstractBlobstore, entry::Blobentry) - return hasBlob(store, entry.blobid) -end -function hasBlob(dfg::AbstractDFG, entry::Blobentry) - return hasBlob(getBlobstore(dfg, entry.storelabel), entry.blobid) -end - -#TODO -# """ -# $(SIGNATURES) -# Copies all the entries from the source into the destination. -# Can specify which entries to copy with the `sourceEntries` parameter. -# Returns the list of copied entries. -# """ -# function copyBlobstore(sourceStore::D1, destStore::D2; sourceEntries=listEntries(sourceStore))::Vector{E} where {T, D1 <: AbstractDataStore{T}, D2 <: AbstractDataStore{T}, E <: Blobentry} -# # Quick check -# destEntries = listBlobs(destStore) -# typeof(sourceEntries) != typeof(destEntries) && error("Can't copy stores, source has entries of type $(typeof(sourceEntries)), destination has entries of type $(typeof(destEntries)).") -# # Same source/destination check -# sourceStore == destStore && error("Can't specify same store for source and destination.") -# # Otherwise, continue -# for sourceEntry in sourceEntries -# addBlob!(destStore, deepcopy(sourceEntry), getBlob(sourceStore, sourceEntry)) -# end -# return sourceEntries -# end - -##============================================================================== -## FolderStore -##============================================================================== +# ============================================================================== +# FolderStore +# TODO rename to FolderBlobstore +# ============================================================================== struct FolderStore{T} <: AbstractBlobstore{T} label::Symbol folder::String @@ -200,9 +87,10 @@ function listBlobs(store::FolderStore) return blobids end -##============================================================================== -## InMemoryBlobstore -##============================================================================== +# ============================================================================== +# InMemoryBlobstore +# TODO rename to MemoryBlobstore +# ============================================================================== struct InMemoryBlobstore{T} <: AbstractBlobstore{T} label::Symbol @@ -241,9 +129,10 @@ hasBlob(store::InMemoryBlobstore, blobid::UUID) = haskey(store.blobs, blobid) listBlobs(store::InMemoryBlobstore) = collect(keys(store.blobs)) -##============================================================================== -## LinkStore Link blobid to a existing local folder -##============================================================================== +# ============================================================================== +# LinkStore Link blobid to a existing local folder +# TODO Rename to LinkBlobstore +# ============================================================================== #TODO consider using a deterministic blobid (uuid5) with ns stored in the csv? @tags struct LinkStore <: AbstractBlobstore{String} label::Symbol @@ -292,9 +181,9 @@ end deleteBlob!(store::LinkStore, ::Blobentry) = deleteBlob!(store) deleteBlob!(store::LinkStore, ::UUID) = deleteBlob!(store) -##============================================================================== -## RowBlobstore Ordered Dict Row Table Blob Store -##============================================================================== +# ============================================================================== +# RowBlobstore Ordered Dict Row Table Blob Store +# ============================================================================== # RowBlob # T must be compatable with the AbstactRow iterator @@ -320,7 +209,7 @@ function Tables.columnnames(row::RowBlob) return (:id, Tables.columnnames(getfield(row, :blob))...) end -## RowBlobstore +# RowBlobstore struct RowBlobstore{T} <: AbstractBlobstore{T} label::Symbol @@ -350,7 +239,7 @@ Tables.rows(store::RowBlobstore) = values(store.blobs) #TODO # Tables.materializer(::Type{RowBlobstore{T}}) where T = Tables.rowtable -## +# function getBlob(store::RowBlobstore, blobid::UUID) if !haskey(store.blobs, blobid) throw(IdNotFoundError("Blob", blobid)) @@ -377,7 +266,7 @@ hasBlob(store::RowBlobstore, blobid::UUID) = haskey(store.blobs, blobid) listBlobs(store::RowBlobstore) = collect(keys(store.blobs)) # TODO also see about wrapping a table directly -## +# if false rb = RowBlob(uuid4(), (a = [1, 2], b = [3, 4])) @@ -399,7 +288,7 @@ if false # Tables.materializer(tstore) - ## + # struct Foo a::Float64 b::Float64 @@ -413,5 +302,5 @@ if false Tables.rowtable(sstore) end -## -## +# +# diff --git a/src/Common.jl b/src/Common.jl deleted file mode 100644 index 7ced5ad1..00000000 --- a/src/Common.jl +++ /dev/null @@ -1,223 +0,0 @@ - -##============================================================================== -## Sorting -##============================================================================== -#Natural Sorting less than - -# Adapted from https://rosettacode.org/wiki/Natural_sorting -# split at digit to not digit change -splitbynum(x::AbstractString) = split(x, r"(?<=\D)(?=\d)|(?<=\d)(?=\D)") -#parse to Int -function numstringtonum(arr::Vector{<:AbstractString}) - return [(n = tryparse(Int, e)) !== nothing ? n : e for e in arr] -end -#natural less than -function natural_lt(x::T, y::T) where {T <: AbstractString} - xarr = numstringtonum(splitbynum(x)) - yarr = numstringtonum(splitbynum(y)) - for i = 1:min(length(xarr), length(yarr)) - if typeof(xarr[i]) != typeof(yarr[i]) - return isa(xarr[i], Int) - elseif xarr[i] == yarr[i] - continue - else - return xarr[i] < yarr[i] - end - end - return length(xarr) < length(yarr) -end - -natural_lt(x::Symbol, y::Symbol) = natural_lt(string(x), string(y)) - -""" - $SIGNATURES - -Convenience wrapper for `Base.sort`. -Sort variable (factor) lists in a meaningful way (by `timestamp`, `label`, etc), for example `[:april;:x1_3;:x1_6;]` -Defaults to sorting by timestamp for variables and factors and using `natural_lt` for Symbols. -See Base.sort for more detail. - -Notes -- Not fool proof, but does better than native sort. - -Example - -`sortDFG(ls(dfg))` -`sortDFG(ls(dfg), by=getLabel, lt=natural_lt)` - -Related - -ls, lsf -""" -function sortDFG(vars::Vector{<:AbstractGraphNode}; by = getTimestamp, kwargs...) - return sort(vars; by = by, kwargs...) -end -sortDFG(vars::Vector{Symbol}; lt = natural_lt, kwargs...) = sort(vars; lt = lt, kwargs...) - -##============================================================================== -## Filtering -##============================================================================== - -# std_numeric_predicates = [==, <, <=, >, >=, in] -# std_string_predicates = [==, in, contains, startswith, endswith] - -# # full list -# tags_includes(tag::Symbol) = Base.Fix1(in, tag) -# solvable_eq(x::Int) = ==(x) -# solvable_in(x::Vector{Int}) = in(x) -# solvable_lt(x::Int) = <(x) -# solvable_lte(x::Int) = <=(x) -# solvable_gt(x::Int) = >(x) -# solvable_gte(x::Int) = >=(x) -# label_in(x::Vector{Symbol}) = in(x) -# label_contains(x::String) = contains(x) -# label_startswith(x::String) = startswith(x) -# label_endswith(x::String) = endswith(x) -# type_eq(x::AbstractStateType) = ==(x) -# type_in(x::Vector{<:AbstractStateType}) = in(x) -# type_contains(x::String) = contains(x) -# type_startswith(x::String) = startswith(x) -# type_endswith(x::String) = endswith(x) - -# Set predicates -# collection_includes(item) = Base.Fix1(in, item) # collection includes item (item in collection) - -# not supported helper -# collection_overlap(collection) = !isdisjoint(collection) # collection overlaps with another collection - -""" - $SIGNATURES -Filter nodes in a DFG based on a predicate function. -This function modifies the input `nodes` vector in place, removing nodes that do not satisfy the predicate. - -- **For cross-backend compatibility:** - Use only the standard predicates (`==`, `<`, `<=`, `>`, `>=`, `in`, `contains`, `startswith`, `endswith`) - when you need your code to work with both in-memory and database-backed DFGs. - These are likely to be supported by database query languages and are defined in `std_numeric_predicates` and `std_string_predicates`. - -- **For in-memory only operations:** - You can use any Julia predicate, since you have full access to the data and Julia's capabilities. - This is more flexible but will not work if you later switch to a database backend or another programming language. - -Standard predicates -- Numeric predicates: `==`, `<`, `<=`, `>`, `>=`, `in` -- String predicates: `==`, `contains`, `startswith`, `endswith`, `in` -""" -function filterDFG! end - -filterDFG!(nodes, predicate::Nothing, by::Function = identity) = nodes -# function filterDFG!(nodes, predicate::Base.Fix2, by=identity) -function filterDFG!(nodes, predicate::Function, by::Function = identity) - return filter!(predicate ∘ by, nodes) -end - -# specialized for label::Symbol filtering -function filterDFG!(nodes, predicate::Function, by::typeof(getLabel)) - # TODO this is not as clean as it should be, revisit if any issues arise - # Standard predicates that needs to be converted to string to work with Symbols - # OR look for the type if predicate isa Base.Fix2 && (predicate.x isa AbstractString || predicate.x isa Regex) - if predicate isa Base.Fix2 && - typeof(predicate.f) in [typeof(contains), typeof(startswith), typeof(endswith)] - return filter!(predicate ∘ string ∘ by, nodes) - else - return filter!(predicate ∘ by, nodes) - end -end - -##============================================================================== -## Validation of session, robot, and user labels. -##============================================================================== -global _invalidIds = ["GRAPH", "AGENT", "VARIABLE", "FACTOR", "BLOB_ENTRY", "FACTORGRAPH"] - -const global _validLabelRegex::Regex = r"^[a-zA-Z][-\w\.\@]*$" - -""" -$(SIGNATURES) - -Returns true if the label is valid for node. -""" -function isValidLabel(id::Union{Symbol, String}) - _id = string(id) - return occursin(_validLabelRegex, _id) && !in(uppercase(_id), _invalidIds) -end - -""" - $SIGNATURES - -Small utility to return `::Int`, e.g. `0` from `getVariableLabelNumber(:x0)` - -Examples --------- -```julia -getVariableLabelNumber(:l10) # 10 -getVariableLabelNumber(:x1) # 1 -getVariableLabelNumber(:x1_10, "x1_") # 10 -``` - -DevNotes -- make prefix Regex based for longer -- i.e. `:apriltag578`, `:lm1_4` - -""" -function getVariableLabelNumber(vs::Symbol, prefix = string(vs)[1]) - return parse(Int, string(vs)[(length(prefix) + 1):end]) -end - -## ================================= -## Additional Downstream dispatches -## ================================= - -""" - $SIGNATURES - -Default non-parametric graph solution. -""" -function solveGraph! end - -""" - $SIGNATURES - -Standard parametric graph solution (Experimental). -""" -function solveGraphParametric! end - -# delta timestamps -calcDeltatime(from::Nanosecond, to::Nanosecond) = Dates.value(to - from) / 10^9 - -function calcDeltatime_ns(from::TimeDateZone, to::TimeDateZone) - # TODO (-)(x::TimeDateZone, y::TimeDateZone) was very slow so manually calculated here - # return Dates.value(convert(Nanosecond, to - from)) / 10^9 - # calculate the "fast part" (microsecond) of the delta timestamp in nanoseconds - fast_to = convert(Nanosecond, Microsecond(to)) + Nanosecond(to) - fast_from = convert(Nanosecond, Microsecond(from)) + Nanosecond(from) - delta_fast = fast_to - fast_from - # calculate the "slow part" (zoned date time) of the delta timestamp in nanoseconds - delta_zoned = convert(Nanosecond, ZonedDateTime(to) - ZonedDateTime(from)) - return delta_zoned + delta_fast -end - -function calcDeltatime(from::TimeDateZone, to::TimeDateZone) - # TODO (-)(x::TimeDateZone, y::TimeDateZone) was very slow so manually calculated here - # return Dates.value(convert(Nanosecond, to - from)) / 10^9 - return Dates.value(calcDeltatime_ns(from, to)) / 10^9 -end - -calcDeltatime(from_node, to_node) = calcDeltatime(from_node.timestamp, to_node.timestamp) - -Timestamp(args...) = TimeDateZone(args...) -Timestamp(t::Nanosecond, zone = tz"UTC") = Timestamp(Val(:unix), t, zone) -Timestamp(t::Microsecond, zone = tz"UTC") = Timestamp(Val(:unix), Nanosecond(t), zone) -function Timestamp(epoch::Val{:unix}, t::Nanosecond, zone = tz"UTC") - return TimeDateZone(TimeDate(1970) + t, zone) -end -function Timestamp(epoch::Val{:unix}, t::Float64, zone = tz"UTC") - return Timestamp(epoch, Nanosecond(t * 10^9), zone) -end -Timestamp(t::Float64, zone = tz"UTC") = Timestamp(Val(:unix), t, zone) -function Timestamp(epoch::Val{:rata}, t::Float64, zone = tz"UTC") - return TimeDateZone(convert(DateTime, Millisecond(t * 10^3)), zone) -end - -function now_tdz(zone = tz"UTC") - t = time() - return Timestamp(t, zone) -end diff --git a/src/DataBlobs/services/BlobEntry.jl b/src/DataBlobs/services/BlobEntry.jl deleted file mode 100644 index b1de9844..00000000 --- a/src/DataBlobs/services/BlobEntry.jl +++ /dev/null @@ -1,471 +0,0 @@ -##============================================================================== -## Blobentry - Generic node CRUD -##============================================================================== - -""" - $(SIGNATURES) -""" -function getBlobentry(node, label::Symbol) - !haskey(refBlobentries(node), label) && throw(LabelNotFoundError("Blobentry", label)) - return refBlobentries(node)[label] -end - -function getBlobentries( - node; - whereLabel::Union{Nothing, Function} = nothing, - whereBlobid::Union{Nothing, Function} = nothing, -) - entries = collect(values(refBlobentries(node))) - filterDFG!(entries, whereLabel, getLabel) - filterDFG!(entries, whereBlobid, x -> string(x.blobid)) - return entries -end - -""" - $(SIGNATURES) -""" -function addBlobentry!(node, entry::Blobentry) - label = getLabel(entry) - haskey(refBlobentries(node), label) && throw(LabelExistsError("Blobentry", label)) - refBlobentries(node)[label] = entry - return entry -end - -function addBlobentries!(node, entries::Vector{Blobentry}) - addBlobentry!.(node, entries) - return entries -end - -""" - $(SIGNATURES) -""" -function mergeBlobentry!(node, entry::Blobentry) - label = getLabel(entry) - refBlobentries(node)[label] = entry - return 1 -end - -function mergeBlobentries!(node, entries::Vector{Blobentry}) - #TODO optimize with something like: merge!(refBlobentries(node), entries) - mergeBlobentry!.(node, entries) - return length(entries) -end - -""" - $(SIGNATURES) -""" -function deleteBlobentry!(node, label::Symbol) - !haskey(refBlobentries(node), label) && return 0 - pop!(refBlobentries(node), label) - return 1 -end - -deleteBlobentry!(node, entry) = deleteBlobentry!(node, getLabel(entry)) - -function deleteBlobentries!(node, labels::Vector{Symbol}) - return sum(deleteBlobentry!.(node, labels)) -end - -""" - $(SIGNATURES) -List all Blobentry keys for a variable `label` in `dfg` -""" -function listBlobentries(node) - return collect(keys(refBlobentries(node))) -end - -""" - $SIGNATURES - -Does a blob entry exist with `label`. -""" -hasBlobentry(node, label::Symbol) = haskey(refBlobentries(node), label) - -##============================================================================== -## Blobentry - utils -##============================================================================== - -""" - checkHash(entry::Blobentry, blob) -> Union{Bool,Nothing} - -Checks the integrity of a blob against the hashes (crc32c, sha256) stored in the given `Blobentry`. - -- Returns `true` if all present hashes (`crchash`, `shahash`) match the computed values from `blob`. -- Returns `false` if any present hash does not match. -- Returns `nothing` if no hashes are stored in the `Blobentry` to check against. -""" -function checkHash(entry::Blobentry, blob) - if !isnothing(entry.crchash) - crc32c(blob) != entry.crchash && return false - end - if entry.shahash != "" - sha256(blob) != entry.shahash && return false - end - if isnothing(entry.crchash) && entry.shahash == "" - return nothing - end - return true -end - -function Base.show(io::IO, ::MIME"text/plain", entry::Blobentry) - println(io, "Blobentry {") - println(io, " blobid: ", entry.blobid) - println(io, " label: ", entry.label) - println(io, " storelabel: ", entry.storelabel) - println(io, " origin: ", entry.origin) - println(io, " description: ", entry.description) - println(io, " mimetype: ", entry.mimetype) - println(io, " timestamp ", entry.timestamp) - println(io, " version: ", entry.version) - return println(io, "}") -end - -# TODO Consider autogenerating all methods of the form: -# verbNoun(dfg::VariableCompute, label::Symbol, args...; kwargs...) = verbNoun(getVariable(dfg, label), args...; kwargs...) -# with something like: -# getvariablemethod = [ -# :getfirstBlobentry, -# ] -# for met in methodstooverload -# @eval DistributedFactorGraphs $met(dfg::AbstractDFG, label::Symbol, args...; kwargs...) = $met(getVariable(dfg, label), args...; kwargs...) -# end - -##============================================================================== -## Blobentry - CRUD -##============================================================================== -function getVariableBlobentry end -function getVariableBlobentries end -function addVariableBlobentry! end -function addVariableBlobentries! end -function mergeVariableBlobentry! end -function mergeVariableBlobentries! end -function deleteVariableBlobentry! end -function deleteVariableBlobentries! end - -function getFactorBlobentry end -function getFactorBlobentries end -function addFactorBlobentry! end -function addFactorBlobentries! end -function mergeFactorBlobentry! end -function mergeFactorBlobentries! end -function deleteFactorBlobentry! end -function deleteFactorBlobentries! end - -function listVariableBlobentries end -function listFactorBlobentries end - -function hasVariableBlobentry end -function hasFactorBlobentry end - -##============================================================================== -## Agent/Graph/Model Blob Entries CRUD -##============================================================================== - -function getGraphBlobentry end -function getGraphBlobentries end -function addGraphBlobentry! end -function addGraphBlobentries! end -function mergeGraphBlobentry! end -function mergeGraphBlobentries! end -function deleteGraphBlobentry! end -function deleteGraphBlobentries! end - -function getAgentBlobentry end -function getAgentBlobentries end -function addAgentBlobentry! end -function addAgentBlobentries! end -function mergeAgentBlobentry! end -function mergeAgentBlobentries! end -function deleteAgentBlobentry! end -function deleteAgentBlobentries! end - -function getModelBlobentry end -function getModelBlobentries end -function addModelBlobentry! end -function addModelBlobentries! end -function mergeModelBlobentry! end -function mergeModelBlobentries! end -function deleteModelBlobentry! end -function deleteModelBlobentries! end - -function listGraphBlobentries end -function listAgentBlobentries end -function listModelBlobentries end - -function hasGraphBlobentry end -function hasAgentBlobentry end -function hasModelBlobentry end - -##============================================================================== -## Default Variable/Factor implementations -##============================================================================== - -function getVariableBlobentry(dfg::AbstractDFG, variableLabel::Symbol, label::Symbol) - return getBlobentry(getVariable(dfg, variableLabel), label) -end - -function getVariableBlobentries( - dfg::AbstractDFG, - variableLabel::Symbol; - whereLabel::Union{Nothing, Function} = nothing, - whereBlobid::Union{Nothing, Function} = nothing, -) - return getBlobentries(getVariable(dfg, variableLabel); whereLabel, whereBlobid) -end - -function listVariableBlobentries(dfg::AbstractDFG, variableLabel::Symbol) - return listBlobentries(getVariable(dfg, variableLabel)) -end - -function hasVariableBlobentry(dfg::AbstractDFG, variableLabel::Symbol, label::Symbol) - return hasBlobentry(getVariable(dfg, variableLabel), label) -end - -function getFactorBlobentry(dfg::AbstractDFG, factorLabel::Symbol, label::Symbol) - return getBlobentry(getFactor(dfg, factorLabel), label) -end - -function getFactorBlobentries( - dfg::AbstractDFG, - factorLabel::Symbol; - whereLabel::Union{Nothing, Function} = nothing, - whereBlobid::Union{Nothing, Function} = nothing, -) - return getBlobentries(getFactor(dfg, factorLabel); whereLabel, whereBlobid) -end - -function listFactorBlobentries(dfg::AbstractDFG, factorLabel::Symbol) - return listBlobentries(getFactor(dfg, factorLabel)) -end - -function hasFactorBlobentry(dfg::AbstractDFG, factorLabel::Symbol, label::Symbol) - return hasBlobentry(getFactor(dfg, factorLabel), label) -end -##============================================================================== -## Blobentry [default] bulk operations -##============================================================================== - -function addVariableBlobentries!(dfg::AbstractDFG, vLbl::Symbol, entries::Vector{Blobentry}) - addVariableBlobentry!.(dfg, vLbl, entries) - return entries -end - -function addFactorBlobentries!(dfg::AbstractDFG, fLbl::Symbol, entries::Vector{Blobentry}) - addFactorBlobentry!.(dfg, fLbl, entries) - return entries -end - -function addAgentBlobentries!( - dfg::AbstractDFG, - agentlabel::Symbol, - entries::Vector{Blobentry}, -) - addAgentBlobentry!.(dfg, agentlabel, entries) - return entries -end - -function addGraphBlobentries!(dfg::AbstractDFG, entries::Vector{Blobentry}) - addGraphBlobentry!.(dfg, entries) - return entries -end - -function mergeVariableBlobentries!( - dfg::AbstractDFG, - vLbl::Symbol, - entries::Vector{Blobentry}, -) - mergeVariableBlobentry!.(dfg, vLbl, entries) - return length(entries) -end -function mergeFactorBlobentries!(dfg::AbstractDFG, fLbl::Symbol, entries::Vector{Blobentry}) - mergeFactorBlobentry!.(dfg, fLbl, entries) - return length(entries) -end -function mergeAgentBlobentries!( - dfg::AbstractDFG, - agentlabel::Symbol, - entries::Vector{Blobentry}, -) - mergeAgentBlobentry!.(dfg, agentlabel, entries) - return length(entries) -end -function mergeGraphBlobentries!(dfg::AbstractDFG, entries::Vector{Blobentry}) - mergeGraphBlobentry!.(dfg, entries) - return length(entries) -end - -function deleteVariableBlobentries!( - dfg::AbstractDFG, - varLabel::Symbol, - labels::Vector{Symbol}, -) - cnts = map(labels) do label - return deleteVariableBlobentry!(dfg, varLabel, label) - end - return sum(cnts) -end - -function deleteFactorBlobentries!( - dfg::AbstractDFG, - facLabel::Symbol, - labels::Vector{Symbol}, -) - cnts = map(labels) do label - return deleteFactorBlobentry!(dfg, facLabel, label) - end - return sum(cnts) -end - -function deleteAgentBlobentries!( - dfg::AbstractDFG, - agentlabel::Symbol, - labels::Vector{Symbol}, -) - cnts = map(labels) do label - return deleteAgentBlobentry!(dfg, agentlabel, label) - end - return sum(cnts) -end - -function deleteGraphBlobentries!(dfg::AbstractDFG, labels::Vector{Symbol}) - cnts = map(labels) do label - return deleteGraphBlobentry!(dfg, label) - end - return sum(cnts) -end - -##============================================================================== -## Blobentry - Helper functions, Lists, etc -##============================================================================== - -function gatherBlobentries( - dfg::AbstractDFG; - whereLabel::Union{Nothing, Function} = nothing, - whereBlobid::Union{Nothing, Function} = nothing, - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - whereType::Union{Nothing, Function} = nothing, - whereVariableLabel::Union{Nothing, Function} = nothing, -) - vls = listVariables( - dfg; - whereSolvable, - whereTags, - whereType, - whereLabel = whereVariableLabel, - ) - return map(vls) do vl - return vl => getVariableBlobentries(dfg, vl; whereLabel, whereBlobid) - end -end -const collectBlobentries = gatherBlobentries - -""" - $(SIGNATURES) -Finds and returns the first blob entry that matches the filter. -The result is sorted by `sortby[=getLabel]` and `sortlt[=natural_lt]` before returning the first entry. -Also see: [`getBlobentry`](@ref) -""" -function getfirstBlobentry( - node; - whereLabel::Union{Nothing, Function} = nothing, - whereBlobid::Union{Nothing, Function} = nothing, - sortby::Function = getLabel, - sortlt::Function = natural_lt, -) - entries = getBlobentries(node; whereLabel, whereBlobid) - if isempty(entries) - return nothing - else - return sort(entries; by = sortby, lt = sortlt)[1] - end -end - -function getfirstVariableBlobentry( - dfg::AbstractDFG, - label::Symbol; - whereLabel::Union{Nothing, Function} = nothing, - whereBlobid::Union{Nothing, Function} = nothing, -) - return getfirstBlobentry(getVariable(dfg, label); whereLabel, whereBlobid) -end - -## ============================================================================= -## TODO Maybe deprecate/remove -## ============================================================================= - -""" - $SIGNATURES -List a collection of blob entries per variable that match a particular `pattern::Regex`. - -Notes -- Optional sort function argument, default is unsorted. - - Likely use of `sortDFG` for basic Symbol sorting. - -Example -```julia -listBlobentrySequence(fg, :x0, r"IMG_CENTER", sortDFG) -15-element Vector{Symbol}: - :IMG_CENTER_21676 - :IMG_CENTER_21677 - :IMG_CENTER_21678 - :IMG_CENTER_21679 -... -``` -""" -function listBlobentrySequence( - dfg::AbstractDFG, - lb::Symbol, - pattern::Regex, - _sort::Function = (x) -> x, -) - # - ents_ = listVariableBlobentries(dfg, lb) - entReg = map(l -> match(pattern, string(l)), ents_) - entMsk = entReg .!== nothing - return ents_[findall(entMsk)] |> _sort -end - -""" - $SIGNATURES - -If the blob label `datalabel` already exists, then this function will return the name `datalabel_1`. -If the blob label `datalabel_1` already exists, then this function will return the name `datalabel_2`. -""" -function incrDataLabelSuffix( - dfg::AbstractDFG, - vla::Symbol, - bllb::Union{Symbol, <:AbstractString}; - datalabel = Ref(""), -) - count = 1 - hasund = false - len = 0 - try - de = getfirstVariableBlobentry(dfg, vla; whereLabel = contains(string(bllb))) - isnothing(de) && return Symbol(bllb) # no match, return as is - bllb = string(bllb) - # bllb *= bllb[end] != '_' ? "_" : "" - datalabel[] = string(de.label) - dlb = match(r"\d*", reverse(datalabel[])) - # slightly complicated search if blob name already has an underscore number suffix, e.g. `_4` - count, hasund, len = if occursin(Regex(dlb.match * "_"), reverse(datalabel[])) - parse(Int, dlb.match |> reverse) + 1, true, length(dlb.match) - else - 1, datalabel[][end] == '_', 0 - end - catch err - # append latest count - if !(err isa KeyError) - throw(err) - end - end - # the piece from old label without the suffix count number - bllb = datalabel[][1:(end - len)] - if !hasund || bllb[end] != '_' - bllb *= "_" - end - bllb *= string(count) - - return Symbol(bllb) -end diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 80580ebf..df63165b 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -717,3 +717,24 @@ function findShortestPathDijkstra( return findPath(dfg, from, to).path end end + +# """ +# $SIGNATURES + +# Small utility to return `::Int`, e.g. `0` from `getVariableLabelNumber(:x0)` + +# Examples +# -------- +# ```julia +# getVariableLabelNumber(:l10) # 10 +# getVariableLabelNumber(:x1) # 1 +# getVariableLabelNumber(:x1_10, "x1_") # 10 +# ``` + +# DevNotes +# - make prefix Regex based for longer -- i.e. `:apriltag578`, `:lm1_4` + +# """ +function getVariableLabelNumber(vs::Symbol, prefix = string(vs)[1]) + return parse(Int, string(vs)[(length(prefix) + 1):end]) +end diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index 4b2f535c..e0c21725 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -402,10 +402,6 @@ export hasAgent # public refAgents ## TODO maybe move to DFG from SDK -# addGraph! -# deleteGraph! -# listGraphs -# getGraphs # getModel # getModels # addModel! @@ -423,6 +419,7 @@ public pack, unpack # list of unstable functions not exported any more # will move to public or deprecate over time const unstable_functions::Vector{Symbol} = [ + :getGraph, :VariableSummary, :FactorSummary, :listNeighborhood, @@ -466,7 +463,6 @@ const unstable_functions::Vector{Symbol} = [ :getPointIdentity, :getPoint, :getCoordinates, - :getVariableLabelNumber,# TODO somewhat used, do we deprecate? :getfirstBlobentry,# TODO somewhat used, do we deprecate? :isVariable, :isFactor, @@ -509,6 +505,7 @@ const unstable_functions::Vector{Symbol} = [ # no set on these #deprecated in v0.29 + :getVariableLabelNumber,# TODO somewhat used, deprecated :setTags!, :VariableCompute, :AbstractPackedBelief, @@ -550,44 +547,42 @@ end ##============================================================================== # Entities -include("errors.jl") - include("entities/AbstractDFG.jl") +include("entities/Error.jl") +include("entities/Blobstore.jl") include("entities/Bloblet.jl") - -# Data Blob extensions -include("DataBlobs/entities/BlobEntry.jl") -include("DataBlobs/entities/BlobStores.jl") - -include("serialization/DFGStructStyles.jl") -include("serialization/PackedSerialization.jl") -include("serialization/DistributionSerialization.jl") - -include("entities/DFGFactor.jl") -# include("serialization/FactorSerialization.jl") - -include("entities/DFGVariable.jl") -include("serialization/StateSerialization.jl") - +include("entities/Blobentry.jl") +include("entities/Tags.jl") +include("entities/Timestamp.jl") include("entities/Agent_and_Graph.jl") - +include("entities/Factor.jl") +include("entities/State.jl") +include("entities/Variable.jl") +include("entities/equality.jl") +# Services include("services/AbstractDFG.jl") +include("services/blob_save_load.jl") +include("services/blobentry_ops.jl") +include("services/bloblet_ops.jl") +include("services/tag_ops.jl") +include("services/blobstore_ops.jl") +include("services/compare.jl") +include("services/factor_ops.jl") include("services/list.jl") -include("services/find.jl") -include("services/CommonAccessors.jl") -include("Common.jl") - -#Blobs -include("DataBlobs/services/BlobEntry.jl") -include("DataBlobs/services/BlobStores.jl") -include("DataBlobs/services/BlobPacking.jl") -include("DataBlobs/services/BlobWrappers.jl") - -#FIXME -function getSolvable end -function getStateKind end -function isInitialized end -function listTags end +include("services/agent_ops.jl") +include("services/graph_ops.jl") +include("services/print.jl") +include("services/state_ops.jl") +include("services/discovery.jl") +include("services/variable_ops.jl") + +# Modules and Drivers +include("Serialization/BlobPacking.jl") +include("Serialization/DFGStructStyles.jl") +include("Serialization/DistributionSerialization.jl") +include("Serialization/PackedSerialization.jl") +include("Serialization/StateSerialization.jl") + # In Memory Types include("GraphsDFG/GraphsDFG.jl") using .GraphsDFGs @@ -596,23 +591,13 @@ using .GraphsDFGs const InMemoryDFGTypes = Union{GraphsDFG} const LocalDFG = GraphsDFG -include("services/Tags.jl") -include("services/Bloblet.jl") - -# Common includes -include("services/DFGVariable.jl") -include("services/DFGFactor.jl") -include("Deprecated.jl") -include("services/CompareUtils.jl") - -# include("services/Sync.jl") - # Include the FilesDFG API. include("FileDFG/FileDFG.jl") +# Blobstore implementations +include("Blobstores/Blobstores.jl") -# Custom show and printing for variable factor etc. -include("services/CustomPrinting.jl") +include("extension_stubs.jl") -include("weakdeps_prototypes.jl") +include("Deprecated.jl") end diff --git a/src/GraphsDFG/GraphsDFG.jl b/src/GraphsDFG/GraphsDFG.jl index 2a76ee77..c9bddf53 100644 --- a/src/GraphsDFG/GraphsDFG.jl +++ b/src/GraphsDFG/GraphsDFG.jl @@ -9,6 +9,8 @@ using OrderedCollections using ...DistributedFactorGraphs using ...DistributedFactorGraphs: Agent, + refAgents, + getAgent, LabelNotFoundError, LabelExistsError, MergeConflictError, @@ -19,54 +21,30 @@ using ...DistributedFactorGraphs: filterDFG!, getSolvable, getStateKind, - getAgent, getGraphLabel, isInitialized, Bloblets, + addBloblet!, + addBloblets!, + getBloblet, + getBloblets, + mergeBloblet!, + mergeBloblets!, + deleteBloblet!, + deleteBloblets!, + listBloblets, + listNeighbors, + hasBloblet, Blobentries, - FolderStore, refTags, listTags, - patch! - -# import DFG functions to extend -import ...DistributedFactorGraphs: - setSolverParams!, - getFactor, - # getLabelDict, - addVariable!, - getVariable, - addFactor!, - getSolverParams, - hasVariable, - hasFactor, - isVariable, - isFactor, - mergeVariable!, - mergeFactor!, - deleteVariable!, - deleteFactor!, - getVariables, - listVariables, - ls, - getFactors, - listFactors, - lsf, - isConnected, - listNeighbors, - findPaths, - findPath, - getSubgraph, - getBiadjacencyMatrix, - toDot, - toDotFile, - findShortestPathDijkstra, - getGraphBlobentry, - getGraphBlobentries, - addGraphBlobentry!, - addGraphBlobentries!, - listGraphBlobentries, - listAgentBlobentries + patch!, + mergeTags!, + deleteTags!, + refStates, + refBlobstores, + hasBlobentry, + hasBlobstore include("FactorGraphs/FactorGraphs.jl") using .FactorGraphs @@ -74,7 +52,15 @@ using .FactorGraphs # export SymbolEdge, is_directed, has_edge # Imports include("entities/GraphsDFG.jl") -include("services/GraphsDFG.jl") +include("services/agent_ops.jl") +include("services/graph_ops.jl") +include("services/state_ops.jl") +include("services/variable_ops.jl") +include("services/factor_ops.jl") +include("services/blobentry_ops.jl") +include("services/bloblet_ops.jl") +include("services/blobstore_ops.jl") +include("services/tag_ops.jl") # Exports export GraphsDFG diff --git a/src/GraphsDFG/entities/GraphsDFG.jl b/src/GraphsDFG/entities/GraphsDFG.jl index 385e0819..d8c38cd2 100644 --- a/src/GraphsDFG/entities/GraphsDFG.jl +++ b/src/GraphsDFG/entities/GraphsDFG.jl @@ -22,14 +22,6 @@ DFG.refAgents(dfg::GraphsDFG) = dfg.agents DFG.getGraphLabel(dfg::GraphsDFG) = dfg.graph.label DFG.getDescription(dfg::GraphsDFG) = dfg.graph.description -""" - $(SIGNATURES) - -Create an in-memory GraphsDFG with the following parameters: -- T: Solver parameters (defaults to `NoSolverParams()`) -- V: Variable type -- F: Factor type -""" function GraphsDFG{T, V, F}( g::FactorGraph{Int, V, F} = FactorGraph{Int, V, F}(); # addHistory::Vector{Symbol} = Symbol[], diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl deleted file mode 100644 index 03dde187..00000000 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ /dev/null @@ -1,655 +0,0 @@ -function hasVariable(dfg::GraphsDFG, label::Symbol) - return haskey(dfg.g.variables, label) -end - -function hasFactor(dfg::GraphsDFG, label::Symbol) - return haskey(dfg.g.factors, label) -end - -function isVariable( - dfg::GraphsDFG{P, V, F}, - sym::Symbol, -) where {P <: AbstractDFGParams, V <: AbstractGraphVariable, F <: AbstractGraphFactor} - return haskey(dfg.g.variables, sym) -end - -function isFactor( - dfg::GraphsDFG{P, V, F}, - sym::Symbol, -) where {P <: AbstractDFGParams, V <: AbstractGraphVariable, F <: AbstractGraphFactor} - return haskey(dfg.g.factors, sym) -end - -function addVariable!( - dfg::GraphsDFG{<:AbstractDFGParams, V, <:AbstractGraphFactor}, - variable::V, -) where {V <: AbstractGraphVariable} - if haskey(dfg.g.variables, variable.label) - throw(LabelExistsError("Variable", variable.label)) - end - - FactorGraphs.addVariable!(dfg.g, variable) || return false - - # Track insertion - # push!(dfg.addHistory, variable.label) - - return variable -end - -function addVariable!( - dfg::GraphsDFG{<:AbstractDFGParams, VD, <:AbstractGraphFactor}, - variable::AbstractGraphVariable, -) where {VD <: AbstractGraphVariable} - return addVariable!(dfg, VD(variable)) -end - -function addFactor!( - dfg::GraphsDFG{<:AbstractDFGParams, <:AbstractGraphVariable, F}, - factor::F, -) where {F <: AbstractGraphFactor} - if haskey(dfg.g.factors, factor.label) - throw(LabelExistsError("Factor", factor.label)) - end - # TODO - # @assert FactorGraphs.addFactor!(dfg.g, getVariableOrder(factor), factor) - variableLabels = Symbol[factor.variableorder...] - for vlabel in variableLabels - !hasVariable(dfg, vlabel) && throw(LabelNotFoundError("Variable", vlabel)) - end - @assert FactorGraphs.addFactor!(dfg.g, variableLabels, factor) - return factor -end - -function addFactor!( - dfg::GraphsDFG{<:AbstractDFGParams, <:AbstractGraphVariable, F}, - factor::AbstractGraphFactor, -) where {F <: AbstractGraphFactor} - return addFactor!(dfg, F(factor)) -end - -function getVariable(dfg::GraphsDFG, label::Symbol) - if !haskey(dfg.g.variables, label) - throw(LabelNotFoundError("Variable", label)) - end - - return dfg.g.variables[label] -end - -function getFactor(dfg::GraphsDFG, label::Symbol) - if !haskey(dfg.g.factors, label) - throw(LabelNotFoundError("Factor", label)) - end - return dfg.g.factors[label] -end - -function mergeVariable!(dfg::GraphsDFG, variable::AbstractGraphVariable) - if !haskey(dfg.g.variables, variable.label) - addVariable!(dfg, variable) - else - patch!(dfg.g.variables[variable.label], variable) - end - # metrics = (; - # tags = length(variable.tags), - # states = length(variable.states), - # bloblets = length(variable.bloblets), - # blobentries = length(variable.blobentries), - # ) - #TODO return metrics or 1 to keep it simple? - # if 1, the merge result does not include the children. - # if metrics, the merge result includes the children counts - return 1 -end - -function mergeFactor!(dfg::GraphsDFG, factor::AbstractGraphFactor) - label = getLabel(factor) - if !haskey(dfg.g.factors, label) - addFactor!(dfg, factor) - else - patch!(dfg.g.factors[label], factor) - end - #TODO also same metrics consideration as mergeVariable! - return 1 -end - -function deleteVariable!(dfg::GraphsDFG, label::Symbol)#::Tuple{AbstractGraphVariable, Vector{<:AbstractGraphFactor}} - !haskey(dfg.g.variables, label) && return 0 - - # orphaned factors are not supported. - del_facs = map(l -> deleteFactor!(dfg, l), listNeighbors(dfg, label)) - - rem_vertex!(dfg.g, dfg.g.labels[label]) - return sum(del_facs; init = 0) + 1 -end - -function deleteFactor!(dfg::GraphsDFG, label::Symbol) - !haskey(dfg.g.factors, label) && return 0 - rem_vertex!(dfg.g, dfg.g.labels[label]) - return 1 -end - -# """ -# whereTags = ⊇([:x1]) -# whereTags([:x1, :x2]) -# true -# whereTags = Base.Fix1(in, :x1) -# whereTags([:x1, :x2]) -# true -# """ - -function getVariables( - dfg::GraphsDFG; - whereSolvable::Union{Nothing, Function} = nothing, - whereLabel::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - whereType::Union{Nothing, Function} = nothing, -) - variables = collect(values(dfg.g.variables)) - - filterDFG!(variables, whereLabel, getLabel) - filterDFG!(variables, whereSolvable, getSolvable) - filterDFG!(variables, whereTags, refTags) - filterDFG!(variables, whereType, getStateKind) - - return variables -end - -function listVariables( - dfg::GraphsDFG; - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - whereType::Union{Nothing, Function} = nothing, - whereLabel::Union{Nothing, Function} = nothing, -) - if !isnothing(whereSolvable) || !isnothing(whereTags) || !isnothing(whereType) - return map( - getLabel, - getVariables(dfg; whereSolvable, whereTags, whereType, whereLabel), - )::Vector{Symbol} - else - # Is it ok to continue using the internal keys property? collect(keys(dfg.g.variables)) allowcates a lot. - labels = copy(dfg.g.variables.keys) - filterDFG!(labels, whereLabel, string) - return labels - end -end - -function getFactors( - dfg::GraphsDFG; - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - whereType::Union{Nothing, Function} = nothing, - whereLabel::Union{Nothing, Function} = nothing, -) - factors = collect(values(dfg.g.factors)) - filterDFG!(factors, whereLabel, getLabel) - filterDFG!(factors, whereSolvable, getSolvable) - filterDFG!(factors, whereTags, refTags) - filterDFG!(factors, whereType, typeof ∘ DFG.getObservation) - return factors -end - -function listFactors( - dfg::GraphsDFG; - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - whereType::Union{Nothing, Function} = nothing, - whereLabel::Union{Nothing, Function} = nothing, -) - if !isnothing(whereSolvable) || !isnothing(whereTags) || !isnothing(whereType) - return map( - getLabel, - getFactors(dfg; whereSolvable, whereTags, whereType, whereLabel), - )::Vector{Symbol} - else - # Is it ok to continue using the internal keys property? collect(keys(dfg.g.factors)) allowcates a lot. - labels = copy(dfg.g.factors.keys) - filterDFG!(labels, whereLabel, string) - return labels - end -end - -function isConnected(dfg::GraphsDFG) - return Graphs.is_connected(dfg.g) - # return length(Graphs.connected_components(dfg.g)) == 1 -end - -function listNeighbors( - dfg::GraphsDFG, - label::Symbol; - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - solvable::Union{Nothing, Int} = nothing, #TODO deprecated for whereSolvable v0.29 -) - if !isnothing(solvable) - Base.depwarn( - "solvable kwarg is deprecated, use kwarg `whereSolvable = >=(solvable)` instead", #v0.29 - :listNeighbors, - ) - !isnothing(whereSolvable) && - error("Cannot use both solvable and whereSolvable kwargs.") - whereSolvable = >=(solvable) - end - - if !(hasVariable(dfg, label) || hasFactor(dfg, label)) - throw(LabelNotFoundError(label)) - end - - neighbors_il = FactorGraphs.outneighbors(dfg.g, dfg.g.labels[label]) - neighbors_ll = [dfg.g.labels[i] for i in neighbors_il] - - # Additional filtering - # solvable != 0 && filter!(lbl -> _isSolvable(dfg, lbl, solvable), neighbors_ll) - filterDFG!(neighbors_ll, whereSolvable, l -> getSolvable(dfg, l)) - filterDFG!(neighbors_ll, whereTags, l -> listTags(dfg, l)) - - # Variable sorting (order is important) - if haskey(dfg.g.factors, label) - order = intersect(dfg.g.factors[label].variableorder, neighbors_ll)#map(v->v.dfgNode.label, neighbors)) - return order::Vector{Symbol} - end - - return neighbors_ll::Vector{Symbol} -end - -function listNeighborhood( - dfg::GraphsDFG, - variableFactorLabels::Vector{Symbol}, - distance::Int; - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - solvable::Union{Nothing, Int} = nothing, #TODO deprecated for whereSolvable v0.29 -) - if !isnothing(solvable) - Base.depwarn( - "solvable kwarg is deprecated, use kwarg `whereSolvable = >=(solvable)` instead", #v0.29 - :listNeighborhood, - ) - !isnothing(whereSolvable) && - error("Cannot use both solvable and whereSolvable kwargs.") - whereSolvable = >=(solvable) - end - - # find neighbors at distance to add - nbhood = Int[] - - for l in variableFactorLabels - union!(nbhood, neighborhood(dfg.g, dfg.g.labels[l], distance)) - end - - allvarfacs = [dfg.g.labels[id] for id in nbhood] - - filterDFG!(allvarfacs, whereSolvable, l -> getSolvable(dfg, l)) - filterDFG!(allvarfacs, whereTags, l -> listTags(dfg, l)) - - variableLabels = intersect(listVariables(dfg), allvarfacs) - factorLabels = intersect(listFactors(dfg), allvarfacs) - - # if filterOrphans - # filter!(factorLabels) do lbl - # issubset(getVariableOrder(fg, lbl), variableLabels) - # end - # end - - return variableLabels, factorLabels -end - -# TODO copy GraphsDFG to GraphsDFG overwrite -# function copyGraph!(destDFG::GraphsDFG, -# sourceDFG::GraphsDFG, -# variableFactorLabels::Vector{Symbol}; -# copyGraphMetadata::Bool=false, -# overwriteDest::Bool=false, -# deepcopyNodes::Bool=false, -# verbose::Bool = true) - -# Biadjacency Matrix https://en.wikipedia.org/wiki/Adjacency_matrix#Of_a_bipartite_graph -function getBiadjacencyMatrix( - dfg::GraphsDFG; - solvable::Union{Nothing, Int} = nothing, #TODO deprecated for whereSolvable v0.29 - whereSolvable = isnothing(solvable) ? nothing : >=(solvable), - varLabels = listVariables(dfg; whereSolvable), - factLabels = listFactors(dfg; whereSolvable), -) - varIndex = [dfg.g.labels[s] for s in varLabels] - factIndex = [dfg.g.labels[s] for s in factLabels] - - adj = adjacency_matrix(dfg.g) - - adjvf = adj[factIndex, varIndex] - return (B = adjvf, varLabels = varLabels, facLabels = factLabels) -end - -#TODO JT test. -""" - $(SIGNATURES) -A replacement for to_dot that saves only hardcoded factor graph plotting attributes. -""" -function savedot_attributes(io::IO, dfg::GraphsDFG) - write(io, "graph G {\n") - - for vl in listVariables(dfg) - write(io, "$vl [color=red, shape=ellipse];\n") - end - for fl in listFactors(dfg) - write( - io, - "$fl [color=blue, shape=box, fontsize=8, fixedsize=false, height=0.1, width=0.1];\n", - ) - end - - for e in edges(dfg.g) - write(io, "$(dfg.g.labels[src(e)]) -- $(dfg.g.labels[dst(e)])\n") - end - return write(io, "}\n") -end - -function toDotFile(dfg::GraphsDFG, fileName::String = "/tmp/dfg.dot") - open(fileName, "w") do fid - return savedot_attributes(fid, dfg) - end - return nothing -end - -function toDot(dfg::GraphsDFG) - m = PipeBuffer() - savedot_attributes(m, dfg) - data = take!(m) - close(m) - return String(data) -end - -#API design NOTE: -# Do not create new Verbs or Nouns for metric vs. topological pathfinding. findPaths is the universal router... findPaths(..., metric) -# for now we only look at topological paths. - -function findPaths(::typeof(all_simple_paths), dfg, from::Symbol, to::Symbol; kwargs...) - gpaths = Graphs.all_simple_paths(dfg.g, dfg.g.labels[from], dfg.g.labels[to]; kwargs...) - return map(p -> (path = map(i -> dfg.g.labels[i], p), dist = length(p) - 1), gpaths) -end - -function findPaths( - ::typeof(yen_k_shortest_paths), - dfg::GraphsDFG, - from::Symbol, - to::Symbol, - k::Int; - distmx = weights(dfg.g), - kwargs..., -) - (; paths, dists) = Graphs.yen_k_shortest_paths( - dfg.g, - dfg.g.labels[from], - dfg.g.labels[to], - distmx, - k; - kwargs..., - ) - return map(zip(paths, dists)) do (path, dist) - return (path = map(i -> dfg.g.labels[i], path), dist = dist) - end -end - -# note with default heuristic this is just dijkstra's algorithm -function findPaths( - ::typeof(a_star), - dfg::GraphsDFG, - from::Symbol, - to::Symbol; - distmx::AbstractMatrix{T} = weights(dfg.g), - heuristic = nothing, -) where {T} - #TODO make it easier to use label in the heuristic - heuristic = something(heuristic, (n) -> zero(T)) - edgepath = Graphs.a_star(dfg.g, dfg.g.labels[from], dfg.g.labels[to], distmx, heuristic) - - if isempty(edgepath) - return @NamedTuple{path::Vector{Symbol}, dist::T}[] - end - - path = [dfg.g.labels[edgepath[1].src]] - dist = zero(T) - for (; dst, src) in edgepath - push!(path, dfg.g.labels[dst]) - dist += distmx[src, dst] - end - - return [(path = path, dist = dist)] -end - -#TODO Move findPaths and findPath to AbstractDFG services as default implementations. -function findPaths( - dfg::AbstractDFG, - from::Symbol, - to::Symbol, - k::Int; - variableLabels::Union{Nothing, Vector{Symbol}} = nothing, - factorLabels::Union{Nothing, Vector{Symbol}} = nothing, - kwargs..., -) - # If the user provided restricted lists, build the subgraph automatically - active_dfg = - if isa(dfg, GraphsDFG) && isnothing(variableLabels) && isnothing(factorLabels) - dfg - else - vlabels = something(variableLabels, listVariables(dfg)) - flabels = something(factorLabels, listFactors(dfg)) - labels = vcat(vlabels, flabels) - DFG.getSubgraph( - GraphsDFG{NoSolverParams, VariableSkeleton, FactorSkeleton}, - dfg, - labels, - ) - end - !hasVariable(active_dfg, from) && - !hasFactor(active_dfg, from) && - throw(DFG.LabelNotFoundError(from)) - !hasVariable(active_dfg, to) && - !hasFactor(active_dfg, to) && - throw(DFG.LabelNotFoundError(to)) - - # optimization for k=1 since A* is more efficient than Yen's for single shortest path - if k == 1 - return findPaths(a_star, active_dfg, from, to; kwargs...) - else - return findPaths(yen_k_shortest_paths, active_dfg, from, to, k; kwargs...) - end -end - -function findPath( - dfg::AbstractDFG, - from::Symbol, - to::Symbol; - variableLabels::Union{Nothing, Vector{Symbol}} = nothing, - factorLabels::Union{Nothing, Vector{Symbol}} = nothing, - kwargs..., -) - paths = findPaths(dfg, from, to, 1; variableLabels, factorLabels, kwargs...) - - if isempty(paths) - return nothing - else - return first(paths) - end -end - -export bfs_tree -export dfs_tree -export traverseGraphTopologicalSort - -function Graphs.bfs_tree(fg::GraphsDFG, s::Symbol) - return bfs_tree(fg.g, fg.g.labels[s]) -end - -function Graphs.dfs_tree(fg::GraphsDFG, s::Symbol) - return dfs_tree(fg.g, fg.g.labels[s]) -end - -""" - $SIGNATURES - -Return a topological sort of a factor graph as a vector of vertex labels in topological order. -Starting from s::Symbol -""" -function traverseGraphTopologicalSort(fg::GraphsDFG, s::Symbol, fs_tree = bfs_tree) - tree = fs_tree(fg, s) - list = topological_sort_by_dfs(tree) - symlist = map(s -> fg.g.labels[s], list) - return symlist -end - -# FG blob entries -function getGraphBlobentry(fg::GraphsDFG, label::Symbol) - if !haskey(fg.graph.blobentries, label) - throw(LabelNotFoundError("GraphBlobentry", label)) - end - return fg.graph.blobentries[label] -end - -function getGraphBlobentries(fg::GraphsDFG; whereLabel::Union{Nothing, Function} = nothing) - entries = collect(values(fg.graph.blobentries)) - filterDFG!(entries, whereLabel, getLabel) - return entries -end - -function listGraphBlobentries(fg::GraphsDFG; whereLabel::Union{Nothing, Function} = nothing) - labels = collect(keys(fg.graph.blobentries)) - filterDFG!(labels, whereLabel, string) - return labels -end - -function listAgentBlobentries(fg::GraphsDFG, agentlabel::Symbol) - return collect(keys(fg.agents[agentlabel].blobentries)) -end - -function addGraphBlobentry!(fg::GraphsDFG, entry::Blobentry) - if haskey(fg.graph.blobentries, entry.label) - throw(LabelExistsError("Blobentry", entry.label)) - end - push!(fg.graph.blobentries, entry.label => entry) - return entry -end - -function addGraphBlobentries!(fg::GraphsDFG, entries::Vector{Blobentry}) - return map(entries) do entry - return addGraphBlobentry!(fg, entry) - end -end - -function DFG.addAgentBlobentry!(fg::GraphsDFG, agentlabel::Symbol, entry::Blobentry) - if haskey(fg.agents[agentlabel].blobentries, entry.label) - throw(LabelExistsError("Blobentry", entry.label)) - end - push!(fg.agents[agentlabel].blobentries, entry.label => entry) - return entry -end - -function DFG.addAgentBlobentries!( - fg::GraphsDFG, - agentlabel::Symbol, - entries::Vector{Blobentry}, -) - return map(entries) do entry - return addAgentBlobentry!(fg, agentlabel, entry) - end -end - -function DFG.getAgentBlobentry(fg::GraphsDFG, agentlabel::Symbol, label::Symbol) - if !haskey(fg.agents[agentlabel].blobentries, label) - throw(LabelNotFoundError("Blobentry", label)) - end - return fg.agents[agentlabel].blobentries[label] -end - -function DFG.getAgentBlobentries( - fg::GraphsDFG, - agentlabel::Symbol; - whereLabel::Union{Nothing, Function} = nothing, -) - entries = collect(values(fg.agents[agentlabel].blobentries)) - filterDFG!(entries, whereLabel, getLabel) - return entries -end - -function DFG.mergeGraphBlobentry!(dfg::GraphsDFG, entry::Blobentry) - DFG.refBlobentries(dfg.graph)[getLabel(entry)] = entry - return 1 -end - -function DFG.mergeAgentBlobentry!(dfg::GraphsDFG, agentlabel::Symbol, entry::Blobentry) - DFG.refBlobentries(dfg.agents[agentlabel])[getLabel(entry)] = entry - return 1 -end - -function DFG.mergeGraphBlobentries!(dfg::GraphsDFG, entries::Vector{Blobentry}) - cnts = map(entries) do entry - return mergeGraphBlobentry!(dfg, entry) - end - return sum(cnts) -end - -function DFG.mergeAgentBlobentries!( - dfg::GraphsDFG, - agentlabel::Symbol, - entries::Vector{Blobentry}, -) - cnts = map(entries) do entry - return mergeAgentBlobentry!(dfg, agentlabel, entry) - end - return sum(cnts) -end - -##============================================================================= -## Variable Blobentries -##============================================================================= - -function DFG.addVariableBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) - variable = getVariable(dfg, label) - addBlobentry!(variable, entry) - return entry -end - -function DFG.mergeVariableBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) - return mergeBlobentry!(getVariable(dfg, label), entry) -end - -function DFG.deleteVariableBlobentry!(dfg::GraphsDFG, label::Symbol, entryLabel::Symbol) - return deleteBlobentry!(getVariable(dfg, label), entryLabel) -end - -##============================================================================= -## Factor Blobentries -##============================================================================= - -function DFG.addFactorBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) - factor = getFactor(dfg, label) - addBlobentry!(factor, entry) - return entry -end - -function DFG.mergeFactorBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) - return mergeBlobentry!(getFactor(dfg, label), entry) -end - -function DFG.deleteFactorBlobentry!(dfg::GraphsDFG, label::Symbol, entryLabel::Symbol) - return deleteBlobentry!(getFactor(dfg, label), entryLabel) -end - -function DFG.deleteGraphBlobentry!(dfg::GraphsDFG, label::Symbol) - !haskey(dfg.graph.blobentries, label) && return 0 - delete!(dfg.graph.blobentries, label) - return 1 -end - -function DFG.deleteAgentBlobentry!(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) - !haskey(dfg.agents[agentlabel].blobentries, label) && return 0 - delete!(dfg.agents[agentlabel].blobentries, label) - return 1 -end - -function DFG.hasGraphBlobentry(dfg::GraphsDFG, label::Symbol) - return haskey(dfg.graph.blobentries, label) -end - -function DFG.hasAgentBlobentry(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) - return haskey(dfg.agents[agentlabel].blobentries, label) -end diff --git a/src/GraphsDFG/services/agent_ops.jl b/src/GraphsDFG/services/agent_ops.jl new file mode 100644 index 00000000..8d05722b --- /dev/null +++ b/src/GraphsDFG/services/agent_ops.jl @@ -0,0 +1,77 @@ +## ============================================================================= +## Agent in DFG CRUD +## ============================================================================= + +""" + $(SIGNATURES) +""" +function DFG.addAgent!(dfg::GraphsDFG, agent::Agent) + label = getLabel(agent) + haskey(refAgents(dfg), label) && throw(LabelExistsError("Agent", label)) + push!(refAgents(dfg), label => agent) + return agent +end + +""" + $(SIGNATURES) +""" +function DFG.getAgent(dfg::GraphsDFG, label::Symbol) + agent = get(refAgents(dfg), label, nothing) + if isnothing(agent) + available = listAgents(dfg) + isempty(available) && + @info "No agents in graph: `$(getGraphLabel(dfg))`. Use `addAgent!(dfg, Agent(; label=:myAgent))` to add one" + throw(LabelNotFoundError("Agent", label, available)) + end + return agent +end + +function DFG.getAgents(dfg::GraphsDFG) + return map(listAgents(dfg)) do label + return getAgent(dfg, label) + end +end + +""" + $(SIGNATURES) +""" +function DFG.mergeAgent!(dfg::GraphsDFG, agent::Agent) + label = getLabel(agent) + if hasAgent(dfg, label) + mergeAgent!(refAgents(dfg)[label], agent) + else + addAgent!(dfg, agent) + end + return 1 +end + +function DFG.mergeAgents!(dfg::GraphsDFG, agents::Vector{Agent}) + count = 0 + for agent in agents + count += mergeAgent!(dfg, agent) + end + return count +end + +""" + $(SIGNATURES) +""" +function DFG.deleteAgent!(dfg::GraphsDFG, label::Symbol) + !haskey(refAgents(dfg), label) && return 0 + pop!(refAgents(dfg), label) + return 1 +end + +""" + $(SIGNATURES) +""" +function DFG.listAgents(dfg::GraphsDFG) + return collect(keys(refAgents(dfg))) +end + +""" + $(SIGNATURES) +""" +function DFG.hasAgent(dfg::GraphsDFG, label::Symbol) + return haskey(refAgents(dfg), label) +end diff --git a/src/GraphsDFG/services/blobentry_ops.jl b/src/GraphsDFG/services/blobentry_ops.jl new file mode 100644 index 00000000..4c38fd95 --- /dev/null +++ b/src/GraphsDFG/services/blobentry_ops.jl @@ -0,0 +1,286 @@ +# ============================================================================== +# Agent Blobentries +# ============================================================================== + +function DFG.addAgentBlobentry!(fg::GraphsDFG, agentlabel::Symbol, entry::Blobentry) + if haskey(fg.agents[agentlabel].blobentries, entry.label) + throw(LabelExistsError("Blobentry", entry.label)) + end + push!(fg.agents[agentlabel].blobentries, entry.label => entry) + return entry +end + +function DFG.addAgentBlobentries!( + fg::GraphsDFG, + agentlabel::Symbol, + entries::Vector{Blobentry}, +) + return map(entries) do entry + return DFG.addAgentBlobentry!(fg, agentlabel, entry) + end +end + +function DFG.getAgentBlobentry(fg::GraphsDFG, agentlabel::Symbol, label::Symbol) + if !haskey(fg.agents[agentlabel].blobentries, label) + throw(LabelNotFoundError("Blobentry", label)) + end + return fg.agents[agentlabel].blobentries[label] +end + +function DFG.getAgentBlobentries( + fg::GraphsDFG, + agentlabel::Symbol; + whereLabel::Union{Nothing, Function} = nothing, +) + entries = collect(values(fg.agents[agentlabel].blobentries)) + filterDFG!(entries, whereLabel, getLabel) + return entries +end + +function DFG.mergeAgentBlobentry!(dfg::GraphsDFG, agentlabel::Symbol, entry::Blobentry) + DFG.refBlobentries(dfg.agents[agentlabel])[getLabel(entry)] = entry + return 1 +end + +function DFG.mergeAgentBlobentries!( + dfg::GraphsDFG, + agentlabel::Symbol, + entries::Vector{Blobentry}, +) + cnts = map(entries) do entry + return DFG.mergeAgentBlobentry!(dfg, agentlabel, entry) + end + return sum(cnts) +end + +function DFG.deleteAgentBlobentry!(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) + !haskey(dfg.agents[agentlabel].blobentries, label) && return 0 + delete!(dfg.agents[agentlabel].blobentries, label) + return 1 +end + +function DFG.deleteAgentBlobentries!( + dfg::GraphsDFG, + agentlabel::Symbol, + labels::Vector{Symbol}, +) + cnts = map(labels) do label + return deleteAgentBlobentry!(dfg, agentlabel, label) + end + return sum(cnts) +end + +function DFG.listAgentBlobentries(fg::GraphsDFG, agentlabel::Symbol) + return collect(keys(fg.agents[agentlabel].blobentries)) +end + +function DFG.hasAgentBlobentry(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) + return haskey(dfg.agents[agentlabel].blobentries, label) +end + +# ============================================================================== +# Graph Blobentries +# ============================================================================== +function DFG.addGraphBlobentry!(fg::GraphsDFG, entry::Blobentry) + if haskey(fg.graph.blobentries, entry.label) + throw(LabelExistsError("Blobentry", entry.label)) + end + push!(fg.graph.blobentries, entry.label => entry) + return entry +end + +function DFG.addGraphBlobentries!(fg::GraphsDFG, entries::Vector{Blobentry}) + return map(entries) do entry + return DFG.addGraphBlobentry!(fg, entry) + end +end + +function DFG.getGraphBlobentry(fg::GraphsDFG, label::Symbol) + if !haskey(fg.graph.blobentries, label) + throw(LabelNotFoundError("GraphBlobentry", label)) + end + return fg.graph.blobentries[label] +end + +function DFG.getGraphBlobentries( + fg::GraphsDFG; + whereLabel::Union{Nothing, Function} = nothing, +) + entries = collect(values(fg.graph.blobentries)) + filterDFG!(entries, whereLabel, getLabel) + return entries +end + +function DFG.mergeGraphBlobentry!(dfg::GraphsDFG, entry::Blobentry) + DFG.refBlobentries(dfg.graph)[getLabel(entry)] = entry + return 1 +end + +function DFG.mergeGraphBlobentries!(dfg::GraphsDFG, entries::Vector{Blobentry}) + cnts = map(entries) do entry + return DFG.mergeGraphBlobentry!(dfg, entry) + end + return sum(cnts) +end + +function DFG.deleteGraphBlobentry!(dfg::GraphsDFG, label::Symbol) + !haskey(dfg.graph.blobentries, label) && return 0 + delete!(dfg.graph.blobentries, label) + return 1 +end + +function DFG.deleteGraphBlobentries!(dfg::GraphsDFG, labels::Vector{Symbol}) + cnts = map(labels) do label + return deleteGraphBlobentry!(dfg, label) + end + return sum(cnts) +end + +function DFG.listGraphBlobentries( + fg::GraphsDFG; + whereLabel::Union{Nothing, Function} = nothing, +) + labels = collect(keys(fg.graph.blobentries)) + filterDFG!(labels, whereLabel, string) + return labels +end + +function DFG.hasGraphBlobentry(dfg::GraphsDFG, label::Symbol) + return haskey(dfg.graph.blobentries, label) +end + +# ============================================================================== +# Variable Blobentries +# ============================================================================== + +function DFG.addVariableBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) + variable = getVariable(dfg, label) + DFG.addBlobentry!(variable, entry) + return entry +end + +function DFG.addVariableBlobentries!( + dfg::GraphsDFG, + vLbl::Symbol, + entries::Vector{Blobentry}, +) + addVariableBlobentry!.(dfg, vLbl, entries) + return entries +end + +function DFG.getVariableBlobentry(dfg::GraphsDFG, variableLabel::Symbol, label::Symbol) + return DFG.getBlobentry(getVariable(dfg, variableLabel), label) +end + +function DFG.getVariableBlobentries( + dfg::GraphsDFG, + variableLabel::Symbol; + whereLabel::Union{Nothing, Function} = nothing, + whereBlobid::Union{Nothing, Function} = nothing, +) + return DFG.getBlobentries(getVariable(dfg, variableLabel); whereLabel, whereBlobid) +end + +function DFG.mergeVariableBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) + return DFG.mergeBlobentry!(getVariable(dfg, label), entry) +end + +function DFG.mergeVariableBlobentries!( + dfg::GraphsDFG, + vLbl::Symbol, + entries::Vector{Blobentry}, +) + cnts = map(entries) do entry + return DFG.mergeVariableBlobentry!(dfg, vLbl, entry) + end + return sum(cnts) +end + +function DFG.deleteVariableBlobentry!(dfg::GraphsDFG, label::Symbol, entryLabel::Symbol) + return DFG.deleteBlobentry!(getVariable(dfg, label), entryLabel) +end + +function DFG.deleteVariableBlobentries!( + dfg::GraphsDFG, + varLabel::Symbol, + labels::Vector{Symbol}, +) + cnts = map(labels) do label + return deleteVariableBlobentry!(dfg, varLabel, label) + end + return sum(cnts) +end + +function DFG.listVariableBlobentries(dfg::GraphsDFG, variableLabel::Symbol) + return DFG.listBlobentries(getVariable(dfg, variableLabel)) +end + +function DFG.hasVariableBlobentry(dfg::GraphsDFG, variableLabel::Symbol, label::Symbol) + return DFG.hasBlobentry(getVariable(dfg, variableLabel), label) +end + +# ============================================================================== +# Factor Blobentries +# ============================================================================== + +function DFG.addFactorBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) + factor = getFactor(dfg, label) + DFG.addBlobentry!(factor, entry) + return entry +end + +function DFG.addFactorBlobentries!(dfg::GraphsDFG, fLbl::Symbol, entries::Vector{Blobentry}) + DFG.addFactorBlobentry!.(dfg, fLbl, entries) + return entries +end + +function DFG.getFactorBlobentry(dfg::GraphsDFG, factorLabel::Symbol, label::Symbol) + return getBlobentry(getFactor(dfg, factorLabel), label) +end + +function DFG.getFactorBlobentries( + dfg::GraphsDFG, + factorLabel::Symbol; + whereLabel::Union{Nothing, Function} = nothing, + whereBlobid::Union{Nothing, Function} = nothing, +) + return getBlobentries(getFactor(dfg, factorLabel); whereLabel, whereBlobid) +end + +function DFG.mergeFactorBlobentry!(dfg::GraphsDFG, label::Symbol, entry::Blobentry) + return DFG.mergeBlobentry!(getFactor(dfg, label), entry) +end + +function DFG.mergeFactorBlobentries!( + dfg::GraphsDFG, + fLbl::Symbol, + entries::Vector{Blobentry}, +) + cnts = map(entries) do entry + return DFG.mergeFactorBlobentry!(dfg, fLbl, entry) + end + return sum(cnts) +end + +function DFG.deleteFactorBlobentry!(dfg::GraphsDFG, label::Symbol, entryLabel::Symbol) + return DFG.deleteBlobentry!(getFactor(dfg, label), entryLabel) +end + +function DFG.deleteFactorBlobentries!( + dfg::GraphsDFG, + facLabel::Symbol, + labels::Vector{Symbol}, +) + cnts = map(labels) do label + return DFG.deleteFactorBlobentry!(dfg, facLabel, label) + end + return sum(cnts) +end + +function DFG.listFactorBlobentries(dfg::GraphsDFG, factorLabel::Symbol) + return listBlobentries(getFactor(dfg, factorLabel)) +end + +function DFG.hasFactorBlobentry(dfg::GraphsDFG, factorLabel::Symbol, label::Symbol) + return hasBlobentry(getFactor(dfg, factorLabel), label) +end diff --git a/src/GraphsDFG/services/bloblet_ops.jl b/src/GraphsDFG/services/bloblet_ops.jl new file mode 100644 index 00000000..8059dbe1 --- /dev/null +++ b/src/GraphsDFG/services/bloblet_ops.jl @@ -0,0 +1,196 @@ +# TODO add whereLabel to get_Bloblets + +# ============================================================================== +# Agent Bloblets +# ============================================================================== +function DFG.addAgentBloblet!(dfg::GraphsDFG, agentlabel::Symbol, bloblet::Bloblet) + return addBloblet!(getAgent(dfg, agentlabel), bloblet) +end + +function DFG.addAgentBloblets!( + dfg::GraphsDFG, + agentlabel::Symbol, + bloblets::Vector{Bloblet}, +) + return addBloblets!(getAgent(dfg, agentlabel), bloblets) +end + +function DFG.getAgentBloblet(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) + return getBloblet(getAgent(dfg, agentlabel), label) +end + +function DFG.getAgentBloblets(dfg::GraphsDFG, agentlabel::Symbol) + return getBloblets(getAgent(dfg, agentlabel)) +end + +function DFG.mergeAgentBloblet!(dfg::GraphsDFG, agentlabel::Symbol, bloblet::Bloblet) + return mergeBloblet!(getAgent(dfg, agentlabel), bloblet) +end + +function DFG.mergeAgentBloblets!( + dfg::GraphsDFG, + agentlabel::Symbol, + bloblets::Vector{Bloblet}, +) + return mergeBloblets!(getAgent(dfg, agentlabel), bloblets) +end + +function DFG.deleteAgentBloblet!(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) + return deleteBloblet!(getAgent(dfg, agentlabel), label) +end + +function DFG.deleteAgentBloblets!( + dfg::GraphsDFG, + agentlabel::Symbol, + labels::Vector{Symbol}, +) + return deleteBloblets!(getAgent(dfg, agentlabel), labels) +end + +function DFG.listAgentBloblets(dfg::GraphsDFG, agentlabel::Symbol) + return listBloblets(getAgent(dfg, agentlabel)) +end + +function DFG.hasAgentBloblet(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) + return hasBloblet(getAgent(dfg, agentlabel), label) +end + +# ============================================================================== +# Graph Bloblets +# ============================================================================== + +DFG.addGraphBloblet!(dfg::GraphsDFG, bloblet::Bloblet) = addBloblet!(dfg.graph, bloblet) + +function DFG.addGraphBloblets!(dfg::GraphsDFG, bloblets::Vector{Bloblet}) + return addBloblets!(dfg.graph, bloblets) +end + +DFG.getGraphBloblet(dfg::GraphsDFG, label::Symbol) = getBloblet(dfg.graph, label) + +DFG.getGraphBloblets(dfg::GraphsDFG) = getBloblets(dfg.graph) + +DFG.mergeGraphBloblet!(dfg::GraphsDFG, bloblet::Bloblet) = mergeBloblet!(dfg.graph, bloblet) + +function DFG.mergeGraphBloblets!(dfg::GraphsDFG, bloblets::Vector{Bloblet}) + return mergeBloblets!(dfg.graph, bloblets) +end + +DFG.deleteGraphBloblet!(dfg::GraphsDFG, label::Symbol) = deleteBloblet!(dfg.graph, label) + +function DFG.deleteGraphBloblets!(dfg::GraphsDFG, labels::Vector{Symbol}) + return deleteBloblets!(dfg.graph, labels) +end + +DFG.listGraphBloblets(dfg::GraphsDFG) = listBloblets(dfg.graph) + +DFG.hasGraphBloblet(dfg::GraphsDFG, label::Symbol) = hasBloblet(dfg.graph, label) + +# ============================================================================== +# Variable Bloblets +# ============================================================================== +function DFG.addVariableBloblet!(dfg::GraphsDFG, var_label::Symbol, bloblet::Bloblet) + return addBloblet!(getVariable(dfg, var_label), bloblet) +end + +function DFG.addVariableBloblets!( + dfg::GraphsDFG, + var_label::Symbol, + bloblets::Vector{Bloblet}, +) + return addBloblets!(getVariable(dfg, var_label), bloblets) +end + +function DFG.getVariableBloblet(dfg::GraphsDFG, var_label::Symbol, label::Symbol) + return getBloblet(getVariable(dfg, var_label), label) +end + +function DFG.getVariableBloblets(dfg::GraphsDFG, var_label::Symbol) + return getBloblets(getVariable(dfg, var_label)) +end + +function DFG.mergeVariableBloblet!(dfg::GraphsDFG, var_label::Symbol, bloblet::Bloblet) + return mergeBloblet!(getVariable(dfg, var_label), bloblet) +end + +function DFG.mergeVariableBloblets!( + dfg::GraphsDFG, + var_label::Symbol, + bloblets::Vector{Bloblet}, +) + return mergeBloblets!(getVariable(dfg, var_label), bloblets) +end + +function DFG.deleteVariableBloblet!(dfg::GraphsDFG, var_label::Symbol, label::Symbol) + return deleteBloblet!(getVariable(dfg, var_label), label) +end + +function DFG.deleteVariableBloblets!( + dfg::GraphsDFG, + var_label::Symbol, + labels::Vector{Symbol}, +) + return deleteBloblets!(getVariable(dfg, var_label), labels) +end + +function DFG.listVariableBloblets(dfg::GraphsDFG, var_label::Symbol) + return listBloblets(getVariable(dfg, var_label)) +end + +function DFG.hasVariableBloblet(dfg::GraphsDFG, var_label::Symbol, label::Symbol) + return hasBloblet(getVariable(dfg, var_label), label) +end + +# ============================================================================== +# Factor Bloblets +# ============================================================================== +function DFG.addFactorBloblet!(dfg::GraphsDFG, fac_label::Symbol, bloblet::Bloblet) + return addBloblet!(getFactor(dfg, fac_label), bloblet) +end + +function DFG.addFactorBloblets!( + dfg::GraphsDFG, + fac_label::Symbol, + bloblets::Vector{Bloblet}, +) + return addBloblets!(getFactor(dfg, fac_label), bloblets) +end + +function DFG.getFactorBloblet(dfg::GraphsDFG, fac_label::Symbol, label::Symbol) + return getBloblet(getFactor(dfg, fac_label), label) +end + +function DFG.getFactorBloblets(dfg::GraphsDFG, fac_label::Symbol) + return getBloblets(getFactor(dfg, fac_label)) +end + +function DFG.mergeFactorBloblet!(dfg::GraphsDFG, fac_label::Symbol, bloblet::Bloblet) + return mergeBloblet!(getFactor(dfg, fac_label), bloblet) +end + +function DFG.mergeFactorBloblets!( + dfg::GraphsDFG, + fac_label::Symbol, + bloblets::Vector{Bloblet}, +) + return mergeBloblets!(getFactor(dfg, fac_label), bloblets) +end + +function DFG.deleteFactorBloblet!(dfg::GraphsDFG, fac_label::Symbol, label::Symbol) + return deleteBloblet!(getFactor(dfg, fac_label), label) +end + +function DFG.deleteFactorBloblets!( + dfg::GraphsDFG, + fac_label::Symbol, + labels::Vector{Symbol}, +) + return deleteBloblets!(getFactor(dfg, fac_label), labels) +end + +function DFG.listFactorBloblets(dfg::GraphsDFG, fac_label::Symbol) + return listBloblets(getFactor(dfg, fac_label)) +end + +function DFG.hasFactorBloblet(dfg::GraphsDFG, fac_label::Symbol, label::Symbol) + return hasBloblet(getFactor(dfg, fac_label), label) +end diff --git a/src/GraphsDFG/services/blobstore_ops.jl b/src/GraphsDFG/services/blobstore_ops.jl new file mode 100644 index 00000000..f1430757 --- /dev/null +++ b/src/GraphsDFG/services/blobstore_ops.jl @@ -0,0 +1,72 @@ +##============================================================================== +## AbstractBlobstore CRUD +##============================================================================== +# AbstractBlobstore should have label or overwrite getLabel + +DFG.refBlobstores(dfg::GraphsDFG) = dfg.blobstores + +function DFG.getBlobstore(dfg::GraphsDFG, storeLabel::Symbol) + store = get(refBlobstores(dfg), storeLabel, nothing) + if isnothing(store) + isempty(refBlobstores(dfg)) && + @info "No blobstores in graph: `$(getGraphLabel(dfg))`. Use `addBlobstore!(dfg, FolderStore(\"path/to/store\"))` to add one." + throw( + LabelNotFoundError("Blobstore", storeLabel, collect(keys(refBlobstores(dfg)))), + ) + end + return store +end + +function DFG.getBlobstores(dfg::GraphsDFG) + stores = map(listBlobstores(dfg)) do label + return getBlobstore(dfg, label) + end + return isempty(stores) ? AbstractBlobstore[] : stores +end + +function DFG.addBlobstore!(dfg::GraphsDFG, store::AbstractBlobstore) + label = getLabel(store) + haskey(refBlobstores(dfg), label) && throw(LabelExistsError("Blobstore", label)) + return push!(refBlobstores(dfg), label => store) +end + +# TODO edge api is a work in progress and only internal +function DFG.mergeStorelink!(dfg::GraphsDFG, link_to_store::AbstractBlobstore) + # we currently only have a label for a storelink, so we do not know if it is the same link + # so we have to look at the Blobstore node to find out. we only merge if the label=>store matches + # + label = getLabel(link_to_store) # the edge in this case is from the virtual dfg object to the Blobstore, so simply label. + if hasBlobstore(dfg, label) + existing_store = getBlobstore(dfg, label) + if existing_store != link_to_store + throw(MergeConflictError("Merge conflict for Blobstore with label $(label)")) + else + return 0 # no merge needed, they are the same store + end + else + push!(refBlobstores(dfg), label => link_to_store) + end + return 1 +end + +function DFG.deleteBlobstore!(dfg::GraphsDFG, key::Symbol) + !haskey(refBlobstores(dfg), key) && return 0 + pop!(refBlobstores(dfg), key) + return 1 +end +DFG.listBlobstores(dfg::GraphsDFG) = collect(keys(DFG.refBlobstores(dfg))) + +function DFG.mergeStorelinks!(destDFG::GraphsDFG, blobstores::Vector{<:AbstractBlobstore}) + count = 0 + for store in blobstores + count += DFG.mergeStorelink!(destDFG, store) + end + return count +end + +function DFG.hasBlobstore(dfg::GraphsDFG, label::Symbol) + return haskey(refBlobstores(dfg), label) +end + +#TODO empty as verb or only `deleteNouns!` +# emptyBlobstore!(dfg::GraphsDFG) = empty!(refBlobstores(dfg)) diff --git a/src/GraphsDFG/services/factor_ops.jl b/src/GraphsDFG/services/factor_ops.jl new file mode 100644 index 00000000..51bdb919 --- /dev/null +++ b/src/GraphsDFG/services/factor_ops.jl @@ -0,0 +1,112 @@ +function DFG.addFactor!( + dfg::GraphsDFG{<:AbstractDFGParams, <:AbstractGraphVariable, F}, + factor::F, +) where {F <: AbstractGraphFactor} + if haskey(dfg.g.factors, factor.label) + throw(LabelExistsError("Factor", factor.label)) + end + # TODO + # @assert FactorGraphs.addFactor!(dfg.g, getVariableOrder(factor), factor) + variableLabels = Symbol[factor.variableorder...] + for vlabel in variableLabels + !hasVariable(dfg, vlabel) && throw(LabelNotFoundError("Variable", vlabel)) + end + @assert FactorGraphs.addFactor!(dfg.g, variableLabels, factor) + return factor +end + +#TODO move to interface level +function DFG.addFactor!( + dfg::GraphsDFG{<:AbstractDFGParams, <:AbstractGraphVariable, F}, + factor::AbstractGraphFactor, +) where {F <: AbstractGraphFactor} + return addFactor!(dfg, F(factor)) +end + +function DFG.addFactors!(dfg::AbstractDFG, factors::Vector{<:AbstractGraphFactor}) + return asyncmap(factors) do f + return addFactor!(dfg, f) + end +end + +function DFG.getFactor(dfg::GraphsDFG, label::Symbol) + if !haskey(dfg.g.factors, label) + throw(LabelNotFoundError("Factor", label)) + end + return dfg.g.factors[label] +end + +function DFG.getFactors( + dfg::GraphsDFG; + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + whereType::Union{Nothing, Function} = nothing, + whereLabel::Union{Nothing, Function} = nothing, +) + factors = collect(values(dfg.g.factors)) + filterDFG!(factors, whereLabel, getLabel) + filterDFG!(factors, whereSolvable, getSolvable) + filterDFG!(factors, whereTags, refTags) + filterDFG!(factors, whereType, typeof ∘ DFG.getObservation) + return factors +end + +function DFG.mergeFactor!(dfg::GraphsDFG, factor::AbstractGraphFactor) + label = getLabel(factor) + if !haskey(dfg.g.factors, label) + addFactor!(dfg, factor) + else + patch!(dfg.g.factors[label], factor) + end + #TODO also same metrics consideration as mergeVariable! + return 1 +end + +function DFG.mergeFactors!(dfg::AbstractDFG, factors::Vector{<:AbstractGraphFactor}) + counts = asyncmap(f -> mergeFactor!(dfg, f), factors) + return sum(counts; init = 0) +end + +function DFG.deleteFactor!(dfg::GraphsDFG, label::Symbol) + !haskey(dfg.g.factors, label) && return 0 + rem_vertex!(dfg.g, dfg.g.labels[label]) + return 1 +end + +function DFG.deleteFactors!(dfg::AbstractDFG, labels::Vector{Symbol}) + counts = asyncmap(labels) do l + return deleteFactor!(dfg, l) + end + return sum(counts) +end + +function DFG.deleteFactors!(dfg::AbstractDFG; kwargs...) + labels = listFactors(dfg; kwargs...) + return deleteFactors!(dfg, labels) +end + +## + +function DFG.listFactors( + dfg::GraphsDFG; + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + whereType::Union{Nothing, Function} = nothing, + whereLabel::Union{Nothing, Function} = nothing, +) + if !isnothing(whereSolvable) || !isnothing(whereTags) || !isnothing(whereType) + return map( + getLabel, + getFactors(dfg; whereSolvable, whereTags, whereType, whereLabel), + )::Vector{Symbol} + else + # Is it ok to continue using the internal keys property? collect(keys(dfg.g.factors)) allowcates a lot. + labels = copy(dfg.g.factors.keys) + filterDFG!(labels, whereLabel, string) + return labels + end +end + +function DFG.hasFactor(dfg::GraphsDFG, label::Symbol) + return haskey(dfg.g.factors, label) +end diff --git a/src/GraphsDFG/services/graph_ops.jl b/src/GraphsDFG/services/graph_ops.jl new file mode 100644 index 00000000..a191ad79 --- /dev/null +++ b/src/GraphsDFG/services/graph_ops.jl @@ -0,0 +1,247 @@ + +function DFG.isVariable( + dfg::GraphsDFG{P, V, F}, + sym::Symbol, +) where {P <: AbstractDFGParams, V <: AbstractGraphVariable, F <: AbstractGraphFactor} + return haskey(dfg.g.variables, sym) +end + +function DFG.isFactor( + dfg::GraphsDFG{P, V, F}, + sym::Symbol, +) where {P <: AbstractDFGParams, V <: AbstractGraphVariable, F <: AbstractGraphFactor} + return haskey(dfg.g.factors, sym) +end + +function DFG.isConnected(dfg::GraphsDFG) + return Graphs.is_connected(dfg.g) + # return length(Graphs.connected_components(dfg.g)) == 1 +end + +function DFG.listNeighbors( + dfg::GraphsDFG, + label::Symbol; + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + solvable::Union{Nothing, Int} = nothing, #TODO deprecated for whereSolvable v0.29 +) + if !isnothing(solvable) + Base.depwarn( + "solvable kwarg is deprecated, use kwarg `whereSolvable = >=(solvable)` instead", #v0.29 + :listNeighbors, + ) + !isnothing(whereSolvable) && + error("Cannot use both solvable and whereSolvable kwargs.") + whereSolvable = >=(solvable) + end + + if !(hasVariable(dfg, label) || hasFactor(dfg, label)) + throw(LabelNotFoundError(label)) + end + + neighbors_il = FactorGraphs.outneighbors(dfg.g, dfg.g.labels[label]) + neighbors_ll = [dfg.g.labels[i] for i in neighbors_il] + + # Additional filtering + # solvable != 0 && filter!(lbl -> _isSolvable(dfg, lbl, solvable), neighbors_ll) + filterDFG!(neighbors_ll, whereSolvable, l -> getSolvable(dfg, l)) + filterDFG!(neighbors_ll, whereTags, l -> listTags(dfg, l)) + + # Variable sorting (order is important) + if haskey(dfg.g.factors, label) + order = intersect(dfg.g.factors[label].variableorder, neighbors_ll)#map(v->v.dfgNode.label, neighbors)) + return order::Vector{Symbol} + end + + return neighbors_ll::Vector{Symbol} +end + +function DFG.listNeighborhood( + dfg::GraphsDFG, + variableFactorLabels::Vector{Symbol}, + distance::Int; + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + solvable::Union{Nothing, Int} = nothing, #TODO deprecated for whereSolvable v0.29 +) + if !isnothing(solvable) + Base.depwarn( + "solvable kwarg is deprecated, use kwarg `whereSolvable = >=(solvable)` instead", #v0.29 + :listNeighborhood, + ) + !isnothing(whereSolvable) && + error("Cannot use both solvable and whereSolvable kwargs.") + whereSolvable = >=(solvable) + end + + # find neighbors at distance to add + nbhood = Int[] + + for l in variableFactorLabels + union!(nbhood, neighborhood(dfg.g, dfg.g.labels[l], distance)) + end + + allvarfacs = [dfg.g.labels[id] for id in nbhood] + + filterDFG!(allvarfacs, whereSolvable, l -> getSolvable(dfg, l)) + filterDFG!(allvarfacs, whereTags, l -> listTags(dfg, l)) + + variableLabels = intersect(listVariables(dfg), allvarfacs) + factorLabels = intersect(listFactors(dfg), allvarfacs) + + # if filterOrphans + # filter!(factorLabels) do lbl + # issubset(getVariableOrder(fg, lbl), variableLabels) + # end + # end + + return variableLabels, factorLabels +end + +# TODO copy GraphsDFG to GraphsDFG overwrite +# function copyGraph!(destDFG::GraphsDFG, +# sourceDFG::GraphsDFG, +# variableFactorLabels::Vector{Symbol}; +# copyGraphMetadata::Bool=false, +# overwriteDest::Bool=false, +# deepcopyNodes::Bool=false, +# verbose::Bool = true) + +# Biadjacency Matrix https://en.wikipedia.org/wiki/Adjacency_matrix#Of_a_bipartite_graph +function DFG.getBiadjacencyMatrix( + dfg::GraphsDFG; + solvable::Union{Nothing, Int} = nothing, #TODO deprecated for whereSolvable v0.29 + whereSolvable = isnothing(solvable) ? nothing : >=(solvable), + varLabels = listVariables(dfg; whereSolvable), + factLabels = listFactors(dfg; whereSolvable), +) + varIndex = [dfg.g.labels[s] for s in varLabels] + factIndex = [dfg.g.labels[s] for s in factLabels] + + adj = adjacency_matrix(dfg.g) + + adjvf = adj[factIndex, varIndex] + return (B = adjvf, varLabels = varLabels, facLabels = factLabels) +end + +#TODO JT test. +""" + $(SIGNATURES) +A replacement for to_dot that saves only hardcoded factor graph plotting attributes. +""" +function savedot_attributes(io::IO, dfg::GraphsDFG) + write(io, "graph G {\n") + + for vl in listVariables(dfg) + write(io, "$vl [color=red, shape=ellipse];\n") + end + for fl in listFactors(dfg) + write( + io, + "$fl [color=blue, shape=box, fontsize=8, fixedsize=false, height=0.1, width=0.1];\n", + ) + end + + for e in edges(dfg.g) + write(io, "$(dfg.g.labels[src(e)]) -- $(dfg.g.labels[dst(e)])\n") + end + return write(io, "}\n") +end + +function DFG.toDotFile(dfg::GraphsDFG, fileName::String = "/tmp/dfg.dot") + open(fileName, "w") do fid + return savedot_attributes(fid, dfg) + end + return nothing +end + +function DFG.toDot(dfg::GraphsDFG) + m = PipeBuffer() + savedot_attributes(m, dfg) + data = take!(m) + close(m) + return String(data) +end + +#API design NOTE: +# Do not create new Verbs or Nouns for metric vs. topological pathfinding. findPaths is the universal router... findPaths(..., metric) +# for now we only look at topological paths. + +function DFG.findPaths(::typeof(all_simple_paths), dfg, from::Symbol, to::Symbol; kwargs...) + gpaths = Graphs.all_simple_paths(dfg.g, dfg.g.labels[from], dfg.g.labels[to]; kwargs...) + return map(p -> (path = map(i -> dfg.g.labels[i], p), dist = length(p) - 1), gpaths) +end + +function DFG.findPaths( + ::typeof(yen_k_shortest_paths), + dfg::GraphsDFG, + from::Symbol, + to::Symbol, + k::Int; + distmx = weights(dfg.g), + kwargs..., +) + (; paths, dists) = Graphs.yen_k_shortest_paths( + dfg.g, + dfg.g.labels[from], + dfg.g.labels[to], + distmx, + k; + kwargs..., + ) + return map(zip(paths, dists)) do (path, dist) + return (path = map(i -> dfg.g.labels[i], path), dist = dist) + end +end + +# note with default heuristic this is just dijkstra's algorithm +function DFG.findPaths( + ::typeof(a_star), + dfg::GraphsDFG, + from::Symbol, + to::Symbol; + distmx::AbstractMatrix{T} = weights(dfg.g), + heuristic = nothing, +) where {T} + #TODO make it easier to use label in the heuristic + heuristic = something(heuristic, (n) -> zero(T)) + edgepath = Graphs.a_star(dfg.g, dfg.g.labels[from], dfg.g.labels[to], distmx, heuristic) + + if isempty(edgepath) + return @NamedTuple{path::Vector{Symbol}, dist::T}[] + end + + path = [dfg.g.labels[edgepath[1].src]] + dist = zero(T) + for (; dst, src) in edgepath + push!(path, dfg.g.labels[dst]) + dist += distmx[src, dst] + end + + return [(path = path, dist = dist)] +end + +export bfs_tree +export dfs_tree +export traverseGraphTopologicalSort + +function Graphs.bfs_tree(fg::GraphsDFG, s::Symbol) + return bfs_tree(fg.g, fg.g.labels[s]) +end + +function Graphs.dfs_tree(fg::GraphsDFG, s::Symbol) + return dfs_tree(fg.g, fg.g.labels[s]) +end + +""" + $SIGNATURES + +Return a topological sort of a factor graph as a vector of vertex labels in topological order. +Starting from s::Symbol +""" +function traverseGraphTopologicalSort(fg::GraphsDFG, s::Symbol, fs_tree = bfs_tree) + tree = fs_tree(fg, s) + list = topological_sort_by_dfs(tree) + symlist = map(s -> fg.g.labels[s], list) + return symlist +end diff --git a/src/GraphsDFG/services/state_ops.jl b/src/GraphsDFG/services/state_ops.jl new file mode 100644 index 00000000..11a5d8d3 --- /dev/null +++ b/src/GraphsDFG/services/state_ops.jl @@ -0,0 +1,141 @@ + +# ============================================================================= + +function DFG.addState!(dfg::GraphsDFG, variableLabel::Symbol, state::State) + var = getVariable(dfg, variableLabel) + return addState!(var, state) +end + +function DFG.addStates!(dfg::GraphsDFG, variableLabel::Symbol, states::Vector{<:State}) + cnt = asyncmap(states) do state + addState!(dfg, variableLabel, state) + return 1 + end + return sum(cnt) +end + +function DFG.addStates!( + dfg::GraphsDFG, + varLabel_state_pairs::Vector{<:Pair{Symbol, <:State}}, +) + cnt = asyncmap(varLabel_state_pairs) do (varLabel, state) + addState!(dfg, varLabel, state) + return 1 + end + return sum(cnt) +end + +# ============================================================================= + +function DFG.getState(dfg::GraphsDFG, variableLabel::Symbol, label::Symbol) + v = getVariable(dfg, variableLabel) + return getState(v, label) +end + +#TODO add filters +function DFG.getStates(dfg::GraphsDFG, variableLabel::Symbol) + v = getVariable(dfg, variableLabel) + return collect(values(refStates(v))) +end + +# ============================================================================== + +function DFG.mergeState!(dfg::GraphsDFG, variableLabel::Symbol, vnd::State) + return mergeState!(getVariable(dfg, variableLabel), vnd) +end + +function DFG.mergeStates!( + dfg::GraphsDFG, + varLabel_state_pairs::Vector{<:Pair{Symbol, <:State}}, +) + cnt = asyncmap(varLabel_state_pairs) do (varLabel, state) + return mergeState!(dfg, varLabel, state) + end + return sum(cnt) +end + +function DFG.mergeStates!(dfg::GraphsDFG, variableLabel::Symbol, states::Vector{<:State}) + cnt = asyncmap(states) do state + return mergeState!(dfg, variableLabel, state) + end + return sum(cnt) +end + +# ============================================================================= +function DFG.deleteState!(dfg::GraphsDFG, variableLabel::Symbol, label::Symbol) + return deleteState!(getVariable(dfg, variableLabel), label) +end + +function DFG.deleteState!(dfg::GraphsDFG, sourceVariable::VariableDFG, label::Symbol) + return deleteState!(dfg, sourceVariable.label, label) +end + +function DFG.deleteStates!(dfg::GraphsDFG, variableLabel::Symbol, labels::Vector{Symbol}) + cnt = asyncmap(labels) do label + return deleteState!(dfg, variableLabel, label) + end + return sum(cnt) +end + +function DFG.deleteStates!( + dfg::GraphsDFG, + varLabel_stateLabel_pairs::Vector{Pair{Symbol, Symbol}}, +) + cnt = asyncmap(varLabel_stateLabel_pairs) do (varLabel, stateLabel) + return deleteState!(dfg, varLabel, stateLabel) + end + return sum(cnt) +end + +# ============================================================================= + +function DFG.listStates( + dfg::GraphsDFG, + lbl::Symbol; + whereLabel::Union{Nothing, Function} = nothing, +) + return listStates(getVariable(dfg, lbl); whereLabel) +end + +function DFG.listStates( + dfg::GraphsDFG; + whereLabel::Union{Nothing, Function} = nothing, + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + whereType::Union{Nothing, Function} = nothing, + whereVariableLabel::Union{Nothing, Function} = nothing, +) + labels = Set{Symbol}() + vls = listVariables( + dfg; + whereSolvable, + whereTags, + whereType, + whereLabel = whereVariableLabel, + ) + for vl in vls + union!(labels, listStates(dfg, vl; whereLabel)) + end + return collect(labels) +end + +# ============================================================================= + +DFG.hasState(v::VariableDFG, label::Symbol) = haskey(v.states, label) +function DFG.hasState(dfg::GraphsDFG, variableLabel::Symbol, label::Symbol) + return hasState(getVariable(dfg, variableLabel), label) +end + +# ============================================================================= +# other TODO +# ============================================================================= + +function DFG.copytoState!( + dfg::GraphsDFG, + variableLabel::Symbol, + stateLabel::Symbol, + state::State, +) + newstate = State(state; label = stateLabel) + return mergeState!(dfg, variableLabel, newstate) +end diff --git a/src/GraphsDFG/services/tag_ops.jl b/src/GraphsDFG/services/tag_ops.jl new file mode 100644 index 00000000..9fdf00c7 --- /dev/null +++ b/src/GraphsDFG/services/tag_ops.jl @@ -0,0 +1,79 @@ +# ============================================================================== +# Agent Tags +# ============================================================================== +function DFG.mergeAgentTags!(dfg::GraphsDFG, agentlabel::Symbol, tags) + mergeTags!(getAgent(dfg, agentlabel), tags) + return length(tags) +end + +function DFG.deleteAgentTags!(dfg::GraphsDFG, agentlabel::Symbol, tags) + deleteTags!(getAgent(dfg, agentlabel), tags) + return length(tags) +end + +function DFG.listAgentTags(dfg::GraphsDFG, agentlabel::Symbol) + return listTags(getAgent(dfg, agentlabel)) +end + +function DFG.hasAgentTags(dfg::GraphsDFG, agentlabel::Symbol, tags::Vector{Symbol}) + return tags ⊆ listAgentTags(dfg, agentlabel) +end + +# ============================================================================== +# Graph Tags +# ============================================================================== +function DFG.mergeGraphTags!(dfg::GraphsDFG, tags) + mergeTags!(dfg.graph, tags) + return length(tags) +end + +function DFG.deleteGraphTags!(dfg::GraphsDFG, tags) + deleteTags!(dfg.graph, tags) + return length(tags) +end + +function DFG.listGraphTags(dfg::GraphsDFG) + return listTags(dfg.graph) +end + +function DFG.hasGraphTags(dfg::GraphsDFG, tags::Vector{Symbol}) + return tags ⊆ listGraphTags(dfg) +end + +# ============================================================================== +# Variable Tags +# ============================================================================== +function DFG.mergeVariableTags!(dfg::GraphsDFG, label::Symbol, tags) + return mergeTags!(getVariable(dfg, label), tags) +end + +function DFG.deleteVariableTags!(dfg::GraphsDFG, label::Symbol, tags) + return deleteTags!(getVariable(dfg, label), tags) +end + +function DFG.listVariableTags(dfg::GraphsDFG, sym::Symbol) + return listTags(getVariable(dfg, sym)) +end + +function DFG.hasVariableTags(dfg::GraphsDFG, sym::Symbol, tags::Vector{Symbol}) + return tags ⊆ listVariableTags(dfg, sym) +end + +# ============================================================================== +# Factor Tags +# ============================================================================== +function DFG.mergeFactorTags!(dfg::GraphsDFG, label::Symbol, tags) + return mergeTags!(getFactor(dfg, label), tags) +end + +function DFG.deleteFactorTags!(dfg::GraphsDFG, label::Symbol, tags) + return deleteTags!(getFactor(dfg, label), tags) +end + +function DFG.listFactorTags(dfg::GraphsDFG, sym::Symbol) + return listTags(getFactor(dfg, sym)) +end + +function DFG.hasFactorTags(dfg::GraphsDFG, sym::Symbol, tags::Vector{Symbol}) + return tags ⊆ listFactorTags(dfg, sym) +end diff --git a/src/GraphsDFG/services/variable_ops.jl b/src/GraphsDFG/services/variable_ops.jl new file mode 100644 index 00000000..03b3f106 --- /dev/null +++ b/src/GraphsDFG/services/variable_ops.jl @@ -0,0 +1,108 @@ +function DFG.addVariable!( + dfg::GraphsDFG{<:AbstractDFGParams, V, <:AbstractGraphFactor}, + variable::V, +) where {V <: AbstractGraphVariable} + if haskey(dfg.g.variables, variable.label) + throw(LabelExistsError("Variable", variable.label)) + end + + @assert FactorGraphs.addVariable!(dfg.g, variable) + + return variable +end + +#TODO move to interface level +function DFG.addVariable!( + dfg::GraphsDFG{<:AbstractDFGParams, VD, <:AbstractGraphFactor}, + variable::AbstractGraphVariable, +) where {VD <: AbstractGraphVariable} + return addVariable!(dfg, VD(variable)) +end + +function DFG.addVariables!(dfg::AbstractDFG, variables::Vector{<:AbstractGraphVariable}) + return asyncmap(variables) do v + return addVariable!(dfg, v) + end +end + +function DFG.getVariable(dfg::GraphsDFG, label::Symbol) + if !haskey(dfg.g.variables, label) + throw(LabelNotFoundError("Variable", label)) + end + + return dfg.g.variables[label] +end + +function DFG.getVariables( + dfg::GraphsDFG; + whereSolvable::Union{Nothing, Function} = nothing, + whereLabel::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + whereType::Union{Nothing, Function} = nothing, +) + variables = collect(values(dfg.g.variables)) + + filterDFG!(variables, whereLabel, getLabel) + filterDFG!(variables, whereSolvable, getSolvable) + filterDFG!(variables, whereTags, refTags) + filterDFG!(variables, whereType, getStateKind) + + return variables +end + +function DFG.mergeVariable!(dfg::GraphsDFG, variable::AbstractGraphVariable) + if !haskey(dfg.g.variables, variable.label) + addVariable!(dfg, variable) + else + patch!(dfg.g.variables[variable.label], variable) + end + # metrics = (; + # tags = length(variable.tags), + # states = length(variable.states), + # bloblets = length(variable.bloblets), + # blobentries = length(variable.blobentries), + # ) + #TODO return metrics or 1 to keep it simple? + # if 1, the merge result does not include the children. + # if metrics, the merge result includes the children counts + return 1 +end + +function DFG.mergeVariables!(dfg::AbstractDFG, variables::Vector{<:AbstractGraphVariable}) + counts = asyncmap(v -> mergeVariable!(dfg, v), variables) + return sum(counts; init = 0) +end + +function DFG.deleteVariable!(dfg::GraphsDFG, label::Symbol)#::Tuple{AbstractGraphVariable, Vector{<:AbstractGraphFactor}} + !haskey(dfg.g.variables, label) && return 0 + + # orphaned factors are not supported. + del_facs = map(l -> deleteFactor!(dfg, l), listNeighbors(dfg, label)) + + rem_vertex!(dfg.g, dfg.g.labels[label]) + return sum(del_facs; init = 0) + 1 +end + +function DFG.listVariables( + dfg::GraphsDFG; + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + whereType::Union{Nothing, Function} = nothing, + whereLabel::Union{Nothing, Function} = nothing, +) + if !isnothing(whereSolvable) || !isnothing(whereTags) || !isnothing(whereType) + return map( + getLabel, + getVariables(dfg; whereSolvable, whereTags, whereType, whereLabel), + )::Vector{Symbol} + else + # Is it ok to continue using the internal keys property? collect(keys(dfg.g.variables)) allowcates a lot. + labels = copy(dfg.g.variables.keys) + filterDFG!(labels, whereLabel, string) + return labels + end +end + +function DFG.hasVariable(dfg::GraphsDFG, label::Symbol) + return haskey(dfg.g.variables, label) +end diff --git a/src/DataBlobs/services/BlobPacking.jl b/src/Serialization/BlobPacking.jl similarity index 100% rename from src/DataBlobs/services/BlobPacking.jl rename to src/Serialization/BlobPacking.jl diff --git a/src/serialization/DFGStructStyles.jl b/src/Serialization/DFGStructStyles.jl similarity index 98% rename from src/serialization/DFGStructStyles.jl rename to src/Serialization/DFGStructStyles.jl index 0f6bb0a1..d1532fda 100644 --- a/src/serialization/DFGStructStyles.jl +++ b/src/Serialization/DFGStructStyles.jl @@ -52,7 +52,7 @@ function StructUtils.lift(::DFGJSONStyle, ::Type{T}, x::JSON.LazyValue) where {T return T(x[1][], x[2][]), nothing end -# Above does not work for Arrya{ComplexF64, 0} +# Above does not work for Array{ComplexF64, 0} StructUtils.lower(::DFGJSONStyle, x::AbstractArray{<:Complex, 0}) = (real(x), imag(x)) function StructUtils.lift( ::DFGJSONStyle, diff --git a/src/serialization/DistributionSerialization.jl b/src/Serialization/DistributionSerialization.jl similarity index 100% rename from src/serialization/DistributionSerialization.jl rename to src/Serialization/DistributionSerialization.jl diff --git a/src/serialization/PackedSerialization.jl b/src/Serialization/PackedSerialization.jl similarity index 100% rename from src/serialization/PackedSerialization.jl rename to src/Serialization/PackedSerialization.jl diff --git a/src/serialization/StateSerialization.jl b/src/Serialization/StateSerialization.jl similarity index 100% rename from src/serialization/StateSerialization.jl rename to src/Serialization/StateSerialization.jl diff --git a/src/entities/AbstractDFG.jl b/src/entities/AbstractDFG.jl index cabacf6a..12a5887f 100644 --- a/src/entities/AbstractDFG.jl +++ b/src/entities/AbstractDFG.jl @@ -1,6 +1,5 @@ - # TODO consider enforcing the full structure. -# This is not explicitly inforced, but surves as extra information of how the structure is put together. +# This is not explicitly enforced, but serves as extra information of how the structure is put together. # AbstractDFGNode are all nodes that make up a DFG, including Agent, Graph, Variable, Factor, Blobstore, Blobentry etc. # abstract type AbstractDFGNode end # any DFGNode shall have a label. @@ -54,3 +53,75 @@ function StructUtils.lower(::StructUtils.StructStyle, p::AbstractDFGParams) return StructUtils.lower(Packed(p)) end @choosetype AbstractDFGParams resolvePackedType + +##============================================================================== +## AbstractDFG +##============================================================================== +##------------------------------------------------------------------------------ +## Broadcasting +##------------------------------------------------------------------------------ +# to allow stuff like `getObservation.(dfg, [:x1x2f1;:x10l3f2])` +# https://docs.julialang.org/en/v1/manual/interfaces/# +Base.Broadcast.broadcastable(dfg::AbstractDFG) = Ref(dfg) + +# ------------------------------------------------------------------------------ +# References to containers +# ------------------------------------------------------------------------------ + +refTags(node) = node.tags +refBlobentries(node) = node.blobentries +refBloblets(node) = node.bloblets +refSolvable(node) = node.solvable + +""" + $(SIGNATURES) +""" +function refAgents end +function refGraph end + +# ============================================================================= + +""" + $(SIGNATURES) +Get the label of the node. +""" +getLabel(node) = node.label + +""" +$SIGNATURES + +Get the timestamp of a AbstractGraphNode. +""" +getTimestamp(node) = node.timestamp + +""" + $(SIGNATURES) +""" +getDescription(node) = node.description + +function Base.show(io::IO, ::MIME"text/plain", dfg::AbstractDFG) + summary(io, dfg) + println(io) + println(io, " GraphLabel: ", getGraphLabel(dfg)) + println(io, " Description: ", getDescription(dfg)) + println(io, " Nr variables: ", length(ls(dfg))) + println(io, " Nr factors: ", length(lsf(dfg))) + println(io, " Graph Bloblets: ", listGraphBloblets(dfg)) + println(io, " Agents: ", listAgents(dfg)) + println(io, " Blobstores: ", listBlobstores(dfg)) + return +end + +# ============================================================================== +# Validation of labels. +# ============================================================================== + +""" +$(SIGNATURES) + +Returns true if the label is valid for node. +""" +function isValidLabel(label::Union{Symbol, String}) + validLabelRegex::Regex = r"^[a-zA-Z][\w]*$" + return occursin(validLabelRegex, string(label)) +end diff --git a/src/DataBlobs/entities/BlobEntry.jl b/src/entities/Blobentry.jl similarity index 55% rename from src/DataBlobs/entities/BlobEntry.jl rename to src/entities/Blobentry.jl index bcddb0dc..c45f2d17 100644 --- a/src/DataBlobs/entities/BlobEntry.jl +++ b/src/entities/Blobentry.jl @@ -1,6 +1,6 @@ -##============================================================================== -## Blobentry -##============================================================================== +# ============================================================================== +# Blobentry +# ============================================================================== """ $(TYPEDEF) @@ -134,3 +134,135 @@ function StructUtils.makedict(s::StructUtils.StructStyle, T::Type{Blobentries}, end return entries, nothing end + +# ============================================================================== +# Blobentry - Generic node CRUD +# ============================================================================== + +""" + $(SIGNATURES) +""" +function getBlobentry(node, label::Symbol) + !haskey(refBlobentries(node), label) && throw(LabelNotFoundError("Blobentry", label)) + return refBlobentries(node)[label] +end + +function getBlobentries( + node; + whereLabel::Union{Nothing, Function} = nothing, + whereBlobid::Union{Nothing, Function} = nothing, +) + entries = collect(values(refBlobentries(node))) + filterDFG!(entries, whereLabel, getLabel) + filterDFG!(entries, whereBlobid, x -> string(x.blobid)) + return entries +end + +""" + $(SIGNATURES) +""" +function addBlobentry!(node, entry::Blobentry) + label = getLabel(entry) + haskey(refBlobentries(node), label) && throw(LabelExistsError("Blobentry", label)) + refBlobentries(node)[label] = entry + return entry +end + +function addBlobentries!(node, entries::Vector{Blobentry}) + addBlobentry!.(node, entries) + return entries +end + +""" + $(SIGNATURES) +""" +function mergeBlobentry!(node, entry::Blobentry) + label = getLabel(entry) + refBlobentries(node)[label] = entry + return 1 +end + +function mergeBlobentries!(node, entries::Vector{Blobentry}) + #TODO optimize with something like: merge!(refBlobentries(node), entries) + mergeBlobentry!.(node, entries) + return length(entries) +end + +""" + $(SIGNATURES) +""" +function deleteBlobentry!(node, label::Symbol) + !haskey(refBlobentries(node), label) && return 0 + pop!(refBlobentries(node), label) + return 1 +end + +deleteBlobentry!(node, entry) = deleteBlobentry!(node, getLabel(entry)) + +function deleteBlobentries!(node, labels::Vector{Symbol}) + return sum(deleteBlobentry!.(node, labels)) +end + +""" + $(SIGNATURES) +List all Blobentry keys for a variable `label` in `dfg` +""" +function listBlobentries(node) + return collect(keys(refBlobentries(node))) +end + +""" + $SIGNATURES + +Does a blob entry exist with `label`. +""" +hasBlobentry(node, label::Symbol) = haskey(refBlobentries(node), label) + +# ============================================================================== +# Blobentry - utils +# ============================================================================== + +""" + checkHash(entry::Blobentry, blob) -> Union{Bool,Nothing} + +Checks the integrity of a blob against the hashes (crc32c, sha256) stored in the given `Blobentry`. + +- Returns `true` if all present hashes (`crchash`, `shahash`) match the computed values from `blob`. +- Returns `false` if any present hash does not match. +- Returns `nothing` if no hashes are stored in the `Blobentry` to check against. +""" +function checkHash(entry::Blobentry, blob) + if !isnothing(entry.crchash) + crc32c(blob) != entry.crchash && return false + end + if entry.shahash != "" + sha256(blob) != entry.shahash && return false + end + if isnothing(entry.crchash) && entry.shahash == "" + return nothing + end + return true +end + +function Base.show(io::IO, ::MIME"text/plain", entry::Blobentry) + println(io, "Blobentry {") + println(io, " blobid: ", entry.blobid) + println(io, " label: ", entry.label) + println(io, " storelabel: ", entry.storelabel) + println(io, " origin: ", entry.origin) + println(io, " description: ", entry.description) + println(io, " mimetype: ", entry.mimetype) + println(io, " timestamp ", entry.timestamp) + println(io, " version: ", entry.version) + return println(io, "}") +end + +# TODO Consider autogenerating all methods of the form: +# verbNoun(dfg::VariableCompute, label::Symbol, args...; kwargs...) = verbNoun(getVariable(dfg, label), args...; kwargs...) +# with something like: +# getvariablemethod = [ +# :getfirstBlobentry, +# ] +# for met in methodstooverload +# @eval DistributedFactorGraphs $met(dfg::AbstractDFG, label::Symbol, args...; kwargs...) = $met(getVariable(dfg, label), args...; kwargs...) +# end diff --git a/src/entities/Bloblet.jl b/src/entities/Bloblet.jl index 66edcc7a..192e4a4a 100644 --- a/src/entities/Bloblet.jl +++ b/src/entities/Bloblet.jl @@ -45,31 +45,34 @@ end """ $(SIGNATURES) """ -function getBloblet(node, label::Symbol) - !haskey(refBloblets(node), label) && throw(LabelNotFoundError("Bloblet", label)) - return refBloblets(node)[label] +function addBloblet!(node, bloblet::Bloblet) + label = getLabel(bloblet) + haskey(refBloblets(node), label) && throw(LabelExistsError("Bloblet", label)) + refBloblets(node)[label] = bloblet + return bloblet end """ $(SIGNATURES) """ -function getBloblets(node) - return collect(values(refBloblets(node))) +function addBloblets!(node, bloblets::Vector{Bloblet}) + foreach(bl -> addBloblet!(node, bl), bloblets) + return bloblets end """ $(SIGNATURES) """ -function addBloblet!(node, bloblet::Bloblet) - label = getLabel(bloblet) - haskey(refBloblets(node), label) && throw(LabelExistsError("Bloblet", label)) - refBloblets(node)[label] = bloblet - return bloblet +function getBloblet(node, label::Symbol) + !haskey(refBloblets(node), label) && throw(LabelNotFoundError("Bloblet", label)) + return refBloblets(node)[label] end -function addBloblets!(node, bloblets::Vector{Bloblet}) - foreach(bl -> addBloblet!(node, bl), bloblets) - return bloblets +""" + $(SIGNATURES) +""" +function getBloblets(node) + return collect(values(refBloblets(node))) end """ diff --git a/src/DataBlobs/entities/BlobStores.jl b/src/entities/Blobstore.jl similarity index 100% rename from src/DataBlobs/entities/BlobStores.jl rename to src/entities/Blobstore.jl diff --git a/src/errors.jl b/src/entities/Error.jl similarity index 100% rename from src/errors.jl rename to src/entities/Error.jl diff --git a/src/entities/DFGFactor.jl b/src/entities/Factor.jl similarity index 99% rename from src/entities/DFGFactor.jl rename to src/entities/Factor.jl index ffbebe72..4004a474 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/Factor.jl @@ -417,3 +417,7 @@ function patch!(dest::FactorSkeleton, src::FactorSkeleton) union!(dest.tags, src.tags) return dest end + +function Base.show(io::IO, ::MIME"text/plain", f::FactorDFG) + return printFactor(io, f; short = true, limit = false) +end diff --git a/src/entities/State.jl b/src/entities/State.jl new file mode 100644 index 00000000..7f4781f2 --- /dev/null +++ b/src/entities/State.jl @@ -0,0 +1,306 @@ +##============================================================================== +## Abstract Types +##============================================================================== + +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 separately. + """Discriminator for which representation is active.""" + densitykind::AbstractDensityKind = NonparametricDensityKind() + + #--- Parametric fields (Gaussian / GMM / Homotopy leading modes) --- + """On-manifold component means. + Gaussian: length 1. Homotopy: leading (tree_kernel) means.""" + means::Vector{P} = P[] # previously `val[1]` for Gaussian + """Component covariances, matching `means`.""" + covariances::Vector{Matrix{Float64}} = Matrix{Float64}[] # previously `covar` existed but was stored in `bw` (hacky) + "Component weights, matching `means`." + 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 + +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 +##============================================================================== + +""" +$(TYPEDEF) +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. +Fields: +$(TYPEDFIELDS) +""" +@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() + """ + Generic Belief representation for this state, including the discriminator for which representation is active + and the associated fields for each representation kind. + """ + 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.""" + initialized::Bool = false + """Stores the amount of information 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)}(; kwargs...) +end +function State(label::Symbol, variableType::StateType; kwargs...) + return State{typeof(variableType)}(; label, kwargs...) +end + +function State(state::State; kwargs...) + return State{typeof(getStateKind(state))}(; + (key => deepcopy(getproperty(state, key)) for key in fieldnames(State))..., + kwargs..., + ) +end + +# 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 + +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} + +StructUtils.dictlike(::Type{<:States}) = false +StructUtils.structlike(::Type{<:States}) = false +StructUtils.arraylike(::Type{<:States}) = false + +function StructUtils.lower(states::States) + return map(collect(values(states))) do (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( + style::StructUtils.StructStyle, + S::Type{<:States{T}}, + lazystates::JSON.LazyValue, + tags::NamedTuple = (;), +) where {T} + StateT = State{T, getPointType(T)} + states = S() + 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 + +""" + @defStateType StructName manifold point_identity + +A macro to create a new variable type with name `StructName` associated with a given manifold and identity point. + +- `StructName` is the name of the new variable type, which will be defined as a subtype of `StateType`. +- `manifold` is an object that must be a subtype of `ManifoldsBase.AbstractManifold`. +- `point_identity` is the identity point on the manifold, used as a reference for operations. + +This macro is useful for defining variable types that are not parameterized by dimension, and for associating them with a specific manifold and identity point. + +See the [Manifolds.jl documentation on creating your own manifolds](https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html) for more information. + +Example: +``` +DFG.@defStateType Pose2 SpecialEuclideanGroup(2) ArrayPartition([0;0.0],[1 0; 0 1.0]) +``` +""" +macro defStateType(structname, manifold, point_identity) + return esc( + quote + Base.@__doc__ struct $structname <: StateType{Any} end + + # user manifold must be a <:Manifold + @assert ($manifold isa AbstractManifold) "defStateType of " * + string($structname) * + " requires that the " * + string($manifold) * + " be a subtype of `ManifoldsBase.AbstractManifold`" + + DFG.getManifold(::Type{$structname}) = $manifold + + DFG.getPointType(::Type{$structname}) = typeof($point_identity) + + DFG.getPointIdentity(::Type{$structname}) = $point_identity + end, + ) +end + +""" + @defStateTypeN StructName manifold point_identity + +A macro to create a new variable type with name `StructName` that is parameterized by `N` and associated with a given manifold and identity point. + +- `StructName` is the name of the new variable type, which will be defined as a subtype of `StateType{N}`. +- `manifold` is an object that must be a subtype of `ManifoldsBase.AbstractManifold`. +- `point_identity` is the identity point on the manifold, used as a reference for operations. + +This macro is useful for defining variable types that are parameterized by dimension or other type parameters (e.g., `Pose{N}`), and for associating them with a specific manifold and identity point. + +See the [Manifolds.jl documentation on creating your own manifolds](https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html) for more information. + +Example: +``` +DFG.@defStateTypeN Pose{N} SpecialEuclideanGroup(N) ArrayPartition(zeros(SVector{N, Float64}), SMatrix{N, N, Float64}(I)) +``` +""" +macro defStateTypeN(structname, manifold, point_identity) + return esc( + quote + Base.@__doc__ struct $structname <: StateType{N} end + + DFG.getManifold(::Type{$structname}) where {N} = $manifold + + DFG.getPointType(::Type{$structname}) where {N} = typeof($point_identity) + + DFG.getPointIdentity(::Type{$structname}) where {N} = $point_identity + end, + ) +end diff --git a/src/entities/Tags.jl b/src/entities/Tags.jl new file mode 100644 index 00000000..1fdb5be9 --- /dev/null +++ b/src/entities/Tags.jl @@ -0,0 +1,44 @@ +# from CommonAccessors.jl +##============================================================================== +## TAGS as a set -- list, merge, delete, (empty?) +##============================================================================== +# Node level functions (in-memory) +""" +$SIGNATURES +""" +listTags(node) = collect(refTags(node)) + +""" + $SIGNATURES + +Merge add tags to a variable or factor (union) +""" +function mergeTags!(node, tags) + union!(refTags(node), tags) + return length(tags) +end +""" +$SIGNATURES + +Remove the tags from the node (setdiff) +""" +function deleteTags!(node, tags) + setdiff!(refTags(node), tags) + return length(tags) +end + +""" +$SIGNATURES + +Empty all tags from the node (empty) +""" +emptyTags!(node) = empty!(refTags(node)) + +""" + $SIGNATURES + +Determine if the variable or factor neighbors have the `tags::Vector{Symbol}``. +""" +function hasTags(node, tags::Vector{Symbol}) + return tags ⊆ listTags(node) +end diff --git a/src/entities/Timestamp.jl b/src/entities/Timestamp.jl new file mode 100644 index 00000000..71212929 --- /dev/null +++ b/src/entities/Timestamp.jl @@ -0,0 +1,45 @@ +# ============================================================================== +# Timestamps +# ============================================================================== + +# delta timestamps +calcDeltatime(from::Nanosecond, to::Nanosecond) = Dates.value(to - from) / 10^9 + +function calcDeltatime_ns(from::TimeDateZone, to::TimeDateZone) + # TODO (-)(x::TimeDateZone, y::TimeDateZone) was very slow so manually calculated here + # return Dates.value(convert(Nanosecond, to - from)) / 10^9 + # calculate the "fast part" (microsecond) of the delta timestamp in nanoseconds + fast_to = convert(Nanosecond, Microsecond(to)) + Nanosecond(to) + fast_from = convert(Nanosecond, Microsecond(from)) + Nanosecond(from) + delta_fast = fast_to - fast_from + # calculate the "slow part" (zoned date time) of the delta timestamp in nanoseconds + delta_zoned = convert(Nanosecond, ZonedDateTime(to) - ZonedDateTime(from)) + return delta_zoned + delta_fast +end + +function calcDeltatime(from::TimeDateZone, to::TimeDateZone) + # TODO (-)(x::TimeDateZone, y::TimeDateZone) was very slow so manually calculated here + # return Dates.value(convert(Nanosecond, to - from)) / 10^9 + return Dates.value(calcDeltatime_ns(from, to)) / 10^9 +end + +calcDeltatime(from_node, to_node) = calcDeltatime(from_node.timestamp, to_node.timestamp) + +Timestamp(args...) = TimeDateZone(args...) +Timestamp(t::Nanosecond, zone = tz"UTC") = Timestamp(Val(:unix), t, zone) +Timestamp(t::Microsecond, zone = tz"UTC") = Timestamp(Val(:unix), Nanosecond(t), zone) +function Timestamp(epoch::Val{:unix}, t::Nanosecond, zone = tz"UTC") + return TimeDateZone(TimeDate(1970) + t, zone) +end +function Timestamp(epoch::Val{:unix}, t::Float64, zone = tz"UTC") + return Timestamp(epoch, Nanosecond(t * 10^9), zone) +end +Timestamp(t::Float64, zone = tz"UTC") = Timestamp(Val(:unix), t, zone) +function Timestamp(epoch::Val{:rata}, t::Float64, zone = tz"UTC") + return TimeDateZone(convert(DateTime, Millisecond(t * 10^3)), zone) +end + +function now_tdz(zone = tz"UTC") + t = time() + return Timestamp(t, zone) +end diff --git a/src/entities/DFGVariable.jl b/src/entities/Variable.jl similarity index 52% rename from src/entities/DFGVariable.jl rename to src/entities/Variable.jl index d68031c8..3ab258bc 100644 --- a/src/entities/DFGVariable.jl +++ b/src/entities/Variable.jl @@ -1,238 +1,3 @@ -##============================================================================== -## Abstract Types -##============================================================================== - -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 separately. - """Discriminator for which representation is active.""" - densitykind::AbstractDensityKind = NonparametricDensityKind() - - #--- Parametric fields (Gaussian / GMM / Homotopy leading modes) --- - """On-manifold component means. - Gaussian: length 1. Homotopy: leading (tree_kernel) means.""" - means::Vector{P} = P[] # previously `val[1]` for Gaussian - """Component covariances, matching `means`.""" - covariances::Vector{Matrix{Float64}} = Matrix{Float64}[] # previously `covar` existed but was stored in `bw` (hacky) - "Component weights, matching `means`." - 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 - -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 -##============================================================================== - -""" -$(TYPEDEF) -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. -Fields: -$(TYPEDFIELDS) -""" -@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() - """ - Generic Belief representation for this state, including the discriminator for which representation is active - and the associated fields for each representation kind. - """ - 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.""" - 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 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)}(; kwargs...) -end -function State(label::Symbol, variableType::StateType; kwargs...) - return State{typeof(variableType)}(; label, kwargs...) -end - -function State(state::State; kwargs...) - return State{typeof(getStateKind(state))}(; - (key => deepcopy(getproperty(state, key)) for key in fieldnames(State))..., - kwargs..., - ) -end - -# 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 - -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} - -StructUtils.dictlike(::Type{<:States}) = false -StructUtils.structlike(::Type{<:States}) = false -StructUtils.arraylike(::Type{<:States}) = false - -function StructUtils.lower(states::States) - return map(collect(values(states))) do (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( - style::StructUtils.StructStyle, - S::Type{<:States{T}}, - lazystates::JSON.LazyValue, - tags::NamedTuple = (;), -) where {T} - StateT = State{T, getPointType(T)} - states = S() - 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 ##============================================================================== ## DFG Variables @@ -325,7 +90,7 @@ end # JSON.omit_empty(::DistributedFactorGraphs.DFGJSONStyle, ::Type{<:VariableDFG}) = true -const VariableCompute = VariableDFG +const VariableCompute = VariableDFG #TODO deprecated in v0.29 ##------------------------------------------------------------------------------ ## Constructors @@ -524,3 +289,44 @@ function patch!(dest::VariableSkeleton, src::VariableSkeleton) union!(dest.tags, src.tags) return dest end + +function Base.show(io::IO, ::MIME"text/plain", v::VariableDFG) + return printVariable(io, v; short = true, limit = false) +end + +# ============================================================================== +# State(::VariableDFG,...) operations +# ============================================================================== + +function addState!(v::VariableDFG, state::State) + if haskey(refStates(v), state.label) + throw(LabelExistsError("State", state.label)) + end + refStates(v)[state.label] = state + return state +end + +function getState(v::VariableDFG, label::Symbol) + !haskey(refStates(v), label) && throw(LabelNotFoundError("State", label)) + return refStates(v)[label] +end + +function mergeState!(v::VariableDFG, vnd::State) + if !haskey(v.states, vnd.label) + addState!(v, vnd) + else + v.states[vnd.label] = vnd + end + return 1 +end + +function deleteState!(v::VariableDFG, label::Symbol) + !haskey(v.states, label) && return 0 + delete!(v.states, label) + return 1 +end + +function listStates(v::VariableDFG; whereLabel::Union{Nothing, Function} = nothing) + labels = collect(keys(v.states)) + return filterDFG!(labels, whereLabel) +end diff --git a/src/entities/equality.jl b/src/entities/equality.jl new file mode 100644 index 00000000..c8fb1cc0 --- /dev/null +++ b/src/entities/equality.jl @@ -0,0 +1,53 @@ +# ============================================================================== +# (==) +# ============================================================================== +import Base.== +## @generated compare +# Reference https://github.com/JuliaLang/julia/issues/4648 + +#= +For now abstract `StateType`s are considered equal if they are the same type, dims, and manifolds (labels are deprecated) +If your implentation has aditional properties such as `DynPose2` with `ut::Int64` (microsecond time) or support different manifolds +implement compare if needed. +=# +# ==(a::StateType,b::StateType) = typeof(a) == typeof(b) && a.dims == b.dims && a.manifolds == b.manifolds + +==(a::FactorCache, b::FactorCache) = typeof(a) == typeof(b) + +==(a::AbstractObservation, b::AbstractObservation) = typeof(a) == typeof(b) + +# Generate compares automatically for all in this union +const GeneratedCompareUnion = Union{ + BeliefRepresentation, + State, + Blobentry, + Bloblet, + VariableSkeleton, + FactorSkeleton, + Recipehyper, + Recipestate, +} + +@generated function ==(x::T, y::T) where {T <: GeneratedCompareUnion} + return mapreduce(n -> :(x.$n == y.$n), (a, b) -> :($a && $b), fieldnames(x)) +end + +function ==(x::T, y::T) where {T <: AbstractGraphFactor} + ignored = [:solvercache, :solvable] + tp = mapreduce( + n -> getproperty(x, n) == getproperty(y, n), + (a, b) -> a && b, + setdiff(propertynames(x), ignored), + ) + return tp && getSolvable(x) == getSolvable(y) +end + +function ==(x::T, y::T) where {T <: AbstractGraphVariable} + ignored = [:solvable] + tp = mapreduce( + n -> getproperty(x, n) == getproperty(y, n), + (a, b) -> a && b, + setdiff(propertynames(x), ignored), + ) + return tp && getSolvable(x) == getSolvable(y) +end diff --git a/src/weakdeps_prototypes.jl b/src/extension_stubs.jl similarity index 100% rename from src/weakdeps_prototypes.jl rename to src/extension_stubs.jl diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index c6ecb545..d72c4abf 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -1,12 +1,3 @@ -##============================================================================== -## AbstractDFG -##============================================================================== -##------------------------------------------------------------------------------ -## Broadcasting -##------------------------------------------------------------------------------ -# to allow stuff like `getObservation.(dfg, [:x1x2f1;:x10l3f2])` -# https://docs.julialang.org/en/v1/manual/interfaces/# -Base.Broadcast.broadcastable(dfg::AbstractDFG) = Ref(dfg) ##============================================================================== ## Interface for an AbstractDFG @@ -76,615 +67,6 @@ function setSolverParams!(dfg::AbstractDFG, solverParams::AbstractDFGParams) return dfg.solverParams = solverParams end -## ============================================================================= -## Agent in DFG CRUD -## ============================================================================= -""" - $(SIGNATURES) -""" -function refAgents end - -""" - $(SIGNATURES) -""" -function getAgent(dfg::AbstractDFG, label::Symbol) - agent = get(refAgents(dfg), label, nothing) - if isnothing(agent) - available = listAgents(dfg) - isempty(available) && - @info "No agents in graph: `$(getGraphLabel(dfg))`. Use `addAgent!(dfg, Agent(; label=:myAgent))` to add one" - throw(LabelNotFoundError("Agent", label, available)) - end - return agent -end - -function getAgents(dfg::AbstractDFG) - return map(listAgents(dfg)) do label - return getAgent(dfg, label) - end -end - -""" - $(SIGNATURES) -""" -function addAgent!(dfg::AbstractDFG, agent::Agent) - label = getLabel(agent) - haskey(refAgents(dfg), label) && throw(LabelExistsError("Agent", label)) - push!(refAgents(dfg), label => agent) - return agent -end - -""" - $(SIGNATURES) -""" -function deleteAgent!(dfg::AbstractDFG, label::Symbol) - !haskey(refAgents(dfg), label) && return 0 - pop!(refAgents(dfg), label) - return 1 -end - -""" - $(SIGNATURES) -""" -function listAgents(dfg::AbstractDFG) - return collect(keys(refAgents(dfg))) -end - -""" - $(SIGNATURES) -""" -function hasAgent(dfg::AbstractDFG, label::Symbol) - return haskey(refAgents(dfg), label) -end - -""" - $(SIGNATURES) -""" -function mergeAgent!(dfg::AbstractDFG, agent::Agent) - label = getLabel(agent) - if hasAgent(dfg, label) - mergeAgent!(refAgents(dfg)[label], agent) - else - addAgent!(dfg, agent) - end - return 1 -end - -function mergeAgents!(dfg::AbstractDFG, agents::Vector{Agent}) - count = 0 - for agent in agents - count += mergeAgent!(dfg, agent) - end - return count -end - -##============================================================================== -## AbstractBlobstore CRUD -##============================================================================== -# AbstractBlobstore should have label or overwrite getLabel - -refBlobstores(dfg::AbstractDFG) = dfg.blobstores - -function getBlobstore(dfg::AbstractDFG, storeLabel::Symbol) - store = get(refBlobstores(dfg), storeLabel, nothing) - if isnothing(store) - isempty(refBlobstores(dfg)) && - @info "No blobstores in graph: `$(getGraphLabel(dfg))`. Use `addBlobstore!(dfg, FolderStore(\"path/to/store\"))` to add one." - throw( - LabelNotFoundError("Blobstore", storeLabel, collect(keys(refBlobstores(dfg)))), - ) - end - return store -end - -function getBlobstores(dfg::AbstractDFG) - stores = map(listBlobstores(dfg)) do label - return getBlobstore(dfg, label) - end - return isempty(stores) ? AbstractBlobstore[] : stores -end - -function addBlobstore!(dfg::AbstractDFG, store::AbstractBlobstore) - label = getLabel(store) - haskey(refBlobstores(dfg), label) && throw(LabelExistsError("Blobstore", label)) - return push!(refBlobstores(dfg), label => store) -end - -# TODO edge api is a work in progress and only internal -function mergeStorelink!(dfg::AbstractDFG, link_to_store::AbstractBlobstore) - # we currently only have a label for a storelink, so we do not know if it is the same link - # so we have to look at the Blobstore node to find out. we only merge if the label=>store matches - # - label = getLabel(link_to_store) # the edge in this case is from the virtual dfg object to the Blobstore, so simply label. - if hasBlobstore(dfg, label) - existing_store = getBlobstore(dfg, label) - if existing_store != link_to_store - throw(MergeConflictError("Merge conflict for Blobstore with label $(label)")) - else - return 0 # no merge needed, they are the same store - end - else - push!(refBlobstores(dfg), label => link_to_store) - end - return 1 -end - -function deleteBlobstore!(dfg::AbstractDFG, key::Symbol) - !haskey(refBlobstores(dfg), key) && return 0 - pop!(refBlobstores(dfg), key) - return 1 -end -listBlobstores(dfg::AbstractDFG) = collect(keys(refBlobstores(dfg))) - -function mergeStorelinks!(destDFG::AbstractDFG, blobstores::Vector{<:AbstractBlobstore}) - count = 0 - for store in blobstores - count += mergeStorelink!(destDFG, store) - end - return count -end - -function hasBlobstore(dfg::AbstractDFG, label::Symbol) - return haskey(refBlobstores(dfg), label) -end - -#TODO empty as verb or only `deleteNouns!` -# emptyBlobstore!(dfg::AbstractDFG) = empty!(refBlobstores(dfg)) - -##============================================================================== -## CRUD Interfaces -##============================================================================== -##------------------------------------------------------------------------------ -## Variable And Factor CRUD -##------------------------------------------------------------------------------ - -""" - $(SIGNATURES) -True if the variable exists in the graph. -Implement `hasVariable(dfg::AbstractDFG, label::Symbol)` -""" -function hasVariable end - -""" - $(SIGNATURES) -True if the factor exists in the graph. -Implement `hasFactor(dfg::AbstractDFG, label::Symbol)` -""" -function hasFactor end - -""" - $(SIGNATURES) -Add a VariableDFG to a DFG. -Implement `addVariable!(dfg::AbstractDFG, variable::AbstractGraphVariable)` -""" -function addVariable! end - -""" - $(SIGNATURES) -Add a Vector{VariableDFG} to a DFG. -Implement `addVariables!(dfg::AbstractDFG, variables::Vector{<:AbstractGraphVariable})` -""" -function addVariables!(dfg::AbstractDFG, variables::Vector{<:AbstractGraphVariable}) - return asyncmap(variables) do v - return addVariable!(dfg, v) - end -end - -""" - $(SIGNATURES) -Add a FactorDFG to a DFG. -Implement `addFactor!(dfg::AbstractDFG, factor::AbstractGraphFactor)` -""" -function addFactor! end - -""" - $(SIGNATURES) -Add a Vector{FactorDFG} to a DFG. -""" -function addFactors!(dfg::AbstractDFG, factors::Vector{<:AbstractGraphFactor}) - return asyncmap(factors) do f - return addFactor!(dfg, f) - end -end - -""" - $(SIGNATURES) -Get a VariableDFG from a DFG using its label. -Implement `getVariable(dfg::AbstractDFG, label::Symbol)` -""" -function getVariable end - -""" - $(SIGNATURES) -Get a VariableSummary from a DFG. -""" -function getVariableSummary end - -""" - $(SIGNATURES) -Get the variables from a DFG as a Vector{VariableSummary}. -""" -function getVariablesSummary end - -""" - $(SIGNATURES) -Get a VariableSkeleton from a DFG. -""" -function getVariableSkeleton end - -""" - $(SIGNATURES) -Get the variables from a DFG as a Vector{VariableSkeleton}. -""" -function getVariablesSkeleton end - -""" - $(SIGNATURES) -Get a FactorDFG from a DFG using its label. -Implement `getFactor(dfg::AbstractDFG, label::Symbol)` -""" -function getFactor end - -#TODO implement -function getFactorSkeleton end -function getFactorSummary end -""" - $(SIGNATURES) -Get the skeleton factors from a DFG as a Vector{FactorSkeleton}. -""" -function getFactorsSkeleton end -#TODO implement -function getFactorsSummary end - -function Base.getindex(dfg::AbstractDFG, lbl::Symbol) - if isVariable(dfg, lbl) - getVariable(dfg, lbl) - elseif isFactor(dfg, lbl) - getFactor(dfg, lbl) - else - throw(LabelNotFoundError("GraphNode", lbl)) - end -end - -""" - $(SIGNATURES) -Merge a variable into the DFG. If a variable with the same label exists, it will be overwritten; -otherwise, the variable will be added to the graph. -Implement `mergeVariable!(dfg::AbstractDFG, variable::AbstractGraphVariable)` -""" -function mergeVariable! end - -function mergeVariables!(dfg::AbstractDFG, variables::Vector{<:AbstractGraphVariable}) - counts = asyncmap(v -> mergeVariable!(dfg, v), variables) - return sum(counts; init = 0) -end - -""" - $(SIGNATURES) -Merge a factor into the DFG. If a factor with the same label exists, it will be overwritten; -otherwise, the factor will be added to the graph. -Implement `mergeFactor!(dfg::AbstractDFG, factor::AbstractGraphFactor)` -""" -function mergeFactor! end - -function mergeFactors!(dfg::AbstractDFG, factors::Vector{<:AbstractGraphFactor}) - counts = asyncmap(f -> mergeFactor!(dfg, f), factors) - return sum(counts; init = 0) -end - -""" - $(SIGNATURES) -Delete a VariableDFG from the DFG. -Implement `deleteVariable!(dfg::AbstractDFG, label::Symbol)` -""" -function deleteVariable! end -""" - $(SIGNATURES) -Delete a FactorDFG from the DFG using its label. -Implement `deleteFactor!(dfg::AbstractDFG, label::Symbol)` -""" -function deleteFactor! end - -""" - $(SIGNATURES) -Get the variables in the DFG as a Vector, supporting various filters. - -Keyword arguments -- `whereSolvable`: Optional function to filter on the `solvable` property, eg `>=(1)`. -- `whereLabel`: Optional function to filter on label e.g., `contains(r"x1")`. -- `whereTags`: Optional function to filter on tags, eg. `⊇([:POSE])`. -- `whereType`: Optional function to filter on the variable type. - -Returns -- `Vector{<:AbstractGraphVariable}` matching the filters. - -See also: [`listVariables`](@ref), [`ls`](@ref) -""" -function getVariables end - -function getVariables(dfg::AbstractDFG, labels::Vector{Symbol}) - return map(label -> getVariable(dfg, label), labels) -end - -""" - $(SIGNATURES) -List the DFGFactors in the DFG. -Optionally specify a label regular expression to retrieves a subset of the factors. -""" -function getFactors end - -function getFactors(dfg::AbstractDFG, labels::Vector{Symbol}) - return map(label -> getFactor(dfg, label), labels) -end - -##------------------------------------------------------------------------------ -## Checking Types -##------------------------------------------------------------------------------ - -""" - $SIGNATURES - -Return whether `sym::Symbol` represents a variable vertex in the graph DFG. -Checks whether it both exists in the graph and is a variable. -(If you rather want a quick for type, just do node isa VariableDFG) -Implement `isVariable(dfg::AbstractDFG, label::Symbol)` -""" -function isVariable end - -""" - $SIGNATURES - -Return whether `sym::Symbol` represents a factor vertex in the graph DFG. -Checks whether it both exists in the graph and is a factor. -(If you rather want a quicker for type, just do node isa FactorDFG) -Implement `isFactor(dfg::AbstractDFG, label::Symbol)` -""" -function isFactor end - -##------------------------------------------------------------------------------ -## Neighbors -##------------------------------------------------------------------------------ -""" - $(SIGNATURES) -Checks if the graph is fully connected, returns true if so. -Implement `isConnected(dfg::AbstractDFG)` -""" -function isConnected end - -""" - $(SIGNATURES) -Retrieve a list of labels of the immediate neighbors around a given variable or factor specified by its label. -Implement `listNeighbors(dfg::AbstractDFG, label::Symbol; whereSolvable, whereTags)` -""" -function listNeighbors end - -""" - findPaths(dfg, from::Symbol, to::Symbol, k::Int; variableLabels, factorLabels, kwargs...) - -Return the `k` shortest paths between `from` and `to` in the factor graph. -Each result is a `(path = Vector{Symbol}, dist)` named tuple. - -Optional keyword arguments restrict which variables and/or factors may appear on -the path. When neither is given the full graph is used. When only one is -provided the other defaults to all labels of that kind in `dfg`. - -Typical usage with filters: -```julia -vars = listVariables(dfg; whereSolvable = >=(1)) -facs = listFactors(dfg; whereSolvable = >=(1)) -findPaths(dfg, :x1, :x5, 3; variableLabels = vars, factorLabels = facs) -``` - -See also: [`findPath`](@ref), [`listVariables`](@ref), [`listFactors`](@ref) -""" -function findPaths end - -""" - findPath(dfg, from::Symbol, to::Symbol; variableLabels, factorLabels, kwargs...) - -Return the single shortest path between `from` and `to`. -Errors if no path exists (use `findPaths` for graphs that may be disconnected). - -Accepts the same restriction keywords as [`findPaths`](@ref). -""" -function findPath end - -function listNeighbors(dfg::AbstractDFG, node::AbstractGraphNode; kwargs...) - return listNeighbors(dfg, getLabel(node); kwargs...) -end -##------------------------------------------------------------------------------ -## copy and duplication -##------------------------------------------------------------------------------ - -##------------------------------------------------------------------------------ -## CRUD Aliases -##------------------------------------------------------------------------------ - -function deleteVariable!(dfg::AbstractDFG, variable::AbstractGraphVariable) - return deleteVariable!(dfg, variable.label) -end - -function deleteVariables!(dfg::AbstractDFG, labels::Vector{Symbol}) - counts = asyncmap(labels) do l - return deleteVariable!(dfg, l) - end - return sum(counts) -end - -function deleteVariables!(dfg::AbstractDFG; kwargs...) - labels = listVariables(dfg; kwargs...) - return deleteVariables!(dfg, labels) -end - -""" - $(SIGNATURES) -Delete the referenced Factor from the DFG. -""" -function deleteFactor!(dfg::AbstractDFG, factor::AbstractGraphFactor) - return deleteFactor!(dfg, factor.label) -end - -function deleteFactors!(dfg::AbstractDFG, labels::Vector{Symbol}) - counts = asyncmap(labels) do l - return deleteFactor!(dfg, l) - end - return sum(counts) -end - -function deleteFactors!(dfg::AbstractDFG; kwargs...) - labels = listFactors(dfg; kwargs...) - return deleteFactors!(dfg, labels) -end - -# rather use isa in code, but ok, here it is -isVariable(dfg::AbstractDFG, node::AbstractGraphVariable) = true -isFactor(dfg::AbstractDFG, node::AbstractGraphFactor) = true - -##============================================================================== -## exists - alias for hasVariable || hasFactor -##============================================================================== -# exists alone is ambiguous and only for variables and factors where there rest of the nouns use has, -# TODO therefore, keep as internal or deprecate? -# additionally - variables and factors can possibly have the same label in other drivers such as NvaSDK - -""" - $(SIGNATURES) -True if a variable or factor with `label` exists in the graph. -""" -function exists(dfg::AbstractDFG, label::Symbol) - return hasVariable(dfg, label) || hasFactor(dfg, label) -end - -function exists(dfg::AbstractDFG, node::AbstractGraphNode) - return exists(dfg, node.label) -end - -##============================================================================== -## Subgraphs and Neighborhoods -##============================================================================== - -#TODO add pruning filters that is applied during traversal. -""" - $(SIGNATURES) -Build a list of all unique neighbors inside 'distance'. Neighbors can be filtered by using keyword arguments, eg. [`whereTags`] and [`whereSolvable`]. -Filters are applied to final neighborhood result. - -Notes -- Returns a tuple `(variableLabels, factorLabels)`, where each element is a `Vector{Symbol}`. - -Related: -- [`getSubgraph`](@ref) -- [`mergeGraph!`](@ref) -""" -function listNeighborhood(dfg::AbstractDFG, label::Symbol, distance::Int; filters...) - neighborList = Set{Symbol}([label]) - curList = Set{Symbol}([label]) - - for dist = 1:distance - newNeighbors = Set{Symbol}() - for node in curList - neighbors = listNeighbors(dfg, node) - union!(neighborList, neighbors) - union!(newNeighbors, neighbors) - end - curList = newNeighbors - end - - variableLabels = intersect(listVariables(dfg; filters...), neighborList) - factorLabels = intersect(listFactors(dfg; filters...), neighborList) - - return variableLabels, factorLabels -end - -function listNeighborhood( - dfg::AbstractDFG, - variableFactorLabels::Vector{Symbol}, - distance::Int; - filters..., -) - if distance > 0 - variableLabels = Symbol[] - factorLabels = Symbol[] - for l in variableFactorLabels - varls, facls = listNeighborhood(dfg, l, distance; filters...) - union!(variableLabels, varls) - union!(factorLabels, facls) - end - else - variableLabels = intersect(listVariables(dfg; filters...), variableFactorLabels) - factorLabels = intersect(listFactors(dfg; filters...), variableFactorLabels) - end - - return variableLabels, factorLabels -end - -""" - $(SIGNATURES) -Build a deep subgraph copy from the DFG given a list of variables and factors and an optional distance. -Note: Orphaned factors (where the subgraph does not contain all the related variables) are not returned. -Related: -- [`listNeighborhood`](@ref) -- [`mergeGraph!`](@ref) -Dev Notes -- Bulk vs node for node: a list of labels are compiled and the sugraph is copied in bulk. -""" -function getSubgraph( - ::Type{G}, - dfg::AbstractDFG, - variableFactorLabels::Vector{Symbol}, - distance::Int = 0; - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - graphLabel::Symbol = Symbol(getGraphLabel(dfg), "_sub_$(string(uuid4())[1:6])"), - solvable = nothing, #TODO deprecated in v0.29 - kwargs..., -) where {G <: AbstractDFG} - if !isnothing(solvable) - Base.depwarn( - "solvable kwarg is deprecated, use kwarg `whereSolvable = >=(solvable)` instead", #v0.29 - :getSubgraph, - ) - !isnothing(whereSolvable) && - error("Cannot use both solvable and whereSolvable kwargs.") - whereSolvable = >=(solvable) - end - #build up the neighborhood from variableFactorLabels - variableLabels, factorLabels = - listNeighborhood(dfg, variableFactorLabels, distance; whereSolvable, whereTags) - - # Copy the section of graph we want - destDFG = deepcopyGraph(G, dfg, variableLabels, factorLabels; graphLabel, kwargs...) - return destDFG -end - -function getSubgraph( - dfg::AbstractDFG, - variableFactorLabels::Vector{Symbol}, - distance::Int = 0; - kwargs..., -) - return getSubgraph(LocalDFG, dfg, variableFactorLabels, distance; kwargs...) -end - -""" - $(SIGNATURES) -Merge the source DFG into the destination DFG cascading down the hierarchy of DFG nodes. -Merge rules: -- Variables, Factors, Agents, and Graphroot, with the same label are merged if they are equal. - - On conflicts, a `MergeConflictError` is thrown. - - Child nodes (eg. tags, Bloblets, Blobentries, States, etc.) are using `merge!`. -- The Blobstore links are merged provided they point to the same Blobstore. - - On conflicts, a `MergeConflictError` is thrown. -""" -function mergeGraph!(destDFG::AbstractDFG, srcDFG::AbstractDFG) - patch!(destDFG.graph, srcDFG.graph) - mergeVariables!(destDFG, getVariables(srcDFG)) - mergeFactors!(destDFG, getFactors(srcDFG)) - mergeAgents!(destDFG, getAgents(srcDFG)) - mergeStorelinks!(destDFG, getBlobstores(srcDFG)) - return destDFG -end - ##============================================================================== ## Graphs Structures (Abstract, overwrite for performance) ##============================================================================== @@ -780,3 +162,218 @@ function getSummaryGraph(dfg::G) where {G <: AbstractDFG} # end return summaryDfg end + +## +##============================================================================== +## Common Accessors +##============================================================================== + +##------------------------------------------------------------------------------ +## By value accessors +##------------------------------------------------------------------------------ + +# Common get and set methods + +# NOTE this could be reduced with macros and function generation to even less code. + +##------------------------------------------------------------------------------ +## solvable +##------------------------------------------------------------------------------ + +""" + $SIGNATURES + +Variables or factors may or may not be 'solvable', depending on a user definition. Useful for ensuring atomic transactions. + +Related: +- isSolveInProgress +""" +function getSolvable(node::Union{VariableDFG, VariableSummary, FactorDFG, FactorSummary}) + return node.solvable[] +end + +""" + $SIGNATURES + +Get 'solvable' parameter for either a variable or factor. +""" +function getSolvable(dfg::AbstractDFG, sym::Symbol) + if isVariable(dfg, sym) + return getVariable(dfg, sym).solvable[] + elseif isFactor(dfg, sym) + return getFactor(dfg, sym).solvable[] + end +end + +""" + $SIGNATURES + +Set the `solvable` parameter for either a variable or factor. +""" +function setSolvable!(node::Union{VariableDFG, FactorDFG}, solvable::Int) + node.solvable[] = solvable + return solvable +end + +#FIXME this is only for in memory DFGs +function setSolvable!(dfg::AbstractDFG, sym::Symbol, solvable::Int) + if isVariable(dfg, sym) + getVariable(dfg, sym).solvable[] = solvable + elseif isFactor(dfg, sym) + getFactor(dfg, sym).solvable[] = solvable + end + return solvable +end + +""" + $SIGNATURES + +Variables or factors may or may not be 'solvable', depending on a user definition. +returns true if `getSolvable` > 0 +Related: +- `getSolvable`(@ref) +""" +isSolvable(node::Union{VariableDFG, FactorDFG}) = getSolvable(node) > 0 + +# ================================= +# Additional Downstream dispatches +# ================================= + +""" + $SIGNATURES + +Default non-parametric graph solution. +""" +function solveGraph! end + +""" + $SIGNATURES + +Standard parametric graph solution (Experimental). +""" +function solveGraphParametric! end + +##============================================================================== +## Sorting +##============================================================================== +#Natural Sorting less than + +# Adapted from https://rosettacode.org/wiki/Natural_sorting +# split at digit to not digit change +splitbynum(x::AbstractString) = split(x, r"(?<=\D)(?=\d)|(?<=\d)(?=\D)") +#parse to Int +function numstringtonum(arr::Vector{<:AbstractString}) + return [(n = tryparse(Int, e)) !== nothing ? n : e for e in arr] +end +#natural less than +function natural_lt(x::T, y::T) where {T <: AbstractString} + xarr = numstringtonum(splitbynum(x)) + yarr = numstringtonum(splitbynum(y)) + for i = 1:min(length(xarr), length(yarr)) + if typeof(xarr[i]) != typeof(yarr[i]) + return isa(xarr[i], Int) + elseif xarr[i] == yarr[i] + continue + else + return xarr[i] < yarr[i] + end + end + return length(xarr) < length(yarr) +end + +natural_lt(x::Symbol, y::Symbol) = natural_lt(string(x), string(y)) + +""" + $SIGNATURES + +Convenience wrapper for `Base.sort`. +Sort variable (factor) lists in a meaningful way (by `timestamp`, `label`, etc), for example `[:april;:x1_3;:x1_6;]` +Defaults to sorting by timestamp for variables and factors and using `natural_lt` for Symbols. +See Base.sort for more detail. + +Notes +- Not fool proof, but does better than native sort. + +Example + +`sortDFG(ls(dfg))` +`sortDFG(ls(dfg), by=getLabel, lt=natural_lt)` + +Related + +ls, lsf +""" +function sortDFG(vars::Vector{<:AbstractGraphNode}; by = getTimestamp, kwargs...) + return sort(vars; by = by, kwargs...) +end +sortDFG(vars::Vector{Symbol}; lt = natural_lt, kwargs...) = sort(vars; lt = lt, kwargs...) + +##============================================================================== +## Filtering +##============================================================================== + +# std_numeric_predicates = [==, <, <=, >, >=, in] +# std_string_predicates = [==, in, contains, startswith, endswith] + +# # full list +# tags_includes(tag::Symbol) = Base.Fix1(in, tag) +# solvable_eq(x::Int) = ==(x) +# solvable_in(x::Vector{Int}) = in(x) +# solvable_lt(x::Int) = <(x) +# solvable_lte(x::Int) = <=(x) +# solvable_gt(x::Int) = >(x) +# solvable_gte(x::Int) = >=(x) +# label_in(x::Vector{Symbol}) = in(x) +# label_contains(x::String) = contains(x) +# label_startswith(x::String) = startswith(x) +# label_endswith(x::String) = endswith(x) +# type_eq(x::AbstractStateType) = ==(x) +# type_in(x::Vector{<:AbstractStateType}) = in(x) +# type_contains(x::String) = contains(x) +# type_startswith(x::String) = startswith(x) +# type_endswith(x::String) = endswith(x) + +# Set predicates +# collection_includes(item) = Base.Fix1(in, item) # collection includes item (item in collection) + +# not supported helper +# collection_overlap(collection) = !isdisjoint(collection) # collection overlaps with another collection + +""" + $SIGNATURES +Filter nodes in a DFG based on a predicate function. +This function modifies the input `nodes` vector in place, removing nodes that do not satisfy the predicate. + +- **For cross-backend compatibility:** + Use only the standard predicates (`==`, `<`, `<=`, `>`, `>=`, `in`, `contains`, `startswith`, `endswith`) + when you need your code to work with both in-memory and database-backed DFGs. + These are likely to be supported by database query languages and are defined in `std_numeric_predicates` and `std_string_predicates`. + +- **For in-memory only operations:** + You can use any Julia predicate, since you have full access to the data and Julia's capabilities. + This is more flexible but will not work if you later switch to a database backend or another programming language. + +Standard predicates +- Numeric predicates: `==`, `<`, `<=`, `>`, `>=`, `in` +- String predicates: `==`, `contains`, `startswith`, `endswith`, `in` +""" +function filterDFG! end + +filterDFG!(nodes, predicate::Nothing, by::Function = identity) = nodes +# function filterDFG!(nodes, predicate::Base.Fix2, by=identity) +function filterDFG!(nodes, predicate::Function, by::Function = identity) + return filter!(predicate ∘ by, nodes) +end + +# specialized for label::Symbol filtering +function filterDFG!(nodes, predicate::Function, by::typeof(getLabel)) + # TODO this is not as clean as it should be, revisit if any issues arise + # Standard predicates that needs to be converted to string to work with Symbols + # OR look for the type if predicate isa Base.Fix2 && (predicate.x isa AbstractString || predicate.x isa Regex) + if predicate isa Base.Fix2 && + typeof(predicate.f) in [typeof(contains), typeof(startswith), typeof(endswith)] + return filter!(predicate ∘ string ∘ by, nodes) + else + return filter!(predicate ∘ by, nodes) + end +end diff --git a/src/services/Bloblet.jl b/src/services/Bloblet.jl deleted file mode 100644 index e20e390c..00000000 --- a/src/services/Bloblet.jl +++ /dev/null @@ -1,264 +0,0 @@ -# TODO add whereLabel to get_Bloblets -## ============================================================================== -## Variable Bloblets -## ============================================================================== -""" - $(SIGNATURES) -""" -function getVariableBloblet(dfg::GraphsDFG, var_label::Symbol, label::Symbol) - return getBloblet(getVariable(dfg, var_label), label) -end - -""" - $(SIGNATURES) -""" -function getVariableBloblets(dfg::GraphsDFG, var_label::Symbol) - return getBloblets(getVariable(dfg, var_label)) -end - -""" - $(SIGNATURES) -""" -function addVariableBloblet!(dfg::GraphsDFG, var_label::Symbol, bloblet::Bloblet) - return addBloblet!(getVariable(dfg, var_label), bloblet) -end - -""" - $(SIGNATURES) -""" -function addVariableBloblets!(dfg::GraphsDFG, var_label::Symbol, bloblets::Vector{Bloblet}) - return addBloblets!(getVariable(dfg, var_label), bloblets) -end - -""" - $(SIGNATURES) -""" -function mergeVariableBloblet!(dfg::GraphsDFG, var_label::Symbol, bloblet::Bloblet) - return mergeBloblet!(getVariable(dfg, var_label), bloblet) -end - -""" - $(SIGNATURES) -""" -function mergeVariableBloblets!( - dfg::GraphsDFG, - var_label::Symbol, - bloblets::Vector{Bloblet}, -) - return mergeBloblets!(getVariable(dfg, var_label), bloblets) -end - -""" - $(SIGNATURES) -""" -function deleteVariableBloblet!(dfg::GraphsDFG, var_label::Symbol, label::Symbol) - return deleteBloblet!(getVariable(dfg, var_label), label) -end - -""" - $(SIGNATURES) -""" -function deleteVariableBloblets!(dfg::GraphsDFG, var_label::Symbol, labels::Vector{Symbol}) - return deleteBloblets!(getVariable(dfg, var_label), labels) -end - -""" - $(SIGNATURES) -""" -function listVariableBloblets(dfg::GraphsDFG, var_label::Symbol) - return listBloblets(getVariable(dfg, var_label)) -end - -""" - $(SIGNATURES) -""" -function hasVariableBloblet(dfg::GraphsDFG, var_label::Symbol, label::Symbol) - return hasBloblet(getVariable(dfg, var_label), label) -end - -## ============================================================================== -## Factor Bloblets -## ============================================================================== -""" - $(SIGNATURES) -""" -function getFactorBloblet(dfg::GraphsDFG, fac_label::Symbol, label::Symbol) - return getBloblet(getFactor(dfg, fac_label), label) -end - -""" - $(SIGNATURES) -""" -function getFactorBloblets(dfg::GraphsDFG, fac_label::Symbol) - return getBloblets(getFactor(dfg, fac_label)) -end - -""" - $(SIGNATURES) -""" -function addFactorBloblet!(dfg::GraphsDFG, fac_label::Symbol, bloblet::Bloblet) - return addBloblet!(getFactor(dfg, fac_label), bloblet) -end - -""" - $(SIGNATURES) -""" -function addFactorBloblets!(dfg::GraphsDFG, fac_label::Symbol, bloblets::Vector{Bloblet}) - return addBloblets!(getFactor(dfg, fac_label), bloblets) -end - -""" - $(SIGNATURES) -""" -function mergeFactorBloblet!(dfg::GraphsDFG, fac_label::Symbol, bloblet::Bloblet) - return mergeBloblet!(getFactor(dfg, fac_label), bloblet) -end - -""" - $(SIGNATURES) -""" -function mergeFactorBloblets!(dfg::GraphsDFG, fac_label::Symbol, bloblets::Vector{Bloblet}) - return mergeBloblets!(getFactor(dfg, fac_label), bloblets) -end - -""" - $(SIGNATURES) -""" -function deleteFactorBloblet!(dfg::GraphsDFG, fac_label::Symbol, label::Symbol) - return deleteBloblet!(getFactor(dfg, fac_label), label) -end - -""" - $(SIGNATURES) -""" -function deleteFactorBloblets!(dfg::GraphsDFG, fac_label::Symbol, labels::Vector{Symbol}) - return deleteBloblets!(getFactor(dfg, fac_label), labels) -end - -""" - $(SIGNATURES) -""" -function listFactorBloblets(dfg::GraphsDFG, fac_label::Symbol) - return listBloblets(getFactor(dfg, fac_label)) -end - -""" - $(SIGNATURES) -""" -function hasFactorBloblet(dfg::GraphsDFG, fac_label::Symbol, label::Symbol) - return hasBloblet(getFactor(dfg, fac_label), label) -end - -##============================================================================== -## Agent Bloblets -##============================================================================== -""" - $(SIGNATURES) -""" -function getAgentBloblet(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) - return getBloblet(getAgent(dfg, agentlabel), label) -end -""" - $(SIGNATURES) -""" -function addAgentBloblet!(dfg::GraphsDFG, agentlabel::Symbol, bloblet::Bloblet) - return addBloblet!(getAgent(dfg, agentlabel), bloblet) -end -""" - $(SIGNATURES) -""" -function mergeAgentBloblet!(dfg::GraphsDFG, agentlabel::Symbol, bloblet::Bloblet) - return mergeBloblet!(getAgent(dfg, agentlabel), bloblet) -end -""" - $(SIGNATURES) -""" -function deleteAgentBloblet!(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) - return deleteBloblet!(getAgent(dfg, agentlabel), label) -end -""" - $(SIGNATURES) -""" -function getAgentBloblets(dfg::GraphsDFG, agentlabel::Symbol) - return getBloblets(getAgent(dfg, agentlabel)) -end -""" - $(SIGNATURES) -""" -function addAgentBloblets!(dfg::GraphsDFG, agentlabel::Symbol, bloblets::Vector{Bloblet}) - return addBloblets!(getAgent(dfg, agentlabel), bloblets) -end -""" - $(SIGNATURES) -""" -function mergeAgentBloblets!(dfg::GraphsDFG, agentlabel::Symbol, bloblets::Vector{Bloblet}) - return mergeBloblets!(getAgent(dfg, agentlabel), bloblets) -end -""" - $(SIGNATURES) -""" -function deleteAgentBloblets!(dfg::GraphsDFG, agentlabel::Symbol, labels::Vector{Symbol}) - return deleteBloblets!(getAgent(dfg, agentlabel), labels) -end -""" - $(SIGNATURES) -""" -function listAgentBloblets(dfg::GraphsDFG, agentlabel::Symbol) - return listBloblets(getAgent(dfg, agentlabel)) -end -""" - $(SIGNATURES) -""" -function hasAgentBloblet(dfg::GraphsDFG, agentlabel::Symbol, label::Symbol) - return hasBloblet(getAgent(dfg, agentlabel), label) -end - -##============================================================================== -## Graph Bloblets -##============================================================================== -""" - $(SIGNATURES) -""" -getGraphBloblet(dfg::GraphsDFG, label::Symbol) = getBloblet(dfg.graph, label) -""" - $(SIGNATURES) -""" -addGraphBloblet!(dfg::GraphsDFG, bloblet::Bloblet) = addBloblet!(dfg.graph, bloblet) -""" - $(SIGNATURES) -""" -mergeGraphBloblet!(dfg::GraphsDFG, bloblet::Bloblet) = mergeBloblet!(dfg.graph, bloblet) -""" - $(SIGNATURES) -""" -deleteGraphBloblet!(dfg::GraphsDFG, label::Symbol) = deleteBloblet!(dfg.graph, label) -""" - $(SIGNATURES) -""" -getGraphBloblets(dfg::GraphsDFG) = getBloblets(dfg.graph) -""" - $(SIGNATURES) -""" -function addGraphBloblets!(dfg::GraphsDFG, bloblets::Vector{Bloblet}) - return addBloblets!(dfg.graph, bloblets) -end -""" - $(SIGNATURES) -""" -function mergeGraphBloblets!(dfg::GraphsDFG, bloblets::Vector{Bloblet}) - return mergeBloblets!(dfg.graph, bloblets) -end -""" - $(SIGNATURES) -""" -function deleteGraphBloblets!(dfg::GraphsDFG, labels::Vector{Symbol}) - return deleteBloblets!(dfg.graph, labels) -end -""" - $(SIGNATURES) -""" -listGraphBloblets(dfg::GraphsDFG) = listBloblets(dfg.graph) -""" - $(SIGNATURES) -""" -hasGraphBloblet(dfg::GraphsDFG, label::Symbol) = hasBloblet(dfg.graph, label) diff --git a/src/services/CommonAccessors.jl b/src/services/CommonAccessors.jl deleted file mode 100644 index 23186a64..00000000 --- a/src/services/CommonAccessors.jl +++ /dev/null @@ -1,96 +0,0 @@ -##============================================================================== -## Common Accessors -##============================================================================== - -##------------------------------------------------------------------------------ -## References to containers -##------------------------------------------------------------------------------ - -refTags(node) = node.tags -refBlobentries(node) = node.blobentries -refBloblets(node) = node.bloblets - -##------------------------------------------------------------------------------ -## By value accessors -##------------------------------------------------------------------------------ - -# Common get and set methods - -# NOTE this could be reduced with macros and function generation to even less code. - -""" - $(SIGNATURES) -Get the label of the node. -""" -getLabel(node) = node.label - -""" -$SIGNATURES - -Get the timestamp of a AbstractGraphNode. -""" -getTimestamp(node) = node.timestamp - -""" - $(SIGNATURES) -""" -getDescription(node) = node.description - -##------------------------------------------------------------------------------ -## solvable -##------------------------------------------------------------------------------ - -""" - $SIGNATURES - -Variables or factors may or may not be 'solvable', depending on a user definition. Useful for ensuring atomic transactions. - -Related: -- isSolveInProgress -""" -function getSolvable(node::Union{VariableDFG, VariableSummary, FactorDFG, FactorSummary}) - return node.solvable[] -end - -""" - $SIGNATURES - -Get 'solvable' parameter for either a variable or factor. -""" -function getSolvable(dfg::AbstractDFG, sym::Symbol) - if isVariable(dfg, sym) - return getVariable(dfg, sym).solvable[] - elseif isFactor(dfg, sym) - return getFactor(dfg, sym).solvable[] - end -end - -""" - $SIGNATURES - -Set the `solvable` parameter for either a variable or factor. -""" -function setSolvable!(node::Union{VariableDFG, FactorDFG}, solvable::Int) - node.solvable[] = solvable - return solvable -end - -#FIXME this is only for in memory DFGs -function setSolvable!(dfg::AbstractDFG, sym::Symbol, solvable::Int) - if isVariable(dfg, sym) - getVariable(dfg, sym).solvable[] = solvable - elseif isFactor(dfg, sym) - getFactor(dfg, sym).solvable[] = solvable - end - return solvable -end - -""" - $SIGNATURES - -Variables or factors may or may not be 'solvable', depending on a user definition. -returns true if `getSolvable` > 0 -Related: -- `getSolvable`(@ref) -""" -isSolvable(node::Union{VariableDFG, FactorDFG}) = getSolvable(node) > 0 diff --git a/src/services/DFGVariable.jl b/src/services/DFGVariable.jl deleted file mode 100644 index 08537002..00000000 --- a/src/services/DFGVariable.jl +++ /dev/null @@ -1,533 +0,0 @@ -##============================================================================== -## Accessors -##============================================================================== - -##============================================================================== -## Variable Node Data -##============================================================================== - -##------------------------------------------------------------------------------ -## variableType -##------------------------------------------------------------------------------ -""" - $(SIGNATURES) - -Get the kind of the variable's state, eg. `Pose2`, `Point3`, etc. as an instance of `StateType`. -""" -getStateKind(::VariableDFG{T}) where {T} = T() - -getStateKind(::State{T}) where {T} = T() - -getStateKind(dfg::AbstractDFG, lbl::Symbol) = getStateKind(getVariable(dfg, lbl)) - -##------------------------------------------------------------------------------ -## StateType -##------------------------------------------------------------------------------ - -# """ -# $SIGNATURES -# Interface function to return the `variableType` manifolds of an StateType, extend this function for all Types<:StateType. -# """ -# function getManifolds end - -# getManifolds(::Type{<:T}) where {T <: ManifoldsBase.AbstractManifold} = convert(Tuple, T) -# getManifolds(::T) where {T <: ManifoldsBase.AbstractManifold} = getManifolds(T) - -""" - @defStateType StructName manifold point_identity - -A macro to create a new variable type with name `StructName` associated with a given manifold and identity point. - -- `StructName` is the name of the new variable type, which will be defined as a subtype of `StateType`. -- `manifold` is an object that must be a subtype of `ManifoldsBase.AbstractManifold`. -- `point_identity` is the identity point on the manifold, used as a reference for operations. - -This macro is useful for defining variable types that are not parameterized by dimension, and for associating them with a specific manifold and identity point. - -See the [Manifolds.jl documentation on creating your own manifolds](https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html) for more information. - -Example: -``` -DFG.@defStateType Pose2 SpecialEuclideanGroup(2) ArrayPartition([0;0.0],[1 0; 0 1.0]) -``` -""" -macro defStateType(structname, manifold, point_identity) - return esc( - quote - Base.@__doc__ struct $structname <: StateType{Any} end - - # user manifold must be a <:Manifold - @assert ($manifold isa AbstractManifold) "defStateType of " * - string($structname) * - " requires that the " * - string($manifold) * - " be a subtype of `ManifoldsBase.AbstractManifold`" - - DFG.getManifold(::Type{$structname}) = $manifold - - DFG.getPointType(::Type{$structname}) = typeof($point_identity) - - DFG.getPointIdentity(::Type{$structname}) = $point_identity - end, - ) -end - -""" - @defStateTypeN StructName manifold point_identity - -A macro to create a new variable type with name `StructName` that is parameterized by `N` and associated with a given manifold and identity point. - -- `StructName` is the name of the new variable type, which will be defined as a subtype of `StateType{N}`. -- `manifold` is an object that must be a subtype of `ManifoldsBase.AbstractManifold`. -- `point_identity` is the identity point on the manifold, used as a reference for operations. - -This macro is useful for defining variable types that are parameterized by dimension or other type parameters (e.g., `Pose{N}`), and for associating them with a specific manifold and identity point. - -See the [Manifolds.jl documentation on creating your own manifolds](https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html) for more information. - -Example: -``` -DFG.@defStateTypeN Pose{N} SpecialEuclideanGroup(N) ArrayPartition(zeros(SVector{N, Float64}), SMatrix{N, N, Float64}(I)) -``` -""" -macro defStateTypeN(structname, manifold, point_identity) - return esc( - quote - Base.@__doc__ struct $structname <: StateType{N} end - - DFG.getManifold(::Type{$structname}) where {N} = $manifold - - DFG.getPointType(::Type{$structname}) where {N} = typeof($point_identity) - - DFG.getPointIdentity(::Type{$structname}) where {N} = $point_identity - end, - ) -end - -#TODO why this convert? rather enforce explicit use of getManifold -function Base.convert( - ::Type{<:AbstractManifold}, - ::Union{<:T, Type{<:T}}, -) where {T <: StateType} - #TODO Deprecate v0.29 - Base.depwarn( - "convert(AbstractManifold, StateType) is deprecated, use getManifold instead", - :convert, - ) - return getManifold(T) -end - -""" - $SIGNATURES -Interface function to return the `<:ManifoldsBase.AbstractManifold` object of `variableType<:StateType`. -""" -getManifold(::T) where {T <: StateType} = getManifold(T) -getManifold(vari::VariableDFG) = getStateKind(vari) |> getManifold -getManifold(state::State) = getStateKind(state) |> getManifold -# covers both <:StateType and <:AbstractObservation -getManifold(dfg::AbstractDFG, lbl::Symbol) = getManifold(dfg[lbl]) - -""" - $SIGNATURES -Interface function to return the `variableType` dimension of an StateType, extend this function for all Types<:StateType. -""" -function getDimension end - -getDimension(::Type{T}) where {T <: StateType} = manifold_dimension(getManifold(T)) -getDimension(::T) where {T <: StateType} = manifold_dimension(getManifold(T)) -getDimension(M::ManifoldsBase.AbstractManifold) = manifold_dimension(M) -getDimension(p::Distributions.Distribution) = length(p) -getDimension(var::VariableDFG) = getDimension(getStateKind(var)) - -""" - $SIGNATURES -Interface function to return the manifold point type of an StateType, extend this function for all Types<:StateType. -""" -function getPointType end -getPointType(::T) where {T <: StateType} = getPointType(T) - -""" - $SIGNATURES -Interface function to return the user provided identity point for this StateType manifold, extend this function for all Types<:StateType. - -Notes -- Used in transition period for Serialization. This function will likely be changed or deprecated entirely. -""" -function getPointIdentity end -getPointIdentity(::T) where {T <: StateType} = getPointIdentity(T) - -##------------------------------------------------------------------------------ -## solvedCount -##------------------------------------------------------------------------------ - -""" - $SIGNATURES - -Get the number of times a variable has been inferred -- i.e. `solvedCount`. - -Related - -isSolved, setSolvedCount! -""" -getSolvedCount(v::State) = v.solves -function getSolvedCount(v::VariableDFG, solveKey::Symbol = :default) - return getState(v, solveKey) |> getSolvedCount -end -function getSolvedCount(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default) - return getSolvedCount(getVariable(dfg, sym), solveKey) -end - -""" - $SIGNATURES - -Update/set the `solveCount` value. - -Related - -getSolved, isSolved -""" -setSolvedCount!(v::State, val::Int) = v.solves = val -function setSolvedCount!(v::VariableDFG, val::Int, solveKey::Symbol = :default) - return setSolvedCount!(getState(v, solveKey), val) -end -function setSolvedCount!( - dfg::AbstractDFG, - sym::Symbol, - val::Int, - solveKey::Symbol = :default, -) - return setSolvedCount!(getVariable(dfg, sym), val, solveKey) -end - -""" - $SIGNATURES - -Boolean on whether the variable has been solved. - -Related - -getSolved, setSolved! -""" -isSolved(v::State) = 0 < v.solves -function isSolved(v::VariableDFG, solveKey::Symbol = :default) - return getState(v, solveKey) |> isSolved -end -function isSolved(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default) - return isSolved(getVariable(dfg, sym), solveKey) -end - -##------------------------------------------------------------------------------ -## initialized -##------------------------------------------------------------------------------ -""" - $SIGNATURES - -Returns state of variable data `.initialized` flag. - -Notes: -- used by both factor graph variable and Bayes tree clique logic. -""" -function isInitialized(var::VariableDFG, key::Symbol = :default) - return getState(var, key).initialized -end - -function isInitialized(dfg::AbstractDFG, label::Symbol, key::Symbol = :default) - return isInitialized(getVariable(dfg, label), key)::Bool -end - -""" - $SIGNATURES - -Return `::Bool` on whether this variable has been marginalized. - -Notes: -- State default `solveKey=:default` -""" -function isMarginalized(vert::VariableDFG, solveKey::Symbol = :default) - return getState(vert, solveKey).marginalized -end -function isMarginalized(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default) - return isMarginalized(DFG.getVariable(dfg, sym), solveKey) -end - -""" - $SIGNATURES - -Mark a variable as marginalized `true` or `false`. -""" -function setMarginalized!(vnd::State, val::Bool) - return vnd.marginalized = val -end -function setMarginalized!(vari::VariableDFG, val::Bool, solveKey::Symbol = :default) - return setMarginalized!(getState(vari, solveKey), val) -end -function setMarginalized!( - dfg::AbstractDFG, - sym::Symbol, - val::Bool, - solveKey::Symbol = :default, -) - return setMarginalized!(getVariable(dfg, sym), val, solveKey) -end - -##============================================================================== -## Variables -##============================================================================== -# -# | | label | tags | timestamp | variableTypeName | solvable | solverData | smallData | dataEntries | -# |---------------------|:-----:|:----:|:---------:|:----------------:|:--------:|:----------:|:---------:|:-----------:| -# | VariableSkeleton | X | X | | | | | | | -# | VariableSummary | X | X | X | X | | | | X | -# | VariableDFG | X | X | x | | X | X | X | X | -# -##------------------------------------------------------------------------------ - -##------------------------------------------------------------------------------ -## label -##------------------------------------------------------------------------------ - -## COMMON -# getLabel - -##------------------------------------------------------------------------------ -## tags -##------------------------------------------------------------------------------ - -## COMMON - -##------------------------------------------------------------------------------ -## timestamp -##------------------------------------------------------------------------------ - -## COMMON -# getTimestamp - -##------------------------------------------------------------------------------ -## solvable -##------------------------------------------------------------------------------ - -## COMMON: solvable -# getSolvable -# setSolvable! -# isSolvable - -## COMMON: - -##------------------------------------------------------------------------------ -## Variable Metadata -##------------------------------------------------------------------------------ - -# Generic Metadata CRUD -# TODO optimize for difference in in-memory by extending in other drivers. - -##------------------------------------------------------------------------------ -## Blobentries and Blobs -##------------------------------------------------------------------------------ -## see DataEntryBlob Folder - -##============================================================================== -## Layer 2 CRUD and SET -##============================================================================== - -##============================================================================== -## TAGS - See CommonAccessors -##============================================================================== - -##============================================================================== -## Variable Node Data -##============================================================================== -##------------------------------------------------------------------------------ -## CRUD: get, add, update, delete -##------------------------------------------------------------------------------ -hasState(v::VariableDFG, label::Symbol) = haskey(v.states, label) -function hasState(dfg::AbstractDFG, variableLabel::Symbol, label::Symbol) - return hasState(getVariable(dfg, variableLabel), label) -end - -function getState(v::VariableDFG, label::Symbol) - !haskey(refStates(v), label) && throw(LabelNotFoundError("State", label)) - return refStates(v)[label] -end - -""" - $(SIGNATURES) -Get the variable `State` for a given state label. -""" -function getState(dfg::AbstractDFG, variableLabel::Symbol, label::Symbol) - v = getVariable(dfg, variableLabel) - return getState(v, label) -end - -#TODO add filters -function getStates(dfg::AbstractDFG, variableLabel::Symbol) - v = getVariable(dfg, variableLabel) - return collect(values(refStates(v))) -end - -""" - $(SIGNATURES) -Add variable solver data, errors if it already exists. -""" -function addState!(dfg::GraphsDFG, variableLabel::Symbol, state::State) - var = getVariable(dfg, variableLabel) - return addState!(var, state) -end - -function addState!(v::VariableDFG, state::State) - if haskey(refStates(v), state.label) - throw(LabelExistsError("State", state.label)) - end - refStates(v)[state.label] = state - return state -end - -""" - $(SIGNATURES) -Add variable `State`s by calling `addState!`. -NOTE: If an error occurs while adding one of the states, previously added states will not be rolled back. -""" -function addStates!(dfg::AbstractDFG, variableLabel::Symbol, states::Vector{<:State}) - cnt = asyncmap(states) do state - addState!(dfg, variableLabel, state) - return 1 - end - return sum(cnt) -end - -function addStates!(dfg::AbstractDFG, varLabel_state_pairs::Vector{<:Pair{Symbol, <:State}}) - cnt = asyncmap(varLabel_state_pairs) do (varLabel, state) - addState!(dfg, varLabel, state) - return 1 - end - return sum(cnt) -end - -""" - $(SIGNATURES) -Update the variable state if it exists, otherwise add it. - -Related - -mergeStates! -""" -function mergeState!(dfg::GraphsDFG, variableLabel::Symbol, vnd::State) - return mergeState!(getVariable(dfg, variableLabel), vnd) -end - -function mergeState!(v::VariableDFG, vnd::State) - if !haskey(v.states, vnd.label) - addState!(v, vnd) - else - v.states[vnd.label] = vnd - end - return 1 -end - -function mergeStates!( - dfg::AbstractDFG, - varLabel_state_pairs::Vector{<:Pair{Symbol, <:State}}, -) - cnt = asyncmap(varLabel_state_pairs) do (varLabel, state) - return mergeState!(dfg, varLabel, state) - end - return sum(cnt) -end - -function mergeStates!(dfg::AbstractDFG, variableLabel::Symbol, states::Vector{<:State}) - cnt = asyncmap(states) do state - return mergeState!(dfg, variableLabel, state) - end - return sum(cnt) -end - -function copytoState!( - dfg::AbstractDFG, - variableLabel::Symbol, - stateLabel::Symbol, - state::State, -) - newstate = State(state; label = stateLabel) - return mergeState!(dfg, variableLabel, newstate) -end - -""" - $(SIGNATURES) -Delete the variable `State` by label, returns the number of deleted elements. -""" -function deleteState!(dfg::GraphsDFG, variableLabel::Symbol, label::Symbol) - return deleteState!(getVariable(dfg, variableLabel), label) -end - -function deleteState!(v::VariableDFG, label::Symbol) - !haskey(v.states, label) && return 0 - delete!(v.states, label) - return 1 -end - -function deleteState!(dfg::AbstractDFG, sourceVariable::VariableDFG, label::Symbol) - return deleteState!(dfg, sourceVariable.label, label) -end - -""" - $(SIGNATURES) -Delete the variable `State`s by label, returns the number of deleted elements. -""" -function deleteStates!(dfg::AbstractDFG, variableLabel::Symbol, labels::Vector{Symbol}) - cnt = asyncmap(labels) do label - return deleteState!(dfg, variableLabel, label) - end - return sum(cnt) -end - -function deleteStates!( - dfg::AbstractDFG, - varLabel_stateLabel_pairs::Vector{Pair{Symbol, Symbol}}, -) - cnt = asyncmap(varLabel_stateLabel_pairs) do (varLabel, stateLabel) - return deleteState!(dfg, varLabel, stateLabel) - end - return sum(cnt) -end - -##------------------------------------------------------------------------------ -## SET: list, merge -##------------------------------------------------------------------------------ - -""" - $(SIGNATURES) -List all the variable state labels. -""" -function listStates(v::VariableDFG; whereLabel::Union{Nothing, Function} = nothing) - labels = collect(keys(v.states)) - return filterDFG!(labels, whereLabel) -end - -function listStates( - dfg::AbstractDFG, - lbl::Symbol; - whereLabel::Union{Nothing, Function} = nothing, -) - return listStates(getVariable(dfg, lbl); whereLabel) -end - -function listStates( - dfg::AbstractDFG; - whereLabel::Union{Nothing, Function} = nothing, - whereSolvable::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - whereType::Union{Nothing, Function} = nothing, - whereVariableLabel::Union{Nothing, Function} = nothing, -) - labels = Set{Symbol}() - vls = listVariables( - dfg; - whereSolvable, - whereTags, - whereType, - whereLabel = whereVariableLabel, - ) - for vl in vls - union!(labels, listStates(dfg, vl; whereLabel)) - end - return collect(labels) -end diff --git a/src/services/Tags.jl b/src/services/Tags.jl deleted file mode 100644 index 5c376740..00000000 --- a/src/services/Tags.jl +++ /dev/null @@ -1,164 +0,0 @@ -# from CommonAccessors.jl -##============================================================================== -## TAGS as a set -- list, merge, delete, (empty?) -##============================================================================== -# Node level functions (in-memory) -""" -$SIGNATURES -""" -listTags(node) = collect(refTags(node)) - -""" - $SIGNATURES - -Merge add tags to a variable or factor (union) -""" -function mergeTags!(node, tags) - union!(refTags(node), tags) - return length(tags) -end -""" -$SIGNATURES - -Remove the tags from the node (setdiff) -""" -function deleteTags!(node, tags) - setdiff!(refTags(node), tags) - return length(tags) -end - -""" -$SIGNATURES - -Empty all tags from the node (empty) -""" -emptyTags!(node) = empty!(refTags(node)) - -# DFG level functions -##============================================================================== - -""" -$SIGNATURES - -List the tags for a variable. -""" -function listVariableTags(dfg::AbstractDFG, sym::Symbol) - return listTags(getVariable(dfg, sym)) -end - -function listFactorTags(dfg::AbstractDFG, sym::Symbol) - return listTags(getFactor(dfg, sym)) -end - -function listGraphTags(dfg::InMemoryDFGTypes) - return listTags(dfg.graph) -end - -function listAgentTags(dfg::InMemoryDFGTypes, agentlabel::Symbol) - return listTags(getAgent(dfg, agentlabel)) -end - -# function mergeVariableTags!(dfg::AbstractDFG, sym::Symbol, tags) -# v = getVariable(dfg, sym) -# mergeTags!(v, tags) -# mergeVariable!(dfg, v) -# return length(tags) -# end - -# function mergeFactorTags!(dfg::AbstractDFG, sym::Symbol, tags) -# f = getFactor(dfg, sym) -# mergeTags!(f, tags) -# mergeFactor!(dfg, f) -# return length(tags) -# end - -function mergeVariableTags!(dfg::InMemoryDFGTypes, label::Symbol, tags) - return mergeTags!(getVariable(dfg, label), tags) -end - -function mergeFactorTags!(dfg::InMemoryDFGTypes, label::Symbol, tags) - return mergeTags!(getFactor(dfg, label), tags) -end - -function mergeGraphTags!(dfg::InMemoryDFGTypes, tags) - mergeTags!(dfg.graph, tags) - return length(tags) -end - -function mergeAgentTags!(dfg::InMemoryDFGTypes, agentlabel::Symbol, tags) - mergeTags!(getAgent(dfg, agentlabel), tags) - return length(tags) -end - -##------------------------------------------------------------------------------ - -function deleteVariableTags!(dfg::InMemoryDFGTypes, label::Symbol, tags) - return deleteTags!(getVariable(dfg, label), tags) -end - -function deleteFactorTags!(dfg::InMemoryDFGTypes, label::Symbol, tags) - return deleteTags!(getFactor(dfg, label), tags) -end - -function deleteGraphTags!(dfg::InMemoryDFGTypes, tags) - deleteTags!(dfg.graph, tags) - return length(tags) -end - -function deleteAgentTags!(dfg::InMemoryDFGTypes, agentlabel::Symbol, tags) - deleteTags!(getAgent(dfg, agentlabel), tags) - return length(tags) -end - -##------------------------------------------------------------------------------ - -function hasVariableTags(dfg::AbstractDFG, sym::Symbol, tags::Vector{Symbol}) - return tags ⊆ listVariableTags(dfg, sym) -end - -function hasFactorTags(dfg::AbstractDFG, sym::Symbol, tags::Vector{Symbol}) - return tags ⊆ listFactorTags(dfg, sym) -end - -function hasGraphTags(dfg::AbstractDFG, tags::Vector{Symbol}) - return tags ⊆ listGraphTags(dfg) -end - -function hasAgentTags(dfg::AbstractDFG, agentlabel::Symbol, tags::Vector{Symbol}) - return tags ⊆ listAgentTags(dfg, agentlabel) -end - -## -function listTags(dfg::AbstractDFG, sym::Symbol) - getFnc = isVariable(dfg, sym) ? getVariable : getFactor - return listTags(getFnc(dfg, sym)) -end - -function mergeTags!(dfg::InMemoryDFGTypes, sym::Symbol, tags) - getFnc = isVariable(dfg, sym) ? getVariable : getFactor - union!(refTags(getFnc(dfg, sym)), tags) - return length(tags) -end - -function deleteTags!(dfg::InMemoryDFGTypes, sym::Symbol, tags) - getFnc = isVariable(dfg, sym) ? getVariable : getFactor - setdiff!(refTags(getFnc(dfg, sym)), tags) - return length(tags) -end - -function emptyTags!(dfg::InMemoryDFGTypes, sym::Symbol) - getFnc = isVariable(dfg, sym) ? getVariable : getFactor - return empty!(refTags(getFnc(dfg, sym))) -end - -##------------------------------------------------------------------------------ -## TODO -##------------------------------------------------------------------------------ -""" - $SIGNATURES - -Determine if the variable or factor neighbors have the `tags::Vector{Symbol}``. -""" -function hasTags(dfg::AbstractDFG, sym::Symbol, tags::Vector{Symbol}) - return tags ⊆ listTags(dfg, sym) -end diff --git a/src/services/agent_ops.jl b/src/services/agent_ops.jl new file mode 100644 index 00000000..1c095f81 --- /dev/null +++ b/src/services/agent_ops.jl @@ -0,0 +1,62 @@ +# ============================================================================== +# Agent CRUD +# ============================================================================== +""" + $(SIGNATURES) +Get an agent by label. +""" +function getAgent end + +""" + $(SIGNATURES) +Get all agents in the DFG. +""" +function getAgents end + +""" + $(SIGNATURES) +Add an agent to the DFG. +""" +function addAgent! end + +""" + $(SIGNATURES) +Add multiple agents to the DFG. +""" +function addAgents! end + +""" + $(SIGNATURES) +Merge an agent into the DFG (add or update). +""" +function mergeAgent! end + +""" + $(SIGNATURES) +Merge multiple agents into the DFG. +""" +function mergeAgents! end + +""" + $(SIGNATURES) +Delete an agent by label. +""" +function deleteAgent! end + +""" + $(SIGNATURES) +Delete multiple agents by label. +""" +function deleteAgents! end + +""" + $(SIGNATURES) +List all agent labels in the DFG. +""" +function listAgents end + +""" + $(SIGNATURES) +Check whether an agent with the given label exists. +""" +function hasAgent end diff --git a/src/DataBlobs/services/BlobWrappers.jl b/src/services/blob_save_load.jl similarity index 100% rename from src/DataBlobs/services/BlobWrappers.jl rename to src/services/blob_save_load.jl diff --git a/src/services/blobentry_ops.jl b/src/services/blobentry_ops.jl new file mode 100644 index 00000000..7b741bdf --- /dev/null +++ b/src/services/blobentry_ops.jl @@ -0,0 +1,203 @@ +##============================================================================== +## Blobentry - CRUD +##============================================================================== +function addVariableBlobentry! end +function addVariableBlobentries! end +function getVariableBlobentry end +function getVariableBlobentries end +function mergeVariableBlobentry! end +function mergeVariableBlobentries! end +function deleteVariableBlobentry! end +function deleteVariableBlobentries! end +function listVariableBlobentries end +function hasVariableBlobentry end + +function getFactorBlobentry end +function getFactorBlobentries end +function addFactorBlobentry! end +function addFactorBlobentries! end +function mergeFactorBlobentry! end +function mergeFactorBlobentries! end +function deleteFactorBlobentry! end +function deleteFactorBlobentries! end +function listFactorBlobentries end +function hasFactorBlobentry end + +##============================================================================== +## Agent/Graph/Model Blob Entries CRUD +##============================================================================== + +function getGraphBlobentry end +function getGraphBlobentries end +function addGraphBlobentry! end +function addGraphBlobentries! end +function mergeGraphBlobentry! end +function mergeGraphBlobentries! end +function deleteGraphBlobentry! end +function deleteGraphBlobentries! end + +function getAgentBlobentry end +function getAgentBlobentries end +function addAgentBlobentry! end +function addAgentBlobentries! end +function mergeAgentBlobentry! end +function mergeAgentBlobentries! end +function deleteAgentBlobentry! end +function deleteAgentBlobentries! end + +function getModelBlobentry end +function getModelBlobentries end +function addModelBlobentry! end +function addModelBlobentries! end +function mergeModelBlobentry! end +function mergeModelBlobentries! end +function deleteModelBlobentry! end +function deleteModelBlobentries! end + +function listGraphBlobentries end +function listAgentBlobentries end +function listModelBlobentries end + +function hasGraphBlobentry end +function hasAgentBlobentry end +function hasModelBlobentry end + +# ============================================================================== +# THE FOLLOWING ARE UNSTABLE OPPERATIONS +# ============================================================================== + +# ============================================================================== +# Blobentry - Helper functions, Lists, etc +# ============================================================================== + +function gatherBlobentries( + dfg::AbstractDFG; + whereLabel::Union{Nothing, Function} = nothing, + whereBlobid::Union{Nothing, Function} = nothing, + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + whereType::Union{Nothing, Function} = nothing, + whereVariableLabel::Union{Nothing, Function} = nothing, +) + vls = listVariables( + dfg; + whereSolvable, + whereTags, + whereType, + whereLabel = whereVariableLabel, + ) + return map(vls) do vl + return vl => getVariableBlobentries(dfg, vl; whereLabel, whereBlobid) + end +end +const collectBlobentries = gatherBlobentries + +""" + $(SIGNATURES) +Finds and returns the first blob entry that matches the filter. +The result is sorted by `sortby[=getLabel]` and `sortlt[=natural_lt]` before returning the first entry. +Also see: [`getBlobentry`](@ref) +""" +function getfirstBlobentry( + node; + whereLabel::Union{Nothing, Function} = nothing, + whereBlobid::Union{Nothing, Function} = nothing, + sortby::Function = getLabel, + sortlt::Function = natural_lt, +) + entries = getBlobentries(node; whereLabel, whereBlobid) + if isempty(entries) + return nothing + else + return sort(entries; by = sortby, lt = sortlt)[1] + end +end + +function getfirstVariableBlobentry( + dfg::AbstractDFG, + label::Symbol; + whereLabel::Union{Nothing, Function} = nothing, + whereBlobid::Union{Nothing, Function} = nothing, +) + return getfirstBlobentry(getVariable(dfg, label); whereLabel, whereBlobid) +end + +## ============================================================================= +## TODO Maybe deprecate/remove +## ============================================================================= + +""" + $SIGNATURES +List a collection of blob entries per variable that match a particular `pattern::Regex`. + +Notes +- Optional sort function argument, default is unsorted. + - Likely use of `sortDFG` for basic Symbol sorting. + +Example +```julia +listBlobentrySequence(fg, :x0, r"IMG_CENTER", sortDFG) +15-element Vector{Symbol}: + :IMG_CENTER_21676 + :IMG_CENTER_21677 + :IMG_CENTER_21678 + :IMG_CENTER_21679 +... +``` +""" +function listBlobentrySequence( + dfg::AbstractDFG, + lb::Symbol, + pattern::Regex, + _sort::Function = (x) -> x, +) + # + ents_ = listVariableBlobentries(dfg, lb) + entReg = map(l -> match(pattern, string(l)), ents_) + entMsk = entReg .!== nothing + return ents_[findall(entMsk)] |> _sort +end + +""" + $SIGNATURES + +If the blob label `datalabel` already exists, then this function will return the name `datalabel_1`. +If the blob label `datalabel_1` already exists, then this function will return the name `datalabel_2`. +""" +function incrDataLabelSuffix( + dfg::AbstractDFG, + vla::Symbol, + bllb::Union{Symbol, <:AbstractString}; + datalabel = Ref(""), +) + count = 1 + hasund = false + len = 0 + try + de = getfirstVariableBlobentry(dfg, vla; whereLabel = contains(string(bllb))) + isnothing(de) && return Symbol(bllb) # no match, return as is + bllb = string(bllb) + # bllb *= bllb[end] != '_' ? "_" : "" + datalabel[] = string(de.label) + dlb = match(r"\d*", reverse(datalabel[])) + # slightly complicated search if blob name already has an underscore number suffix, e.g. `_4` + count, hasund, len = if occursin(Regex(dlb.match * "_"), reverse(datalabel[])) + parse(Int, dlb.match |> reverse) + 1, true, length(dlb.match) + else + 1, datalabel[][end] == '_', 0 + end + catch err + # append latest count + if !(err isa KeyError) + throw(err) + end + end + # the piece from old label without the suffix count number + bllb = datalabel[][1:(end - len)] + if !hasund || bllb[end] != '_' + bllb *= "_" + end + bllb *= string(count) + + return Symbol(bllb) +end diff --git a/src/services/bloblet_ops.jl b/src/services/bloblet_ops.jl new file mode 100644 index 00000000..125b3cee --- /dev/null +++ b/src/services/bloblet_ops.jl @@ -0,0 +1,205 @@ +# ============================================================================== +# Variable Bloblets +# ============================================================================== +""" + $(SIGNATURES) +""" +function getVariableBloblet end + +""" + $(SIGNATURES) +""" +function getVariableBloblets end + +""" + $(SIGNATURES) +""" +function addVariableBloblet! end + +""" + $(SIGNATURES) +""" +function addVariableBloblets! end + +""" + $(SIGNATURES) +""" +function mergeVariableBloblet! end + +""" + $(SIGNATURES) +""" +function mergeVariableBloblets! end + +""" + $(SIGNATURES) +""" +function deleteVariableBloblet! end + +""" + $(SIGNATURES) +""" +function deleteVariableBloblets! end + +""" + $(SIGNATURES) +""" +function listVariableBloblets end + +""" + $(SIGNATURES) +""" +function hasVariableBloblet end + +# ============================================================================== +# Factor Bloblets +# ============================================================================== +""" + $(SIGNATURES) +""" +function getFactorBloblet end + +""" + $(SIGNATURES) +""" +function getFactorBloblets end + +""" + $(SIGNATURES) +""" +function addFactorBloblet! end + +""" + $(SIGNATURES) +""" +function addFactorBloblets! end + +""" + $(SIGNATURES) +""" +function mergeFactorBloblet! end + +""" + $(SIGNATURES) +""" +function mergeFactorBloblets! end + +""" + $(SIGNATURES) +""" +function deleteFactorBloblet! end + +""" + $(SIGNATURES) +""" +function deleteFactorBloblets! end + +""" + $(SIGNATURES) +""" +function listFactorBloblets end + +""" + $(SIGNATURES) +""" +function hasFactorBloblet end + +# ============================================================================== +# Agent Bloblets +# ============================================================================== +""" + $(SIGNATURES) +""" +function getAgentBloblet end + +""" + $(SIGNATURES) +""" +function addAgentBloblet! end + +""" + $(SIGNATURES) +""" +function mergeAgentBloblet! end + +""" + $(SIGNATURES) +""" +function deleteAgentBloblet! end + +""" + $(SIGNATURES) +""" +function getAgentBloblets end + +""" + $(SIGNATURES) +""" +function addAgentBloblets! end + +""" + $(SIGNATURES) +""" +function mergeAgentBloblets! end + +""" + $(SIGNATURES) +""" +function deleteAgentBloblets! end + +""" + $(SIGNATURES) +""" +function listAgentBloblets end + +""" + $(SIGNATURES) +""" +function hasAgentBloblet end + +# ============================================================================== +# Graph Bloblets +# ============================================================================== +""" + $(SIGNATURES) +""" +function getGraphBloblet end +""" + $(SIGNATURES) +""" +function addGraphBloblet! end +""" + $(SIGNATURES) +""" +function mergeGraphBloblet! end +""" + $(SIGNATURES) +""" +function deleteGraphBloblet! end +""" + $(SIGNATURES) +""" +function getGraphBloblets end +""" + $(SIGNATURES) +""" +function addGraphBloblets! end + +""" + $(SIGNATURES) +""" +function mergeGraphBloblets! end + +""" + $(SIGNATURES) +""" +function deleteGraphBloblets! end + +""" + $(SIGNATURES) +""" +function listGraphBloblets end +""" + $(SIGNATURES) +""" +function hasGraphBloblet end diff --git a/src/services/blobstore_ops.jl b/src/services/blobstore_ops.jl new file mode 100644 index 00000000..ac9a7795 --- /dev/null +++ b/src/services/blobstore_ops.jl @@ -0,0 +1,171 @@ +# ============================================================================== +# Blobstore Operations +# ============================================================================== + +""" + $(SIGNATURES) +Return the internal blobstore dictionary reference for `dfg`. + """ +function refBlobstores end + +""" + $(SIGNATURES) +Add a blobstore to the DFG. +""" +function addBlobstore! end + +""" + $(SIGNATURES) +Get a blobstore by label. +""" +function getBlobstore end + +""" + $(SIGNATURES) +Get all blobstores in the DFG. +""" +function getBlobstores end + +""" + $(SIGNATURES) +Merge a single blobstore link into the DFG (idempotent if identical). +""" +function mergeStorelink! end + +""" + $(SIGNATURES) +Merge a vector of blobstore links into the DFG. +""" +function mergeStorelinks! end + +""" + $(SIGNATURES) +Delete a blobstore by label. Returns 0 if not found. +""" +function deleteBlobstore! end + +""" + $(SIGNATURES) +List all blobstore labels in the DFG. +""" +function listBlobstores end + +""" + $(SIGNATURES) +Check whether a blobstore with the given label exists. +""" +function hasBlobstore end + +# ============================================================================== +# Blob Operations +# ============================================================================== +""" +Get the data blob for the specified Blobstore or DFG. + +Related +[`getBlobentry`](@ref) +Implement +`getBlob(store::AbstractBlobstore, blobid::UUID)` + +$(METHODLIST) +""" +function getBlob end + +""" +Adds a blob to the Blobstore with the blobid. + +Related +[`addBlobentry!`](@ref) +Implement +`addBlob!(store::AbstractBlobstore, blobid::UUID, data)` +$(METHODLIST) +""" +function addBlob! end + +""" +Delete a blob from the blob store or dfg with the given entry. + +Related +[`deleteBlobentry!`](@ref) +Implement +`deleteBlob!(store::AbstractBlobstore, blobid::UUID)` +$(METHODLIST) +""" +function deleteBlob! end + +""" + $(SIGNATURES) +List all `blobid`s in the blob store. +Implement +`listBlobs(store::AbstractBlobstore)` +""" +function listBlobs end + +""" + $(SIGNATURES) +Check if the blob store has a blob with the given `blobid`. +""" +function hasBlob end + +# ============================================================================== +# AbstractBlobstore derived CRUD for Blob +# ============================================================================== +#TODO maybe we should generalize and move the cached Blobstore to DFG. +function getBlob(dfg::AbstractDFG, entry::Blobentry) + storeLabel = entry.storelabel + store = getBlobstore(dfg, storeLabel) + return getBlob(store, entry.blobid) +end + +function getBlob(store::AbstractBlobstore, entry::Blobentry) + return getBlob(store, entry.blobid) +end + +#add +function addBlob!(dfg::AbstractDFG, entry::Blobentry, blob) + return addBlob!(getBlobstore(dfg, entry.storelabel), entry, blob) +end + +function addBlob!(store::AbstractBlobstore{T}, entry::Blobentry, blob::T) where {T} + return addBlob!(store, entry.blobid, blob) +end + +# also creates an blobid as uuid4 +addBlob!(store::AbstractBlobstore, blob) = addBlob!(store, uuid4(), blob) + +#delete +function deleteBlob!(dfg::AbstractDFG, entry::Blobentry) + return deleteBlob!(getBlobstore(dfg, entry.storelabel), entry) +end + +function deleteBlob!(store::AbstractBlobstore, entry::Blobentry) + return deleteBlob!(store, entry.blobid) +end + +#has +function hasBlob(store::AbstractBlobstore, entry::Blobentry) + return hasBlob(store, entry.blobid) +end +function hasBlob(dfg::AbstractDFG, entry::Blobentry) + return hasBlob(getBlobstore(dfg, entry.storelabel), entry.blobid) +end + +#TODO Copy is wrong verb here, merge works better, blobs are immutable so id should be enough, but we can maybe check blob and error with merge conflict. +# """ +# $(SIGNATURES) +# Merges all the entries from the source into the destination. +# Can specify which entries to merge with the `sourceEntries` parameter. +# Returns the list of merged entries. +# """ +# function mergeBlobstore!(sourceStore::D1, destStore::D2; sourceEntries=listEntries(sourceStore))::Vector{E} where {T, D1 <: AbstractDataStore{T}, D2 <: AbstractDataStore{T}, E <: Blobentry} +# # Quick check +# destEntries = listBlobs(destStore) +# typeof(sourceEntries) != typeof(destEntries) && error("Can't merge stores, source has entries of type $(typeof(sourceEntries)), destination has entries of type $(typeof(destEntries)).") +# # Same source/destination check +# sourceStore == destStore && error("Can't specify same store for source and destination.") +# # Otherwise, continue +# for sourceEntry in sourceEntries +# addBlob!(destStore, deepcopy(sourceEntry), getBlob(sourceStore, sourceEntry)) +# end +# return sourceEntries +# end diff --git a/src/services/CompareUtils.jl b/src/services/compare.jl similarity index 88% rename from src/services/CompareUtils.jl rename to src/services/compare.jl index f6b45531..aa6383bc 100644 --- a/src/services/CompareUtils.jl +++ b/src/services/compare.jl @@ -1,57 +1,4 @@ ## TODO update this file for DFG v1 -##============================================================================== -## (==) -##============================================================================== -import Base.== -## @generated compare -# Reference https://github.com/JuliaLang/julia/issues/4648 - -#= -For now abstract `StateType`s are considered equal if they are the same type, dims, and manifolds (abels are deprecated) -If your implentation has aditional properties such as `DynPose2` with `ut::Int64` (microsecond time) or support different manifolds -implement compare if needed. -=# -# ==(a::StateType,b::StateType) = typeof(a) == typeof(b) && a.dims == b.dims && a.manifolds == b.manifolds - -==(a::FactorCache, b::FactorCache) = typeof(a) == typeof(b) - -==(a::AbstractObservation, b::AbstractObservation) = typeof(a) == typeof(b) - -# Generate compares automatically for all in this union -const GeneratedCompareUnion = Union{ - BeliefRepresentation, - State, - Blobentry, - Bloblet, - VariableSkeleton, - FactorSkeleton, - Recipehyper, - Recipestate, -} - -@generated function ==(x::T, y::T) where {T <: GeneratedCompareUnion} - return mapreduce(n -> :(x.$n == y.$n), (a, b) -> :($a && $b), fieldnames(x)) -end - -function ==(x::T, y::T) where {T <: AbstractGraphFactor} - ignored = [:solvercache, :solvable] - tp = mapreduce( - n -> getproperty(x, n) == getproperty(y, n), - (a, b) -> a && b, - setdiff(propertynames(x), ignored), - ) - return tp && getSolvable(x) == getSolvable(y) -end - -function ==(x::T, y::T) where {T <: AbstractGraphVariable} - ignored = [:solvable] - tp = mapreduce( - n -> getproperty(x, n) == getproperty(y, n), - (a, b) -> a && b, - setdiff(propertynames(x), ignored), - ) - return tp && getSolvable(x) == getSolvable(y) -end ##============================================================================== ## Compare @@ -282,8 +229,8 @@ DevNotes - TODO `getSolverData(A).fnc.varValsAll / varidx` are only defined downstream, so should should this function not be in IIF? """ function compareFactor( - A::FactorCompute, - B::FactorCompute; + A::FactorDFG, + B::FactorDFG; show::Bool = true, skip::Vector{Symbol} = Symbol[], skipsamples::Bool = true, diff --git a/src/services/discovery.jl b/src/services/discovery.jl new file mode 100644 index 00000000..3706f4b5 --- /dev/null +++ b/src/services/discovery.jl @@ -0,0 +1,296 @@ +""" + $(SIGNATURES) +Checks if the graph is fully connected, returns true if so. +Implement `isConnected(dfg::AbstractDFG)` +""" +function isConnected end + +""" + $(SIGNATURES) +Retrieve a list of labels of the immediate neighbors around a given variable or factor specified by its label. +Implement `listNeighbors(dfg::AbstractDFG, label::Symbol; whereSolvable, whereTags)` +""" +function listNeighbors end + +""" + findPaths(dfg, from::Symbol, to::Symbol, k::Int; variableLabels, factorLabels, kwargs...) + +Return the `k` shortest paths between `from` and `to` in the factor graph. +Each result is a `(path = Vector{Symbol}, dist)` named tuple. + +Optional keyword arguments restrict which variables and/or factors may appear on +the path. When neither is given the full graph is used. When only one is +provided the other defaults to all labels of that kind in `dfg`. + +Typical usage with filters: +```julia +vars = listVariables(dfg; whereSolvable = >=(1)) +facs = listFactors(dfg; whereSolvable = >=(1)) +findPaths(dfg, :x1, :x5, 3; variableLabels = vars, factorLabels = facs) +``` + +See also: [`findPath`](@ref), [`listVariables`](@ref), [`listFactors`](@ref) +""" +function findPaths end + +""" + findPath(dfg, from::Symbol, to::Symbol; variableLabels, factorLabels, kwargs...) + +Return the single shortest path between `from` and `to`. +Errors if no path exists (use `findPaths` for graphs that may be disconnected). + +Accepts the same restriction keywords as [`findPaths`](@ref). +""" +function findPath end + +#TODO Move findPaths and findPath to AbstractDFG services as default implementations. +function findPaths( + dfg::AbstractDFG, + from::Symbol, + to::Symbol, + k::Int; + variableLabels::Union{Nothing, Vector{Symbol}} = nothing, + factorLabels::Union{Nothing, Vector{Symbol}} = nothing, + kwargs..., +) + # If the user provided restricted lists, build the subgraph automatically + active_dfg = + if isa(dfg, GraphsDFG) && isnothing(variableLabels) && isnothing(factorLabels) + dfg + else + vlabels = something(variableLabels, listVariables(dfg)) + flabels = something(factorLabels, listFactors(dfg)) + labels = vcat(vlabels, flabels) + DFG.getSubgraph( + GraphsDFG{NoSolverParams, VariableSkeleton, FactorSkeleton}, + dfg, + labels, + ) + end + !hasVariable(active_dfg, from) && + !hasFactor(active_dfg, from) && + throw(DFG.LabelNotFoundError(from)) + !hasVariable(active_dfg, to) && + !hasFactor(active_dfg, to) && + throw(DFG.LabelNotFoundError(to)) + + # optimization for k=1 since A* is more efficient than Yen's for single shortest path + if k == 1 + return findPaths(GraphsDFGs.a_star, active_dfg, from, to; kwargs...) + else + return findPaths(GraphsDFGs.yen_k_shortest_paths, active_dfg, from, to, k; kwargs...) + end +end + +function findPath( + dfg::AbstractDFG, + from::Symbol, + to::Symbol; + variableLabels::Union{Nothing, Vector{Symbol}} = nothing, + factorLabels::Union{Nothing, Vector{Symbol}} = nothing, + kwargs..., +) + paths = findPaths(dfg, from, to, 1; variableLabels, factorLabels, kwargs...) + + if isempty(paths) + return nothing + else + return first(paths) + end +end + +function listNeighbors(dfg::AbstractDFG, node::AbstractGraphNode; kwargs...) + return listNeighbors(dfg, getLabel(node); kwargs...) +end + +##============================================================================== +## Subgraphs and Neighborhoods +##============================================================================== + +#TODO add pruning filters that is applied during traversal. +""" + $(SIGNATURES) +Build a list of all unique neighbors inside 'distance'. Neighbors can be filtered by using keyword arguments, eg. [`whereTags`] and [`whereSolvable`]. +Filters are applied to final neighborhood result. + +Notes +- Returns a tuple `(variableLabels, factorLabels)`, where each element is a `Vector{Symbol}`. + +Related: +- [`getSubgraph`](@ref) +- [`mergeGraph!`](@ref) +""" +function listNeighborhood(dfg::AbstractDFG, label::Symbol, distance::Int; filters...) + neighborList = Set{Symbol}([label]) + curList = Set{Symbol}([label]) + + for dist = 1:distance + newNeighbors = Set{Symbol}() + for node in curList + neighbors = listNeighbors(dfg, node) + union!(neighborList, neighbors) + union!(newNeighbors, neighbors) + end + curList = newNeighbors + end + + variableLabels = intersect(listVariables(dfg; filters...), neighborList) + factorLabels = intersect(listFactors(dfg; filters...), neighborList) + + return variableLabels, factorLabels +end + +function listNeighborhood( + dfg::AbstractDFG, + variableFactorLabels::Vector{Symbol}, + distance::Int; + filters..., +) + if distance > 0 + variableLabels = Symbol[] + factorLabels = Symbol[] + for l in variableFactorLabels + varls, facls = listNeighborhood(dfg, l, distance; filters...) + union!(variableLabels, varls) + union!(factorLabels, facls) + end + else + variableLabels = intersect(listVariables(dfg; filters...), variableFactorLabels) + factorLabels = intersect(listFactors(dfg; filters...), variableFactorLabels) + end + + return variableLabels, factorLabels +end + +##============================================================================== +## Finding +##============================================================================== +# function findClosestTimestamp(setA::Vector{Tuple{DateTime,T}}, +# setB::Vector{Tuple{DateTime,S}}) where {S,T} +""" + $SIGNATURES + +Find and return the closest timestamp from two sets of Tuples. Also return the minimum delta-time (`::Nanosecond`) and how many elements match from the two sets are separated by the minimum delta-time. +""" +function findClosestTimestamp( + setA::Vector{Tuple{TimeDateZone, T}}, + setB::Vector{Tuple{TimeDateZone, S}}, +) where {S, T} + # + # build matrix of delta times, ranges on rows x vars on columns + DT = map(Iterators.product(setA, setB)) do (a, b) + return abs(DFG.calcDeltatime_ns(a[1], b[1])) + end + + # absolute time differences + # DTi = (x->x.value).(DT) .|> abs + + # find the smallest element + mdt = minimum(DT) + corrs = findall(x -> x == mdt, DT) + + # return the closest timestamp, deltaT, number of correspondences + return corrs[1].I, mdt, length(corrs) +end + +""" + $SIGNATURES + +Find and return nearest variable labels per delta time. Function will filter on `regexFilter`, `tags`, and `solvable`. + +Notes +- Returns `Vector{Tuple{Vector{Symbol}, Nanosecond}}` + +DevNotes: +- TODO `number` should allow returning more than one for k-nearest matches. +- Future versions likely will require some optimization around the internal `getVariable` call. + - Perhaps a dedicated/efficient `getVariableTimestamp` for all DFG flavors. + +Related + +ls, listVariables, findClosestTimestamp +""" +function findVariablesNearTimestamp( + dfg::AbstractDFG, + query_timestamp::TimeDateZone; + whereLabel::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + whereSolvable::Union{Nothing, Function} = nothing, + number::Int = 1, +) + # + # get the variable labels based on filters + vls = listVariables(dfg; whereLabel, whereTags, whereSolvable) + # compile timestamps with label + # vars = map( x->getVariable(dfg, x), vls ) + timeset = map(x -> (getTimestamp(getVariable(dfg, x)), x), vls) + mask = BitArray{1}(undef, length(vls)) + fill!(mask, true) + + RET = Vector{Tuple{Vector{Symbol}, Nanosecond}}() + SYMS = Symbol[] + CORRS = 1 + NUMBER = number + while 0 < CORRS + NUMBER + # get closest + link, mdt, corrs = findClosestTimestamp([(query_timestamp, 0)], timeset[mask]) + newsym = vls[link[2]] + union!(SYMS, !isa(newsym, Vector) ? [newsym] : newsym) + mask[link[2]] = false + CORRS = corrs - 1 + # last match, done with this delta time + if corrs == 1 + NUMBER -= 1 + push!(RET, (deepcopy(SYMS), mdt)) + SYMS = Symbol[] + end + end + + return RET +end + +function findVariablesNearTimestamp( + dfg::AbstractDFG, + query_timestamp::DateTime; + timezone = tz"UTC", + kwargs..., +) + return findVariablesNearTimestamp( + dfg, + TimeDateZone(query_timestamp, timezone); + kwargs..., + ) +end + +##============================================================================== +## Automated Graph Searching +##============================================================================== +""" + $SIGNATURES + +Speciallized function available to only GraphsDFG at this time. + +Notes +- Has option for various types of filters (increases memory usage) + +Example +```julia +using IncrementalInference + +# canonical example graph as example +fg = generateGraph_Kaess() + +@show path = findShortestPathDijkstra(fg, :x1, :x3) +@show isVariable.(fg, path) +@show isFactor.(fg, path) +``` + +DevNotes +- TODO expand to other AbstractDFG entities. +- TODO use of filter resource consumption can be improved. + +Related + +`Graphs.dijkstra_shortest_paths` +""" +function findShortestPathDijkstra end diff --git a/src/services/DFGFactor.jl b/src/services/factor_ops.jl similarity index 75% rename from src/services/DFGFactor.jl rename to src/services/factor_ops.jl index f40c2189..2fde28f0 100644 --- a/src/services/DFGFactor.jl +++ b/src/services/factor_ops.jl @@ -1,3 +1,91 @@ +""" + $(SIGNATURES) +Add a FactorDFG to a DFG. +Implement `addFactor!(dfg::AbstractDFG, factor::AbstractGraphFactor)` +""" +function addFactor! end + +""" + $(SIGNATURES) +Add a Vector{FactorDFG} to a DFG. +""" +function addFactors! end + +""" + $(SIGNATURES) +Get a FactorDFG from a DFG using its label. +Implement `getFactor(dfg::AbstractDFG, label::Symbol)` +""" +function getFactor end + +""" + $(SIGNATURES) +List the DFGFactors in the DFG. +Optionally specify a label regular expression to retrieves a subset of the factors. +""" +function getFactors end + +""" + $(SIGNATURES) +Merge a factor into the DFG. If a factor with the same label exists, it will be overwritten; +otherwise, the factor will be added to the graph. +Implement `mergeFactor!(dfg::AbstractDFG, factor::AbstractGraphFactor)` +""" +function mergeFactor! end + +function mergeFactors! end + +""" + $(SIGNATURES) +Delete a FactorDFG from the DFG using its label. +Implement `deleteFactor!(dfg::AbstractDFG, label::Symbol)` +""" +function deleteFactor! end + +""" + $(SIGNATURES) +Delete Factors from the DFG using their labels or filters. +""" +function deleteFactors! end + +""" + $(SIGNATURES) +Get a list of the labels of the DFGFactors in the DFG. +Optionally specify a label regular expression to retrieves a subset of the factors. +""" +function listFactors end + +""" + $(SIGNATURES) +True if the factor exists in the graph. +Implement `hasFactor(dfg::AbstractDFG, label::Symbol)` +""" +function hasFactor end + +# ============================================================================== +#TODO implement +function getFactorSkeleton end +function getFactorSummary end +""" + $(SIGNATURES) +Get the skeleton factors from a DFG as a Vector{FactorSkeleton}. +""" +function getFactorsSkeleton end +#TODO implement +function getFactorsSummary end + +# ======================================================================================= +function getFactors(dfg::AbstractDFG, labels::Vector{Symbol}) + return map(label -> getFactor(dfg, label), labels) +end + +function deleteFactor!(dfg::AbstractDFG, factor::AbstractGraphFactor) + return deleteFactor!(dfg, factor.label) +end + +# ======================================================================================= +# ======================================================================================= + ##============================================================================== ## Accessors ##============================================================================== @@ -195,11 +283,3 @@ end isPrior(f::AbstractGraphFactor) = isPrior(getObservation(f)) isPrior(dfg::AbstractDFG, fl::Symbol) = isPrior(getFactor(dfg, fl)) - -##============================================================================== -## Layer 2 CRUD (none) and Sets -##============================================================================== - -##============================================================================== -## TAGS - See CommonAccessors -##============================================================================== diff --git a/src/services/find.jl b/src/services/find.jl deleted file mode 100644 index 43912b7e..00000000 --- a/src/services/find.jl +++ /dev/null @@ -1,132 +0,0 @@ -##============================================================================== -## Finding -##============================================================================== -# function findClosestTimestamp(setA::Vector{Tuple{DateTime,T}}, -# setB::Vector{Tuple{DateTime,S}}) where {S,T} -""" - $SIGNATURES - -Find and return the closest timestamp from two sets of Tuples. Also return the minimum delta-time (`::Nanosecond`) and how many elements match from the two sets are separated by the minimum delta-time. -""" -function findClosestTimestamp( - setA::Vector{Tuple{TimeDateZone, T}}, - setB::Vector{Tuple{TimeDateZone, S}}, -) where {S, T} - # - # build matrix of delta times, ranges on rows x vars on columns - DT = map(Iterators.product(setA, setB)) do (a, b) - return abs(DFG.calcDeltatime_ns(a[1], b[1])) - end - - # absolute time differences - # DTi = (x->x.value).(DT) .|> abs - - # find the smallest element - mdt = minimum(DT) - corrs = findall(x -> x == mdt, DT) - - # return the closest timestamp, deltaT, number of correspondences - return corrs[1].I, mdt, length(corrs) -end - -""" - $SIGNATURES - -Find and return nearest variable labels per delta time. Function will filter on `regexFilter`, `tags`, and `solvable`. - -Notes -- Returns `Vector{Tuple{Vector{Symbol}, Nanosecond}}` - -DevNotes: -- TODO `number` should allow returning more than one for k-nearest matches. -- Future versions likely will require some optimization around the internal `getVariable` call. - - Perhaps a dedicated/efficient `getVariableTimestamp` for all DFG flavors. - -Related - -ls, listVariables, findClosestTimestamp -""" -function findVariablesNearTimestamp( - dfg::AbstractDFG, - query_timestamp::TimeDateZone; - whereLabel::Union{Nothing, Function} = nothing, - whereTags::Union{Nothing, Function} = nothing, - whereSolvable::Union{Nothing, Function} = nothing, - number::Int = 1, -) - # - # get the variable labels based on filters - vls = listVariables(dfg; whereLabel, whereTags, whereSolvable) - # compile timestamps with label - # vars = map( x->getVariable(dfg, x), vls ) - timeset = map(x -> (getTimestamp(getVariable(dfg, x)), x), vls) - mask = BitArray{1}(undef, length(vls)) - fill!(mask, true) - - RET = Vector{Tuple{Vector{Symbol}, Nanosecond}}() - SYMS = Symbol[] - CORRS = 1 - NUMBER = number - while 0 < CORRS + NUMBER - # get closest - link, mdt, corrs = findClosestTimestamp([(query_timestamp, 0)], timeset[mask]) - newsym = vls[link[2]] - union!(SYMS, !isa(newsym, Vector) ? [newsym] : newsym) - mask[link[2]] = false - CORRS = corrs - 1 - # last match, done with this delta time - if corrs == 1 - NUMBER -= 1 - push!(RET, (deepcopy(SYMS), mdt)) - SYMS = Symbol[] - end - end - - return RET -end - -function findVariablesNearTimestamp( - dfg::AbstractDFG, - query_timestamp::DateTime; - timezone = tz"UTC", - kwargs..., -) - return findVariablesNearTimestamp( - dfg, - TimeDateZone(query_timestamp, timezone); - kwargs..., - ) -end - -##============================================================================== -## Automated Graph Searching -##============================================================================== -""" - $SIGNATURES - -Speciallized function available to only GraphsDFG at this time. - -Notes -- Has option for various types of filters (increases memory usage) - -Example -```julia -using IncrementalInference - -# canonical example graph as example -fg = generateGraph_Kaess() - -@show path = findShortestPathDijkstra(fg, :x1, :x3) -@show isVariable.(fg, path) -@show isFactor.(fg, path) -``` - -DevNotes -- TODO expand to other AbstractDFG entities. -- TODO use of filter resource consumption can be improved. - -Related - -`Graphs.dijkstra_shortest_paths` -""" -function findShortestPathDijkstra end diff --git a/src/services/graph_ops.jl b/src/services/graph_ops.jl new file mode 100644 index 00000000..78a96e17 --- /dev/null +++ b/src/services/graph_ops.jl @@ -0,0 +1,131 @@ +# ============================================================================== +# Graph Operations (not applicable to a DFG) +# ============================================================================== +function addGraph! end +function deleteGraph! end +function listGraphs end +function getGraphs end + +# ============================================================================== +# AbstractGraphNode Functions +# ============================================================================== + +function Base.getindex(dfg::AbstractDFG, lbl::Symbol) + if isVariable(dfg, lbl) + getVariable(dfg, lbl) + elseif isFactor(dfg, lbl) + getFactor(dfg, lbl) + else + throw(LabelNotFoundError("GraphNode", lbl)) + end +end + +""" + $SIGNATURES + +Return whether `sym::Symbol` represents a variable vertex in the graph DFG. +Checks whether it both exists in the graph and is a variable. +(If you rather want a quick for type, just do node isa VariableDFG) +Implement `isVariable(dfg::AbstractDFG, label::Symbol)` +""" +function isVariable end + +""" + $SIGNATURES + +Return whether `sym::Symbol` represents a factor vertex in the graph DFG. +Checks whether it both exists in the graph and is a factor. +(If you rather want a quicker for type, just do node isa FactorDFG) +Implement `isFactor(dfg::AbstractDFG, label::Symbol)` +""" +function isFactor end + +# rather use isa in code, but ok, here it is +isVariable(dfg::AbstractDFG, node::AbstractGraphVariable) = true +isFactor(dfg::AbstractDFG, node::AbstractGraphFactor) = true + +##============================================================================== +## exists - alias for hasVariable || hasFactor +##============================================================================== +# exists alone is ambiguous and only for variables and factors where there rest of the nouns use has, +# TODO therefore, keep as internal or deprecate? +# additionally - variables and factors can possibly have the same label in other drivers such as NvaSDK + +""" + $(SIGNATURES) +True if a variable or factor with `label` exists in the graph. +""" +function exists(dfg::AbstractDFG, label::Symbol) + return hasVariable(dfg, label) || hasFactor(dfg, label) +end + +function exists(dfg::AbstractDFG, node::AbstractGraphNode) + return exists(dfg, node.label) +end + +""" + $(SIGNATURES) +Build a deep subgraph copy from the DFG given a list of variables and factors and an optional distance. +Note: Orphaned factors (where the subgraph does not contain all the related variables) are not returned. +Related: +- [`listNeighborhood`](@ref) +- [`mergeGraph!`](@ref) +Dev Notes +- Bulk vs node for node: a list of labels are compiled and the sugraph is copied in bulk. +""" +function getSubgraph( + ::Type{G}, + dfg::AbstractDFG, + variableFactorLabels::Vector{Symbol}, + distance::Int = 0; + whereSolvable::Union{Nothing, Function} = nothing, + whereTags::Union{Nothing, Function} = nothing, + graphLabel::Symbol = Symbol(getGraphLabel(dfg), "_sub_$(string(uuid4())[1:6])"), + solvable = nothing, #TODO deprecated in v0.29 + kwargs..., +) where {G <: AbstractDFG} + if !isnothing(solvable) + Base.depwarn( + "solvable kwarg is deprecated, use kwarg `whereSolvable = >=(solvable)` instead", #v0.29 + :getSubgraph, + ) + !isnothing(whereSolvable) && + error("Cannot use both solvable and whereSolvable kwargs.") + whereSolvable = >=(solvable) + end + #build up the neighborhood from variableFactorLabels + variableLabels, factorLabels = + listNeighborhood(dfg, variableFactorLabels, distance; whereSolvable, whereTags) + + # Copy the section of graph we want + destDFG = deepcopyGraph(G, dfg, variableLabels, factorLabels; graphLabel, kwargs...) + return destDFG +end + +function getSubgraph( + dfg::AbstractDFG, + variableFactorLabels::Vector{Symbol}, + distance::Int = 0; + kwargs..., +) + return getSubgraph(LocalDFG, dfg, variableFactorLabels, distance; kwargs...) +end + +""" + $(SIGNATURES) +Merge the source DFG into the destination DFG cascading down the hierarchy of DFG nodes. +Merge rules: +- Variables, Factors, Agents, and Graphroot, with the same label are merged if they are equal. + - On conflicts, a `MergeConflictError` is thrown. + - Child nodes (eg. tags, Bloblets, Blobentries, States, etc.) are using `merge!`. +- The Blobstore links are merged provided they point to the same Blobstore. + - On conflicts, a `MergeConflictError` is thrown. +""" +function mergeGraph!(destDFG::AbstractDFG, srcDFG::AbstractDFG) + patch!(destDFG.graph, srcDFG.graph) + mergeVariables!(destDFG, getVariables(srcDFG)) + mergeFactors!(destDFG, getFactors(srcDFG)) + mergeAgents!(destDFG, getAgents(srcDFG)) + mergeStorelinks!(destDFG, getBlobstores(srcDFG)) + return destDFG +end diff --git a/src/services/list.jl b/src/services/list.jl index 27fab873..58ff59b1 100644 --- a/src/services/list.jl +++ b/src/services/list.jl @@ -5,33 +5,6 @@ ##------------------------------------------------------------------------------ ## Overwrite in driver for performance ##------------------------------------------------------------------------------ -""" - $(SIGNATURES) -Get a list of labels of the DFGVariables in the graph. -Supports optional arguments to filter the variables returned. - -Notes -- Returns `::Vector{Symbol}` - -Example -```julia -listVariables(dfg) -``` - -See also: [`ls`](@ref) -""" -function listVariables(dfg::AbstractDFG, args...; kwargs...) - return map(getLabel, getVariables(dfg, args...; kwargs...))::Vector{Symbol} -end - -""" - $(SIGNATURES) -Get a list of the labels of the DFGFactors in the DFG. -Optionally specify a label regular expression to retrieves a subset of the factors. -""" -function listFactors(dfg::AbstractDFG, args...; kwargs...) - return map(getLabel, getFactors(dfg, args...; kwargs...))::Vector{Symbol} -end ##------------------------------------------------------------------------------ ## Aliases and Other filtered lists diff --git a/src/services/CustomPrinting.jl b/src/services/print.jl similarity index 89% rename from src/services/CustomPrinting.jl rename to src/services/print.jl index cf675271..87e6553e 100644 --- a/src/services/CustomPrinting.jl +++ b/src/services/print.jl @@ -175,23 +175,3 @@ end ## Overloading show ##============================================================================== # Base.show_default(io, v) -function Base.show(io::IO, ::MIME"text/plain", v::VariableDFG) - return printVariable(io, v; short = true, limit = false) -end - -function Base.show(io::IO, ::MIME"text/plain", f::FactorDFG) - return printFactor(io, f; short = true, limit = false) -end - -function Base.show(io::IO, ::MIME"text/plain", dfg::AbstractDFG) - summary(io, dfg) - println(io) - println(io, " GraphLabel: ", getGraphLabel(dfg)) - println(io, " Description: ", getDescription(dfg)) - println(io, " Nr variables: ", length(ls(dfg))) - println(io, " Nr factors: ", length(lsf(dfg))) - println(io, " Graph Bloblets: ", listGraphBloblets(dfg)) - println(io, " Agents: ", listAgents(dfg)) - println(io, " Blobstores: ", listBlobstores(dfg)) - return -end diff --git a/src/services/state_ops.jl b/src/services/state_ops.jl new file mode 100644 index 00000000..aed659a4 --- /dev/null +++ b/src/services/state_ops.jl @@ -0,0 +1,68 @@ +""" + $(SIGNATURES) +Add variable solver data, errors if it already exists. +""" +function addState! end + +""" + $(SIGNATURES) +Add variable `State`s by calling `addState!`. +NOTE: If an error occurs while adding one of the states, previously added states will not be rolled back. +""" +function addStates! end + +""" + $(SIGNATURES) +Get the variable `State` for a given state label. +""" +function getState end + +""" + $(SIGNATURES) +Get all the variable `State`s for a given variable label. +""" +function getStates end + +""" + $(SIGNATURES) +Merge the variable state to the variable if it exists, otherwise add it. + +Related +mergeStates! +""" +function mergeState! end + +""" + $(SIGNATURES) +Merge variable states to the variable if they exist, otherwise add them. +""" +function mergeStates! end + +""" + $(SIGNATURES) +Delete the variable `State` by label, returns the number of deleted elements. +""" +function deleteState! end + +""" + $(SIGNATURES) +Delete variable `State`s by label, returns the number of deleted elements. +""" +function deleteStates! end + +""" + $(SIGNATURES) +List all the variable state labels. +""" +function listStates end + +""" + $(SIGNATURES) +True if the variable has a state with the given label. +""" +function hasState end + +""" + $(SIGNATURES) +""" +function copytoState! end diff --git a/src/services/tag_ops.jl b/src/services/tag_ops.jl new file mode 100644 index 00000000..1b6c4046 --- /dev/null +++ b/src/services/tag_ops.jl @@ -0,0 +1,122 @@ +# ============================================================================== +# Tag CRUD — Agent +# ============================================================================== + +""" + $(SIGNATURES) +Merge `tags` into an agent's tag set (union). +""" +function mergeAgentTags! end + +""" + $(SIGNATURES) +Remove `tags` from an agent's tag set (setdiff). +""" +function deleteAgentTags! end + +""" + $(SIGNATURES) +List all tags on an agent. +""" +function listAgentTags end + +""" + $(SIGNATURES) +Check whether an agent has all of the given `tags`. +""" +function hasAgentTags end + +# ============================================================================== +# Tag CRUD — Graph +# ============================================================================== + +""" + $(SIGNATURES) +Merge `tags` into the graph-root tag set (union). +""" +function mergeGraphTags! end + +""" + $(SIGNATURES) +Remove `tags` from the graph-root tag set (setdiff). +""" +function deleteGraphTags! end + +""" + $(SIGNATURES) +List all tags on the graph root. +""" +function listGraphTags end + +""" + $(SIGNATURES) +Check whether the graph root has all of the given `tags`. +""" +function hasGraphTags end + +# ============================================================================== +# Tag CRUD — Variable +# ============================================================================== + +""" + $(SIGNATURES) +Merge `tags` into a variable's tag set (union). +""" +function mergeVariableTags! end + +""" + $(SIGNATURES) +Remove `tags` from a variable's tag set (setdiff). +""" +function deleteVariableTags! end + +""" + $(SIGNATURES) +List all tags on a variable. +""" +function listVariableTags end + +""" + $(SIGNATURES) +Check whether a variable has all of the given `tags`. +""" +function hasVariableTags end + +# ============================================================================== +# Tag CRUD — Factor +# ============================================================================== + +""" + $(SIGNATURES) +Merge `tags` into a factor's tag set (union). +""" +function mergeFactorTags! end + +""" + $(SIGNATURES) +Remove `tags` from a factor's tag set (setdiff). +""" +function deleteFactorTags! end + +""" + $(SIGNATURES) +List all tags on a factor. +""" +function listFactorTags end + +""" + $(SIGNATURES) +Check whether a factor has all of the given `tags`. +""" +function hasFactorTags end + +# ============================================================================== +# Generic +# ============================================================================== + +# TODO deprecate in favor of explicit listVariableTags, and listFactorTags. +# Only used in GraphsDFG listNeighborhood whereTags filter. +function listTags(dfg::AbstractDFG, sym::Symbol) + getFnc = isVariable(dfg, sym) ? getVariable : getFactor + return listTags(getFnc(dfg, sym)) +end diff --git a/src/services/variable_ops.jl b/src/services/variable_ops.jl new file mode 100644 index 00000000..39df178e --- /dev/null +++ b/src/services/variable_ops.jl @@ -0,0 +1,322 @@ +""" + $(SIGNATURES) +Add a VariableDFG to a DFG. +Implement `addVariable!(dfg::AbstractDFG, variable::AbstractGraphVariable)` +""" +function addVariable! end + +""" + $(SIGNATURES) +Add a Vector{VariableDFG} to a DFG. +Implement `addVariables!(dfg::AbstractDFG, variables::Vector{<:AbstractGraphVariable})` +""" +function addVariables! end + +""" + $(SIGNATURES) +Get a VariableDFG from a DFG using its label. +Implement `getVariable(dfg::AbstractDFG, label::Symbol)` +""" +function getVariable end + +""" + $(SIGNATURES) +Get the variables in the DFG as a Vector, supporting various filters. + +Keyword arguments +- `whereSolvable`: Optional function to filter on the `solvable` property, eg `>=(1)`. +- `whereLabel`: Optional function to filter on label e.g., `contains(r"x1")`. +- `whereTags`: Optional function to filter on tags, eg. `⊇([:POSE])`. +- `whereType`: Optional function to filter on the variable type. + +Returns +- `Vector{<:AbstractGraphVariable}` matching the filters. + +See also: [`listVariables`](@ref), [`ls`](@ref) +""" +function getVariables end + +""" + $(SIGNATURES) +Merge a variable into the DFG. If a variable with the same label exists, it will be overwritten; +otherwise, the variable will be added to the graph. +Implement `mergeVariable!(dfg::AbstractDFG, variable::AbstractGraphVariable)` +""" +function mergeVariable! end + +""" + $(SIGNATURES) +Merge a vector of variables into the DFG. If a variable with the same label exists, it will be overwritten; otherwise, the variable will be added to the graph. +Implement `mergeVariables!(dfg::AbstractDFG, variables::Vector{<:AbstractGraphVariable})` +""" +function mergeVariables! end + +""" + $(SIGNATURES) +Delete a VariableDFG from the DFG. +Implement `deleteVariable!(dfg::AbstractDFG, label::Symbol)` +""" +function deleteVariable! end + +""" + $(SIGNATURES) +Get a list of labels of the DFGVariables in the graph. +Supports optional arguments to filter the variables returned. + +Notes +- Returns `::Vector{Symbol}` + +Example +```julia +listVariables(dfg) +``` + +See also: [`ls`](@ref) +""" +function listVariables end + +""" + $(SIGNATURES) +True if the variable exists in the graph. +Implement `hasVariable(dfg::AbstractDFG, label::Symbol)` +""" +function hasVariable end + +# ============================================================================== +""" + $(SIGNATURES) +Get a VariableSummary from a DFG. +""" +function getVariableSummary end + +""" + $(SIGNATURES) +Get the variables from a DFG as a Vector{VariableSummary}. +""" +function getVariablesSummary end + +""" + $(SIGNATURES) +Get a VariableSkeleton from a DFG. +""" +function getVariableSkeleton end + +""" + $(SIGNATURES) +Get the variables from a DFG as a Vector{VariableSkeleton}. +""" +function getVariablesSkeleton end + +# ============================================================================= + +function getVariables(dfg::AbstractDFG, labels::Vector{Symbol}) + return map(label -> getVariable(dfg, label), labels) +end + +function deleteVariable!(dfg::AbstractDFG, variable::AbstractGraphVariable) + return deleteVariable!(dfg, variable.label) +end + +function deleteVariables!(dfg::AbstractDFG, labels::Vector{Symbol}) + counts = asyncmap(labels) do l + return deleteVariable!(dfg, l) + end + return sum(counts) +end + +function deleteVariables!(dfg::AbstractDFG; kwargs...) + labels = listVariables(dfg; kwargs...) + return deleteVariables!(dfg, labels) +end + +# ============================================================================= +# TODO SORT OUT BELLOW +# ============================================================================= + +""" + $(SIGNATURES) + +Get the kind of the variable's state, eg. `Pose2`, `Point3`, etc. as an instance of `StateType`. +""" +getStateKind(::VariableDFG{T}) where {T} = T() + +getStateKind(::State{T}) where {T} = T() + +getStateKind(dfg::AbstractDFG, lbl::Symbol) = getStateKind(getVariable(dfg, lbl)) + +#TODO why this convert? rather enforce explicit use of getManifold +function Base.convert( + ::Type{<:AbstractManifold}, + ::Union{<:T, Type{<:T}}, +) where {T <: StateType} + #TODO Deprecate v0.29 + Base.depwarn( + "convert(AbstractManifold, StateType) is deprecated, use getManifold instead", + :convert, + ) + return getManifold(T) +end + +""" + $SIGNATURES +Interface function to return the `<:ManifoldsBase.AbstractManifold` object of `variableType<:StateType`. +""" +getManifold(::T) where {T <: StateType} = getManifold(T) +getManifold(vari::VariableDFG) = getStateKind(vari) |> getManifold +getManifold(state::State) = getStateKind(state) |> getManifold +# covers both <:StateType and <:AbstractObservation +getManifold(dfg::AbstractDFG, lbl::Symbol) = getManifold(dfg[lbl]) + +""" + $SIGNATURES +Interface function to return the `variableType` dimension of an StateType, extend this function for all Types<:StateType. +""" +function getDimension end + +getDimension(::Type{T}) where {T <: StateType} = manifold_dimension(getManifold(T)) +getDimension(::T) where {T <: StateType} = manifold_dimension(getManifold(T)) +getDimension(M::ManifoldsBase.AbstractManifold) = manifold_dimension(M) +getDimension(p::Distributions.Distribution) = length(p) +getDimension(var::VariableDFG) = getDimension(getStateKind(var)) + +""" + $SIGNATURES +Interface function to return the manifold point type of an StateType, extend this function for all Types<:StateType. +""" +function getPointType end +getPointType(::T) where {T <: StateType} = getPointType(T) + +""" + $SIGNATURES +Interface function to return the user provided identity point for this StateType manifold, extend this function for all Types<:StateType. + +Notes +- Used in transition period for Serialization. This function will likely be changed or deprecated entirely. +""" +function getPointIdentity end +getPointIdentity(::T) where {T <: StateType} = getPointIdentity(T) + +##------------------------------------------------------------------------------ +## solvedCount +##------------------------------------------------------------------------------ + +""" + $SIGNATURES + +Get the number of times a variable has been inferred -- i.e. `solvedCount`. + +Related + +isSolved, setSolvedCount! +""" +getSolvedCount(v::State) = v.solves +function getSolvedCount(v::VariableDFG, solveKey::Symbol = :default) + return getState(v, solveKey) |> getSolvedCount +end +function getSolvedCount(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default) + return getSolvedCount(getVariable(dfg, sym), solveKey) +end + +""" + $SIGNATURES + +Update/set the `solveCount` value. + +Related + +getSolved, isSolved +""" +setSolvedCount!(v::State, val::Int) = v.solves = val +function setSolvedCount!(v::VariableDFG, val::Int, solveKey::Symbol = :default) + return setSolvedCount!(getState(v, solveKey), val) +end +function setSolvedCount!( + dfg::AbstractDFG, + sym::Symbol, + val::Int, + solveKey::Symbol = :default, +) + return setSolvedCount!(getVariable(dfg, sym), val, solveKey) +end + +""" + $SIGNATURES + +Boolean on whether the variable has been solved. + +Related + +getSolved, setSolved! +""" +isSolved(v::State) = 0 < v.solves +function isSolved(v::VariableDFG, solveKey::Symbol = :default) + return getState(v, solveKey) |> isSolved +end +function isSolved(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default) + return isSolved(getVariable(dfg, sym), solveKey) +end + +##------------------------------------------------------------------------------ +## initialized +##------------------------------------------------------------------------------ +""" + $SIGNATURES + +Returns state of variable data `.initialized` flag. + +Notes: +- used by both factor graph variable and Bayes tree clique logic. +""" +function isInitialized(var::VariableDFG, key::Symbol = :default) + return getState(var, key).initialized +end + +function isInitialized(dfg::AbstractDFG, label::Symbol, key::Symbol = :default) + return isInitialized(getVariable(dfg, label), key)::Bool +end + +""" + $SIGNATURES + +Return `::Bool` on whether this variable has been marginalized. + +Notes: +- State default `solveKey=:default` +""" +function isMarginalized(vert::VariableDFG, solveKey::Symbol = :default) + return getState(vert, solveKey).marginalized +end +function isMarginalized(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default) + return isMarginalized(DFG.getVariable(dfg, sym), solveKey) +end + +""" + $SIGNATURES + +Mark a variable as marginalized `true` or `false`. +""" +function setMarginalized!(vnd::State, val::Bool) + return vnd.marginalized = val +end +function setMarginalized!(vari::VariableDFG, val::Bool, solveKey::Symbol = :default) + return setMarginalized!(getState(vari, solveKey), val) +end +function setMarginalized!( + dfg::AbstractDFG, + sym::Symbol, + val::Bool, + solveKey::Symbol = :default, +) + return setMarginalized!(getVariable(dfg, sym), val, solveKey) +end + +##============================================================================== +## Variables +##============================================================================== +# +# | | label | tags | timestamp | variableTypeName | solvable | solverData | smallData | dataEntries | +# |---------------------|:-----:|:----:|:---------:|:----------------:|:--------:|:----------:|:---------:|:-----------:| +# | VariableSkeleton | X | X | | | | | | | +# | VariableSummary | X | X | X | X | | | | X | +# | VariableDFG | X | X | x | | X | X | X | X | +# diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index 34502153..000238e0 100644 --- a/test/interfaceTests.jl +++ b/test/interfaceTests.jl @@ -12,7 +12,7 @@ if false include("testBlocks.jl") testDFGAPI = GraphsDFG - + DFG.@usingDFG true # Enable debug logging using Logging logger = SimpleLogger(stdout, Logging.Debug) diff --git a/test/testBlocks.jl b/test/testBlocks.jl index c4a7bf18..bbca5cd1 100644 --- a/test/testBlocks.jl +++ b/test/testBlocks.jl @@ -81,16 +81,7 @@ function DFGStructureAndAccessors( @test getGraphLabel(fg) == :workspace # Test the validation of the robot, session, and user IDs. - notAllowedList = [ - Symbol("!notValid"), - Symbol("1notValid"), - :_notValid, - :AGENT, - :VARIABLE, - :FACTOR, - :BLOB_ENTRY, - :FACTORGRAPH, - ] + notAllowedList = [Symbol("!notValid"), Symbol("1notValid"), :_notValid] for s in notAllowedList @test_throws ArgumentError T(solverParams = solparams, graphLabel = s) @@ -132,7 +123,7 @@ function DFGStructureAndAccessors( # _getDuplicatedEmptyDFG # copyEmptyDFG(::Type{T}, sourceDFG) where T <: AbstractDFG = T(getDFGInfo(sourceDFG)) # copyEmptyDFG(sourceDFG::T) where T <: AbstractDFG = copyEmptyDFG(T, sourceDFG) - + display(fg) return fg end @@ -549,24 +540,24 @@ function tagsTestBlock!(fg, v1, v1_tags) # v1Tags = deepcopy(DFG.refTags(v1)) @test issetequal(v1Tags, v1_tags) - @test issetequal(listTags(fg, :a), v1Tags) - @test mergeTags!(fg, :a, [:TAG]) == 1 - @test issetequal(listTags(fg, :a), v1Tags ∪ [:TAG]) - @test deleteTags!(fg, :a, [:TAG]) == 1 - @test issetequal(listTags(fg, :a), v1Tags) - @test emptyTags!(fg, :a) == Set{Symbol}() - - v2Tags = listTags(fg, :b) - @test hasTags(fg, :b, v2Tags) - @test hasTags(fg, :b, [:LANDMARK]) - @test !hasTags(fg, :b, [:LANDMARK, :TAG]) + @test issetequal(listVariableTags(fg, :a), v1Tags) + @test mergeVariableTags!(fg, :a, [:TAG]) == 1 + @test issetequal(listVariableTags(fg, :a), v1Tags ∪ [:TAG]) + @test deleteVariableTags!(fg, :a, [:TAG]) == 1 + @test issetequal(listVariableTags(fg, :a), v1Tags) + @test emptyTags!(getVariable(fg, :a)) == Set{Symbol}() + + v2Tags = listVariableTags(fg, :b) + @test hasVariableTags(fg, :b, v2Tags) + @test hasVariableTags(fg, :b, [:LANDMARK]) + @test !hasVariableTags(fg, :b, [:LANDMARK, :TAG]) @test listNeighbors(fg, :abf1; whereTags = ⊇([:LANDMARK])) == [:b] @test isempty(listNeighbors(fg, :abf1; whereTags = ⊇([:LANDMARK, :TAG]))) # Test specific type tag accessors - @test issetequal(listVariableTags(fg, :a), listTags(fg, :a)) - @test issetequal(listFactorTags(fg, :abf1), listTags(fg, :abf1)) + @test issetequal(listVariableTags(fg, :a), listTags(getVariable(fg, :a))) + @test issetequal(listFactorTags(fg, :abf1), listTags(getFactor(fg, :abf1))) # Test mergeVariableTags! and mergeFactorTags! @test mergeVariableTags!(fg, :a, [:NEW_VAR_TAG]) == 1 @@ -1473,6 +1464,10 @@ function GettingNeighbors(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDFG @test listNeighbors(dfg, getFactor(dfg, :x1x2f1)) == ls(dfg, getFactor(dfg, :x1x2f1)) @test listNeighbors(dfg, :x1x2f1) == ls(dfg, :x1x2f1) + varneighls, facneighls = listNeighborhood(dfg, [:x1, :x3], 2) + @test issetequal(varneighls, [:x1, :x2, :x3, :x4]) + @test issetequal(facneighls, [:x1x2f1, :x2x3f1, :x3x4f1]) + # Solvable #TODO if not a GraphsDFG with and summary or skeleton if VARTYPE == VariableDFG @@ -1973,8 +1968,8 @@ function PathFindingTests(testDFGAPI) # --- With whereTags --- # By default all variables have :VARIABLE tag. Tag some for testing. - mergeTags!(dfg, :x3, Set([:LANDMARK])) - mergeTags!(dfg, :x4, Set([:LANDMARK])) + mergeVariableTags!(dfg, :x3, Set([:LANDMARK])) + mergeVariableTags!(dfg, :x4, Set([:LANDMARK])) landmark_vars = listVariables(dfg; whereTags = ⊇([:LANDMARK])) @test :x3 ∈ landmark_vars @test :x4 ∈ landmark_vars From b64f292c8da9c230e31f64ec6d68f00ee7a12c61 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 8 Apr 2026 10:52:48 +0200 Subject: [PATCH 2/3] format --- src/services/discovery.jl | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/services/discovery.jl b/src/services/discovery.jl index 3706f4b5..6891521b 100644 --- a/src/services/discovery.jl +++ b/src/services/discovery.jl @@ -78,7 +78,14 @@ function findPaths( if k == 1 return findPaths(GraphsDFGs.a_star, active_dfg, from, to; kwargs...) else - return findPaths(GraphsDFGs.yen_k_shortest_paths, active_dfg, from, to, k; kwargs...) + return findPaths( + GraphsDFGs.yen_k_shortest_paths, + active_dfg, + from, + to, + k; + kwargs..., + ) end end From 7e15b6fc3103450157ca95cf2810c39b61f287be Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 8 Apr 2026 12:22:08 +0200 Subject: [PATCH 3/3] fix docs for refactor --- docs/src/blob_ref.md | 13 ++++--- docs/src/func_ref.md | 46 ++++++++++++++++++----- docs/src/services_ref.md | 48 +++++++++++++++--------- src/Serialization/PackedSerialization.jl | 2 +- 4 files changed, 76 insertions(+), 33 deletions(-) diff --git a/docs/src/blob_ref.md b/docs/src/blob_ref.md index 5175cdcc..0a755540 100644 --- a/docs/src/blob_ref.md +++ b/docs/src/blob_ref.md @@ -12,8 +12,8 @@ Depth = 3 Modules = [DistributedFactorGraphs] Pages = [ - "DataBlobs/entities/BlobEntry.jl", - "DataBlobs/entities/BlobStores.jl", + "entities/Blobentry.jl", + "entities/Blobstore.jl", ] ``` @@ -23,10 +23,11 @@ Pages = [ Modules = [DistributedFactorGraphs] Pages = [ - "DataBlobs/services/BlobEntry.jl", - "DataBlobs/services/BlobStores.jl", - "DataBlobs/services/BlobPacking.jl", - "DataBlobs/services/BlobWrappers.jl", + "services/blobentry_ops.jl", + "services/blobstore_ops.jl", + "Serialization/BlobPacking.jl", + "services/blob_save_load.jl", + "Blobstores/Blobstores.jl", ] ``` diff --git a/docs/src/func_ref.md b/docs/src/func_ref.md index 632c7dc9..7eb6334e 100644 --- a/docs/src/func_ref.md +++ b/docs/src/func_ref.md @@ -23,38 +23,66 @@ Modules = [DistributedFactorGraphs] Pages = ["entities/AbstractDFG.jl"] ``` -### Summary DFG +### DFG Variable Nodes ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["entities/AbstractDFGSummary.jl"] +Pages = ["entities/Variable.jl"] ``` -### DFG Variable Nodes +### DFG Factor Nodes ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["entities/DFGVariable.jl"] +Pages = ["entities/Factor.jl"] ``` -### DFG Factor Nodes +### State + +```@autodocs +Modules = [DistributedFactorGraphs] +Pages = ["entities/State.jl"] +``` + +### Agent and Graph + +```@autodocs +Modules = [DistributedFactorGraphs] +Pages = ["entities/Agent_and_Graph.jl"] +``` + +### Tags + +```@autodocs +Modules = [DistributedFactorGraphs] +Pages = ["entities/Tags.jl"] +``` + +### Timestamp + +```@autodocs +Modules = [DistributedFactorGraphs] +Pages = ["entities/Timestamp.jl"] +``` + +### Equality ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["entities/DFGFactor.jl"] +Pages = ["entities/equality.jl"] ``` ### Error Types ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["errors.jl"] +Pages = ["entities/Error.jl"] ``` ## DFG Plots [GraphMakie.jl] ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["weakdeps_prototypes.jl"] +Pages = ["extension_stubs.jl"] ``` ## Drivers @@ -69,5 +97,5 @@ Modules = [GraphsDFGs] ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["FileDFG.jl"] +Pages = ["FileDFG/FileDFG.jl", "FileDFG/services/FileDFG.jl"] ``` \ No newline at end of file diff --git a/docs/src/services_ref.md b/docs/src/services_ref.md index 6560e453..b8177a0d 100644 --- a/docs/src/services_ref.md +++ b/docs/src/services_ref.md @@ -18,20 +18,15 @@ Pages = ["services/AbstractDFG.jl"] Common Accessors to both variable and factor nodes -```@autodocs -Modules = [DistributedFactorGraphs] -Pages = ["services/CommonAccessors.jl"] -``` - ## Common ```@autodocs Modules = [DistributedFactorGraphs] Pages = [ "services/list.jl", - "services/find.jl", - "services/Tags.jl", + "services/discovery.jl", + "services/tag_ops.jl", "entities/Bloblet.jl", - "services/Bloblet.jl", + "services/bloblet_ops.jl", ] ``` @@ -39,40 +34,59 @@ Pages = [ ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["services/DFGVariable.jl"] +Pages = ["services/variable_ops.jl"] ``` ## DFG Factor Accessors CRUD and SET opperations ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["services/DFGFactor.jl"] +Pages = ["services/factor_ops.jl"] ``` -## Printing +## State Operations ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["services/CustomPrinting.jl"] +Pages = ["services/state_ops.jl"] ``` -## Compare Utilities +## Agent Operations + +```@autodocs +Modules = [DistributedFactorGraphs] +Pages = ["services/agent_ops.jl"] +``` + +## Graph Operations + +```@autodocs +Modules = [DistributedFactorGraphs] +Pages = ["services/graph_ops.jl"] +``` + +## Printing ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["services/CompareUtils.jl"] +Pages = ["services/print.jl"] ``` -## Common Functions +## Compare Utilities ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["src/Common.jl"] +Pages = ["services/compare.jl"] ``` ## Serialization ```@autodocs Modules = [DistributedFactorGraphs] -Pages = ["services/Serialization.jl"] +Pages = [ + "Serialization/DFGStructStyles.jl", + "Serialization/DistributionSerialization.jl", + "Serialization/PackedSerialization.jl", + "Serialization/StateSerialization.jl", +] ``` diff --git a/src/Serialization/PackedSerialization.jl b/src/Serialization/PackedSerialization.jl index 3abdee34..f51478c8 100644 --- a/src/Serialization/PackedSerialization.jl +++ b/src/Serialization/PackedSerialization.jl @@ -114,7 +114,7 @@ This is equivalent to writing: end ``` -See also: [`Packed`](@ref), [`pack`](@ref), [`unpack`](@ref), [`resolvePackedType`](@ref) +See also: `Packed`, `pack`, `unpack`, `resolvePackedType` """ macro packed() return esc(:(lower = DFG.Packed, choosetype = DFG.resolvePackedType))