Consolidate test and some cleanup (#287)

* WIP Sorting and grouping tests. 

* Deprecate some PPE functions

* Skeleton and Summary mostly consolidated

* update return type and bugfix in mergeVariableSolverData!

* test better types, getFactors tags

Author: Affie

src/Deprecated.jl

@@ -80,9 +80,27 @@ Base.setproperty!(x::DFGFactor,f::Symbol, val) = begin
     end
   end
 
-@deprecate getEstimates(v::VariableDataLevel1) getVariablePPEs(v)
+@deprecate getEstimates(v::VariableDataLevel1) getPPEDict(v)
+
+@deprecate estimates(v::VariableDataLevel1) getPPEDict(v)
+
+@deprecate getVariablePPEs(v::VariableDataLevel1) getPPEDict(v)
+
+@deprecate getVariablePPE(args...) getPPE(args...)
+#FIXME SORT UIT en deprecate, too many aliases
+
+
+"""
+    $SIGNATURES
+
+Return dictionary with Parametric Point Estimates (PPE) values.
+
+Notes:
+- Equivalent to `getVariablePPEs`.
+"""
+#TODO deprecate, maar gebruik dalk naam om vector te kry soos getVariables/getFactors
+getPPEs(vari::VariableDataLevel1)::Dict = getVariablePPEs(vari)
 
-@deprecate estimates(v::VariableDataLevel1) getVariablePPEs(v)
 
 @deprecate getEstimate(v::VariableDataLevel1, key::Symbol=:default) getVariablePPE(v, key)
 
@@ -120,6 +138,8 @@ Base.setproperty!(x::DFGFactor,f::Symbol, val) = begin
 
 @deprecate internalId(args...) getInternalId(args...)
 
+#FIXME TODO based on API definition of merge, in some cases the Noun is really not needed.
+# @deprecate mergeUpdateVariableSolverData!(args...) mergeVariableSolverData!(args...)
 
 
 export getLabelDict

src/DistributedFactorGraphs.jl

@@ -10,6 +10,11 @@ using JSON2
 using LinearAlgebra
 using SparseArrays
 
+# TODO
+# - hasOrphans: actually check if there are orphaned factors
+# getFactorFunction vs getFactorType, does the same thing
+#
+
 # Entities
 include("entities/AbstractDFG.jl")
 
@@ -36,6 +41,7 @@ export DFGVariableSummary, DFGFactorSummary, AbstractDFGSummary
 export getNeighborhood, getSubgraph, getSubgraphAroundNode
 
 export getUserId, getRobotId, getSessionId
+export getDFGInfo
 
 # Define variable levels
 const VariableDataLevel0 = Union{DFGVariable, DFGVariableSummary, SkeletonDFGVariable}
@@ -71,7 +77,7 @@ export getVariableOrder
 
 # Services/AbstractDFG Exports
 export isInitialized, getFactorFunction, isVariable, isFactor
-export isSolveInProgress, getSolvable, setSolvable!, getSolveInProgress
+export isSolveInProgress, getSolvable, setSolvable!, getSolveInProgress, isSolvable
 export mergeUpdateVariableSolverData!, mergeUpdateGraphSolverData!
 
 # Solver (IIF) Exports

src/GraphsDFG/services/GraphsDFG.jl

@@ -202,12 +202,17 @@ function getVariables(dfg::GraphsDFG,
     return variables
 end
 
-function getFactors(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing; solvable::Int=0)::Vector{DFGFactor}
+function getFactors(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[], solvable::Int=0)::Vector{DFGFactor}
     factors = map(v -> v.dfgNode, filter(n -> (n.dfgNode isa DFGFactor) && (solvable != 0 ? solvable <= isSolvable(n.dfgNode) : true), Graphs.vertices(dfg.g)))
 
     if regexFilter != nothing
         factors = filter(f -> occursin(regexFilter, String(f.label)), factors)
     end
+    
+    if length(tags) > 0
+        mask = map(v -> length(intersect(v.tags, tags)) > 0, factors )
+        return factors[mask]
+    end
     return factors
 end
 

src/LightDFG/services/LightDFG.jl

@@ -168,7 +168,7 @@ function listVariables(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing
     end
 end
 
-function getFactors(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; solvable::Int=0)::Vector{AbstractDFGFactor}
+function getFactors(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[], solvable::Int=0)::Vector{AbstractDFGFactor}
     # factors = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGFactor, vertices(dfg.g)))
     factors = collect(values(dfg.g.factors))
     if regexFilter != nothing
@@ -177,6 +177,10 @@ function getFactors(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; s
     if solvable != 0
         factors = filter(f -> _isSolvable(dfg, f.label, solvable), factors)
     end
+    if length(tags) > 0
+        mask = map(v -> length(intersect(v.tags, tags)) > 0, factors )
+        return factors[mask]
+    end
     return factors
 end
 

src/entities/AbstractDFG.jl

@@ -27,9 +27,8 @@ The common node parameters for variables and factors.
 mutable struct DFGNodeParams
     solvable::Int
     _internalId::Int64
-    DFGNodeParams() = new(0, 0)
-    DFGNodeParams(solvable::Int, _internalId::Int64) = new(solvable, _internalId)
 end
+DFGNodeParams() = DFGNodeParams(0, 0)
 
 """
 $(TYPEDEF)

src/services/AbstractDFG.jl

@@ -15,6 +15,7 @@
 # Getters
 """
     $(SIGNATURES)
+Convenience function to get all the matadata of a DFG
 """
 getDFGInfo(dfg::AbstractDFG) = (dfg.description, dfg.userId, dfg.robotId, dfg.sessionId, dfg.userData, dfg.robotData, dfg.sessionData, dfg.solverParams)
 
@@ -268,12 +269,14 @@ end
 Get a DFGVariable with a specific solver key.
 In memory types still return a reference, other types returns a variable with only solveKey.
 """
-function getVariable(dfg::G, label::Symbol, solveKey::Symbol)::AbstractDFGVariable where G <: AbstractDFG
+function getVariable(dfg::AbstractDFG, label::Symbol, solveKey::Symbol)::AbstractDFGVariable
 
     var = getVariable(dfg, label)
 
-    if !haskey(var.solverDataDict, solveKey)
+    if isa(var, DFGVariable) && !haskey(var.solverDataDict, solveKey)
         error("Solvekey '$solveKey' does not exists in the variable")
+    elseif !isa(var, DFGVariable)
+        @warn "getVariable(dfg, label, solveKey) only supported for type DFGVariable."
     end
 
     return var
@@ -394,7 +397,7 @@ end
 List the DFGFactors in the DFG.
 Optionally specify a label regular expression to retrieves a subset of the factors.
 """
-function getFactors(dfg::G, regexFilter::Union{Nothing, Regex}=nothing; solvable::Int=0)::Vector{AbstractDFGFactor} where G <: AbstractDFG
+function getFactors(dfg::G, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[], solvable::Int=0)::Vector{AbstractDFGFactor} where G <: AbstractDFG
     error("getFactors not implemented for $(typeof(dfg))")
 end
 
@@ -712,7 +715,13 @@ end
 
 """
     $(SIGNATURES)
-Get variable PPE for a given solve key.
+Get the parametric point estimate (PPE) for a variable in the factor graph for a given solve key.
+
+Notes
+- Defaults on keywords `solveKey` and `method`
+
+Related
+getMeanPPE, getMaxPPE, getKDEMean, getKDEFit, getPPEs, getVariablePPEs
 """
 function getPPE(dfg::AbstractDFG, variablekey::Symbol, ppekey::Symbol=:default)::AbstractPointParametricEst
     v = getVariable(dfg, variablekey)
@@ -721,7 +730,7 @@ function getPPE(dfg::AbstractDFG, variablekey::Symbol, ppekey::Symbol=:default):
 end
 
 # Not the most efficient call but it at least reuses above (in memory it's probably ok)
-getPPE(dfg::AbstractDFG, sourceVariable::DFGVariable, ppekey::Symbol=default)::AbstractPointParametricEst = getPPE(dfg, sourceVariable.label, ppekey)
+getPPE(dfg::AbstractDFG, sourceVariable::VariableDataLevel1, ppekey::Symbol=default)::AbstractPointParametricEst = getPPE(dfg, sourceVariable.label, ppekey)
 
 """
     $(SIGNATURES)
@@ -765,14 +774,14 @@ end
 Update PPE data if it exists, otherwise add it.
 NOTE: Copies the PPE data.
 """
-updatePPE!(dfg::AbstractDFG, sourceVariable::DFGVariable, ppekey::Symbol=:default) =
+updatePPE!(dfg::AbstractDFG, sourceVariable::VariableDataLevel1, ppekey::Symbol=:default) =
     updatePPE!(dfg, sourceVariable.label, deepcopy(getPPE(sourceVariable, ppekey)), ppekey)
 
 """
     $(SIGNATURES)
 Update PPE data if it exists, otherwise add it.
 """
-function updatePPE!(dfg::AbstractDFG, sourceVariables::Vector{<:DFGVariable}, ppekey::Symbol=:default)
+function updatePPE!(dfg::AbstractDFG, sourceVariables::Vector{<:VariableDataLevel1}, ppekey::Symbol=:default)
     #I think cloud would do this in bulk for speed
     for var in sourceVariables
         updatePPE!(dfg, var.label, getPPE(dfg, var, ppekey), ppekey)
@@ -802,34 +811,69 @@ deletePPE!(dfg::AbstractDFG, sourceVariable::DFGVariable, ppekey::Symbol=:defaul
 
 ####
 
+
+export mergeVariableSolverData!, mergePPEs!, mergeVariableData!, mergeGraphVariableData!
+# update is implied, see API wiki
+@deprecate mergeUpdateVariableSolverData!(dfg, sourceVariable) mergeVariableData!(dfg, sourceVariable)
+@deprecate mergeUpdateGraphSolverData!(sourceDFG, destDFG, varSyms) mergeGraphVariableData!(destDFG, sourceDFG, varSyms)
+
 """
     $(SIGNATURES)
 Merges and updates solver and estimate data for a variable (variable can be from another graph).
-Note: Makes a copy of the estimates and solver data so that there is no coupling
-between graphs.
+If the same key is present in another collection, the value for that key will be the value it has in the last collection listed (updated).
+Note: Makes a copy of the estimates and solver data so that there is no coupling between graphs.
 """
-function mergeUpdateVariableSolverData!(dfg::AbstractDFG, sourceVariable::AbstractDFGVariable)::AbstractDFGVariable
-    if !exists(dfg, sourceVariable)
-        error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.")
-    end
-    var = getVariable(dfg, sourceVariable.label)
+#TODO API
+function mergeVariableSolverData!(destVariable::DFGVariable, sourceVariable::DFGVariable)::DFGVariable
+    # We don't know which graph this came from, must be copied!
+    merge!(destVariable.solverDataDict, deepcopy(sourceVariable.solverDataDict))
+    return destVariable
+end
+
+"""
+    $(SIGNATURES)
+Merges and updates solver and estimate data for a variable (variable can be from another graph).
+Note: Makes a copy of the estimates and solver data so that there is no coupling between graphs.
+"""
+#TODO API and only correct level
+function mergePPEs!(destVariable::AbstractDFGVariable, sourceVariable::AbstractDFGVariable)::AbstractDFGVariable
     # We don't know which graph this came from, must be copied!
-    merge!(var.ppeDict, deepcopy(sourceVariable.ppeDict))
+    merge!(destVariable.ppeDict, deepcopy(sourceVariable.ppeDict))
+    return destVariable
+end
+
+"""
+    $(SIGNATURES)
+Merges and updates solver and estimate data for a variable (variable can be from another graph).
+Note: Makes a copy of the estimates and solver data so that there is no coupling between graphs.
+"""
+#TODO API
+function mergeVariableData!(dfg::AbstractDFG, sourceVariable::AbstractDFGVariable)::AbstractDFGVariable
+
+    var = getVariable(dfg, sourceVariable.label)
+
+    mergePPEs!(var, sourceVariable)
     # If this variable has solverDataDict (summaries do not)
-    :solverDataDict in fieldnames(typeof(var)) && merge!(var.solverDataDict, deepcopy(sourceVariable.solverDataDict))
-    return sourceVariable
+    :solverDataDict in fieldnames(typeof(var)) && mergeVariableSolverData!(var, sourceVariable)
+
+    #update if its not a InMemoryDFGTypes, otherwise it was a reference
+    # if satelite nodes are used it can be updated seprarately
+    # !(isa(dfg, InMemoryDFGTypes)) && updateVariable!(dfg, var)
+
+    return var
 end
 
 """
     $(SIGNATURES)
 Common function to update all solver data and estimates from one graph to another.
 This should be used to push local solve data back into a cloud graph, for example.
 """
-function mergeUpdateGraphSolverData!(sourceDFG::G, destDFG::H, varSyms::Vector{Symbol})::Nothing where {G <: AbstractDFG, H <: AbstractDFG}
+#TODO API
+function mergeGraphVariableData!(destDFG::H, sourceDFG::G, varSyms::Vector{Symbol})::Nothing where {G <: AbstractDFG, H <: AbstractDFG}
     # Update all variables in the destination
     # (For now... we may change this soon)
     for variableId in varSyms
-        mergeUpdateVariableSolverData!(destDFG, getVariable(sourceDFG, variableId))
+        mergeVariableData!(destDFG, getVariable(sourceDFG, variableId))
     end
 end
 
@@ -924,13 +968,15 @@ Related
 
 isSolvable
 """
-function getSolveInProgress(var::Union{DFGVariable, DFGFactor}; solveKey::Symbol=:default)::Int
+function getSolveInProgress(var::Union{DFGVariable, DFGFactor}, solveKey::Symbol=:default)::Int
     # Variable
     var isa DFGVariable && return haskey(getSolverDataDict(var), solveKey) ? getSolverDataDict(var)[solveKey].solveInProgress : 0
     # Factor
     return getSolverData(var).solveInProgress
 end
 
+isSolveInProgress(node::Union{DFGVariable, DFGFactor}, solvekey::Symbol=:default) = getSolveInProgress(node, solvekey) > 0
+
 """
     $SIGNATURES
 

src/services/CompareUtils.jl

@@ -4,12 +4,11 @@
 import Base.==
 
 #=
-TODO <:InferenceVariable: please someone confirm this, I'm not fully up to date why softtype was created.
-For now abstract `InferenceVariable`s are considered equal if they are the same type (except for labels, that I feel are deprecated)
+For now abstract `InferenceVariable`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. softtype sounds a bit if it can be a trait, or in the future, rather a normal julia type.
+implement compare if needed.
 =#
-==(a::InferenceVariable,b::InferenceVariable) = typeof(a) == typeof(b)
+==(a::InferenceVariable,b::InferenceVariable) = typeof(a) == typeof(b) && a.dims == b.dims && a.manifolds == b.manifolds
 
 ==(a::ConvolutionObject, b::ConvolutionObject) = typeof(a) == typeof(b)
 

src/services/DFGFactor.jl

@@ -106,3 +106,6 @@ Retrieve solver data structure stored in a factor.
 function getSolverData(f::F) where F <: DFGFactor
   return f.solverData
 end
+
+#TODO don't know if this is used, added for completeness
+setSolverData!(f::DFGFactor, data::GenericFunctionNodeData) = f.solverData = data