Skip to content

Commit 974e4a7

Browse files
AffieAffie
authored
Merge pull request #1351 from JuliaRobotics/21Q3/refac/mvapproxconvcirc
mv approxConvCircular, depr printSummaryGraph, approxConvCircular
2 parents 7b1a456 + 9ac0e50 commit 974e4a7

File tree

6 files changed

+170
-162
lines changed

6 files changed

+170
-162
lines changed

‎NEWS.md‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,7 @@ The list below highlights major breaking changes, and please note that significa
2323
- Deprecate use of `getParametricMeasurement` and use `getMeasurementParametric` instead, and add `<:AbstractManifold` to API.
2424
- Deprecate use of `solveBinaryFactorParameteric`, instead use `solveFactorParameteric`.
2525
- Deprecating `approxConvBinary`, use `approxConvBelief` instead.
26-
- Deprecating `accumulateFactorChain`, use `approxConvBelief` instead.
27-
26+
- Removing obsolete `approxConvCircular`, use `approxConvBelief` instead.
2827

2928
# Major changes in v0.24
3029

‎src/AdditionalUtils.jl‎

Lines changed: 0 additions & 87 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
11

2-
export approxConvCircular
32

43
"""
54
$SIGNATURES
@@ -132,91 +131,5 @@ end
132131

133132

134133

135-
"""
136-
$SIGNATURES
137-
138-
Print basic summary of graph to `logger=ConsoleLogger()`.
139-
"""
140-
function printGraphSummary(dfg::G, logger=ConsoleLogger())::Nothing where {G <: AbstractDFG}
141-
vars = ls(dfg)
142-
fcts = lsf(dfg)
143-
144-
prio = lsfPriors(dfg)
145-
146-
isinit = map(x->isInitialized(dfg,x), vars)
147-
infdim = map(x->getVariableInferredDim(dfg, x), vars)
148-
numedges = map(v->length(ls(dfg, v)), vars)
149-
numfed = map(fc->length(ls(dfg, fc)), fcts)
150-
vardims = map(v->getDimension(getVariable(dfg, v)), vars)
151-
fctdims = map(v->getDimension(getFactor(dfg, v)), fcts)
152-
priodims = map(v->getDimension(getFactor(dfg, v)), prio)
153-
154-
with_logger(logger) do
155-
@info "Distributed Factor Graph summary:"
156-
@info " num variables: $(length(vars))"
157-
@info " num factors: $(length(fcts)), w/ $(length(prio)) priors"
158-
@info " var initialized: $(sum(isinit))"
159-
@info ""
160-
@info " var num edges: min. $(minimum(numedges)) | mean $(round(Statistics.mean(numedges),digits=2)) | 90% $(round(quantile(numedges,0.9),digits=2)) | max. $(maximum(numedges))"
161-
@info " fct num edges: min. $(minimum(numfed)) | mean $(round(Statistics.mean(numfed),digits=2)) | 90% $(round(quantile(numfed,0.9),digits=2)) | max. $(maximum(numfed))"
162-
@info " Variable dims: min. $(minimum(vardims)) | mean $(round(Statistics.mean(vardims),digits=2)) | 90% $(round(quantile(vardims,0.9),digits=2)) | max. $(maximum(vardims))"
163-
@info " Factor dims: min. $(minimum(fctdims)) | mean $(round(Statistics.mean(fctdims),digits=2)) | 90% $(round(quantile(fctdims,0.9),digits=2)) | max. $(maximum(fctdims))"
164-
@info " Prior dimens: min. $(minimum(priodims)) | mean $(round(Statistics.mean(priodims),digits=2)) | 90% $(round(quantile(priodims,0.9),digits=2)) | max. $(maximum(priodims))"
165-
@info " var infr'dims: min. $(minimum(infdim)) | mean $(round(Statistics.mean(infdim),digits=2)) | 90% $(round(quantile(infdim,0.9),digits=2)) | max. $(maximum(infdim))"
166-
end
167-
nothing
168-
end
169-
170-
"""
171-
$SIGNATURES
172-
173-
Print basic summary of graph to `logger=ConsoleLogger()`.
174-
"""
175-
function printSummary(dfg::G, logger=ConsoleLogger()) where G <: AbstractDFG
176-
printGraphSummary(dfg, logger)
177-
end
178-
179-
180-
"""
181-
$SIGNATURES
182-
183-
Build an approximate density `[Y|X,DX,.]=[X|Y,DX][DX|.]` as proposed by the conditional convolution.
184-
185-
Notes
186-
- Assume both are on circular manifold, `manikde!(pts, (:Circular,))`
187-
"""
188-
function approxConvCircular(pX::ManifoldKernelDensity,
189-
pDX::ManifoldKernelDensity; N::Int=100)
190-
#
191-
192-
# building basic factor graph
193-
tfg = initfg()
194-
addVariable!(tfg, :s1, Sphere1)
195-
addVariable!(tfg, :s2, Sphere1)
196-
addFactor!(tfg, [:s1;:s2], Sphere1Sphere1(pDX), graphinit=false)
197-
initManual!(tfg,:s1, pX)
198-
199-
# solve for outgoing proposal value
200-
approxConv(tfg,:s1s2f1,:s2)
201-
end
202-
203-
function approxConvCircular(pX::ManifoldKernelDensity,
204-
pDX::SamplableBelief; N::Int=100)
205-
#
206-
pts = reshape(rand(pDX, N), 1, :)
207-
pC = manikde!(pts, Sphere1)
208-
approxConvCircular(pX, pC)
209-
end
210-
211-
212-
function approxConvCircular(pX::SamplableBelief,
213-
pDX::ManifoldKernelDensity; N::Int=100)
214-
#
215-
pts = reshape(rand(pX, N), 1, :)
216-
pC = manikde!(pts, Sphere1)
217-
approxConvCircular(pC, pDX)
218-
end
219-
220-
221134

222135
#

‎src/ConsolidateParametricRelatives.jl‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
# functions relating to parametric solutions of a single factor that is likely in need of consolidation
2+
3+
4+
5+
"""
6+
$SIGNATURES
7+
8+
Helper function to propagate a parametric estimate along a factor chain.
9+
This function takes and returns variable values as coordinates.
10+
11+
Notes
12+
- Not used during MM-iSAM inference.
13+
- Expected uses are for user analysis of factors and estimates.
14+
- real-time dead reckoning chain prediction.
15+
- Parametric binary factor utility function, used by DRT
16+
17+
DevNotes
18+
- TODO ensure type stability, likely returning types `Any` at this time.
19+
- TODO MeanMaxPPE currently stored as coordinates, complicating fast calculation.
20+
21+
Related:
22+
23+
[`getMeasurementParametric`](@ref), [`approxConv`](@ref), [`MutablePose2Pose2Gaussian`](@ref)
24+
"""
25+
function solveFactorParameteric(dfg::AbstractDFG,
26+
fct::DFGFactor,
27+
# currval::P1,
28+
srcsym_vals::AbstractVector{Pair{Symbol, P}},
29+
trgsym::Symbol,
30+
solveKey::Symbol=:default;
31+
evaltmpkw... ) where P
32+
#
33+
34+
varLbls = getVariableOrder(fct)
35+
varTypes = tuple((getVariableType.(dfg, varLbls))...)
36+
sfidx = findfirst( varLbls .== trgsym )
37+
38+
# get the measurement point
39+
fctTyp = getFactorType(fct)
40+
# this is definitely in coordinates, see JuliaRobotics/RoME.jl#465
41+
mea, _ = getMeasurementParametric(fctTyp)
42+
# must change measT to be a tangent vector
43+
M = getManifold(fctTyp)
44+
e0 = identity_element(M)
45+
mea_ = hat(M, e0, mea)
46+
measT = (mea_,)
47+
48+
# get variable points
49+
function _getParametric(vari::DFGVariable, key=:default)
50+
# hasp = haskey(getPPEDict(vari), key)
51+
# FIXME use PPE via Manifold points currently in coordinates
52+
# hasp ? getPPE(vari, key).suggested : calcMean(getBelief(vari, key))
53+
pt = calcMean(getBelief(vari, key))
54+
55+
getCoordinates(getVariableType(vari),pt)
56+
end
57+
58+
# overwrite specific src values from user
59+
coordVals = _getParametric.(getVariable.(dfg, varLbls), solveKey)
60+
for (srcsym, currval) in srcsym_vals
61+
coordVals[findfirst(varLbls .== srcsym)] = currval
62+
end
63+
crds = tuple(coordVals...)
64+
65+
pts = tuple(map(t->getPoint(t...), zip(varTypes,crds))...)
66+
67+
# do the calculation to find solvefor index using the factor, as manifold point
68+
pt = _evalFactorTemporary!( fctTyp, varTypes, sfidx, measT, pts; solveKey=solveKey, evaltmpkw... )[1]
69+
70+
return getCoordinates(getVariableType(dfg, trgsym), pt)
71+
end
72+
73+

‎src/Deprecated.jl‎

Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,99 @@ end
2626
##==============================================================================
2727

2828

29+
30+
# """
31+
# $SIGNATURES
32+
33+
# Build an approximate density `[Y|X,DX,.]=[X|Y,DX][DX|.]` as proposed by the conditional convolution.
34+
35+
# Notes
36+
# - Assume both are on circular manifold, `manikde!(pts, (:Circular,))`
37+
# """
38+
# function approxConvCircular(pX::ManifoldKernelDensity,
39+
# pDX::ManifoldKernelDensity; N::Int=100)
40+
# #
41+
42+
# # building basic factor graph
43+
# tfg = initfg()
44+
# addVariable!(tfg, :s1, Sphere1)
45+
# addVariable!(tfg, :s2, Sphere1)
46+
# addFactor!(tfg, [:s1;:s2], Sphere1Sphere1(pDX), graphinit=false)
47+
# initManual!(tfg,:s1, pX)
48+
49+
# # solve for outgoing proposal value
50+
# approxConv(tfg,:s1s2f1,:s2)
51+
# end
52+
53+
# function approxConvCircular(pX::ManifoldKernelDensity,
54+
# pDX::SamplableBelief; N::Int=100)
55+
# #
56+
# pts = reshape(rand(pDX, N), 1, :)
57+
# pC = manikde!(pts, Sphere1)
58+
# approxConvCircular(pX, pC)
59+
# end
60+
61+
62+
# function approxConvCircular(pX::SamplableBelief,
63+
# pDX::ManifoldKernelDensity; N::Int=100)
64+
# #
65+
# pts = reshape(rand(pX, N), 1, :)
66+
# pC = manikde!(pts, Sphere1)
67+
# approxConvCircular(pC, pDX)
68+
# end
69+
70+
71+
72+
@deprecate printGraphSummary(dfg::AbstractDFG, logger=ConsoleLogger()) show(logger.stream, MIME("text/plain"), dfg)
73+
@deprecate printSummary(dfg::AbstractDFG, logger=ConsoleLogger()) show(logger.stream, MIME("text/plain"), dfg)
74+
75+
76+
# """
77+
# $SIGNATURES
78+
79+
# Print basic summary of graph to `logger=ConsoleLogger()`.
80+
# """
81+
# function printGraphSummary(dfg::G, logger=ConsoleLogger())::Nothing where {G <: AbstractDFG}
82+
# vars = ls(dfg)
83+
# fcts = lsf(dfg)
84+
85+
# prio = lsfPriors(dfg)
86+
87+
# isinit = map(x->isInitialized(dfg,x), vars)
88+
# infdim = map(x->getVariableInferredDim(dfg, x), vars)
89+
# numedges = map(v->length(ls(dfg, v)), vars)
90+
# numfed = map(fc->length(ls(dfg, fc)), fcts)
91+
# vardims = map(v->getDimension(getVariable(dfg, v)), vars)
92+
# fctdims = map(v->getDimension(getFactor(dfg, v)), fcts)
93+
# priodims = map(v->getDimension(getFactor(dfg, v)), prio)
94+
95+
# with_logger(logger) do
96+
# @info "Distributed Factor Graph summary:"
97+
# @info " num variables: $(length(vars))"
98+
# @info " num factors: $(length(fcts)), w/ $(length(prio)) priors"
99+
# @info " var initialized: $(sum(isinit))"
100+
# @info ""
101+
# @info " var num edges: min. $(minimum(numedges)) | mean $(round(Statistics.mean(numedges),digits=2)) | 90% $(round(quantile(numedges,0.9),digits=2)) | max. $(maximum(numedges))"
102+
# @info " fct num edges: min. $(minimum(numfed)) | mean $(round(Statistics.mean(numfed),digits=2)) | 90% $(round(quantile(numfed,0.9),digits=2)) | max. $(maximum(numfed))"
103+
# @info " Variable dims: min. $(minimum(vardims)) | mean $(round(Statistics.mean(vardims),digits=2)) | 90% $(round(quantile(vardims,0.9),digits=2)) | max. $(maximum(vardims))"
104+
# @info " Factor dims: min. $(minimum(fctdims)) | mean $(round(Statistics.mean(fctdims),digits=2)) | 90% $(round(quantile(fctdims,0.9),digits=2)) | max. $(maximum(fctdims))"
105+
# @info " Prior dimens: min. $(minimum(priodims)) | mean $(round(Statistics.mean(priodims),digits=2)) | 90% $(round(quantile(priodims,0.9),digits=2)) | max. $(maximum(priodims))"
106+
# @info " var infr'dims: min. $(minimum(infdim)) | mean $(round(Statistics.mean(infdim),digits=2)) | 90% $(round(quantile(infdim,0.9),digits=2)) | max. $(maximum(infdim))"
107+
# end
108+
# nothing
109+
# end
110+
111+
# """
112+
# $SIGNATURES
113+
114+
# Print basic summary of graph to `logger=ConsoleLogger()`.
115+
# """
116+
# function printSummary(dfg::G, logger=ConsoleLogger()) where G <: AbstractDFG
117+
# printGraphSummary(dfg, logger)
118+
# end
119+
120+
121+
29122
# """
30123
# $SIGNATURES
31124

