Skip to content

Commit cd232b0

Browse files
dehanndehann
committed
wip restoring tests
1 parent 2d7f28f commit cd232b0

3 files changed

Lines changed: 104 additions & 102 deletions

File tree

‎src/services/ApproxConv.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ function approxConvBelief(
1313
)
1414
#
1515
v_trg = getVariable(dfg, target)
16-
N = N == 0 ? getNumPts(v_trg; solveKey = solveKey) : N
16+
N = N == 0 ? getNumPts(v_trg; solveKey) : N
1717
# approxConv should push its result into duplicate memory destination, NOT the variable.VND.val itself. ccw.varValsAll always points directly to variable.VND.val
1818
# points and infoPerCoord
1919

‎src/services/EvalFactor.jl‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -572,10 +572,10 @@ function evalFactor(
572572
dfg::AbstractDFG,
573573
fct::DFGFactor,
574574
solvefor::Symbol,
575-
measurement::AbstractVector = Tuple[];
575+
measurement::AbstractVector = Tuple[]; # FIXME ensure type stable in all cases
576576
needFreshMeasurements::Bool = true,
577577
solveKey::Symbol = :default,
578-
variables = getVariable.(dfg, getVariableOrder(fct)), # because we trying to use StaticArrays, go figure
578+
variables = getVariable.(dfg, getVariableOrder(fct)), # FIXME use tuple instead for type stability
579579
N::Int = length(measurement),
580580
inflateCycles::Int = getSolverParams(dfg).inflateCycles,
581581
nullSurplus::Real = 0,

‎test/testDERelative.jl‎

Lines changed: 101 additions & 99 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ for i in 1:3
6161
problemType=ODEProblem )
6262
#
6363
addFactor!( fg, [prev;nextSym], ode_fac, graphinit=false )
64-
initVariable!(fg, nextSym, [zeros(1) for _ in 1:100])
64+
initVariable!(fg, nextSym, [0.1*randn(1) for _ in 1:100])
6565

6666
prev = nextSym
6767
end
@@ -146,16 +146,17 @@ ref_ = (getBelief(fg, :x0) |> getPoints)
146146

147147
## temp graph solve check
148148

149-
tfg = initfg()
150-
pts_ = approxConv(fg, :x0f1, :x3; setPPE=true, tfg)
149+
tfg = initfg()
150+
tx3_ = approxConvBelief(fg, :x0f1, :x3; setPPE=true, tfg)
151+
pts_ = getPoints(tx3_)
151152
# initVariable!(tfg, :x3, pts)
152153

153154
@cast pts[i,j] := pts_[j][i]
154155

155-
@test getPPE(tfg, :x0).suggested - x0_val_ref |> norm < 0.1
156-
@test getPPE(tfg, :x1).suggested - x1_val_ref |> norm < 0.1
157-
@test getPPE(tfg, :x2).suggested - x2_val_ref |> norm < 0.1
158-
@test Statistics.mean(pts) - x3_val_ref[1] < 1.0
156+
@test isapprox( x0_val_ref, getPPE(tfg, :x0).suggested ; atol = 0.1)
157+
@test isapprox( x1_val_ref, getPPE(tfg, :x1).suggested ; atol = 0.1)
158+
@test isapprox( x2_val_ref, getPPE(tfg, :x2).suggested ; atol = 0.1)
159+
@test isapprox( x3_val_ref, mean(tx3_); atol=0.1)
159160

160161
# using KernelDensityEstimatePlotting
161162
# plotKDE(tfg, [:x0;:x1;:x2;:x3])
@@ -167,13 +168,35 @@ pts_ = approxConv(fg, :x0f1, :x3; setPPE=true, tfg)
167168
@test isapprox( x0_val_ref, mean(getBelief(fg[:x0])); atol=0.1)
168169

169170
@test isInitialized(fg, :x1)
170-
@test isapprox( x1_val_ref, mean(getBelief(fg[:x1])); atol=0.1)
171+
# @test isapprox( x1_val_ref, mean(getBelief(fg[:x1])); atol=0.1)
171172

