Merge pull request #268 from JuliaRobotics/feat/1Q20/solvedcount

add solveCount to DFGVariable

Author: GearsAD

src/DistributedFactorGraphs.jl

@@ -23,6 +23,7 @@ export InferenceType, PackedInferenceType, FunctorInferenceType, InferenceVariab
 export FunctorSingleton, FunctorPairwise, FunctorPairwiseMinimize
 export getMaxPPE, getMeanPPE, getSuggestedPPE, getVariablePPE, getPPE, getVariablePPEs, getPPEs #, getEstimates
 export timestamp # DEPRECATED
+export getSolved, isSolved, setSolved!
 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

src/entities/DFGVariable.jl

@@ -22,8 +22,9 @@ 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)
+  new{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], T(), false, 0.0, false, false, 0, 0)
   VariableNodeData{T}(val::Array{Float64,2},
                       bw::Array{Float64,2},
                       BayesNetOutVertIDs::Array{Symbol,1},
@@ -36,11 +37,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},
@@ -55,20 +57,21 @@ 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)
 
 function VariableNodeData()
     st = stacktrace()
     @warn "VariableNodeData() is deprecated please use VariableNodeData{T}() or VariableNodeData(softtype::T) where T <: InferenceVariable. Enable DEBUG logging for stack trace."
     @debug st
-    VariableNodeData{InferenceVariable}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], SingletonInferenceVariable(), false, 0.0, false, false, 0)
+    VariableNodeData{InferenceVariable}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], SingletonInferenceVariable(), false, 0.0, false, false, 0, 0)
 end
 
 
@@ -93,6 +96,7 @@ mutable struct PackedVariableNodeData
   ismargin::Bool
   dontmargin::Bool
   solveInProgress::Int
+  solvedCount::Int
   PackedVariableNodeData() = new()
   PackedVariableNodeData(x1::Vector{Float64},
                          x2::Int,
@@ -109,7 +113,8 @@ 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
 
 # AbstractPointParametricEst interface
@@ -184,7 +189,7 @@ mutable struct DFGVariableSummary <: AbstractDFGVariable
     timestamp::DateTime
     tags::Vector{Symbol}
     ppeDict::Dict{Symbol, <:AbstractPointParametricEst}
-    softtypename::Symbol
+    softtypename::Symbol # should be removed
     _internalId::Int64
 end
 
@@ -274,8 +279,6 @@ TODO, DO NOT USE v.softtypename in DFGVariableSummary
 getSofttype(v::DFGVariableSummary)::Symbol = v.softtypename
 
 
-
-
 """
     $SIGNATURES
 
@@ -298,6 +301,47 @@ Get solver data dictionary for a variable.
 """
 solverDataDict(v::DFGVariable) = v.solverDataDict
 
+"""
+    $SIGNATURES
+
+Get the number of times a variable has been inferred -- i.e. `solvedCount`.
+
+Related
+
+isSolved, setSolved!
+"""
+getSolved(v::VariableNodeData) = v.solvedCount
+getSolved(v::VariableDataLevel2, solveKey::Symbol=:default) = solverData(v, solveKey) |> getSolved
+getSolved(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol=:default) = getSolved(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) = solverData(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
+"""
+setSolved!(v::VariableNodeData, val::Int) = v.solvedCount = val
+setSolved!(v::VariableDataLevel2, val::Int, solveKey::Symbol=:default) = setSolved!(solverData(v, solveKey), val)
+setSolved!(dfg::AbstractDFG, sym::Symbol, val::Int, solveKey::Symbol=:default) = setSolved!(getVariable(dfg, sym), solveKey, val)
+
+
 """
 $SIGNATURES
 

src/services/DFGVariable.jl

@@ -63,7 +63,8 @@ function pack(dfg::G, d::VariableNodeData)::PackedVariableNodeData where G <: Ab
                                 d.inferdim,
                                 d.ismargin,
                                 d.dontmargin,
-                                d.solveInProgress)
+                                d.solveInProgress,
+                                d.solvedCount)
 end
 
 function unpack(dfg::G, d::PackedVariableNodeData)::VariableNodeData where G <: AbstractDFG

test/interfaceTests.jl

@@ -211,6 +211,14 @@ end
     @test !isVariable(dfg, :doesntexist)
     @test !isFactor(dfg, :doesntexist)
 
+    # test solveCount for variable
+    # vari = getVariable(dfg, v1)
+    @test !isSolved(v1)
+    @test getSolved(v1) == 0
+    setSolved!(v1, 1)
+    @test getSolved(v1) == 1
+    @test isSolved(v1)
+
     # Session, robot, and user small data tests
     smallUserData = Dict{Symbol, String}(:a => "42", :b => "Hello")
     smallRobotData = Dict{Symbol, String}(:a => "43", :b => "Hello")