Merge pull request #230 from JuliaRobotics/feat/4Q19/fcttime

add timestamps to DFGFactor/Summary and closest functions

Author: GearsAD

src/Common.jl

@@ -8,6 +8,9 @@ export getFactorType, getfnctype
 export lsTypes, lsfTypes
 export lsWho, lsfWho
 export *
+export findClosestTimestamp, findVariableNearTimestamp
+export addTags!
+export hasTags, hasTagsNeighbors
 
 *(a::Symbol, b::AbstractString)::Symbol = Symbol(string(a,b))
 
@@ -182,21 +185,6 @@ function lsfPriors(dfg::G)::Vector{Symbol} where G <: AbstractDFG
   return priors
 end
 
-"""
-   $(SIGNATURES)
-
-Variable nodes softtype information holding a variety of meta data associated with the type of variable stored in that node of the factor graph.
-
-Related
-
-getVariableType
-"""
-function getSofttype(vnd::VariableNodeData)
-  return vnd.softtype
-end
-function getSofttype(v::DFGVariable; solveKey::Symbol=:default)
-  return getSofttype(solverData(v, solveKey))
-end
 
 """
     $SIGNATURES
@@ -320,3 +308,135 @@ function lsfWho(dfg::AbstractDFG, type::Symbol)::Vector{Symbol}
     end
     return labels
 end
+
+
+"""
+    $SIGNATURES
+
+Find and return the closest timestamp from two sets of Tuples.  Also return the minimum delta-time (`::Millisecond`) and how many elements match from the two sets are separated by the minimum delta-time.
+"""
+function findClosestTimestamp(setA::Vector{Tuple{DateTime,T}},
+                              setB::Vector{Tuple{DateTime,S}}) where {S,T}
+  #
+  # build matrix of delta times, ranges on rows x vars on columns
+  DT = Array{Millisecond, 2}(undef, length(setA), length(setB))
+  for i in 1:length(setA), j in 1:length(setB)
+    DT[i,j] = setB[j][1] - setA[i][1]
+  end
+
+  DT .= abs.(DT)
+
+  # absolute time differences
+  # DTi = (x->x.value).(DT) .|> abs
+
+  # find the smallest element
+  mdt = minimum(DT)
+  corrs = findall(x->x==mdt, DT)
+
+  # return the closest timestamp, deltaT, number of correspondences
+  return corrs[1].I, mdt, length(corrs)
+end
+
+"""
+    $SIGNATURES
+
+Find and return nearest variable labels per delta time.  Function will filter on `regexFilter`, `tags`, and `solvable`.
+
+DevNotes:
+- TODO `number` should allow returning more than one for k-nearest matches.
+- Future versions likely will require some optimization around the internal `getVariable` call.
+  - Perhaps a dedicated/efficient `getVariableTimestamp` for all DFG flavors.
+
+Related
+
+ls, getVariableIds, findClosestTimestamp
+"""
+function findVariableNearTimestamp(dfg::AbstractDFG,
+                                   timest::DateTime,
+                                   regexFilter::Union{Nothing, Regex}=nothing;
+                                   tags::Vector{Symbol}=Symbol[],
+                                   solvable::Int=0,
+                                   warnDuplicate::Bool=true,
+                                   number::Int=1  )::Vector{Tuple{Vector{Symbol}, Millisecond}}
+  #
+  # get the variable labels based on filters
+  # syms = getVariableIds(dfg, regexFilter, tags=tags, solvable=solvable)
+  syms = getVariableIds(dfg, regexFilter, tags=tags, solvable=solvable)
+  # compile timestamps with label
+  # vars = map( x->getVariable(dfg, x), syms )
+  timeset = map(x->(getTimestamp(getVariable(dfg,x)), x), syms)
+  mask = BitArray{1}(undef, length(syms))
+  fill!(mask, true)
+
+  RET = Vector{Tuple{Vector{Symbol},Millisecond}}()
+  SYMS = Symbol[]
+  CORRS = 1
+  NUMBER = number
+  while 0 < CORRS + NUMBER
+    # get closest
+    link, mdt, corrs = findClosestTimestamp([(timest,0)], timeset[mask])
+    push!(SYMS, syms[link[2]])
+    mask[link[2]] = false
+    CORRS = corrs-1
+    # last match, done with this delta time
+    if corrs == 1
+      NUMBER -=  1
+      push!(RET, (deepcopy(SYMS),mdt))
+      SYMS = Symbol[]
+    end
+  end
+  # warn if duplicates found
+  # warnDuplicate && 1 < corrs ? @warn("getVariableNearTimestamp found more than one variable at $timestamp") :   nothing
+
+  return RET
+end
+
+"""
+$SIGNATURES
+
+Return the tags for a variable or factor.
+"""
+function getTags(dfg::InMemoryDFGTypes, sym::Symbol)
+  getFnc = isVariable(dfg,sym) ? getVariable : getFactor
+  getTags(getFnc(dfg, sym))
+end
+
+"""
+    $SIGNATURES
+
+Add tags to a variable or factor
+"""
+function addTags!(dfg::InMemoryDFGTypes, sym::Symbol, tags::Vector{Symbol})
+  union!(getTags(getFactor(fg, sym)), tags)
+end
+
+"""
+    $SIGNATURES
+
+Determine if the variable or factor neighbors have the `tags:;Vector{Symbol}`, and `matchAll::Bool`.
+"""
+function hasTags(dfg::InMemoryDFGTypes,
+                 sym::Symbol,
+                 tags::Vector{Symbol};
+                 matchAll::Bool=true  )::Bool
+  #
+  alltags = getTags(dfg, sym)
+  length(filter(x->x in alltags, tags)) >= (matchAll ? length(tags) : 1)
+end
+
+
+"""
+    $SIGNATURES
+
+Determine if the variable or factor neighbors have the `tags:;Vector{Symbol}`, and `matchAll::Bool`.
+"""
+function hasTagsNeighbors(dfg::InMemoryDFGTypes,
+                          sym::Symbol,
+                          tags::Vector{Symbol};
+                          matchAll::Bool=true  )::Bool
+  #
+  # assume only variables or factors are neighbors
+  getNeiFnc = isVariable(dfg, sym) ? getFactor : getVariable
+  alltags = union( (ls(dfg, sym) .|> x->getTags(getNeiFnc(dfg,x)))... )
+  length(filter(x->x in alltags, tags)) >= (matchAll ? length(tags) : 1)
+end

src/CommonAccessors.jl

@@ -0,0 +1,111 @@
+# Common get and set methods
+
+# NOTE this could be reduced with macros and function generation to even less code.
+
+export getLabel, label
+export getTags, setTags!, tags
+export getTimestamp, setTimestamp!, timestamp
+export getInternalId, internalId
+
+
+const DataLevel0 = Union{VariableDataLevel0, FactorDataLevel0}
+const DataLevel1 = Union{VariableDataLevel1, FactorDataLevel1}
+const DataLevel2 = Union{VariableDataLevel2, FactorDataLevel2}
+
+
+"""
+$SIGNATURES
+
+Return the label for a variable or factor.
+"""
+getLabel(v::DataLevel0) = v.label
+
+"""
+$SIGNATURES
+
+Return the label for a variable or factor.
+
+DEPRECATED label -> getLabel
+"""
+function label(v::DataLevel0)
+  @warn "Deprecated label, use getLabel instead."
+  getLabel(v)
+end
+
+
+"""
+$SIGNATURES
+
+Return the tags for a variable.
+"""
+getTags(v::DataLevel0) = v.tags
+
+
+"""
+$SIGNATURES
+
+Return the tags for a variable.
+
+DEPRECATED, tags -> getTags
+"""
+function tags(v::DataLevel0)
+  @warn "tags deprecated, use getTags instead"
+  getTags(v)
+end
+
+"""
+$SIGNATURES
+
+Set the tags for a factor.
+"""
+function setTags!(f::DataLevel0, tags::Vector{Symbol})
+  resize!(f.tags, length(tags))
+  f.tags .= tags
+end
+
+
+"""
+$SIGNATURES
+
+Get the timestamp from a DFGFactor object.
+"""
+getTimestamp(v::DataLevel1) = v.timestamp
+
+"""
+$SIGNATURES
+
+Get the timestamp from a DFGFactor object.
+
+DEPRECATED -> use getTimestamp instead.
+"""
+function timestamp(v::DataLevel1)
+    @warn "timestamp deprecated, use getTimestamp instead"
+    getTimestamp(v)
+end
+
+"""
+$SIGNATURES
+
+Set the timestamp of a DFGFactor object.
+"""
+setTimestamp!(v::DataLevel1, ts::DateTime) = v.timestamp = ts
+
+
+"""
+$SIGNATURES
+
+Return the internal ID for a variable.
+"""
+getInternalId(v::DataLevel1) = v._internalId
+
+"""
+$SIGNATURES
+
+Return the internal ID for a variable.
+
+DEPRECATED, internalId -> getInternalId
+"""
+function internalId(v::DataLevel1)
+    @warn "Deprecated interalId, use getInternalId instead."
+    getInternalId(v)
+end

src/DistributedFactorGraphs.jl

@@ -22,7 +22,8 @@ export DFGNode, DFGVariable, DFGFactor, AbstractDFGVariable, AbstractDFGFactor
 export InferenceType, PackedInferenceType, FunctorInferenceType, InferenceVariable, ConvolutionObject
 export FunctorSingleton, FunctorPairwise, FunctorPairwiseMinimize
 export getMaxPPE, getMeanPPE, getSuggestedPPE
-export label, timestamp, setTimestamp!, tags, setTags!, estimates, estimate, data, softtype, solverData, getData, solverDataDict, setSolverData, setSolverData!, internalId, smallData, setSmallData!, bigData
+export timestamp # DEPRECATED
+export label, getTimestamp, setTimestamp!, tags, setTags!, estimates, estimate, data, softtype, solverData, getData, solverDataDict, setSolverData, setSolverData!, internalId, smallData, setSmallData!, bigData
 export DFGVariableSummary, DFGFactorSummary, AbstractDFGSummary
 export addBigDataEntry!, getBigDataEntry, updateBigDataEntry!, deleteBigDataEntry!, getBigDataEntries, getBigDataKeys
 
@@ -78,6 +79,7 @@ export
 include("services/AbstractDFG.jl")
 include("services/DFGVariable.jl")
 include("services/DFGFactor.jl")
+include("CommonAccessors.jl")
 include("services/CompareUtils.jl")
 
 # Include the Graphs.jl API.

src/entities/DFGFactor.jl

@@ -31,20 +31,30 @@ mutable struct GenericFunctionNodeData{T, S}
 end
 
 """
