Merge pull request #486 from JuliaRobotics/feat/2Q20/fctsbetween

getVariableLabelNumber, findFactorsBetweenNaive

Author: dehann

src/Common.jl

@@ -57,7 +57,7 @@ Related
 ls, lsf
 """
 sortDFG(vars::Vector{<:DFGNode}; by=getTimestamp, kwargs...) = sort(vars; by=by, kwargs...)
-sortDFG(vars::Vector{Symbol}; lt=natural_lt, kwargs...)::Vector{Symbol} = sort(vars; lt=lt, kwargs...)
+sortDFG(vars::Vector{Symbol}; lt=natural_lt, kwargs...) = sort(vars; lt=lt, kwargs...)
 
 ##==============================================================================
 ## Validation of session, robot, and user IDs.
@@ -77,3 +77,23 @@ function isValidLabel(id::Union{Symbol, String})::Bool
     end
     return all(t -> t != uppercase(id), _invalidIds) && match(_validLabelRegex, id) != nothing
 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`
+
+"""
+getVariableLabelNumber(vs::Symbol, prefix=string(vs)[1]) = parse(Int, string(vs)[(length(prefix)+1):end])

src/DistributedFactorGraphs.jl

@@ -92,6 +92,7 @@ export DFGFactor, DFGFactorSummary, SkeletonDFGFactor
 # Common
 export getSolvable, setSolvable!, isSolvable
 export getInternalId
+export getVariableLabelNumber
 
 # accessors
 export getLabel, getTimestamp, setTimestamp, setTimestamp!, getTags, setTags!
@@ -161,6 +162,7 @@ export packVariableNodeData, unpackVariableNodeData
 export getSolvedCount, isSolved, setSolvedCount!, isInitialized
 
 export getNeighborhood, getNeighbors, _getDuplicatedEmptyDFG
+export findFactorsBetweenNaive
 export copyGraph!, deepcopyGraph, deepcopyGraph!, buildSubgraph, mergeGraph!
 # Big Data
 ##------------------------------------------------------------------------------

src/services/AbstractDFG.jl

@@ -938,6 +938,41 @@ function deepcopyGraph(::Type{T},
     return destDFG
 end
 
+
+
+##==============================================================================
+## Automated Graph Searching
+##==============================================================================
+
+"""
+    $SIGNATURES
+
+Relatively naive function counting linearly from-to
+
+DevNotes
+- Convert to using LightGraphs shortest path methods instead.
+"""
+function findFactorsBetweenNaive(dfg::AbstractDFG, from::Symbol, to::Symbol, assertSingles::Bool=false)
+  @info "findFactorsBetweenNaive is naive linear number method -- improvements welcome"
+  SRT = getVariableLabelNumber(from)
+  STP = getVariableLabelNumber(to)
+  prefix = string(from)[1]
+  @assert prefix == string(to)[1] "from-to prefixes must match, one is $prefix, other $(string(to)[1])"
+  prev = from
+  fctlist = Symbol[]
+  for num in (SRT+1):STP
+    next = Symbol(prefix,num)
+    fct = intersect(ls(dfg, prev),ls(dfg,next))
+    !assertSingles ? nothing : @assert length(fct) == 1 "assertSingles=true, won't return multiple factors joining variables at this time"
+    union!(fctlist, fct)
+    prev = next
+  end
+
+  return fctlist
+end
+
+
+
 ##==============================================================================
 ## Subgraphs and Neighborhoods
 ##==============================================================================