Merge branch 'master' of https://github.com/JuliaRobotics/DistributedFactorGraphs.jl into 4Q19/poc/v0_6

Author: GearsAD

src/DistributedFactorGraphs.jl

@@ -35,6 +35,7 @@ export DFGNodeParams
 export SkeletonDFGVariable, SkeletonDFGFactor
 export timestamp # DEPRECATED
 export label, getTimestamp, setTimestamp!, tags, setTags!, data, softtype, solverData, getData, solverDataDict, setSolverData, setSolverData!, internalId, smallData, setSmallData!, bigData
+export getSolvedCount, isSolved, setSolvedCount!
 export DFGVariableSummary, DFGFactorSummary, AbstractDFGSummary
 export getNeighborhood, getSubgraph, getSubgraphAroundNode
 

src/entities/DFGVariable.jl

@@ -1,4 +1,3 @@
-
 include("DFGVariable/PackedVariableNodeData.jl")
 include("DFGVariable/PointParametricEst.jl")
 include("DFGVariable/VariableNodeData.jl")

src/entities/DFGVariable/PackedVariableNodeData.jl

@@ -23,6 +23,7 @@ mutable struct PackedVariableNodeData
     ismargin::Bool
     dontmargin::Bool
     solveInProgress::Int
+    solvedCount::Int
     PackedVariableNodeData() = new()
     PackedVariableNodeData(x1::Vector{Float64},
                          x2::Int,
@@ -39,5 +40,6 @@ mutable struct PackedVariableNodeData
                          x13::Float64,
                          x14::Bool,
                          x15::Bool,
-                         x16::Int) = new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16)
+                         x16::Int,
+                         solvedCount::Int) = new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16, solvedCount)
 end

src/entities/DFGVariable/VariableNodeData.jl

@@ -21,6 +21,7 @@ mutable struct VariableNodeData{T<:InferenceVariable}
     ismargin::Bool
     dontmargin::Bool
     solveInProgress::Int
+    solvedCount::Int
     VariableNodeData{T}() where {T <:InferenceVariable} =
     new{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], T(), false, 0.0, false, false, 0)
     VariableNodeData{T}(val::Array{Float64,2},
@@ -35,11 +36,12 @@ mutable struct VariableNodeData{T<:InferenceVariable}
                         inferdim::Float64,
                         ismargin::Bool,
                         dontmargin::Bool,
-                        solveInProgress::Int=0) where T <: InferenceVariable =
+                        solveInProgress::Int=0,
+                        solvedCount::Int=0) where T <: InferenceVariable =
                             new{T}(val,bw,BayesNetOutVertIDs,dimIDs,dims,
                                    eliminated,BayesNetVertID,separator,
                                    softtype::T,initialized,inferdim,ismargin,
-                                   dontmargin, solveInProgress)
+                                   dontmargin, solveInProgress, solvedCount)
 end
 
 VariableNodeData(val::Array{Float64,2},
@@ -54,11 +56,12 @@ VariableNodeData(val::Array{Float64,2},
                  inferdim::Float64,
                  ismargin::Bool,
                  dontmargin::Bool,
-                 solveInProgress::Int=0) where T <: InferenceVariable =
+                 solveInProgress::Int=0,
+                 solvedCount::Int=0) where T <: InferenceVariable =
                    VariableNodeData{T}(val,bw,BayesNetOutVertIDs,dimIDs,dims,
                                        eliminated,BayesNetVertID,separator,
                                        softtype::T,initialized,inferdim,ismargin,
-                                       dontmargin, solveInProgress)
+                                       dontmargin, solveInProgress, solvedCount)
 #
 VariableNodeData(softtype::T) where T <: InferenceVariable =
-    VariableNodeData{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], softtype, false, 0.0, false, false, 0)
+    VariableNodeData{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], softtype, false, 0.0, false, false, 0, 0)

src/services/DFGVariable.jl

@@ -381,3 +381,43 @@ Get the variable ordering for this factor.
 Should be equivalent to getNeighbors unless something was deleted in the graph.
 """
 getVariableOrder(fct::DFGFactor)::Vector{Symbol} = fct._variableOrderSymbols
+
+"""
+    $SIGNATURES
+
+Get the number of times a variable has been inferred -- i.e. `solvedCount`.
+
+Related
+
+isSolved, setSolvedCount!
+"""
+getSolvedCount(v::VariableNodeData) = v.solvedCount
+getSolvedCount(v::VariableDataLevel2, solveKey::Symbol=:default) = getSolverData(v, solveKey) |> getSolvedCount
+getSolvedCount(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol=:default) = getSolvedCount(getVariable(dfg, sym), solveKey)
+
+"""
+    $SIGNATURES
+
+Boolean on whether the variable has been solved.
+
+Related
+
+getSolved, setSolved!
+"""
+isSolved(v::VariableNodeData) = 0 < v.solvedCount
+isSolved(v::VariableDataLevel2, solveKey::Symbol=:default) = getSolverData(v, solveKey) |> isSolved
+isSolved(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol=:default) = isSolved(getVariable(dfg, sym), solveKey)
+
+
+"""
+    $SIGNATURES
+
+Update/set the `solveCount` value.
+
+Related
+
+getSolved, isSolved
+"""
+setSolvedCount!(v::VariableNodeData, val::Int) = v.solvedCount = val
+setSolvedCount!(v::VariableDataLevel2, val::Int, solveKey::Symbol=:default) = setSolvedCount!(solverData(v, solveKey), val)
+setSolvedCount!(dfg::AbstractDFG, sym::Symbol, val::Int, solveKey::Symbol=:default) = setSolvedCount!(getVariable(dfg, sym), val, solveKey)

test/fileDFGTests.jl

@@ -1,3 +1,7 @@
+using DistributedFactorGraphs
+using IncrementalInference
+dfg = LightDFG{NoSolverParams}()
+
 @testset "FileDFG Tests" begin
 
     if typeof(dfg) <: CloudGraphsDFG
@@ -14,6 +18,7 @@
     verts[7].solvable = 1
     verts[8].solvable = 0
     getSolverData(verts[8]).solveInProgress = 1
+    setSolvedCount!(verts[1], 5)
     #call update to set it on cloud
     updateVariable!(dfg, verts[7])
     updateVariable!(dfg, verts[8])

test/interfaceTests.jl

@@ -255,6 +255,15 @@ end
     @test !isVariable(dfg, :doesntexist)
     @test !isFactor(dfg, :doesntexist)
 
+    # test solveCount for variable
+    @test !isSolved(v1)
+    @test getSolvedCount(v1) == 0
+    setSolvedCount!(v1, 1)
+    @test getSolvedCount(v1) == 1
+    @test isSolved(v1)
+    setSolvedCount!(dfg, getLabel(v1), 2)
+    @test getSolvedCount(dfg, getLabel(v1)) == 2
+
     # Session, robot, and user small data tests
     smallUserData = Dict{Symbol, String}(:a => "42", :b => "Hello")
     smallRobotData = Dict{Symbol, String}(:a => "43", :b => "Hello")