-    $(SIGNATURES)
+    $(TYPEDEF)
+
 Fundamental structure for a DFG factor.
 """
 mutable struct DFGFactor{T, S} <: AbstractDFGFactor
     label::Symbol
     tags::Vector{Symbol}
     data::GenericFunctionNodeData{T, S}
     solvable::Int
+    timestamp::DateTime
     _internalId::Int64
     _variableOrderSymbols::Vector{Symbol}
-    DFGFactor{T, S}(label::Symbol) where {T, S} = new{T, S}(label, Symbol[], GenericFunctionNodeData{T, S}(), 0, 0, Symbol[])
-    DFGFactor{T, S}(label::Symbol, _internalId::Int64) where {T, S} = new{T, S}(label, Symbol[], GenericFunctionNodeData{T, S}(), 0, _internalId, Symbol[])
+    # TODO back to front ts and _internalId for legacy reasons
+    DFGFactor{T, S}(label::Symbol, _internalId::Int64=0, timestamp::DateTime=now()) where {T, S} = new{T, S}(label, Symbol[], GenericFunctionNodeData{T, S}(), 0, timestamp, 0, Symbol[])
+    # DFGFactor{T, S}(label::Symbol, _internalId::Int64) where {T, S} = new{T, S}(label, Symbol[], GenericFunctionNodeData{T, S}(), 0, now(), _internalId, Symbol[])
 end
 
+"""
+    $(SIGNATURES)
+
+Convenience constructor for DFG factor.
+"""
+DFGFactor(label::Symbol; tags::Vector{Symbol}=Symbol[], data::GenericFunctionNodeData{T, S}=GenericFunctionNodeData{T, S}(), solvable::Int=0, timestamp::DateTime=now(), _internalId::Int64=0, _variableOrderSymbols::Vector{Symbol}=Symbol[]) where {T, S} = DFGFactor{T,S}(label,tags,data,solvable,timestamp,_internalId,_variableOrderSymbols)
+
 # Simply for convenience - don't export
 const PackedFunctionNodeData{T} = GenericFunctionNodeData{T, <: AbstractString}
 PackedFunctionNodeData(x1, x2, x3, x4, x5::S, x6::T, x7::String="", x8::Vector{Int}=Int[], x9::Int=0) where {T <: PackedInferenceType, S <: AbstractString} = GenericFunctionNodeData(x1, x2, x3, x4, x5, x6, x7, x8, x9)
@@ -76,38 +86,14 @@ end
 #NOTE I feel like a want to force a variableOrderSymbols
 SkeletonDFGFactor(label::Symbol, variableOrderSymbols::Vector{Symbol} = Symbol[]) = SkeletonDFGFactor(label, Symbol[], variableOrderSymbols)
 
+
+
 # Accessors
 
 const FactorDataLevel0 = Union{DFGFactor, DFGFactorSummary, SkeletonDFGFactor}
 const FactorDataLevel1 = Union{DFGFactor, DFGFactorSummary}
+const FactorDataLevel2 = Union{DFGFactor}
 
-"""
-$SIGNATURES
-
-Return the label for a factor.
-"""
-label(f::FactorDataLevel0) = f.label
-
-"""
-$SIGNATURES
-
-Return the tags for a variable.
-"""
-tags(f::FactorDataLevel0) = f.tags
-
-"""
-$SIGNATURES
-
-Set the tags for a factor.
-"""
-setTag!s(f::FactorDataLevel0, tags::Vector{Symbol}) = f.tags = tags
-
-"""
-$SIGNATURES
-
-Return the internal ID for a variable.
-"""
-internalId(f::FactorDataLevel1) = f._internalId
 
 """
     $SIGNATURES