Skip to content

Commit 20d0f7d

Browse files
dehanndehann
authored
Merge pull request #270 from JuliaRobotics/master
v0.5.4-rc1
2 parents e16224f + 6ebb75e commit 20d0f7d

File tree

12 files changed

+193
-91
lines changed

12 files changed

+193
-91
lines changed

‎Project.toml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
name = "DistributedFactorGraphs"
22
uuid = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
3-
version = "0.5.3"
3+
version = "0.5.4"
44

55
[deps]
66
Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

‎src/DFGPlots/DFGPlots.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,7 @@ More information at [GraphPlot.jl](https://github.com/JuliaGraphs/GraphPlot.jl)
8888
function dfgplot(dfg::AbstractDFG, p::DFGPlotProps = DFGPlotProps())
8989
# TODO implement convert functions
9090
@warn "TODO Implement convert"
91-
ldfg = LightDFG{AbstractParams}()
91+
ldfg = LightDFG{NoSolverParams}()
9292
DistributedFactorGraphs._copyIntoGraph!(dfg, ldfg, union(getVariableIds(dfg), getFactorIds(dfg)), true, copyGraphMetadata=false)
9393

9494
dfgplot(ldfg, p)

‎src/DistributedFactorGraphs.jl

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,9 +23,11 @@ export InferenceType, PackedInferenceType, FunctorInferenceType, InferenceVariab
2323
export FunctorSingleton, FunctorPairwise, FunctorPairwiseMinimize
2424
export getMaxPPE, getMeanPPE, getSuggestedPPE, getVariablePPE, getPPE, getVariablePPEs, getPPEs #, getEstimates
2525
export timestamp # DEPRECATED
26+
export getSolvedCount, isSolved, setSolvedCount!
2627
export label, getTimestamp, setTimestamp!, tags, setTags!, estimates, estimate, data, softtype, solverData, getData, solverDataDict, setSolverData, setSolverData!, internalId, smallData, setSmallData!, bigData
2728
export DFGVariableSummary, DFGFactorSummary, AbstractDFGSummary
2829
export addBigDataEntry!, getBigDataEntry, updateBigDataEntry!, deleteBigDataEntry!, getBigDataEntries, getBigDataKeys
30+
export getNeighborhood, getSubgraph, getSubgraphAroundNode
2931

3032
#Skeleton types
3133
export SkeletonDFGVariable, SkeletonDFGFactor

‎src/GraphsDFG/services/GraphsDFG.jl

Lines changed: 35 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -327,40 +327,41 @@ function getNeighbors(dfg::GraphsDFG, label::Symbol; solvable::Int=0)::Vector{Sy
327327
return map(n -> n.dfgNode.label, neighbors)
328328
end
329329

330-
"""
331-
$(SIGNATURES)
332-
Retrieve a deep subgraph copy around a given variable or factor.
333-
Optionally provide a distance to specify the number of edges should be followed.
334-
Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
335-
Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
336-
"""
337-
function getSubgraphAroundNode(dfg::GraphsDFG{P}, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::GraphsDFG=GraphsDFG{P}(); solvable::Int=0)::GraphsDFG where {P <: AbstractParams, T <: DFGNode}
338-
if !haskey(dfg.labelDict, node.label)
339-
error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
340-
end
341-
342-
# Build a list of all unique neighbors inside 'distance'
343-
neighborList = Dict{Symbol, Any}()
344-
push!(neighborList, node.label => dfg.g.vertices[dfg.labelDict[node.label]])
345-
curList = Dict{Symbol, Any}(node.label => dfg.g.vertices[dfg.labelDict[node.label]])
346-
for dist in 1:distance
347-
newNeighbors = Dict{Symbol, Any}()
348-
for (key, node) in curList
349-
neighbors = in_neighbors(node, dfg.g) #Don't use out_neighbors! It enforces directiveness even if we don't want it
350-
for neighbor in neighbors
351-
if !haskey(neighborList, neighbor.dfgNode.label) && (isSolvable(neighbor.dfgNode) >= solvable)
352-
push!(neighborList, neighbor.dfgNode.label => neighbor)
353-
push!(newNeighbors, neighbor.dfgNode.label => neighbor)
354-
end
355-
end
356-
end
357-
curList = newNeighbors
358-
end
359-
360-
# Copy the section of graph we want
361-
_copyIntoGraph!(dfg, addToDFG, collect(keys(neighborList)), includeOrphanFactors)
362-
return addToDFG
363-
end
330+
#NOTE Replaced by abstract function in services/AbstractDFG.jl
331+
# """
332+
# $(SIGNATURES)
333+
# Retrieve a deep subgraph copy around a given variable or factor.
334+
# Optionally provide a distance to specify the number of edges should be followed.
335+
# Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
336+
# Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
337+
# """
338+
# function getSubgraphAroundNode(dfg::GraphsDFG{P}, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::GraphsDFG=GraphsDFG{P}(); solvable::Int=0)::GraphsDFG where {P <: AbstractParams, T <: DFGNode}
339+
# if !haskey(dfg.labelDict, node.label)
340+
# error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
341+
# end
342+
#
343+
# # Build a list of all unique neighbors inside 'distance'
344+
# neighborList = Dict{Symbol, Any}()
345+
# push!(neighborList, node.label => dfg.g.vertices[dfg.labelDict[node.label]])
346+
# curList = Dict{Symbol, Any}(node.label => dfg.g.vertices[dfg.labelDict[node.label]])
347+
# for dist in 1:distance
348+
# newNeighbors = Dict{Symbol, Any}()
349+
# for (key, node) in curList
350+
# neighbors = in_neighbors(node, dfg.g) #Don't use out_neighbors! It enforces directiveness even if we don't want it
351+
# for neighbor in neighbors
352+
# if !haskey(neighborList, neighbor.dfgNode.label) && (isSolvable(neighbor.dfgNode) >= solvable)
353+
# push!(neighborList, neighbor.dfgNode.label => neighbor)
354+
# push!(newNeighbors, neighbor.dfgNode.label => neighbor)
355+
# end
356+
# end
357+
# end
358+
# curList = newNeighbors
359+
# end
360+
#
361+
# # Copy the section of graph we want
362+
# _copyIntoGraph!(dfg, addToDFG, collect(keys(neighborList)), includeOrphanFactors)
363+
# return addToDFG
364+
# end
364365

365366
"""
366367
$(SIGNATURES)

‎src/LightDFG/entities/LightDFG.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ function LightDFG{T,V,F}(g::FactorGraph{Int,V,F}=FactorGraph{Int,V,F}();
3636
userData::Dict{Symbol, String} = Dict{Symbol, String}(),
3737
robotData::Dict{Symbol, String} = Dict{Symbol, String}(),
3838
sessionData::Dict{Symbol, String} = Dict{Symbol, String}(),
39-
params::T=NoSolverParams()) where {T <: AbstractParams, V <:AbstractDFGVariable, F<:AbstractDFGFactor}
39+
params::T=T()) where {T <: AbstractParams, V <:AbstractDFGVariable, F<:AbstractDFGFactor}
4040

4141
LightDFG{T,V,F}(g, description, userId, robotId, sessionId, userData, robotData, sessionData, Symbol[], params)
4242
end

‎src/SymbolDFG/services/SymbolDFG.jl

Lines changed: 38 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -360,44 +360,44 @@ function _copyIntoGraph!(sourceDFG::SymbolDFG, destDFG::SymbolDFG, labels::Vecto
360360
return nothing
361361
end
362362

363-
364-
"""
365-
$(SIGNATURES)
366-
Retrieve a deep subgraph copy around a given variable or factor.
367-
Optionally provide a distance to specify the number of edges should be followed.
368-
Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
369-
Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
370-
"""
371-
function getSubgraphAroundNode(dfg::SymbolDFG, node::DFGNode, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::SymbolDFG=SymbolDFG{AbstractParams}(); solvable::Int=0)::SymbolDFG
372-
if !exists(dfg,node.label)
373-
error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
374-
end
375-
376-
nodelabel = node.label
377-
# Build a list of all unique neighbors inside 'distance'
378-
neighborList = Symbol[nodelabel]
379-
380-
curList = Symbol[nodelabel]
381-
382-
for dist in 1:distance
383-
newNeighbors = Symbol[]
384-
for cl in curList
385-
neighbors = outneighbors(dfg.g, cl)
386-
for neighbor in neighbors
387-
if !(neighbor in neighborList) && _isSolvable(dfg, neighbor, solvable)
388-
push!(neighborList, neighbor)
389-
push!(newNeighbors, neighbor)
390-
end
391-
end
392-
end
393-
curList = newNeighbors
394-
end
395-
396-
397-
# _copyIntoGraph!(dfg, addToDFG, map(n->get_prop(dfg.g, n, :label), ns), includeOrphanFactors)
398-
_copyIntoGraph!(dfg, addToDFG, neighborList, includeOrphanFactors)
399-
return addToDFG
400-
end
363+
# moved to abstract
364+
# """
365+
# $(SIGNATURES)
366+
# Retrieve a deep subgraph copy around a given variable or factor.
367+
# Optionally provide a distance to specify the number of edges should be followed.
368+
# Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
369+
# Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
370+
# """
371+
# function getSubgraphAroundNode(dfg::SymbolDFG, node::DFGNode, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::SymbolDFG=SymbolDFG{AbstractParams}(); solvable::Int=0)::SymbolDFG
372+
# if !exists(dfg,node.label)
373+
# error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
374+
# end
375+
#
376+
# nodelabel = node.label
377+
# # Build a list of all unique neighbors inside 'distance'
378+
# neighborList = Symbol[nodelabel]
379+
#
380+
# curList = Symbol[nodelabel]
381+
#
382+
# for dist in 1:distance
383+
# newNeighbors = Symbol[]
384+
# for cl in curList
385+
# neighbors = outneighbors(dfg.g, cl)
386+
# for neighbor in neighbors
387+
# if !(neighbor in neighborList) && _isSolvable(dfg, neighbor, solvable)
388+
# push!(neighborList, neighbor)
389+
# push!(newNeighbors, neighbor)
390+
# end
391+
# end
392+
# end
393+
# curList = newNeighbors
394+
# end
395+
#
396+
#
397+
# # _copyIntoGraph!(dfg, addToDFG, map(n->get_prop(dfg.g, n, :label), ns), includeOrphanFactors)
398+
# _copyIntoGraph!(dfg, addToDFG, neighborList, includeOrphanFactors)
399+
# return addToDFG
400+
# end
401401

402402

403403
"""

‎src/entities/DFGVariable.jl

Lines changed: 55 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,9 @@ mutable struct VariableNodeData{T<:InferenceVariable}
2222
ismargin::Bool
2323
dontmargin::Bool
2424
solveInProgress::Int
25+
solvedCount::Int
2526
VariableNodeData{T}() where {T <:InferenceVariable} =
26-
new{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], T(), false, 0.0, false, false, 0)
27+
new{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], T(), false, 0.0, false, false, 0, 0)
2728
VariableNodeData{T}(val::Array{Float64,2},
2829
bw::Array{Float64,2},
2930
BayesNetOutVertIDs::Array{Symbol,1},
@@ -36,11 +37,12 @@ mutable struct VariableNodeData{T<:InferenceVariable}
3637
inferdim::Float64,
3738
ismargin::Bool,
3839
dontmargin::Bool,
39-
solveInProgress::Int=0) where T <: InferenceVariable =
40+
solveInProgress::Int=0,
41+
solvedCount::Int=0 ) where T <: InferenceVariable =
4042
new{T}(val,bw,BayesNetOutVertIDs,dimIDs,dims,
4143
eliminated,BayesNetVertID,separator,
4244
softtype::T,initialized,inferdim,ismargin,
43-
dontmargin, solveInProgress)
45+
dontmargin, solveInProgress, solvedCount)
4446
end
4547

4648
VariableNodeData(val::Array{Float64,2},
@@ -55,20 +57,21 @@ VariableNodeData(val::Array{Float64,2},
5557
inferdim::Float64,
5658
ismargin::Bool,
5759
dontmargin::Bool,
58-
solveInProgress::Int=0) where T <: InferenceVariable =
60+
solveInProgress::Int=0,
61+
solvedCount::Int=0) where T <: InferenceVariable =
5962
VariableNodeData{T}(val,bw,BayesNetOutVertIDs,dimIDs,dims,
6063
eliminated,BayesNetVertID,separator,
6164
softtype::T,initialized,inferdim,ismargin,
62-
dontmargin, solveInProgress)
65+
dontmargin, solveInProgress, solvedCount)
6366
#
6467
VariableNodeData(softtype::T) where T <: InferenceVariable =
65-
VariableNodeData{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], softtype, false, 0.0, false, false, 0)
68+
VariableNodeData{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], softtype, false, 0.0, false, false, 0, 0)
6669

6770
function VariableNodeData()
6871
st = stacktrace()
6972
@warn "VariableNodeData() is deprecated please use VariableNodeData{T}() or VariableNodeData(softtype::T) where T <: InferenceVariable. Enable DEBUG logging for stack trace."
7073
@debug st
71-
VariableNodeData{InferenceVariable}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], SingletonInferenceVariable(), false, 0.0, false, false, 0)
74+
VariableNodeData{InferenceVariable}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], SingletonInferenceVariable(), false, 0.0, false, false, 0, 0)
7275
end
7376

7477

@@ -93,6 +96,7 @@ mutable struct PackedVariableNodeData
9396
ismargin::Bool
9497
dontmargin::Bool
9598
solveInProgress::Int
99+
solvedCount::Int
96100
PackedVariableNodeData() = new()
97101
PackedVariableNodeData(x1::Vector{Float64},
98102
x2::Int,
@@ -109,7 +113,8 @@ mutable struct PackedVariableNodeData
109113
x13::Float64,
110114
x14::Bool,
111115
x15::Bool,
112-
x16::Int) = new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16)
116+
x16::Int,
117+
solvedCount::Int) = new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,solvedCount)
113118
end
114119

115120
# AbstractPointParametricEst interface
@@ -184,7 +189,7 @@ mutable struct DFGVariableSummary <: AbstractDFGVariable
184189
timestamp::DateTime
185190
tags::Vector{Symbol}
186191
ppeDict::Dict{Symbol, <:AbstractPointParametricEst}
187-
softtypename::Symbol
192+
softtypename::Symbol # should be removed
188193
_internalId::Int64
189194
end
190195

@@ -274,8 +279,6 @@ TODO, DO NOT USE v.softtypename in DFGVariableSummary
274279
getSofttype(v::DFGVariableSummary)::Symbol = v.softtypename
275280

276281

277-
278-
279282
"""
280283
$SIGNATURES
281284
@@ -298,6 +301,47 @@ Get solver data dictionary for a variable.
298301
"""
299302
solverDataDict(v::DFGVariable) = v.solverDataDict
300303

304+
"""
305+
$SIGNATURES
306+
307+
Get the number of times a variable has been inferred -- i.e. `solvedCount`.
308+
309+
Related
310+
311+
isSolved, setSolvedCount!
312+
"""
313+
getSolvedCount(v::VariableNodeData) = v.solvedCount
314+
getSolvedCount(v::VariableDataLevel2, solveKey::Symbol=:default) = solverData(v, solveKey) |> getSolvedCount
315+
getSolvedCount(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol=:default) = getSolvedCount(getVariable(dfg, sym), solveKey)
316+
317+
"""
318+
$SIGNATURES
319+
320+
Boolean on whether the variable has been solved.
321+
322+
Related
323+
324+
getSolved, setSolved!
325+
"""
326+
isSolved(v::VariableNodeData) = 0 < v.solvedCount
327+
isSolved(v::VariableDataLevel2, solveKey::Symbol=:default) = solverData(v, solveKey) |> isSolved
328+
isSolved(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol=:default) = isSolved(getVariable(dfg, sym), solveKey)
329+
330+
331+
"""
332+
$SIGNATURES
333+
334+
Update/set the `solveCount` value.
335+
336+
Related
337+
338+
getSolved, isSolved
339+
"""
340+
setSolvedCount!(v::VariableNodeData, val::Int) = v.solvedCount = val
341+
setSolvedCount!(v::VariableDataLevel2, val::Int, solveKey::Symbol=:default) = setSolvedCount!(solverData(v, solveKey), val)
342+
setSolvedCount!(dfg::AbstractDFG, sym::Symbol, val::Int, solveKey::Symbol=:default) = setSolvedCount!(getVariable(dfg, sym), val, solveKey)
343+
344+
301345
"""
302346
$SIGNATURES
303347

‎src/needsahome.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ function buildSubgraphFromLabels!(dfg::G,
2525
syms::Vector{Symbol};
2626
subfg::AbstractDFG=(G <: InMemoryDFGTypes ? G : GraphsDFG)(params=getSolverParams(dfg)),
2727
solvable::Int=0,
28-
allowedFactors::Union{Nothing, Vector{Symbol}}=nothing )::G where G <: AbstractDFG
28+
allowedFactors::Union{Nothing, Vector{Symbol}}=nothing )::AbstractDFG where G <: AbstractDFG
2929
#
3030

3131
# add a little too many variables (since we need the factors)

‎src/services/AbstractDFG.jl

Lines changed: 38 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -418,6 +418,30 @@ end
418418

419419
# TODO: NEED TO FIGURE OUT SIGNATURE FOR DEFAULT ARGS
420420

421+
422+
# TODO export, test and overwrite in LightGraphs and CloudGraphsDFG
423+
"""
424+
$(SIGNATURES)
425+
Build a list of all unique neighbors inside 'distance'
426+
"""
427+
function getNeighborhood(dfg::AbstractDFG, label::Symbol, distance::Int)::Vector{Symbol}
428+
neighborList = Set{Symbol}([label])
429+
curList = Set{Symbol}([label])
430+
431+
for dist in 1:distance
432+
newNeighbors = Set{Symbol}()
433+
for node in curList
434+
neighbors = getNeighbors(dfg, node)
435+
for neighbor in neighbors
436+
push!(neighborList, neighbor)
437+
push!(newNeighbors, neighbor)
438+
end
439+
end
440+
curList = newNeighbors
441+
end
442+
return collect(neighborList)
443+
end
444+
421445
"""
422446
$(SIGNATURES)
423447
Retrieve a deep subgraph copy around a given variable or factor.
@@ -426,8 +450,20 @@ Optionally provide an existing subgraph addToDFG, the extracted nodes will be co
426450
Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
427451
Note: Always returns the node at the center, but filters around it if solvable is set.
428452
"""
429-
function getSubgraphAroundNode(dfg::G, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::H=_getDuplicatedEmptyDFG(dfg); solvable::Int=0)::G where {G <: AbstractDFG, H <: AbstractDFG, P <: AbstractParams, T <: DFGNode}
430-
error("getSubgraphAroundNode not implemented for $(typeof(dfg))")
453+
function getSubgraphAroundNode(dfg::AbstractDFG, node::DFGNode, distance::Int=1, includeOrphanFactors::Bool=false, addToDFG::AbstractDFG=_getDuplicatedEmptyDFG(dfg); solvable::Int=0)::AbstractDFG
454+
455+
if !exists(dfg, node.label)
456+
error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
457+
end
458+
459+
neighbors = getNeighborhood(dfg, node.label, distance)
460+
461+
# for some reason: always returns the node at the center with || (nlbl == node.label)
462+
solvable != 0 && filter!(nlbl -> (getSolvable(dfg, nlbl) >= solvable) || (nlbl == node.label), neighbors)
463+
464+
# Copy the section of graph we want
465+
_copyIntoGraph!(dfg, addToDFG, neighbors, includeOrphanFactors)
466+
return addToDFG
431467
end
432468

433469
"""

0 commit comments

Comments
 (0)