172-
@test isInitialized(fg, :x2)
173-
@test isapprox( x2_val_ref, mean(getBelief(fg[:x2])); atol=0.1)
173+
X2_ = approxConvBelief(fg, :x1x2f1, :x2)
174+
@test isapprox( x2_val_ref, mean(X2_); atol=0.1)
175+
176+
# FIXME, X2 and X3 are wrongly initialized to zero above
177+
# X2_ = approxConvBelief(fg, :x2x3f1, :x2)
178+
# @test isapprox( x2_val_ref, mean(X2_); atol=0.1)
179+
# @enter approxConvBelief(fg, :x2x3f1, :x2)
180+
181+
factors = getFactor.(fg, IIF.listNeighbors(fg, :x2))
182+
dens = ManifoldKernelDensity[]
183+
ipc = IIF.proposalbeliefs!(fg, :x2, factors, dens)
184+
185+
#
186+
mkd = *(dens...)
187+
188+
@test isapprox( x2_val_ref, mean(mkd); atol=0.1)
189+
190+
X2_,_ = propagateBelief(fg, :x2, :)
191+
@test isapprox( x2_val_ref, mean(X2_); atol=0.1)
192+
# @enter propagateBelief(fg, :x2, :)
174193

194+
@test isInitialized(fg, :x2)
175195
@test isInitialized(fg, :x3)
176-
@test isapprox( x3_val_ref, mean(getBelief(fg[:x3])); atol=0.1)
196+
197+
# FIXME, wrongly initialized X2 and X3 to near zero above
198+
# @test isapprox( x2_val_ref, mean(getBelief(fg[:x2])); atol=0.1)
199+
# @test isapprox( x3_val_ref, mean(getBelief(fg[:x3])); atol=0.1) # happens to be near zero
177200

178201

179202
## Now test a full graph solve
@@ -202,7 +225,7 @@ dens, ipc = propagateBelief( sfg, :x0, :;)
202225
@test isapprox( x0_val_ref, mean(dens); atol=0.1)
203226

204227
@test isapprox( x0_val_ref, mean(getBelief(sfg[:x0])); atol=0.1)
205-
@test isapprox( x2_val_ref, mean(getBelief(sfg[:x2])); atol=0.1)
228+
# @test isapprox( x2_val_ref, mean(getBelief(sfg[:x2])); atol=0.1) # TODO DELETE THIS LINE
206229

207230
dens, ipc = propagateBelief( sfg, :x1, :;)
208231
@test isapprox( x1_val_ref, mean(dens); atol=0.1)
@@ -212,56 +235,28 @@ _, csmc = repeatCSMStep!(hists[1], 6; duplicate=true)
212235
# @enter repeatCSMStep!(hists[1], 6; duplicate=true)
213236
@test isapprox( x0_val_ref, getPPESuggested(csmc.cliqSubFg, :x0)[1]; atol=0.1 )
214237
nval_x0 = mean(getBelief(csmc.cliqSubFg, :x0))
215-
@test isapprox( x0_val_ref, nval_x0[1]; atol=0.1 )
238+
@test isapprox( x0_val_ref, nval_x0; atol=0.1 )
216239

217240
nval_x0 = mean(getBelief(csmc.cliqSubFg, :x0))
218-
@test isapprox( x0_val_ref, nval_x0[1]; atol=0.1 )
241+
@test isapprox( x0_val_ref, nval_x0; atol=0.1 )
219242

220243

221244
# TODO CHECK vnd.val points istype SArray???
222245

223246
# intended steps at writing are 11,12 (post-root clique downsolve)
224247
val0 = getPPESuggested( hists[1][11][4].cliqSubFg[:x0] )
225-
@test isapprox( x0_val_ref, val0[1]; atol=0.1)
248+
@test isapprox( x0_val_ref, val0; atol=0.1)
226249
val0 = getPPESuggested( hists[1][12][4].cliqSubFg[:x0] )
227-
@test isapprox( x0_val_ref, val0[1]; atol=0.1)
228-
229-
230-
231-
232-
##
233-
234-
235-
sfg = deepcopy( hists[1][6][4].cliqSubFg )
236-
237-
dens, ipc = propagateBelief(
238-
sfg,
239-
:x0,
240-
:
241-
)
242-
243-
244-
vert = getVariable(sfg, :x0)
245-
setBelief!(vert, dens, true, ipc)
246-
247-
248-
250+
@test isapprox( x0_val_ref, val0; atol=0.1)
249251

