Merge pull request #783 from JuliaRobotics/21Q2/enh/valVectorT

towards val::Vector{T}

Author: dehann

Project.toml

@@ -19,6 +19,7 @@ Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 Requires = "ae029012-a4dd-5104-9daa-d747884805df"
 SHA = "ea8e919c-243c-51af-8825-aaa63cd721ce"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
+TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02"
@@ -34,16 +35,18 @@ ManifoldsBase = "0.11, 0.12"
 Neo4j = "2"
 Pkg = "1.4, 1.5"
 Reexport = "0.2, 0.3, 0.4, 0.5, 1"
-Requires = "0.5, 0.6, 0.7, 0.8, 0.9, 0.10, 1"
+Requires = "0.5, 1"
+TensorCast = "0.3.3, 0.4"
 TimeZones = "1.3.1"
 Unmarshal = "0.4"
 julia = "1.4"
 
 [extras]
 GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231"
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "GraphPlot", "Manifolds", "Pkg"]
+test = ["Test", "GraphPlot", "LinearAlgebra", "Manifolds", "Pkg"]

src/DistributedFactorGraphs.jl

@@ -28,6 +28,7 @@ using LinearAlgebra
 using SparseArrays
 using UUIDs
 using Pkg
+using TensorCast
 
 # used for @defVariable
 import ManifoldsBase
@@ -135,7 +136,10 @@ export getSolverData
 export getVariableType
 
 # VariableType functions
-export getDimension, getManifolds, getManifold, getPointType
+export getDimension, getManifold, getPointType
+export getPointIdentity, getPoint, getCoordinates
+
+export getManifolds # TODO Deprecate?
 
 # Small Data CRUD
 export SmallDataTypes, getSmallData, addSmallData!, updateSmallData!, deleteSmallData!, listSmallData, emptySmallData!

src/entities/DFGVariable.jl

