Skip to content

Commit 971e09e

Browse files
GearsADGearsAD
authored
Merge pull request #174 from JuliaRobotics/backport/v04_2_ppe_softtype
Include softtype and VariableEstimate changes and bugfixes
2 parents 2aa3c40 + d510160 commit 971e09e

File tree

13 files changed

+156
-71
lines changed

13 files changed

+156
-71
lines changed

‎src/Common.jl

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,7 @@ function getfnctype(data::GenericFunctionNodeData)
129129
return data.fnc.usrfnc!
130130
end
131131
function getfnctype(fact::DFGFactor; solveKey::Symbol=:default)
132-
data = getData(fact) # TODO , solveKey=solveKey)
132+
data = solverData(fact) # TODO , solveKey=solveKey)
133133
return getfnctype(data)
134134
end
135135
function getfnctype(dfg::T, lbl::Symbol; solveKey::Symbol=:default) where T <: AbstractDFG
@@ -145,7 +145,7 @@ Notes
145145
- Replaces older `getfnctype`.
146146
"""
147147
getFactorType(data::GenericFunctionNodeData) = data.fnc.usrfnc!
148-
getFactorType(fct::DFGFactor) = getFactorType(getData(fct))
148+
getFactorType(fct::DFGFactor) = getFactorType(solverData(fct))
149149
function getFactorType(dfg::G, lbl::Symbol) where G <: AbstractDFG
150150
getFactorType(getFactor(dfg, lbl))
151151
end
@@ -190,7 +190,7 @@ function getSofttype(vnd::VariableNodeData)
190190
return vnd.softtype
191191
end
192192
function getSofttype(v::DFGVariable; solveKey::Symbol=:default)
193-
return getSofttype(getData(v, solveKey=solveKey))
193+
return getSofttype(solverData(v, solveKey))
194194
end
195195

196196
"""

‎src/DistributedFactorGraphs.jl

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ export AbstractParams, NoSolverParams
2121
export DFGNode, DFGVariable, DFGFactor
2222
export InferenceType, PackedInferenceType, FunctorInferenceType, InferenceVariable, ConvolutionObject
2323
export FunctorSingleton, FunctorPairwise, FunctorPairwiseMinimize
24-
export label, timestamp, tags, estimates, estimate, data, solverData, getData, solverDataDict, setSolverData, internalId, smallData, bigData
24+
export label, timestamp, tags, estimates, estimate, data, softtype, solverData, getData, solverDataDict, setSolverData, internalId, smallData, bigData
2525
export DFGVariableSummary, DFGFactorSummary, AbstractDFGSummary
2626

2727
# Services/AbstractDFG Exports
@@ -70,6 +70,7 @@ include("CloudGraphsDFG/CloudGraphsDFG.jl")
7070

7171
function __init__()
7272
@require GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231" begin
73+
@info "Including Plots"
7374
include("DFGPlots/DFGPlots.jl")
7475
@reexport using .DFGPlots
7576
end

‎src/LightDFG/LightDFG.jl

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ import ...DistributedFactorGraphs: setSolverParams,
3434
getSubgraphAroundNode,
3535
getSubgraph,
3636
getAdjacencyMatrix,
37-
getAdjacencyMatrixSparse
37+
getAdjacencyMatrixSparse,
38+
_getDuplicatedEmptyDFG
3839

3940
include("FactorGraphs/FactorGraphs.jl")
4041
using .FactorGraphs