250-
##
251-
252-
msg1 = IIF.getMessageBuffer(tree[1])
253-
msg1.upRx[2].belief[:x2].val
254252

255253
##
256254

257-
calcPPE(fg, :x0).suggested
258-
259255
@test getPPE(fg, :x0).suggested - x0_val_ref |> norm < 0.1
260-
@test_broken getPPE(fg, :x1).suggested - x1_val_ref |> norm < 0.1
261-
@test_broken getPPE(fg, :x2).suggested - x2_val_ref |> norm < 0.1
256+
@test getPPE(fg, :x1).suggested - x1_val_ref |> norm < 0.1
257+
@test getPPE(fg, :x2).suggested - x2_val_ref |> norm < 0.1
262258
@test getPPE(fg, :x3).suggested - x3_val_ref |> norm < 0.1
263259

264-
265260
##
266261

267262
end
@@ -299,7 +294,6 @@ addVariable!(fg, :x0, Position{2}, timestamp=DateTime(2000,1,1,0,0,0))
299294
addFactor!(fg, [:x0], Prior(MvNormal([1;0],0.01*diagm(ones(2)))))
300295

301296

302-
303297
##
304298

305299
prev = :x0
@@ -320,12 +314,53 @@ for i in 1:7
320314
dt=0.05,
321315
problemType=ODEProblem )
322316
#
323-
addFactor!( fg, [prev;nextSym], oder )
317+
addFactor!( fg, [prev;nextSym], oder; graphinit=false )
324318

325319
prev = nextSym
326320
end
327321

328322

323+
##
324+
325+
oder_ = DERelative( fg, [:x0; :x7],
326+
Position{2},
327+
dampedOscillator!,
328+
tstForce,
329+
# (state, var)->(state[1] = var[1]),
330+
# (var, state)->(var[1] = state[1]),
331+
dt=0.05,
332+
problemType=ODEProblem )
333+
334+
oder_.forwardProblem.u0 .= [1.0;0.0]
335+
sl = DifferentialEquations.solve(oder_.forwardProblem)
336+
337+
## check the solve values are correct
338+
339+
x0_val_ref = sl(getVariable(fg, :x0) |> getTimestamp |> DateTime |> datetime2unix)
340+
x1_val_ref = sl(getVariable(fg, :x1) |> getTimestamp |> DateTime |> datetime2unix)
341+
x2_val_ref = sl(getVariable(fg, :x2) |> getTimestamp |> DateTime |> datetime2unix)
342+
x3_val_ref = sl(getVariable(fg, :x3) |> getTimestamp |> DateTime |> datetime2unix)
343+
x4_val_ref = sl(getVariable(fg, :x4) |> getTimestamp |> DateTime |> datetime2unix)
344+
x5_val_ref = sl(getVariable(fg, :x5) |> getTimestamp |> DateTime |> datetime2unix)
345+
x6_val_ref = sl(getVariable(fg, :x6) |> getTimestamp |> DateTime |> datetime2unix)
346+
x7_val_ref = sl(getVariable(fg, :x7) |> getTimestamp |> DateTime |> datetime2unix)
347+
348+
349+
##
350+
351+
initAll!(fg)
352+
353+
##
354+
355+
# tfg = initfg()
356+
# for s in ls(fg)
357+
# initVariable!(fg, s, [0.1.*zeros(2) for _ in 1:100])
358+
# end
359+
360+
# pts = approxConv(fg, :x0f1, :x7, setPPE=true, tfg=tfg)
361+
# # initVariable!(tfg, :x7, pts)
362+
363+
329364
## check forward and backward solving
330365

331366
pts_ = approxConv(fg, :x0f1, :x0)
@@ -344,22 +379,7 @@ initVariable!(fg, :x1, pts_)
344379
pts_ = approxConv(fg, :x0x1f1, :x0)
345380
@cast pts[i,j] := pts_[j][i]
346381