‎src/IncrementalInference.jl‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -132,8 +132,6 @@ export *,
132132
# state machine methods
133133
StateMachine,
134134
exitStateMachine,
135-
printGraphSummary,
136-
printSummary,
137135
print,
138136
getGraphFromHistory,
139137
getCliqSubgraphFromHistory,
@@ -464,14 +462,12 @@ include("Factors/PartialPrior.jl")
464462
include("Factors/PartialPriorPassThrough.jl")
465463
include("DefaultNodeTypes.jl") # older file
466464

467-
468465
# Refactoring in progress
469466
include("services/CalcFactor.jl")
470467
# gradient tools
471468
include("services/FactorGradients.jl")
472469
include("services/CliqueTypes.jl")
473470

474-
475471
# solving graphs
476472
include("SolverUtilities.jl")
477473
include("NumericalCalculations.jl")
@@ -480,6 +476,9 @@ include("ExplicitDiscreteMarginalizations.jl")
480476
include("InferDimensionUtils.jl")
481477
include("services/EvalFactor.jl")
482478
include("services/ApproxConv.jl")
479+
480+
include("ConsolidateParametricRelatives.jl") # FIXME CONSOLIDATE
481+
483482
include("GraphProductOperations.jl")
484483
include("SolveTree.jl")
485484
include("TetherUtils.jl")