‎src/LightDFG/services/LightDFG.jl

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -426,3 +426,15 @@ function getAdjacencyMatrixSparse(dfg::LightDFG)::Tuple{LightGraphs.SparseMatrix
426426
adjvf = adj[factIndex, varIndex]
427427
return adjvf, varLabels, factLabels
428428
end
429+
430+
"""
431+
$(SIGNATURES)
432+
Gets an empty and unique CloudGraphsDFG derived from an existing DFG.
433+
"""
434+
function _getDuplicatedEmptyDFG(dfg::LightDFG{P,V,F})::LightDFG where {P <: AbstractParams, V <: AbstractDFGVariable, F <: AbstractDFGFactor}
435+
newDfg = LightDFG{P,V,F}(;
436+
userId=dfg.userId, robotId=dfg.robotId, sessionId=dfg.sessionId,
437+
params=deepcopy(dfg.solverParams))
438+
newDfg.description ="(Copy of) $(dfg.description)"
439+
return newDfg
440+
end

‎src/MetaGraphsDFG/services/MetaGraphsDFG.jl

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -564,3 +564,15 @@ function _toDotFile(dfg::MetaGraphsDFG, fileName::String="/tmp/dfg.dot")::Nothin
564564
end
565565
return nothing
566566
end
567+
568+
"""
569+
$(SIGNATURES)
570+
Gets an empty and unique CloudGraphsDFG derived from an existing DFG.
571+
"""
572+
function _getDuplicatedEmptyDFG(dfg::MetaGraphsDFG{P})::MetaGraphsDFG where {P <: AbstractParams}
573+
newDfg = MetaGraphsDFG{P}(;
574+
userId=dfg.userId, robotId=dfg.robotId, sessionId=dfg.sessionId,
575+
params=deepcopy(dfg.solverParams))
576+
newDfg.description ="(Copy of) $(dfg.description)"
577+
return newDfg
578+
end

‎src/SymbolDFG/SymbolDFG.jl

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ import ...DistributedFactorGraphs: setSolverParams,
3434
getSubgraphAroundNode,
3535
getSubgraph,
3636
getAdjacencyMatrix,
37-
getAdjacencyMatrixSparse
37+
getAdjacencyMatrixSparse,
38+
_getDuplicatedEmptyDFG
3839

3940
include("SymbolFactorGraphs/SymbolFactorGraphs.jl")
4041
using .SymbolFactorGraphs

‎src/SymbolDFG/services/SymbolDFG.jl

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -518,3 +518,15 @@ function toDotFile(dfg::SymbolDFG, fileName::String="/tmp/dfg.dot")::Nothing
518518
return nothing
519519
end
520520
=#
521+
522+
"""
523+
$(SIGNATURES)
524+
Gets an empty and unique CloudGraphsDFG derived from an existing DFG.
525+
"""
526+
function _getDuplicatedEmptyDFG(dfg::SymbolDFG{T,V,F})::SymbolDFG where {T <: AbstractParams, V <: DFGNode, F <:DFGNode}
527+
newDfg = SymbolDFG{T, V, F}(;
528+
userId=dfg.userId, robotId=dfg.robotId, sessionId=dfg.sessionId,
529+
params=deepcopy(dfg.solverParams))
530+
newDfg.description ="(Copy of) $(dfg.description)"
531+
return newDfg
532+
end

‎src/entities/DFGVariable.jl

Lines changed: 33 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -22,23 +22,9 @@ mutable struct VariableNodeData #TODO v0.5.0 {T<:InferenceVariable}
2222
# Tonio surprise TODO
2323
# frontalonly::Bool
2424
# A valid, packable default constructor is needed.
25-
VariableNodeData() = new(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], SingletonInferenceVariable(), false, false, false, false)
26-
VariableNodeData(x1::Array{Float64,2},
27-
x2::Array{Float64,2},
28-
x3::Vector{Symbol},
29-
x4::Vector{Int},
30-
x5::Int,
31-
x6::Bool,
32-
x7::Symbol,
33-
x8::Vector{Symbol},
34-
# x9::Dict{ Tuple{Symbol, Vector{Float64}} }, # Union{Nothing, },
35-
x10,
36-
x11::Bool,
37-
x12::Float64,
38-
x13::Bool,
39-
x14::Bool) =
40-
new(x1,x2,x3,x4,x5,x6,x7,x8,x10,x11,x12,x13,x14)
25+
4126
end
27+
VariableNodeData() = VariableNodeData(zeros(1,1), zeros(1,1), Symbol[], Int[], 0, false, :NOTHING, Symbol[], SingletonInferenceVariable(), false, 0.0, false, false)
4228

