Skip to content

Commit

Permalink
Merging master
Browse files Browse the repository at this point in the history
  • Loading branch information
@GearsAD
GearsAD committed Sep 23, 2019
2 parents 91f4046 + 306090f commit 15f7237
Show file tree
Hide file tree
Showing 11 changed files with 423 additions and 207 deletions.
12 changes: 6 additions & 6 deletions src/CloudGraphsDFG/services/CloudGraphsDFG.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,8 +63,8 @@ getDescription(dfg::CloudGraphsDFG) = dfg.description
setDescription(dfg::CloudGraphsDFG, description::String) = dfg.description = description
getAddHistory(dfg::CloudGraphsDFG) = dfg.addHistory
getSolverParams(dfg::CloudGraphsDFG) = dfg.solverParams
function setSolverParams(dfg::CloudGraphsDFG, solverParams::T) where T <: AbstractParams
dfg.solverParams = solverParams
function setSolverParams(dfg::CloudGraphsDFG, solverParams::T)::T where T <: AbstractParams
return dfg.solverParams = solverParams
end

"""
Expand Down Expand Up @@ -753,11 +753,11 @@ Get a deep subgraph copy from the DFG given a list of variables and factors.
Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
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.
"""
function getSubgraph(dfg::CloudGraphsDFG, variableFactorLabels::Vector{Symbol}, includeOrphanFactors::Bool=false, addToDFG::Union{Nothing, CloudGraphsDFG}=nothing)::CloudGraphsDFG
function getSubgraph(dfg::CloudGraphsDFG,
variableFactorLabels::Vector{Symbol},
includeOrphanFactors::Bool=false,
addToDFG::G=_getDuplicatedEmptyDFG(dfg) )::G where {G <: AbstractDFG}
# Making a copy session if not specified
if addToDFG == nothing
addToDFG = _getDuplicatedEmptyDFG(dfg)
end

_copyIntoGraph!(dfg, addToDFG, variableFactorLabels, includeOrphanFactors)

Expand Down
45 changes: 44 additions & 1 deletion src/Common.jl
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,18 @@

export sortVarNested
export isPrior, lsfPriors
export getVariableType, getSofttype
export getFactorType, getfnctype
export lsTypes, lsfTypes
export lsWho, lsfWho

## Utility functions for getting type names and modules (from IncrementalInference)
function _getmodule(t::T) where T
T.name.module
end
function _getname(t::T) where T
T.name.name
end

"""
$(SIGNATURES)
Test if all elements of the string is a number: Ex, "123" is true, "1_2" is false.
Expand Down Expand Up @@ -223,10 +230,38 @@ function lsTypes(dfg::G)::Dict{Symbol, Vector{String}} where G <: AbstractDFG
end


function ls(dfg::G, ::Type{T}; solveKey::Symbol=:default) where {G <: AbstractDFG, T <: InferenceVariable}
xx = getVariables(dfg)
mask = getVariableType.(xx, solveKey=solveKey) .|> typeof .== T
vxx = view(xx, mask)
map(x->x.label, vxx)
end


function ls(dfg::G, ::Type{T}) where {G <: AbstractDFG, T <: FunctorInferenceType}
xx = getFactors(dfg)
names = getfield.(typeof.(getFactorType.(xx)), :name) .|> Symbol
vxx = view(xx, names .== Symbol(T))
map(x->x.label, vxx)
end

function lsf(dfg::G, ::Type{T}) where {G <: AbstractDFG, T <: FunctorInferenceType}
ls(dfg, T)
end


"""
$(SIGNATURES)
Gives back all factor labels that fit the bill:
lsWho(dfg, :Pose3)
Dev Notes
- Cloud versions will benefit from less data transfer
- `ls(dfg::C, ::T) where {C <: CloudDFG, T <: ..}`
Related
ls, lsf, lsfPriors
"""
function lsWho(dfg::AbstractDFG, type::Symbol; solveKey::Symbol=:default)::Vector{Symbol}
vars = getVariables(dfg)
Expand All @@ -243,6 +278,14 @@ end
$(SIGNATURES)
Gives back all factor labels that fit the bill:
lsfWho(dfg, :Point2Point2)
Dev Notes
- Cloud versions will benefit from less data transfer
- `ls(dfg::C, ::T) where {C <: CloudDFG, T <: ..}`
Related
ls, lsf, lsfPriors
"""
function lsfWho(dfg::AbstractDFG, type::Symbol)::Vector{Symbol}
facs = getFactors(dfg)
Expand Down
190 changes: 47 additions & 143 deletions src/GraphsDFG/services/GraphsDFG.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,20 @@ setDescription(dfg::GraphsDFG, description::String) = dfg.description = descript
getInnerGraph(dfg::GraphsDFG) = dfg.g
getAddHistory(dfg::GraphsDFG) = dfg.addHistory
getSolverParams(dfg::GraphsDFG) = dfg.solverParams
function setSolverParams(dfg::GraphsDFG, solverParams::T) where T <: AbstractParams
dfg.solverParams = solverParams
end