@@ -16,58 +16,92 @@ Data container for solver-specific data.
 Fields:
 $(TYPEDFIELDS)
 """
-mutable struct VariableNodeData{T<:InferenceVariable}
-    val::Array{Float64,2}
-    bw::Array{Float64,2}
-    BayesNetOutVertIDs::Array{Symbol,1}
-    dimIDs::Array{Int,1} # Likely deprecate
+mutable struct VariableNodeData{T<:InferenceVariable, P}
+    val::Vector{P}
+    bw::Matrix{Float64}
+    BayesNetOutVertIDs::Vector{Symbol}
+    dimIDs::Vector{Int} # Likely deprecate
+
     dims::Int
     eliminated::Bool
     BayesNetVertID::Symbol #  Union{Nothing, }
-    separator::Array{Symbol,1}
+    separator::Vector{Symbol}
+
     variableType::T
     initialized::Bool
     inferdim::Float64
     ismargin::Bool
+
     dontmargin::Bool
     solveInProgress::Int
     solvedCount::Int
     solveKey::Symbol
+
     events::Dict{Symbol,Threads.Condition}
-    VariableNodeData{T}(; solveKey::Symbol=:default) where {T <:InferenceVariable} =
-    new{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], T(), false, 0.0, false, false, 0, 0, solveKey, Dict{Symbol,Threads.Condition}())
-    VariableNodeData{T}(val::Array{Float64,2},
-                        bw::Array{Float64,2},
-                        BayesNetOutVertIDs::Array{Symbol,1},
-                        dimIDs::Array{Int,1},
-                        dims::Int,eliminated::Bool,
-                        BayesNetVertID::Symbol,
-                        separator::Array{Symbol,1},
-                        variableType::T,
-                        initialized::Bool,
-                        inferdim::Float64,
-                        ismargin::Bool,
-                        dontmargin::Bool,
-                        solveInProgress::Int=0,
-                        solvedCount::Int=0,
-                        solveKey::Symbol=:default,
-                        events::Dict{Symbol,Threads.Condition}=Dict{Symbol,Threads.Condition}()) where T <: InferenceVariable =
-                            new{T}(val,bw,BayesNetOutVertIDs,dimIDs,dims,
-                                   eliminated,BayesNetVertID,separator,
-                                   variableType::T,initialized,inferdim,ismargin,
-                                   dontmargin, solveInProgress, solvedCount, solveKey, events)
+
+    # VariableNodeData{T,P}() = new{T,P}()
+    VariableNodeData{T,P}(w...)  where {T <:InferenceVariable, P} = new{T,P}(w...)
+    VariableNodeData{T,P}(;solveKey::Symbol=:default ) where {T <:InferenceVariable, P} = new{T,P}(
+            Vector{P}(), 
+            zeros(0,0), 
+            Symbol[], 
+            Int[], 
+            0, 
+            false, 
+            :NOTHING, 
+            Symbol[], 
+            T(), 
+            false, 
+            0.0, 
+            false, 
+            false, 
+            0, 
+            0, 
+            solveKey, 
+            Dict{Symbol,Threads.Condition}() )
+    #
 end
 
 ##------------------------------------------------------------------------------
 ## Constructors
 
-VariableNodeData(val::Array{Float64,2},
-                 bw::Array{Float64,2},
-                 BayesNetOutVertIDs::Array{Symbol,1},
-                 dimIDs::Array{Int,1},
-                 dims::Int,eliminated::Bool,
+VariableNodeData{T}(;solveKey::Symbol=:default ) where T <: InferenceVariable = VariableNodeData{T, getPointType(T)}(solveKey=solveKey)
+
+VariableNodeData(   val::Vector{P},
+                    bw::Matrix{<:Real},
+                    BayesNetOutVertIDs::AbstractVector{Symbol},
+                    dimIDs::AbstractVector{Int},
+                    dims::Int,
+                    eliminated::Bool,
+                    BayesNetVertID::Symbol,
+                    separator::Array{Symbol,1},
+                    variableType::T,
+                    initialized::Bool,
+                    inferdim::Float64,
+                    ismargin::Bool,
+                    dontmargin::Bool,
+                    solveInProgress::Int=0,
+                    solvedCount::Int=0,
+                    solveKey::Symbol=:default,
+                    events::Dict{Symbol,Threads.Condition}=Dict{Symbol,Threads.Condition}()
+                ) where {T <: InferenceVariable, P} = VariableNodeData{T,P}( 
+                                        val,bw,BayesNetOutVertIDs,dimIDs,dims,
+                                        eliminated,BayesNetVertID,separator,
+                                        variableType,initialized,inferdim,ismargin,
+                                        dontmargin, solveInProgress, solvedCount, solveKey, events  )
+#
+
+
+#
+
+VariableNodeData(val::Vector{P},
+                 bw::Matrix{<:Real},
+                 BayesNetOutVertIDs::AbstractVector{Symbol},
+                 dimIDs::AbstractVector{Int},
+                 dims::Int,
+                 eliminated::Bool,
                  BayesNetVertID::Symbol,
-                 separator::Array{Symbol,1},
+                 separator::AbstractVector{Symbol},
                  variableType::T,
                  initialized::Bool,
                  inferdim::Float64,
@@ -76,16 +110,26 @@ VariableNodeData(val::Array{Float64,2},
                  solveInProgress::Int=0,
                  solvedCount::Int=0,
                  solveKey::Symbol=:default
-                 ) where T <: InferenceVariable =
-                   VariableNodeData{T}(val,bw,BayesNetOutVertIDs,dimIDs,dims,
-                                       eliminated,BayesNetVertID,separator,
-                                       variableType::T,initialized,inferdim,ismargin,
-                                       dontmargin, solveInProgress, solvedCount,
-                                       solveKey)
-
-
-VariableNodeData(variableType::T; solveKey::Symbol=:default) where T <: InferenceVariable =
-    VariableNodeData{T}(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], variableType, false, 0.0, false, false, 0, 0, solveKey)
+                 ) where {T <: InferenceVariable, P} =
+                   VariableNodeData{T,P}(   val,bw,BayesNetOutVertIDs,dimIDs,dims,
+                                            eliminated,BayesNetVertID,separator,
+                                            variableType,initialized,inferdim,ismargin,
+                                            dontmargin, solveInProgress, solvedCount,
+                                            solveKey  )
+#
+
+function VariableNodeData(variableType::T; solveKey::Symbol=:default) where T <: InferenceVariable
+    #
+    # p0 = getPointIdentity(T)
+    P0 = Vector{getPointType(T)}()
+    # P0[1] = p0
+    BW = zeros(0,0)
+    # BW[1] = zeros(getDimension(T))
+    VariableNodeData(   P0, BW, Symbol[], Int[], 
+                        0, false, :NOTHING, Symbol[], 
+                        variableType, false, 0.0, false, 
+                        false, 0, 0, solveKey  )
+end
 
 ##==============================================================================
 ## PackedVariableNodeData.jl
@@ -237,7 +281,7 @@ struct DFGVariable{T<:InferenceVariable} <: AbstractDFGVariable
     ppeDict::Dict{Symbol, <: AbstractPointParametricEst}
     """Dictionary of solver data. May be a subset of all solutions if a solver key was specified in the get call.
     Accessors: [`addVariableSolverData!`](@ref), [`updateVariableSolverData!`](@ref), and [`deleteVariableSolverData!`](@ref)"""
-    solverDataDict::Dict{Symbol, VariableNodeData{T}}
+    solverDataDict::Dict{Symbol, <: VariableNodeData{T}}
     """Dictionary of small data associated with this variable.
     Accessors: [`getSmallData`](@ref), [`setSmallData!`](@ref)"""
     smallData::Dict{Symbol, SmallDataTypes}
@@ -261,10 +305,10 @@ function DFGVariable(label::Symbol, variableType::T;
             nstime::Nanosecond = Nanosecond(0),
             tags::Set{Symbol}=Set{Symbol}(),
             estimateDict::Dict{Symbol, <: AbstractPointParametricEst}=Dict{Symbol, MeanMaxPPE}(),
-            solverDataDict::Dict{Symbol, VariableNodeData{T}}=Dict{Symbol, VariableNodeData{T}}(),
+            solverDataDict::Dict{Symbol, VariableNodeData{T,P}}=Dict{Symbol, VariableNodeData{T,getPointType(T)}}(),
             smallData::Dict{Symbol, SmallDataTypes}=Dict{Symbol, SmallDataTypes}(),
             dataDict::Dict{Symbol, AbstractDataEntry}=Dict{Symbol,AbstractDataEntry}(),
-            solvable::Int=1) where {T <: InferenceVariable}
+            solvable::Int=1) where {T <: InferenceVariable, P}
 
     if timestamp isa DateTime
         DFGVariable{T}(label, ZonedDateTime(timestamp, localzone()), nstime, tags, estimateDict, solverDataDict, smallData, dataDict, Ref(solvable))
@@ -280,12 +324,13 @@ function DFGVariable(label::Symbol,
             tags::Set{Symbol}=Set{Symbol}(),
             estimateDict::Dict{Symbol, <: AbstractPointParametricEst}=Dict{Symbol, MeanMaxPPE}(),
             smallData::Dict{Symbol, SmallDataTypes}=Dict{Symbol, SmallDataTypes}(),
-            dataDict::Dict{Symbol, AbstractDataEntry}=Dict{Symbol,AbstractDataEntry}(),
+            dataDict::Dict{Symbol, <: AbstractDataEntry}=Dict{Symbol,AbstractDataEntry}(),
             solvable::Int=1) where {T <: InferenceVariable}
+    #
     if timestamp isa DateTime
-        DFGVariable{T}(label, ZonedDateTime(timestamp, localzone()), nstime, tags, estimateDict, Dict{Symbol, VariableNodeData{T}}(:default=>solverData), smallData, dataDict, Ref(solvable))
+        DFGVariable{T}(label, ZonedDateTime(timestamp, localzone()), nstime, tags, estimateDict, Dict{Symbol, VariableNodeData{T, getPointType(T)}}(:default=>solverData), smallData, dataDict, Ref(solvable))
     else
-        DFGVariable{T}(label, timestamp, nstime, tags, estimateDict, Dict{Symbol, VariableNodeData{T}}(:default=>solverData), smallData, dataDict, Ref(solvable))
+        DFGVariable{T}(label, timestamp, nstime, tags, estimateDict, Dict{Symbol, VariableNodeData{T, getPointType(T)}}(:default=>solverData), smallData, dataDict, Ref(solvable))
     end
 end
 

src/services/CompareUtils.jl

@@ -239,10 +239,19 @@ function compareFactor(A::DFGFactor,
   TP =  compareAll(A, B, skip=union([:attributes;:solverData;:_variableOrderSymbols],skip), show=show)
   # TP = TP & compareAll(A.attributes, B.attributes, skip=[:data;], show=show)
   TP = TP & compareAllSpecial(getSolverData(A), getSolverData(B), skip=union([:fnc], skip), show=show)
+  if :fnc in skip
+    return TP
+  end
   TP = TP & compareAllSpecial(getSolverData(A).fnc, getSolverData(B).fnc, skip=union([:cpt;:measurement;:params;:varidx;:threadmodel], skip), show=show)
+  if !(:measurement in skip)
   TP = TP & (skipsamples || compareAll(getSolverData(A).fnc.measurement, getSolverData(B).fnc.measurement, show=show, skip=skip))
-  TP = TP & (skipcompute || compareAll(getSolverData(A).fnc.params, getSolverData(B).fnc.params, show=show, skip=skip))
-  TP = TP & (skipcompute || compareAll(getSolverData(A).fnc.varidx, getSolverData(B).fnc.varidx, show=show, skip=skip))
+  end
+  if !(:params in skip)
+    TP = TP & (skipcompute || compareAll(getSolverData(A).fnc.params, getSolverData(B).fnc.params, show=show, skip=skip))
+  end
+  if !(:varidx in skip)
+    TP = TP & (skipcompute || compareAll(getSolverData(A).fnc.varidx, getSolverData(B).fnc.varidx, show=show, skip=skip))
+  end
 
   return TP
 end
@@ -339,11 +348,12 @@ Related:
 
 `compareFactorGraphs`, `compareSimilarVariables`, `compareAllVariables`, `ls`.
 """
