Merge pull request #714 from JuliaRobotics/maint/4Q20/betterdjikstra

isPathFactorsHomogeneous, better path

Author: dehann

Project.toml

@@ -1,6 +1,6 @@
 name = "DistributedFactorGraphs"
 uuid = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
-version = "0.11.1"
+version = "0.11.2"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

src/DistributedFactorGraphs.jl

@@ -238,6 +238,7 @@ export toDot, toDotFile
 
 # shortest path
 export findShortestPathDijkstra
+export isPathFactorsHomogeneous
 
 # Comparisons
 export

src/LightDFG/services/LightDFG.jl

@@ -374,41 +374,31 @@ using IncrementalInference
 fg = generateCanonicalFG_Kaess()
 
 @show path = findShortestPathDijkstra(fg, :x1, :x3)
-@show isVariable.(path)
-@show isFactor.(path)
+@show isVariable.(fg, path)
+@show isFactor.(fg, path)
 ```
 
 DevNotes
 - # TODO expand to other AbstractDFG entities.
 
 Related
 
-[findFactorsBetweenNaive](@ref), `LightGraphs.dijkstra_shortest_paths`
+[`findFactorsBetweenNaive`](@ref), `LightGraphs.dijkstra_shortest_paths`
 """
 function findShortestPathDijkstra(  dfg::LightDFG, 
                                     from::Symbol,
                                     to::Symbol  )
-  #
-  # LightDFG internally uses Integers 
-  frI = dfg.g.labels[from]
-  toI = dfg.g.labels[to]
-  
-  path = LightGraphs.dijkstra_shortest_paths(dfg.g.graph, [toI;])
-  # path = LightGraphs.enumerate_paths(path_state, toI)
-  
-  # assemble into the list
-  dijkpath = Symbol[]
-  # test for connectivity
-  if path.dists[frI] < Inf
-    cursor = frI
-    push!(dijkpath, dfg.g.labels[cursor])
-    # walk the path
-    while cursor != toI
-      cursor = path.parents[cursor]
-      push!(dijkpath, dfg.g.labels[cursor])
-    end
-  end
+    #
+    # LightDFG internally uses Integers 
+    frI = dfg.g.labels[from]
+    toI = dfg.g.labels[to]
+
+    # get shortest path from graph provider
+    path_state = LightGraphs.dijkstra_shortest_paths(dfg.g.graph, [frI;])
+    path = LightGraphs.enumerate_paths(path_state, toI)
+    dijkpath = map(x->dfg.g.labels[x], path)
+
+    # return the list of symbols
+    return dijkpath
+end
 
-  # return the list of symbols
-  return dijkpath
-end

src/services/AbstractDFG.jl

@@ -1085,6 +1085,20 @@ function findFactorsBetweenNaive(   dfg::AbstractDFG,
   return fctlist
 end
 
+"""
+    $SIGNATURES
+Return (::Bool,::Vector{TypeName}) of types between two nodes in the factor graph 
+
+Related
+
+[`LightDFG.findShortestPathDijkstra`](@ref)
+"""
+function isPathFactorsHomogeneous(dfg::AbstractDFG, from::Symbol, to::Symbol)
+    pth = intersect(findShortestPathDijkstra(dfg, from, to), lsf(dfg))
+    types = getFactorType.(dfg, pth) .|> typeof .|> x->(x).name
+    utyp = unique(types)
+    (length(utyp) == 1), utyp
+end
 
 
 ##==============================================================================