4329
"""
4430
$(TYPEDEF)
@@ -77,23 +63,37 @@ mutable struct PackedVariableNodeData
7763
x15::Bool ) = new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15)
7864
end
7965

80-
struct VariableEstimate
66+
67+
abstract type AbstractVariableEstimate end
68+
"""
69+
$TYPEDEF
70+
71+
Data container to store Parameteric Point Estimate (PPE) from a variety of types.
72+
73+
Notes
74+
- `ppeType` is something like `:max/:mean/:modefit` etc.
75+
- `solveKey` is from super-solve concept, starting with `:default`,
76+
- `estimate` is the actual numerical estimate value,
77+
- Additional information such as how the data is represented (ie softtype) is stored alongside this data container in the `DFGVariableSummary` container.
78+
"""
79+
struct VariableEstimate <: AbstractVariableEstimate
8180
solverKey::Symbol
82-
type::Symbol
81+
ppeType::Symbol
8382
estimate::Vector{Float64}
8483
lastUpdatedTimestamp::DateTime
85-
VariableEstimate(solverKey::Symbol, type::Symbol, estimate::Vector{Float64}, lastUpdatedTimestamp::DateTime=now()) = new(solverKey, type, estimate, lastUpdatedTimestamp)
8684
end
85+
VariableEstimate(solverKey::Symbol, type::Symbol, estimate::Vector{Float64}) = VariableEstimate(solverKey, type, estimate, now())
8786

8887
"""
89-
$(SIGNATURES)
90-
Fundamental structure for a DFG variable.
88+
$(TYPEDEF)
89+
Fundamental structure for a DFG variable with fields:
90+
$(TYPEDFIELDS)
9191
"""
9292
mutable struct DFGVariable <: AbstractDFGVariable
9393
label::Symbol
9494
timestamp::DateTime
9595
tags::Vector{Symbol}
96-
estimateDict::Dict{Symbol, Dict{Symbol, VariableEstimate}}
96+
estimateDict::Dict{Symbol, Dict{Symbol, <: AbstractVariableEstimate}}
9797
solverDataDict::Dict{Symbol, VariableNodeData}
9898
smallData::Dict{String, String}
9999
bigData::Any
@@ -110,6 +110,14 @@ timestamp(v::DFGVariable) = v.timestamp
110110
tags(v::DFGVariable) = v.tags
111111
estimates(v::DFGVariable) = v.estimateDict
112112
estimate(v::DFGVariable, key::Symbol=:default) = haskey(v.estimateDict, key) ? v.estimateDict[key] : nothing
113+
114+
"""
115+
$SIGNATURES
116+
117+
Retrieve the soft type name symbol for a DFGVariable or DFGVariableSummary. ie :Point2, Pose2, etc.
118+
"""
119+
softtype(v::DFGVariable)::Symbol = Symbol(typeof(getSofttype(v)))
120+
113121
"""
114122
$SIGNATURES
115123
@@ -145,12 +153,14 @@ mutable struct DFGVariableSummary <: AbstractDFGVariable
145153
label::Symbol
146154
timestamp::DateTime
147155
tags::Vector{Symbol}
148-
estimateDict::Dict{Symbol, Dict{Symbol, VariableEstimate}}
156+
estimateDict::Dict{Symbol, Dict{Symbol, <:AbstractVariableEstimate}}
157+
softtypename::Symbol
149158
_internalId::Int64
150159
end
151160
label(v::DFGVariableSummary) = v.label
152161
timestamp(v::DFGVariableSummary) = v.timestamp
153162
tags(v::DFGVariableSummary) = v.tags
154163
estimates(v::DFGVariableSummary) = v.estimateDict
155164
estimate(v::DFGVariableSummary, key::Symbol=:default) = haskey(v.estimateDict, key) ? v.estimateDict[key] : nothing
165+
softtype(v::DFGVariableSummary)::Symbol = v.softtypename
156166
internalId(v::DFGVariableSummary) = v._internalId

