Merge pull request #894 from JuliaRobotics/22Q3/perf/var_point_param

Point type parameter in DFGVariable and vnd.covar

Author: Affie

Project.toml

@@ -21,6 +21,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"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 StructTypes = "856f2bd8-1eba-4b0a-8007-ebc267875bd4"
 TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"

src/DistributedFactorGraphs.jl

@@ -41,6 +41,7 @@ export AbstractManifold, manifold_dimension
 
 import RecursiveArrayTools: ArrayPartition
 export ArrayPartition
+using StaticArrays
 
 import Base: getindex
 

src/entities/DFGVariable.jl

@@ -13,10 +13,15 @@ $(TYPEDEF)
 Data container for solver-specific data.
 
   ---
+T: Variable type, such as Position1, or RoME.Pose2, etc.
+P: Variable point type, the type of the manifold point.
+N: Manifold dimension.
 Fields:
 $(TYPEDFIELDS)
 """
-Base.@kwdef mutable struct VariableNodeData{T<:InferenceVariable, P}
+Base.@kwdef mutable struct VariableNodeData{T<:InferenceVariable, P, N}
+    "DEPRECATED remove in DFG v0.22"
+    variableType::T=T() #tricky deprecation, also change covar to using N and not variableType
     """
     Globally unique identifier.
     """
@@ -26,37 +31,35 @@ Base.@kwdef mutable struct VariableNodeData{T<:InferenceVariable, P}
     """
     val::Vector{P} = Vector{P}()
     """
-    Common kernel bandwith parameter used with ManifoldKernelDensity, and as legacy also stores covariance until a dedicated field is created for parametric case.
+    Common kernel bandwith parameter used with ManifoldKernelDensity, see field `covar` for the parametric covariance.
     """
     bw::Matrix{Float64} = zeros(0,0)
+    "Parametric (Gaussian) covariance."
+    covar::Vector{SMatrix{N, N, Float64}} = SMatrix{getDimension(variableType), getDimension(variableType), Float64}[]
     BayesNetOutVertIDs::Vector{Symbol} = Symbol[]
     dimIDs::Vector{Int} = Int[] # TODO Likely deprecate
 
-    dims::Int = 0
+    dims::Int = getDimension(variableType) #TODO should we deprecate in favor of N
     """
-    Flag used by junction (Bayes) tree construction algorith to know whether this variable has yet been included in the tree construction.
+    Flag used by junction (Bayes) tree construction algorithm to know whether this variable has yet been included in the tree construction.
     """
     eliminated::Bool = false
     BayesNetVertID::Symbol = :NOTHING #  Union{Nothing, }
     separator::Vector{Symbol} = Symbol[]
     """
-    Variables each have a type, such as Position1, or RoME.Pose2, etc.
-    """
-    variableType::T = T()
-    """
     False if initial numerical values are not yet available or stored values are not ready for further processing yet.
     """
     initialized::Bool = false
     """
     Stores the amount information (per measurement dimension) captured in each coordinate dimension.
     """
-    infoPerCoord::Vector{Float64} = Float64[0.0;]
+    infoPerCoord::Vector{Float64} = zeros(getDimension(variableType))
     """
     Should this variable solveKey be treated as marginalized in inference computations.
     """
     ismargin::Bool = false
     """
-    Shoudl this variable solveKey always be kept fluid and not be automatically marginalized.
+    Should this variable solveKey always be kept fluid and not be automatically marginalized.
     """
     dontmargin::Bool = false
     """
@@ -68,7 +71,7 @@ Base.@kwdef mutable struct VariableNodeData{T<:InferenceVariable, P}
     """
     solvedCount::Int = 0
     """
-    solveKey identifier associated with thsi VariableNodeData object.
+    solveKey identifier associated with this VariableNodeData object.
     """
     solveKey::Symbol = :default
     """
@@ -81,8 +84,8 @@ end
 
 ##------------------------------------------------------------------------------
 ## Constructors
-VariableNodeData{T}(; kwargs...) where T <: InferenceVariable = VariableNodeData{T,getPointType(T)}(; kwargs...)
-VariableNodeData(variableType::InferenceVariable; kwargs...) = VariableNodeData{typeof(variableType)}(; kwargs...)
+VariableNodeData{T}(; kwargs...) where T <: InferenceVariable = VariableNodeData{T,getPointType(T),getDimension(T)}(; kwargs...)
+VariableNodeData(variableType::InferenceVariable; kwargs...) = VariableNodeData{typeof(variableType)}(;kwargs...)
 
 
 ##==============================================================================
@@ -117,6 +120,7 @@ Base.@kwdef mutable struct PackedVariableNodeData
     solveInProgress::Int
     solvedCount::Int
     solveKey::Symbol
+    covar::Vector{Float64}
     _version::String = string(_getDFGVersion())
 end
 
@@ -243,35 +247,35 @@ Complete variable structure for a DistributedFactorGraph variable.
 Fields:
 $(TYPEDFIELDS)
 """
