Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
528 changes: 177 additions & 351 deletions src/DistributedFactorGraphs.jl

Large diffs are not rendered by default.

8 changes: 8 additions & 0 deletions src/GraphsDFG/services/agent_ops.jl
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,10 @@ function DFG.mergeAgents!(dfg::GraphsDFG, agents::Vector{Agent})
return count
end

function DFG.addAgents!(dfg::GraphsDFG, agents::Vector{Agent})
return map(agent -> addAgent!(dfg, agent), agents)
end

"""
$(SIGNATURES)
"""
Expand All @@ -62,6 +66,10 @@ function DFG.deleteAgent!(dfg::GraphsDFG, label::Symbol)
return 1
end

function DFG.deleteAgents!(dfg::GraphsDFG, labels::Vector{Symbol})
return sum(label -> deleteAgent!(dfg, label), labels; init = 0)
end

"""
$(SIGNATURES)
"""
Expand Down
3 changes: 2 additions & 1 deletion src/GraphsDFG/services/blobprovider_ops.jl
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,8 @@ function DFG.addBlobprovider!(dfg::GraphsDFG, provider::AbstractBlobprovider)
)
throw(LabelExistsError("Blobprovider", label))
end
return push!(refBlobproviders(dfg), label => provider)
push!(refBlobproviders(dfg), label => provider)
return provider
end

# TODO edge api is a work in progress and only internal
Expand Down
14 changes: 6 additions & 8 deletions src/GraphsDFG/services/state_ops.jl
Original file line number Diff line number Diff line change
Expand Up @@ -7,22 +7,20 @@ function DFG.addState!(dfg::GraphsDFG, variableLabel::Symbol, state::State)
end

function DFG.addStates!(dfg::GraphsDFG, variableLabel::Symbol, states::Vector{<:State})
cnt = asyncmap(states) do state
addState!(dfg, variableLabel, state)
return 1
s = asyncmap(states) do state
return addState!(dfg, variableLabel, state)
end
return sum(cnt)
return s
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
s = asyncmap(varLabel_state_pairs) do (varLabel, state)
return addState!(dfg, varLabel, state)
end
return sum(cnt)
return s
end

# =============================================================================
Expand Down
12 changes: 4 additions & 8 deletions src/GraphsDFG/services/tag_ops.jl
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,11 @@
# Agent Tags
# ==============================================================================
function DFG.mergeAgentTags!(dfg::GraphsDFG, agentlabel::Symbol, tags)
mergeTags!(getAgent(dfg, agentlabel), tags)
return length(tags)
return mergeTags!(getAgent(dfg, agentlabel), tags)
end

function DFG.deleteAgentTags!(dfg::GraphsDFG, agentlabel::Symbol, tags)
deleteTags!(getAgent(dfg, agentlabel), tags)
return length(tags)
return deleteTags!(getAgent(dfg, agentlabel), tags)
end

function DFG.listAgentTags(dfg::GraphsDFG, agentlabel::Symbol)
Expand All @@ -23,13 +21,11 @@ end
# Graph Tags
# ==============================================================================
function DFG.mergeGraphTags!(dfg::GraphsDFG, tags)
mergeTags!(dfg.graph, tags)
return length(tags)
return mergeTags!(dfg.graph, tags)
end

function DFG.deleteGraphTags!(dfg::GraphsDFG, tags)
deleteTags!(dfg.graph, tags)
return length(tags)
return deleteTags!(dfg.graph, tags)
end