‎src/services/AbstractDFG.jl

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -449,7 +449,7 @@ Return boolean whether a factor `label` is present in `<:AbstractDFG`.
449449
"""
450450
function hasFactor(dfg::G, label::Symbol)::Bool where {G <: AbstractDFG}
451451
@warn "hasFactor() deprecated, please use exists()"
452-
return haskey(dfg.labelDict, label)
452+
return exists(dfg, label)
453453
end
454454

455455
"""
@@ -459,7 +459,7 @@ Return `::Bool` on whether `dfg` contains the variable `lbl::Symbol`.
459459
"""
460460
function hasVariable(dfg::G, label::Symbol)::Bool where {G <: AbstractDFG}
461461
@warn "hasVariable() deprecated, please use exists()"
462-
return haskey(dfg.labelDict, label) # haskey(vertices(dfg.g), label)
462+
return exists(dfg, label) # haskey(vertices(dfg.g), label)
463463
end
464464

465465

@@ -472,14 +472,16 @@ Notes:
472472
- used by both factor graph variable and Bayes tree clique logic.
473473
"""
474474
function isInitialized(var::DFGVariable; key::Symbol=:default)::Bool
475-
solverData(var, key) != nothing && return solverData(var, key).initialized
476-
return false
477-
end
478-
function isInitialized(fct::DFGFactor; key::Symbol=:default)::Bool
479-
solverData(var, key) != nothing && return solverData(fct, key).initialized
480-
return false
475+
data = solverData(var, key)
476+
if data == nothing
477+
@error "Variable does not have solver data $(key)"
478+
return false
479+
else
480+
return data.initialized
481+
end
481482
end
482-
function isInitialized(dfg::G, label::Symbol; key::Symbol=:default)::Bool where G <: AbstractDFG
483+
484+
function isInitialized(dfg::AbstractDFG, label::Symbol; key::Symbol=:default)::Bool
483485
return isInitialized(getVariable(dfg, label), key=key)
484486
end
485487