-Base.@kwdef struct DFGVariable{T<:InferenceVariable} <: AbstractDFGVariable
+Base.@kwdef struct DFGVariable{T<:InferenceVariable, P, N} <: AbstractDFGVariable
     """The ID for the variable"""
-    id::Union{UUID, Nothing}
+    id::Union{UUID, Nothing} = nothing
     """Variable label, e.g. :x1.
     Accessor: [`getLabel`](@ref)"""
     label::Symbol
     """Variable timestamp.
     Accessors: [`getTimestamp`](@ref), [`setTimestamp`](@ref)"""
-    timestamp::ZonedDateTime
+    timestamp::ZonedDateTime = now(localzone())
     """Nano second time, for more resolution on timestamp (only subsecond information)"""
-    nstime::Nanosecond
+    nstime::Nanosecond = Nanosecond(0) 
     """Variable tags, e.g [:POSE, :VARIABLE, and :LANDMARK].
     Accessors: [`getTags`](@ref), [`mergeTags!`](@ref), and [`removeTags!`](@ref)"""
-    tags::Set{Symbol}
+    tags::Set{Symbol} = Set{Symbol}()
     """Dictionary of parametric point estimates keyed by solverDataDict keys
     Accessors: [`addPPE!`](@ref), [`updatePPE!`](@ref), and [`deletePPE!`](@ref)"""
-    ppeDict::Dict{Symbol, <: AbstractPointParametricEst}
+    ppeDict::Dict{Symbol, AbstractPointParametricEst} = 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,P,N}} = Dict{Symbol, VariableNodeData{T,P,N}}()
     """Dictionary of small data associated with this variable.
     Accessors: [`getSmallData`](@ref), [`setSmallData!`](@ref)"""
-    smallData::Dict{Symbol, SmallDataTypes}
+    smallData::Dict{Symbol, SmallDataTypes} = Dict{Symbol, SmallDataTypes}()
     """Dictionary of large data associated with this variable.
     Accessors: [`addBlobEntry!`](@ref), [`getBlobEntry`](@ref), [`updateBlobEntry!`](@ref), and [`deleteBlobEntry!`](@ref)"""
-    dataDict::Dict{Symbol, BlobEntry}
+    dataDict::Dict{Symbol, BlobEntry} = Dict{Symbol, BlobEntry}()
     """Solvable flag for the variable.
     Accessors: [`getSolvable`](@ref), [`setSolvable!`](@ref)"""
-    solvable::Base.RefValue{Int}
+    solvable::Base.RefValue{Int} = Ref(1)
 end
 
 ##------------------------------------------------------------------------------