function DFG.listGraphTags(dfg::GraphsDFG)
Expand Down
10 changes: 3 additions & 7 deletions src/Serialization/StateSerialization.jl
Original file line number Diff line number Diff line change
Expand Up @@ -131,14 +131,10 @@ function unpackOldState(d)
label = Symbol(d.solveKey)
!isempty(d.covar) && error("covar field is not supported")
if label == :parametric
belief = BeliefRepresentation(
GaussianDensityKind(),
statekind;
means = vals,
covariances = [BW],
)
belief =
StoredBelief(GaussianDensityKind(), statekind; means = vals, covariances = [BW])
else
belief = BeliefRepresentation(
belief = StoredBelief(
NonparametricDensityKind(),
statekind;
points = vals,
Expand Down
49 changes: 27 additions & 22 deletions src/entities/State.jl
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ abstract type AbstractStateType{N} end
const StateType = AbstractStateType

##==============================================================================
## BeliefRepresentation
## StoredBelief
##==============================================================================
abstract type AbstractDensityKind end

Expand All @@ -24,9 +24,9 @@ function StructUtils.lower(::StructUtils.StructStyle, p::AbstractDensityKind)
end
@choosetype AbstractDensityKind resolvePackedType

# TODO naming? Density, DensityRepresentation, BeliefRepresentation, BeliefState, etc?
# TODO naming? Density, DensityRepresentation, StoredBelief, BeliefState, etc?
# TODO flatten in State? likeley not for easier serialization of points.
@kwdef struct BeliefRepresentation{T <: StateType, P}
@kwdef struct StoredBelief{T <: StateType, P}
statekind::T = T()# NOTE duplication for serialization, TODO maybe only in State and therefore belief cannot deserialize separately.
"""Discriminator for which representation is active."""
densitykind::AbstractDensityKind = NonparametricDensityKind()
Expand All @@ -50,23 +50,26 @@ end
# JSON.parse(JSON.json(zeros(0, 0)), Matrix{Float64}) errors, so trying with nothing union
end

JSON.omit_empty(::Type{<:BeliefRepresentation}) = true
#FIXME remove old name before v0.29
const BeliefRepresentation = StoredBelief

function BeliefRepresentation(T::AbstractStateType)
return BeliefRepresentation{typeof(T), getPointType(T)}(; statekind = T)
JSON.omit_empty(::Type{<:StoredBelief}) = true

function StoredBelief(T::AbstractStateType)
return StoredBelief{typeof(T), getPointType(T)}(; statekind = T)
end

function BeliefRepresentation(::NonparametricDensityKind, T::AbstractStateType; kwargs...)
return BeliefRepresentation{typeof(T), getPointType(T)}(;
function StoredBelief(::NonparametricDensityKind, T::AbstractStateType; kwargs...)
return StoredBelief{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)}(;
function StoredBelief(::GaussianDensityKind, T::AbstractStateType; kwargs...)
return StoredBelief{typeof(T), getPointType(T)}(;
statekind = T,
densitykind = GaussianDensityKind(),
bandwidth = nothing,
Expand All @@ -76,7 +79,7 @@ end

function StructUtils.fielddefaults(
::StructUtils.StructStyle,
::Type{BeliefRepresentation{T, P}},
::Type{StoredBelief{T, P}},
) where {T, P}
return (
statekind = T(),
Expand All @@ -89,12 +92,12 @@ function StructUtils.fielddefaults(
)
end

function resolveBeliefRepresentationType(lazyobj)
function resolveStoredBeliefType(lazyobj)
statekind = liftStateKind(lazyobj.statekind[])
return BeliefRepresentation{typeof(statekind), getPointType(statekind)}
return StoredBelief{typeof(statekind), getPointType(statekind)}
end

@choosetype BeliefRepresentation resolveBeliefRepresentationType
@choosetype StoredBelief resolveStoredBeliefType

##==============================================================================
## State
Expand All @@ -116,10 +119,9 @@ $(TYPEDFIELDS)
"""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.
Generic stored belief for this state.
"""
belief::BeliefRepresentation{T, P} = BeliefRepresentation{T, P}()#; statekind = T())
belief::StoredBelief{T, P} = StoredBelief{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."""
Expand All @@ -131,10 +133,13 @@ $(TYPEDFIELDS)
"""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,)
#TODO belief container that can be used for active solver beliefs such as a HomotopyDensity
# The type is defined by a trait saved in the StoredBelief and
# verbs such as `hydrate!(state)` `persist!(state)` can be used at data at checkpoints.
# Forcing an explicit `persist!` acts as a state checkpoint,
#ensuring the graph only ever stores fully committed solver results rather than half-computed intermediate math.
# abstract type AbstractActiveBelief end
# active_belief::Base.RefValue{<:AbstractActiveBelief} = Ref{AbstractActiveBelief}() & (ignore = true,)
end

# OLD deprecated fields, removed in v0.29, kept here for reference during transition
Expand Down Expand Up @@ -173,7 +178,7 @@ function StructUtils.fielddefaults(
::Type{State{T, P}},
) where {T, P}
return (
belief = BeliefRepresentation{T, P}(; statekind = T()),
belief = StoredBelief{T, P}(; statekind = T()),
separator = Symbol[],
initialized = false,
observability = Float64[],
Expand Down
6 changes: 4 additions & 2 deletions src/entities/Tags.jl
Original file line number Diff line number Diff line change
Expand Up @@ -14,17 +14,19 @@ listTags(node) = collect(refTags(node))
Merge add tags to a variable or factor (union)
"""
function mergeTags!(node, tags)
n_before = length(refTags(node))
union!(refTags(node), tags)
return length(tags)
return length(refTags(node)) - n_before
end
"""
$SIGNATURES

Remove the tags from the node (setdiff)
"""
function deleteTags!(node, tags)
n_before = length(refTags(node))
setdiff!(refTags(node), tags)
return length(tags)
return n_before - length(refTags(node))
end

"""
Expand Down
2 changes: 1 addition & 1 deletion src/entities/equality.jl
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ implement compare if needed.
const GeneratedCompareUnion = Union{
Agent,
Graphroot,
BeliefRepresentation,
StoredBelief,
State,
Blobentry,
Bloblet,
Expand Down
8 changes: 4 additions & 4 deletions test/testBlocks.jl
Original file line number Diff line number Diff line change
Expand Up @@ -300,7 +300,7 @@ function DFGVariableSCA()
#TODO here for now, don't reccomend usage.
testTags = [:tag1, :tag2]
@test DFG.mergeTags!(v3, testTags) == 2
@test DFG.mergeTags!(v3, Set(testTags)) == 2
@test DFG.mergeTags!(v3, Set(testTags)) == 0

#NOTE a variable's timestamp is considered similar to its label. setTimestamp! (not implemented) would create a new variable and call mergeVariable!
# @test getTimestamp(v1ts) == testTimestamp
Expand Down Expand Up @@ -373,7 +373,7 @@ function DFGFactorSCA()

testTags = [:tag1, :tag2]
@test DFG.mergeTags!(f1, testTags) == 2
@test DFG.mergeTags!(f1, Set(testTags)) == 2
@test DFG.mergeTags!(f1, Set(testTags)) == 0

#follow with mergeFactor!(fg, v1ts)

Expand Down Expand Up @@ -786,13 +786,13 @@ function statesExtendedTestBlock!(fg)
# addStates! bulk
s1 = State{TestVariableType1}(; label = :bulk_s1)
s2 = State{TestVariableType1}(; label = :bulk_s2)
@test addStates!(fg, :a, [s1, s2]) == 2
addStates!(fg, :a, [s1, s2])
@test hasState(fg, :a, :bulk_s1)
@test hasState(fg, :a, :bulk_s2)

# addStates! with pair syntax
s3 = State{TestVariableType2}(; label = :bulk_s3)
@test addStates!(fg, [:b => s3]) == 1
addStates!(fg, [:b => s3])
@test hasState(fg, :b, :bulk_s3)

# deleteStates! bulk
Expand Down
49 changes: 44 additions & 5 deletions test/testSerializingVariables.jl
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,13 @@ DFG.@defStateTypeN Pose{N} SpecialEuclideanGroup(N; variant = :right) ArrayParti
)
Pose2 = Pose{2}

# Complex scalar point type (CircleGroup identity is ComplexF64)
DFG.@defStateType CircleState CircleGroup() (1.0 + 0.0im)

# Complex 0-dimensional array point type (circle manifold)
const MB = DFG.ManifoldsBase
DFG.@defStateType CCircle MB.DefaultManifold(1; field = MB.ℂ) fill(1.0 + 0.0im)

## Build a test variable with states, bloblets, and blobentries
function make_test_variable()
v = DFG.VariableDFG(:x1, Pose{3}())
Expand All @@ -39,27 +46,27 @@ function make_test_variable()
end

@testset "Serializing Variables" begin
@testset "BeliefRepresentation round-trip" begin
bel = DFG.BeliefRepresentation(Pose{3}())
@testset "StoredBelief round-trip" begin
bel = DFG.StoredBelief(Pose{3}())
push!(bel.points, DFG.getPointIdentity(Pose{3}()))
G = DFG.getManifold(Pose{3}())
push!(bel.points, rand(G, ArrayPartition))

jstr = JSON.json(bel; pretty = true, style = DFG.DFGJSONStyle())
parsed = JSON.parse(jstr, DFG.BeliefRepresentation; style = DFG.DFGJSONStyle())
parsed = JSON.parse(jstr, DFG.StoredBelief; style = DFG.DFGJSONStyle())
@test bel == parsed
end

@testset "GaussianDensityKind round-trip" begin
dim = DFG.getDimension(Pose{3}())
bel = DFG.BeliefRepresentation(
bel = DFG.StoredBelief(
DFG.GaussianDensityKind(),
Pose{3}();
means = [DFG.getPointIdentity(Pose{3}())],
covariances = [diagm(ones(dim))],
)
jstr = JSON.json(bel; pretty = true, style = DFG.DFGJSONStyle())
parsed = JSON.parse(jstr, DFG.BeliefRepresentation; style = DFG.DFGJSONStyle())
parsed = JSON.parse(jstr, DFG.StoredBelief; style = DFG.DFGJSONStyle())
@test bel == parsed
end

Expand Down Expand Up @@ -105,6 +112,38 @@ end
@test vsk.label == :x1
@test vsk.tags == v.tags
end

@testset "Complex (CircleGroup) end-to-end" begin
# CircleState uses ComplexF64 as point type
@test DFG.getPointType(CircleState) == ComplexF64
@test DFG.getPointIdentity(CircleState) == 1.0 + 0.0im

v = DFG.VariableDFG(:c1, CircleState())
state = addState!(v, State(:default, CircleState()))
push!(state.belief.points, 1.0 + 0.0im)
push!(state.belief.points, 0.5 + 0.866im)
DFG.addBloblet!(v, DFG.Bloblet(:meta, "circle"))

jstr = JSON.json(v; pretty = true, style = DFG.DFGJSONStyle())
parsed = JSON.parse(jstr, DFG.VariableDFG; style = DFG.DFGJSONStyle())
@test v == parsed
end

@testset "AbstractArray{<:Complex, 0} (CCircle) end-to-end" begin
# CCircle uses Array{ComplexF64, 0} as point type
@test DFG.getPointType(CCircle) == Array{ComplexF64, 0}
@test DFG.getPointIdentity(CCircle) == fill(1.0 + 0.0im)

v = DFG.VariableDFG(:z1, CCircle())
state = addState!(v, State(:default, CCircle()))
push!(state.belief.points, fill(1.0 + 0.0im))
push!(state.belief.points, fill(0.5 + 0.5im))
DFG.addBloblet!(v, DFG.Bloblet(:meta, "ccircle"))

jstr = JSON.json(v; pretty = true, style = DFG.DFGJSONStyle())
parsed = JSON.parse(jstr, DFG.VariableDFG; style = DFG.DFGJSONStyle())
@test v == parsed
end
end

# TODO deprecated v0.29, remove this test and unpackOldState
Expand Down
Loading