-function compareSimilarFactors(fgA::G1,
-                               fgB::G2;
-                               skipsamples::Bool=true,
-                               skipcompute::Bool=true,
-                               show::Bool=true  )::Bool where {G1 <: AbstractDFG, G2 <: AbstractDFG}
+function compareSimilarFactors( fgA::G1,
+                                fgB::G2;
+                                skipsamples::Bool=true,
+                                skipcompute::Bool=true,
+                                skip::AbstractVector{Symbol}=Symbol[],
+                                show::Bool=true  ) where {G1 <: AbstractDFG, G2 <: AbstractDFG}
   #
   xlA = listFactors(fgA)
   xlB = listFactors(fgB)
@@ -354,7 +364,8 @@ function compareSimilarFactors(fgA::G1,
 
   # compare the common set
   for var in xlAB
-    TP = TP && compareFactor(getFactor(fgA, var), getFactor(fgB, var), skipsamples=skipsamples, skipcompute=skipcompute, show=show)
+    TP = TP && compareFactor( getFactor(fgA, var), getFactor(fgB, var), 
+                              skipsamples=skipsamples, skipcompute=skipcompute, skip=skip, show=show)
   end
 
   # return comparison result
@@ -376,12 +387,12 @@ Related:
 
 `compareSimilarVariables`, `compareSimilarFactors`, `compareAllVariables`, `ls`.
 """
-function compareFactorGraphs(fgA::G1,
-                             fgB::G2;
-                             skipsamples::Bool=true,
-                             skipcompute::Bool=true,
-                             skip::Vector{Symbol}=Symbol[],
-                             show::Bool=true  )::Bool where {G1 <: AbstractDFG, G2 <: AbstractDFG}
+function compareFactorGraphs( fgA::G1,
+                              fgB::G2;
+                              skipsamples::Bool=true,
+                              skipcompute::Bool=true,
+                              skip::Vector{Symbol}=Symbol[],
+                              show::Bool=true  ) where {G1 <: AbstractDFG, G2 <: AbstractDFG}
   #
   skiplist = Symbol[:g;:bn;:IDs;:fIDs;:id;:nodeIDs;:factorIDs;:fifo;:solverParams; :factorOperationalMemoryType]
   skiplist = union(skiplist, skip)

src/services/CustomPrinting.jl

@@ -53,7 +53,8 @@ function printVariable( io::IO, vert::DFGVariable;
         println(ioc, "      initilized:        ", isInitialized(vert, :default))
         println(ioc, "      marginalized:      ", isMarginalized(vert, :default))
         println(ioc, "      size bel. samples: ", size(vnd.val))
-        println(ioc, "      kde bandwidths:    ", round.((vnd.bw)[:,1], digits=4))
+        print(ioc, "      kde bandwidths:    ")
+        0 < length(vnd.bw) ? println(ioc, round.(vnd.bw[1], digits=4)) : nothing
         printstyled(ioc, "     VNDs: ",bold=true)
         println(ioc, solk[smsk], 4<lsolk ? "..." : "")
         printstyled(ioc, "  # PPE solveKeys=    ($(length(getPPEDict(vert))))", bold=true)