@@ -281,43 +285,34 @@ end
     $SIGNATURES
 The default DFGVariable constructor.
 """
-function DFGVariable(label::Symbol, variableType::Type{T};
-            id::Union{UUID,Nothing}=nothing,
-            timestamp::ZonedDateTime=now(localzone()),
-            nstime::Nanosecond = Nanosecond(0),
-            tags::Set{Symbol}=Set{Symbol}(),
-            estimateDict::Dict{Symbol, <: AbstractPointParametricEst}=Dict{Symbol, MeanMaxPPE}(),
-            solverDataDict::Dict{Symbol, VariableNodeData{T,P}}=Dict{Symbol, VariableNodeData{T,getPointType(T)}}(),
-            smallData::Dict{Symbol, SmallDataTypes}=Dict{Symbol, SmallDataTypes}(),
-            dataDict::Dict{Symbol, BlobEntry}=Dict{Symbol,BlobEntry}(),
-            solvable::Int=1) where {T <: InferenceVariable, P}
-    #
-    DFGVariable{T}(id, label, timestamp, nstime, tags, estimateDict, solverDataDict, smallData, dataDict, Ref(solvable))
-end
+function DFGVariable(
+    label::Symbol, 
+    T::Type{<:InferenceVariable};
+    estimateDict=nothing,
+    timestamp=now(localzone()),
+    solvable::Union{Int, Base.RefValue{Int}}=Ref(1),
+    kwargs...
+)
+    #TODO deprecated, remove in v0.21 should have already been deprecated
+    if !isnothing(estimateDict)
+        error("Keyword argument `estimateDict` is deprecated use `ppeDict`")
+    end
+    if !isa(timestamp, ZonedDateTime)
+        @warn "timestamp<:DateTime is deprecated, timestamp must be a ZonedDateTime, using local zone."
+        timestamp = ZonedDateTime(timestamp,localzone())
+    end
 
-DFGVariable(label::Symbol, 
-            variableType::T; 
-            solverDataDict::Dict{Symbol, VariableNodeData{T,P}}=Dict{Symbol, VariableNodeData{T,getPointType(T)}}(),
-            kw...) where {T <: InferenceVariable, P} = DFGVariable(label, T; solverDataDict=solverDataDict, kw...)
-#
-# @deprecate DFGVariable(label::Symbol, T_::Type{<:InferenceVariable},w...; timestamp::DateTime=now(),kw...) DFGVariable(label, T_, w...; timestamp=ZonedDateTime(timestamp), kw...)
-#
-
-
-function DFGVariable(label::Symbol,
-            solverData::VariableNodeData{T};
-            id::Union{UUID, Nothing} = nothing,
-            timestamp::ZonedDateTime = now(localzone()),
-            nstime::Nanosecond = Nanosecond(0),
-            tags::Set{Symbol}=Set{Symbol}(),
-            estimateDict::Dict{Symbol, <: AbstractPointParametricEst}=Dict{Symbol, MeanMaxPPE}(),
-            smallData::Dict{Symbol, SmallDataTypes}=Dict{Symbol, SmallDataTypes}(),
-            dataDict::Dict{Symbol, <: BlobEntry}=Dict{Symbol,BlobEntry}(),
-            solvable::Int=1) where {T <: InferenceVariable}
-    #
-    DFGVariable{T}(id, label, timestamp, nstime, tags, estimateDict, Dict{Symbol, VariableNodeData{T, getPointType(T)}}(:default=>solverData), smallData, dataDict, Ref(solvable))
+    solvable isa Int && (solvable = Ref(solvable))
+
+    N = getDimension(T)
+    P = getPointType(T)
+    DFGVariable{T,P,N}(;label, timestamp, solvable, kwargs...)
 end
 
+DFGVariable(label::Symbol, variableType::InferenceVariable; kwargs...) = DFGVariable(label, typeof(variableType); kwargs...)
+
+DFGVariable(label::Symbol, solverData::VariableNodeData; kwargs...) = DFGVariable(;label, solverDataDict=Dict(:default=>solverData), kwargs...)
+
 Base.getproperty(x::DFGVariable,f::Symbol) = begin
     if f == :solvable
         getfield(x,f)[]
@@ -335,14 +330,6 @@ Base.setproperty!(x::DFGVariable,f::Symbol, val) = begin
 end
 
 
-##------------------------------------------------------------------------------
-# TODO: can't see the reason to overwrite copy, leaving it here for now
-# function Base.copy(o::DFGVariable)::DFGVariable
-#     return DFGVariable(o.label, getVariableType(o)(), tags=copy(o.tags), estimateDict=copy(o.estimateDict),
-#                         solverDataDict=copy(o.solverDataDict), smallData=copy(o.smallData),
-#                         dataDict=copy(o.dataDict), solvable=getSolvable(o))
-# end
-
 ##------------------------------------------------------------------------------
 ## DFGVariableSummary lv1
 ##------------------------------------------------------------------------------

src/services/CompareUtils.jl

@@ -175,7 +175,7 @@ function compare(a::VariableNodeData, b::VariableNodeData)
     a.ismargin != b.ismargin && @debug("ismargin is not equal")==nothing && return false
     a.dontmargin != b.dontmargin && @debug("dontmargin is not equal")==nothing && return false
     a.solveInProgress != b.solveInProgress && @debug("solveInProgress is not equal")==nothing && return false
-    typeof(a.variableType) != typeof(b.variableType) && @debug("variableType is not equal")==nothing && return false
+    getVariableType(a) != getVariableType(b) && @debug("variableType is not equal")==nothing && return false
     return true
 end
 
@@ -204,8 +204,8 @@ function compareVariable(A::DFGVariable,
   varskiplist = union(varskiplist, [:variableType])
   union!(varskiplist, skip)
   TP = TP && compareAll(Ad, Bd, skip=varskiplist, show=show)
-  TP = TP && typeof(Ad.variableType) == typeof(Bd.variableType)
-  TP = TP && compareAll(Ad.variableType, Bd.variableType, show=show, skip=skip)
+  TP = TP && typeof(getVariableType(Ad)) == typeof(getVariableType(Bd))
+  TP = TP && compareAll(getVariableType(Ad), getVariableType(Bd), show=show, skip=skip)
   return TP
 end
 

src/services/DFGVariable.jl

@@ -48,12 +48,10 @@ Related
 
 getVariableType
 """