# setSolverParams(dfg::GraphsDFG, solverParams) = dfg.solverParams = solverParams
function setSolverParams(dfg::GraphsDFG, solverParams::P) where P <: AbstractParams
dfg.solverParams = solverParams
"""
$(SIGNATURES)
Gets an empty and unique CloudGraphsDFG derived from an existing DFG.
"""
function _getDuplicatedEmptyDFG(dfg::GraphsDFG)::GraphsDFG
newDfg = GraphsDFG{typeof(dfg.solverParams)}(;
userId=dfg.userId, robotId=dfg.robotId, sessionId=dfg.sessionId,
params=deepcopy(dfg.solverParams))
newDfg.description ="(Copy of) $(dfg.description)"
return newDfg
end

"""
Expand Down Expand Up @@ -190,13 +200,6 @@ function deleteVariable!(dfg::GraphsDFG, label::Symbol)::DFGVariable
return variable
end

#Alias
"""
$(SIGNATURES)
Delete a referenced DFGVariable from the DFG.
"""
deleteVariable!(dfg::GraphsDFG, variable::DFGVariable)::DFGVariable = deleteVariable!(dfg, variable.label)

"""
$(SIGNATURES)
Delete a DFGFactor from the DFG using its label.
Expand All @@ -211,13 +214,6 @@ function deleteFactor!(dfg::GraphsDFG, label::Symbol)::DFGFactor
return factor
end

# Alias
"""
$(SIGNATURES)
Delete the referened DFGFactor from the DFG.
"""
deleteFactor!(dfg::GraphsDFG, factor::DFGFactor)::DFGFactor = deleteFactor!(dfg, factor.label)

"""
$(SIGNATURES)
List the DFGVariables in the DFG.
Expand All @@ -235,34 +231,6 @@ function getVariables(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing
return variables
end

"""
$(SIGNATURES)
Get a list of IDs of the DFGVariables in the DFG.
Optionally specify a label regular expression to retrieves a subset of the variables.
Example
```julia
getVariableIds(dfg, r"l", tags=[:APRILTAG;])
```
Related
ls
"""
function getVariableIds(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[])::Vector{Symbol}
vars = getVariables(dfg, regexFilter, tags=tags)
# mask = map(v -> length(intersect(v.tags, tags)) > 0, vars )
map(v -> v.label, vars)
end

# Alias
"""
$(SIGNATURES)
List the DFGVariables in the DFG.
Optionally specify a label regular expression to retrieves a subset of the variables.
"""
ls(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[])::Vector{Symbol} = getVariableIds(dfg, regexFilter, tags=tags)

"""
$(SIGNATURES)
List the DFGFactors in the DFG.
Expand All @@ -276,29 +244,6 @@ function getFactors(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing):
return factors
end

"""
$(SIGNATURES)
Get a list of the IDs of the DFGFactors in the DFG.
Optionally specify a label regular expression to retrieves a subset of the factors.
"""
getFactorIds(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing)::Vector{Symbol} = map(f -> f.label, getFactors(dfg, regexFilter))

"""
$(SIGNATURES)
List the DFGFactors in the DFG.
Optionally specify a label regular expression to retrieves a subset of the factors.
"""
# Alias
lsf(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing)::Vector{Symbol} = getFactorIds(dfg, regexFilter)

"""
$(SIGNATURES)
Alias for getNeighbors - returns neighbors around a given node label.
"""
function lsf(dfg::GraphsDFG, label::Symbol)::Vector{Symbol}
return getNeighbors(dfg, label)
end