@@ -523,7 +525,6 @@ showFactor(fgl::G, fsym::Symbol) where G <: AbstractDFG = @show getFactor(fgl,fs
523525
Produces a dot-format of the graph for visualization.
524526
"""
525527
function toDot(dfg::AbstractDFG)::String
526-
@warn "Falling Back to convert to GraphsDFG"
527528
#TODO implement convert
528529
graphsdfg = GraphsDFG{AbstractParams}()
529530
DistributedFactorGraphs._copyIntoGraph!(dfg, graphsdfg, union(getVariableIds(dfg), getFactorIds(dfg)), true)
@@ -543,7 +544,6 @@ Note
543544
- Based on graphviz.org
544545
"""
545546
function toDotFile(dfg::AbstractDFG, fileName::String="/tmp/dfg.dot")::Nothing
546-
@warn "Falling Back to convert to GraphsDFG"
547547
#TODO implement convert
548548
graphsdfg = GraphsDFG{AbstractParams}()
549549
DistributedFactorGraphs._copyIntoGraph!(dfg, graphsdfg, union(getVariableIds(dfg), getFactorIds(dfg)), true)

‎src/services/DFGVariable.jl

Lines changed: 24 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -77,27 +77,36 @@ function unpack(dfg::G, d::PackedVariableNodeData)::VariableNodeData where G <:
7777
end
7878
@debug "Net conversion result: $st"
7979

80+
if st == nothing
81+
error("The variable doesn't seem to have a softtype. It needs to set up with an InferenceVariable from IIF. This will happen if you use DFG to add serialized variables directly and try use them. Please use IncrementalInference.addVariable().")
82+
end
83+
8084
return VariableNodeData(M3,M4, d.BayesNetOutVertIDs,
8185
d.dimIDs, d.dims, d.eliminated, d.BayesNetVertID, d.separator,
8286
st, d.initialized, d.inferdim, d.ismargin, d.dontmargin )
8387
end
8488

8589
function compare(a::VariableNodeData, b::VariableNodeData)
86-
TP = true
87-
TP = TP && a.val == b.val
88-
TP = TP && a.bw == b.bw
89-
TP = TP && a.BayesNetOutVertIDs == b.BayesNetOutVertIDs
90-
TP = TP && a.dimIDs == b.dimIDs
91-
TP = TP && a.dims == b.dims
92-
TP = TP && a.eliminated == b.eliminated
93-
TP = TP && a.BayesNetVertID == b.BayesNetVertID
94-
TP = TP && a.separator == b.separator
95-
TP = TP && abs(a.inferdim - b.inferdim) < 1e-14
96-
TP = TP && a.ismargin == b.ismargin
97-
TP = TP && a.softtype == b.softtype
98-
return TP
90+
a.val != b.val && @debug("val is not equal")==nothing && return false
91+
a.bw != b.bw && @debug("bw is not equal")==nothing && return false
92+
a.BayesNetOutVertIDs != b.BayesNetOutVertIDs && @debug("BayesNetOutVertIDs is not equal")==nothing && return false
93+
a.dimIDs != b.dimIDs && @debug("dimIDs is not equal")==nothing && return false
94+
a.dims != b.dims && @debug("dims is not equal")==nothing && return false
95+
a.eliminated != b.eliminated && @debug("eliminated is not equal")==nothing && return false
96+
a.BayesNetVertID != b.BayesNetVertID && @debug("BayesNetVertID is not equal")==nothing && return false
97+
a.separator != b.separator && @debug("separator is not equal")==nothing && return false
98+
a.initialized != b.initialized && @debug("initialized is not equal")==nothing && return false
99+
abs(a.inferdim - b.inferdim) > 1e-14 && @debug("inferdim is not equal")==nothing && return false
100+
a.ismargin != b.ismargin && @debug("ismargin is not equal")==nothing && return false
101+
a.dontmargin != b.dontmargin && @debug("dontmargin is not equal")==nothing && return false
102+
typeof(a.softtype) != typeof(b.softtype) && @debug("softtype is not equal")==nothing && return false
103+
return true
99104
end
100105

106+
"""
107+
$(SIGNATURES)
108+
Equality check for VariableNodeData.
109+
"""
101110
function ==(a::VariableNodeData,b::VariableNodeData, nt::Symbol=:var)
102111
return DistributedFactorGraphs.compare(a,b)
103112
end
@@ -108,12 +117,12 @@ Equality check for VariableEstimate.
108117
"""
109118
function ==(a::VariableEstimate, b::VariableEstimate)::Bool
110119
a.solverKey != b.solverKey && @debug("solverKey are not equal")==nothing && return false
111-
a.type != b.type && @debug("type is not equal")==nothing && return false
120+
a.ppeType != b.ppeType && @debug("ppeType is not equal")==nothing && return false
112121
a.estimate != b.estimate && @debug("estimate are not equal")==nothing && return false
113122
a.lastUpdatedTimestamp != b.lastUpdatedTimestamp && @debug("lastUpdatedTimestamp is not equal")==nothing && return false
114123
return true
115124
end
116125

117126
function convert(::Type{DFGVariableSummary}, v::DFGVariable)
118-
return DFGVariableSummary(v.label, v.timestamp, deepcopy(v.tags), deepcopy(v.estimateDict), v._internalId)
127+
return DFGVariableSummary(v.label, v.timestamp, deepcopy(v.tags), deepcopy(v.estimateDict), Symbol(typeof(getSofttype(v))), v._internalId)
119128
end

0 commit comments

Comments
 (0)