-getVariableType(v::DFGVariable{T}) where T <: InferenceVariable = T()
+getVariableType(::DFGVariable{T}) where T = T()
+
+getVariableType(::VariableNodeData{T}) where T = T()
 
-function getVariableType(vnd::VariableNodeData)
-    # @warn "getVariableType(::VariableNodeData) is being deprecated, use getVariableType(::DFGVariable) instead."
-    return vnd.variableType
-end
 
 
 # TODO: Confirm that we can switch this out, instead of retrieving the complete variable.

src/services/Serialization.jl

@@ -83,7 +83,7 @@ end
 
 # returns a PackedVariableNodeData
 function packVariableNodeData(d::VariableNodeData{T}) where {T <: InferenceVariable}
-  @debug "Dispatching conversion variable -> packed variable for type $(string(d.variableType))"
+  @debug "Dispatching conversion variable -> packed variable for type $(string(getVariableType(d)))"
   castval = if 0 < length(d.val)
     precast = getCoordinates.(T, d.val)
     @cast castval[i,j] := precast[j][i]
@@ -92,19 +92,23 @@ function packVariableNodeData(d::VariableNodeData{T}) where {T <: InferenceVaria
     zeros(1,0)
   end
   _val = castval[:]
+
+  length(d.covar) > 1 && @warn("Packing of more than one parametric covariance is NOT supported yet, only packing first.")
+  
   return PackedVariableNodeData(d.id, _val, size(castval,1),
                                 d.bw[:], size(d.bw,1),
                                 d.BayesNetOutVertIDs,
                                 d.dimIDs, d.dims, d.eliminated,
                                 d.BayesNetVertID, d.separator,
-                                typeModuleName(d.variableType),
+                                typeModuleName(getVariableType(d)),
                                 d.initialized,
                                 d.infoPerCoord,
                                 d.ismargin,
                                 d.dontmargin,
                                 d.solveInProgress,
                                 d.solvedCount,
                                 d.solveKey,
+                                isempty(d.covar) ? Float64[] : vec(d.covar[1]),
                                 string(_getDFGVersion()))
 end
 
@@ -131,25 +135,27 @@ function unpackVariableNodeData(d::PackedVariableNodeData)
     BW = reshape(d.vecbw,r4,c4)
 
     # 
-    return VariableNodeData{T, getPointType(T)}(
-        d.id,
-        vals, 
-        BW, 
-        Symbol.(d.BayesNetOutVertIDs),
-        d.dimIDs, 
-        d.dims, 
-        d.eliminated, 
-        Symbol(d.BayesNetVertID), 
-        Symbol.(d.separator),
-        T(), 
-        d.initialized, 
-        d.infoPerCoord, 
-        d.ismargin, 
-        d.dontmargin, 
-        d.solveInProgress, 
-        d.solvedCount, 
-        Symbol(d.solveKey),
-        Dict{Symbol,Threads.Condition}() )
+    N = getDimension(T)
+    return VariableNodeData{T, getPointType(T), N}(;
+        id = d.id,
+        val = vals, 
+        bw = BW, 
+        #TODO only one covar is currently supported in packed VND
+        covar = isempty(d.covar) ? SMatrix{N, N, Float64}[] : [d.covar],
+        BayesNetOutVertIDs = Symbol.(d.BayesNetOutVertIDs),
+        dimIDs = d.dimIDs, 
+        dims = d.dims, 
+        eliminated = d.eliminated, 
+        BayesNetVertID = Symbol(d.BayesNetVertID), 
+        separator = Symbol.(d.separator),
+        initialized = d.initialized, 
+        infoPerCoord = d.infoPerCoord, 
+        ismargin = d.ismargin, 
+        dontmargin = d.dontmargin, 
+        solveInProgress = d.solveInProgress, 
+        solvedCount = d.solvedCount, 
+        solveKey = Symbol(d.solveKey),
+        events = Dict{Symbol,Threads.Condition}() )
 end
 
 ##==============================================================================
@@ -188,13 +194,13 @@ function unpackVariable(variable::PackedVariable; skipVersionCheck::Bool=false)
     metadata = JSON3.read(base64decode(variable.metadata), Dict{Symbol, DFG.SmallDataTypes})
 
     return DFGVariable(
-        id = variable.id,
         variable.label, 
-        variableType, 
+        variableType;
+        id = variable.id,
         timestamp=variable.timestamp, 
         nstime=Nanosecond(variable.nstime), 
         tags=Set(variable.tags), 
-        estimateDict=ppeDict, 
+        ppeDict=ppeDict, 
         solverDataDict=solverDict,  
         smallData=metadata, 
         dataDict=dataDict, 

test/data/0_21_0.tar.gz

@@ -52,6 +52,7 @@ end
 
 
 @testset "Custom Printing" begin
+    global var1, var2, var3, v1_tags, vorphan
 
     iobuf = IOBuffer()
     # for now just test the interface and a bit of output
@@ -64,7 +65,7 @@ end
     if DistributedFactorGraphs._getDFGVersion() < v"0.19"
         @test String(take!(iobuf)) == "DFGVariable{TestVariableType1}\nid:\nnothing\nlabel:\n:a\ntags:\nSet([:VARIABLE, :POSE])\nsmallData:\nDict{Symbol, Union{Bool, Float64, Int64, Vector{Bool}, Vector{Float64}, Vector{Int64}, Vector{String}, String}}(:small=>\"data\")\ndataDict:\nDict{Symbol, DistributedFactorGraphs.BlobEntry}()\nsolvable:\n0\n"   
     else
-        @test String(take!(iobuf)) == "DFGVariable{TestVariableType1}\nid:\nnothing\nlabel:\n:a\ntags:\nSet([:VARIABLE, :POSE])\nsmallData:\nDict{Symbol, Union{Bool, Float64, Int64, Vector{Bool}, Vector{Float64}, Vector{Int64}, Vector{String}, String}}(:small=>\"data\")\ndataDict:\nDict{Symbol, BlobEntry}()\nsolvable:\n0\n"   
+        @test String(take!(iobuf)) == "DFGVariable{TestVariableType1, Vector{Float64}, 1}\nid:\nnothing\nlabel:\n:a\ntags:\nSet([:VARIABLE, :POSE])\nsmallData:\nDict{Symbol, Union{Bool, Float64, Int64, Vector{Bool}, Vector{Float64}, Vector{Int64}, Vector{String}, String}}(:small=>\"data\")\ndataDict:\nDict{Symbol, BlobEntry}()\nsolvable:\n0\n"   
     end
 
     @test printVariable(iobuf, var1, short=true) === nothing