Merge pull request #719 from JuliaRobotics/feat/4Q20/shortpinit

xtra filter or shortest path, initialized

Author: dehann

src/LightDFG/services/LightDFG.jl

@@ -398,7 +398,8 @@ function findShortestPathDijkstra(  dfg::LightDFG,
                                     tagsFactors::Vector{Symbol}=Symbol[],
                                     typeVariables::Union{Nothing, <:AbstractVector}=nothing,
                                     typeFactors::Union{Nothing, <:AbstractVector}=nothing,
-                                    solvable::Int=0  )
+                                    solvable::Int=0,
+                                    initialized::Union{Nothing,Bool}=nothing  )
     #
     # helper function to filter on vector of types
     function _filterTypeList(thelist::Vector{Symbol}, typeList, listfnc=x->ls(dfg, x) )
@@ -417,6 +418,7 @@ function findShortestPathDijkstra(  dfg::LightDFG,
                 0 < length(tagsFactors)    ||
                 typeVariables !== nothing  ||
                 typeFactors !== nothing    ||
+                initialized !== nothing    ||
                 solvable != 0
     #
     dfg_ = if duplicate
@@ -425,13 +427,23 @@ function findShortestPathDijkstra(  dfg::LightDFG,
         fctList = lsf(dfg, regexFactors, tags=tagsFactors, solvable=solvable)
         varList = typeVariables !== nothing ? _filterTypeList(varList, typeVariables) : varList
         fctList = typeFactors !== nothing ? _filterTypeList(fctList, typeFactors, x->lsf(dfg, x)) : fctList
+        varList = if initialized !== nothing
+            initmask = isInitialized.(dfg, varList) .== initialized
+            varList[initmask]
+        else
+            varList
+        end
         deepcopyGraph(typeof(dfg), dfg, varList, fctList)
     else
         # no filter can be used directly
         dfg
     end
 
     # LightDFG internally uses Integers 
+    if !haskey(dfg_.g.labels, from) || !haskey(dfg_.g.labels, from)
+        # assume filters excluded either `to` or `from` and hence no shortest path
+        return Symbol[]
+    end
     frI = dfg_.g.labels[from]
     toI = dfg_.g.labels[to]
 

src/services/DFGVariable.jl

@@ -136,23 +136,24 @@ isSolved(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol=:default) = isSolved(ge
 """
     $SIGNATURES
 
-Returns state of vertex data `.initialized` flag.
+Returns state of variable data `.initialized` flag.
 
 Notes:
 - used by both factor graph variable and Bayes tree clique logic.
 """
-function isInitialized(var::DFGVariable, key::Symbol=:default)::Bool
-      data = getSolverData(var, key)
-      if data == nothing
-          #TODO we still have a mixture of 2 error behaviours
-        return false
-      else
-          return data.initialized
-    end
+function isInitialized(var::DFGVariable, key::Symbol=:default)
+  data = getSolverData(var, key)
+  if data === nothing
+    #TODO we still have a mixture of 2 error behaviours
+      # DF, not sure I follow the error here?
+  return false
+  else
+    return data.initialized
+  end
 end
 
-function isInitialized(dfg::AbstractDFG, label::Symbol, key::Symbol=:default)::Bool
-  return isInitialized(getVariable(dfg, label), key)
+function isInitialized(dfg::AbstractDFG, label::Symbol, key::Symbol=:default)
+  return isInitialized(getVariable(dfg, label), key)::Bool
 end