347-
try
348-
@test norm(X0_ - pts) < 1e-2
349-
catch
350-
@error "FIXME: Skipping numerical test failure"
351-
end
352-
353-
##
354-
355-
tfg = initfg()
356-
for s in ls(fg)
357-
initVariable!(fg, s, [zeros(2) for _ in 1:100])
358-
end
359-
360-
pts = approxConv(fg, :x0f1, :x7, setPPE=true, tfg=tfg)
361-
# initVariable!(tfg, :x7, pts)
362-
382+
@test isapprox(0, norm(X0_ - pts); atol=1e-2)
363383

364384

365385
##
@@ -368,36 +388,17 @@ pts = approxConv(fg, :x0f1, :x7, setPPE=true, tfg=tfg)
368388

369389
##
370390

371-
372-
oder_ = DERelative( fg, [:x0; :x7],
373-
Position{2},
374-
dampedOscillator!,
375-
tstForce,
376-
# (state, var)->(state[1] = var[1]),
377-
# (var, state)->(var[1] = state[1]),
378-
dt=0.05,
379-
problemType=ODEProblem )
380-
381-
oder_.forwardProblem.u0 .= [1.0;0.0]
382-
sl = DifferentialEquations.solve(oder_.forwardProblem)
383-
384-
385-
## check the solve values are correct
386-
387-
@error "FIXME: Disabling numerical test on DERelative 1"
388-
# try
389-
# for sym = ls(tfg)
390-
# @test getPPE(tfg, sym).suggested - sl(getVariable(fg, sym) |> getTimestamp |> DateTime |> datetime2unix) |> norm < 0.2
391-
# end
392-
# catch
393-
# @error "FIXME: Numerical solution failures on DERelative test"
394-
# end
395-
391+
@test isapprox( getPPESuggested(tfg, :x0), x0_val_ref; atol=0.2)
392+
@test isapprox( getPPESuggested(tfg, :x1), x1_val_ref; atol=0.2)
393+
@test isapprox( getPPESuggested(tfg, :x2), x2_val_ref; atol=0.2)
394+
@test isapprox( getPPESuggested(tfg, :x3), x3_val_ref; atol=0.2)
395+
@test isapprox( getPPESuggested(tfg, :x4), x4_val_ref; atol=0.2)
396+
@test isapprox( getPPESuggested(tfg, :x5), x5_val_ref; atol=0.2)
397+
@test isapprox( getPPESuggested(tfg, :x6), x6_val_ref; atol=0.2)
398+
@test isapprox( getPPESuggested(tfg, :x7), x7_val_ref; atol=0.2)
396399

397400
##
398401

399-
400-
401402
# Plots.plot(sl,linewidth=2,xaxis="unixtime [s]",label=["ω [rad/s]" "θ [rad]"],layout=(2,1))
402403

403404
# for lb in sortDFG(ls(fg))
@@ -418,14 +419,15 @@ solveTree!(fg);
418419

419420
##
420421

421-
@error "FIXME: Disabling numerical test on DERelative 2"
422-
# try
423-
# for sym = ls(fg)
424-
# @test getPPE(fg, sym).suggested - sl(getVariable(fg, sym) |> getTimestamp |> DateTime |> datetime2unix) |> norm < 0.2
425-
# end
426-
# catch
427-
# @error "FIXME: Numerical failure during DERelative tests"
428-
# end
422+
423+
@test isapprox( getPPESuggested(fg, :x0), x0_val_ref; atol=0.2)
424+
@test isapprox( getPPESuggested(fg, :x1), x1_val_ref; atol=0.2)
425+
@test isapprox( getPPESuggested(fg, :x2), x2_val_ref; atol=0.2)
426+
@test isapprox( getPPESuggested(fg, :x3), x3_val_ref; atol=0.2)
427+
@test isapprox( getPPESuggested(fg, :x4), x4_val_ref; atol=0.2)
428+
@test isapprox( getPPESuggested(fg, :x5), x5_val_ref; atol=0.2)
429+
@test isapprox( getPPESuggested(fg, :x6), x6_val_ref; atol=0.2)
430+
@test isapprox( getPPESuggested(fg, :x7), x7_val_ref; atol=0.2)
429431

430432

431433
##

0 commit comments

Comments
 (0)