"""
$(SIGNATURES)
Checks if the graph is fully connected, returns true if so.
Expand All @@ -307,14 +252,6 @@ function isFullyConnected(dfg::GraphsDFG)::Bool
return length(Graphs.connected_components(dfg.g)) == 1
end

#Alias
"""
$(SIGNATURES)
Checks if the graph is not fully connected, returns true if it is not contiguous.
"""
hasOrphans(dfg::GraphsDFG)::Bool = !isFullyConnected(dfg)


"""
$(SIGNATURES)
Retrieve a list of labels of the immediate neighbors around a given variable or factor.
Expand Down Expand Up @@ -359,56 +296,40 @@ function getNeighbors(dfg::GraphsDFG, label::Symbol; ready::Union{Nothing, Int}=
return map(n -> n.dfgNode.label, neighbors)
end

# Aliases
"""
$(SIGNATURES)
Retrieve a list of labels of the immediate neighbors around a given variable or factor.
"""
function ls(dfg::GraphsDFG, node::T)::Vector{Symbol} where T <: DFGNode
return getNeighbors(dfg, node)
end
"""
$(SIGNATURES)
Retrieve a list of labels of the immediate neighbors around a given variable or factor specified by its label.
"""
function ls(dfg::GraphsDFG, label::Symbol)::Vector{Symbol} where T <: DFGNode
return getNeighbors(dfg, label)
end

function _copyIntoGraph!(sourceDFG::GraphsDFG, destDFG::GraphsDFG, variableFactorLabels::Vector{Symbol}, includeOrphanFactors::Bool=false)::Nothing
# Split into variables and factors
verts = map(id -> sourceDFG.g.vertices[sourceDFG.labelDict[id]], variableFactorLabels)
sourceVariables = filter(n -> n.dfgNode isa DFGVariable, verts)
sourceFactors = filter(n -> n.dfgNode isa DFGFactor, verts)

# Now we have to add all variables first,
for variable in sourceVariables
if !haskey(destDFG.labelDict, variable.dfgNode.label)
addVariable!(destDFG, deepcopy(variable.dfgNode))
end
end
# And then all factors to the destDFG.
for factor in sourceFactors
if !haskey(destDFG.labelDict, factor.dfgNode.label)
# Get the original factor variables (we need them to create it)
neighVarIds = getNeighbors(sourceDFG, factor.dfgNode.label) #OLD: in_neighbors(factor, sourceDFG.g)
# Find the labels and associated neighVarIds in our new subgraph
factVariables = DFGVariable[]
for neighVarId in neighVarIds
if haskey(destDFG.labelDict, neighVarId)
push!(factVariables, getVariable(destDFG, neighVarId))
#otherwise ignore
end
end

# Only if we have all of them should we add it (otherwise strange things may happen on evaluation)
if includeOrphanFactors || length(factVariables) == length(neighVarIds)
addFactor!(destDFG, factVariables, deepcopy(factor.dfgNode))
end
end
end
return nothing
end
# function _copyIntoGraph!(sourceDFG::GraphsDFG, destDFG::GraphsDFG, variableFactorLabels::Vector{Symbol}, includeOrphanFactors::Bool=false)::Nothing
# # Split into variables and factors
# verts = map(id -> sourceDFG.g.vertices[sourceDFG.labelDict[id]], variableFactorLabels)
# sourceVariables = filter(n -> n.dfgNode isa DFGVariable, verts)
# sourceFactors = filter(n -> n.dfgNode isa DFGFactor, verts)
#
# # Now we have to add all variables first,
# for variable in sourceVariables
# if !haskey(destDFG.labelDict, variable.dfgNode.label)
# addVariable!(destDFG, deepcopy(variable.dfgNode))
# end
# end
# # And then all factors to the destDFG.
# for factor in sourceFactors
# if !haskey(destDFG.labelDict, factor.dfgNode.label)
# # Get the original factor variables (we need them to create it)
# neighVarIds = getNeighbors(sourceDFG, factor.dfgNode.label) #OLD: in_neighbors(factor, sourceDFG.g)
# # Find the labels and associated neighVarIds in our new subgraph
# factVariables = DFGVariable[]
# for neighVarId in neighVarIds
# if haskey(destDFG.labelDict, neighVarId)
# push!(factVariables, getVariable(destDFG, neighVarId))
# #otherwise ignore
# end
# end
#
# # Only if we have all of them should we add it (otherwise strange things may happen on evaluation)
# if includeOrphanFactors || length(factVariables) == length(neighVarIds)
# addFactor!(destDFG, factVariables, deepcopy(factor.dfgNode))
# end
# end
# end
# return nothing
# end

"""
$(SIGNATURES)
Expand Down Expand Up @@ -445,23 +366,6 @@ function getSubgraphAroundNode(dfg::GraphsDFG{P}, node::T, distance::Int64=1, in
return addToDFG
end

"""
$(SIGNATURES)
Get a deep subgraph copy from the DFG given a list of variables and factors.
Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
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.
"""
function getSubgraph(dfg::GraphsDFG, variableFactorLabels::Vector{Symbol}, includeOrphanFactors::Bool=false, addToDFG::GraphsDFG=GraphsDFG{AbstractParams}())::GraphsDFG
for label in variableFactorLabels
if !haskey(dfg.labelDict, label)
error("Variable/factor with label '$(label)' does not exist in the factor graph")
end
end

_copyIntoGraph!(dfg, addToDFG, variableFactorLabels, includeOrphanFactors)
return addToDFG
end

"""
$(SIGNATURES)
Get an adjacency matrix for the DFG, returned as a Matrix{Union{Nothing, Symbol}}.
Expand Down
3 changes: 2 additions & 1 deletion src/LightDFG/entities/LightDFG.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,8 @@ Base.getproperty(x::LightDFG,f::Symbol) = begin
@error "Depreciated? returning number of nodes"
nv(x.g)
elseif f == :labelDict
@error "Depreciated? Concider using exists(dfg,label) instead. Returing internals copy"
@error "Depreciated? Consider using exists(dfg,label) instead. Returning internals copy"
#TODO: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/111
copy(x.g.labels.sym_int)
else
getfield(x,f)
Expand Down
Loading

0 comments on commit 15f7237

Please sign in to comment.