‎src/services/ApproxConv.jl‎

Lines changed: 0 additions & 69 deletions
Original file line numberDiff line numberDiff line change
@@ -241,75 +241,6 @@ end
241241

242242

243243

244-
"""
245-
$SIGNATURES
246-
247-
Helper function to propagate a parametric estimate along a factor chain.
248-
This function takes and returns variable values as coordinates.
249-
250-
Notes
251-
- Not used during MM-iSAM inference.
252-
- Expected uses are for user analysis of factors and estimates.
253-
- real-time dead reckoning chain prediction.
254-
- Parametric binary factor utility function, used by DRT
255-
256-
DevNotes
257-
- TODO ensure type stability, likely returning types `Any` at this time.
258-
- TODO MeanMaxPPE currently stored as coordinates, complicating fast calculation.
259-
260-
Related:
261-
262-
[`getMeasurementParametric`](@ref), [`approxConv`](@ref), [`MutablePose2Pose2Gaussian`](@ref)
263-
"""
264-
function solveFactorParameteric(dfg::AbstractDFG,
265-
fct::DFGFactor,
266-
# currval::P1,
267-
srcsym_vals::AbstractVector{Pair{Symbol, P}},
268-
trgsym::Symbol,
269-
solveKey::Symbol=:default;
270-
evaltmpkw... ) where P
271-
#
272-
273-
varLbls = getVariableOrder(fct)
274-
varTypes = tuple((getVariableType.(dfg, varLbls))...)
275-
sfidx = findfirst( varLbls .== trgsym )
276-
277-
# get the measurement point
278-
fctTyp = getFactorType(fct)
279-
# this is definitely in coordinates, see JuliaRobotics/RoME.jl#465
280-
mea, _ = getMeasurementParametric(fctTyp)
281-
# must change measT to be a tangent vector
282-
M = getManifold(fctTyp)
283-
e0 = identity_element(M)
284-
mea_ = hat(M, e0, mea)
285-
measT = (mea_,)
286-
287-
# get variable points
288-
function _getParametric(vari::DFGVariable, key=:default)
289-
# hasp = haskey(getPPEDict(vari), key)
290-
# FIXME use PPE via Manifold points currently in coordinates
291-
# hasp ? getPPE(vari, key).suggested : calcMean(getBelief(vari, key))
292-
pt = calcMean(getBelief(vari, key))
293-
294-
getCoordinates(getVariableType(vari),pt)
295-
end
296-
297-
# overwrite specific src values from user
298-
coordVals = _getParametric.(getVariable.(dfg, varLbls), solveKey)
299-
for (srcsym, currval) in srcsym_vals
300-
coordVals[findfirst(varLbls .== srcsym)] = currval
301-
end
302-
crds = tuple(coordVals...)
303-
304-
pts = tuple(map(t->getPoint(t...), zip(varTypes,crds))...)
305-
306-
# do the calculation to find solvefor index using the factor, as manifold point
307-
pt = _evalFactorTemporary!( fctTyp, varTypes, sfidx, measT, pts; solveKey=solveKey, evaltmpkw... )[1]
308-
309-
return getCoordinates(getVariableType(dfg, trgsym), pt)
310-
end
311-
312-
313244

314245

315246
#

0 commit comments

Comments
 (0)