diff --git a/test/testMultihypoFMD.jl b/test/testMultihypoFMD.jl index 7294832b8..bc368ad4a 100644 --- a/test/testMultihypoFMD.jl +++ b/test/testMultihypoFMD.jl @@ -9,31 +9,37 @@ import IncrementalInference: getSample ## -@testset "test FactorMetadata is properly populated" begin + struct MyFactor{T <: SamplableBelief} <: IIF.AbstractRelativeRoots Z::T # specialSampler approach will be deprecated - specialSampler::Function + # specialSampler::Function end -## -function getSample(mf::MyFactor, N::Int=1, fmd_...) - @warn "specialSampler (mf::MyFactor,N) does not get hypo sub-selected FMD data" - @show DFG.getLabel.(fmd_[1].fullvariables) +function getSample(cf::CalcFactor{<:MyFactor}, N::Int=1) + @warn "getSample(cf::CalcFactor{<:MyFactor},::Int) does not get hypo sub-selected FMD data" + @show DFG.getLabel.(cf.metadata.fullvariables) # @assert DFG.getLabel.(fmd_[1].fullvariables) |> length < 3 "this factor is only between two variables" - return (reshape(rand(mf.Z, N),1,N),) + return (reshape(rand(cf.factor.Z, N),1,N),) end -function (mf_::MyFactor)(res,fmd,i,meas, X1,X2) - @assert DFG.getLabel.(fmd.fullvariables) |> length < 3 "this factor is only between two variables" +function (cf::CalcFactor{<:MyFactor})(res, z, X1, X2) + @assert DFG.getLabel.(cf.metadata.fullvariables) |> length < 3 "this factor is only between two variables" # just a linear difference to complete the test - res .= X2[:,i] - (X1[:,i] + meas[1][:,1]) + res .= X2 - (X1 + z) + nothing end +## + + + +@testset "test FactorMetadata is properly populated" begin + ## @@ -47,11 +53,19 @@ addVariable!(fg, :x1_b, ContinuousScalar) addFactor!(fg, [:x0], Prior(Normal())) # create the object and add it to the graph -mf = MyFactor(Normal(10,1),getSample) +mf = MyFactor( Normal(10,1) ) + +## # this sampling might error addFactor!(fg, [:x0;:x1_a;:x1_b], mf, multihypo=[1;1/2;1/2]) +## + +meas = freshSamples(fg, :x0x1_ax1_bf1, 10) + +## + solveTree!(fg); ## diff --git a/test/testmultihypothesisapi.jl b/test/testmultihypothesisapi.jl index 7f4e87439..55d3925cb 100644 --- a/test/testmultihypothesisapi.jl +++ b/test/testmultihypothesisapi.jl @@ -11,66 +11,63 @@ import IncrementalInference: getSample ## -mutable struct DevelopPrior <: AbstractPrior - x::Distribution +mutable struct DevelopPrior{T <: SamplableBelief} <: AbstractPrior + x::T end -getSample(dpl::DevelopPrior, N::Int=1) = (reshape(rand(dpl.x, N),1,N), ) +getSample(cf::CalcFactor{<:DevelopPrior}, N::Int=1) = (reshape(rand(cf.factor.x, N),1,N), ) -mutable struct DevelopLikelihood <: AbstractRelativeRoots - x::Distribution +mutable struct DevelopLikelihood{T <: SamplableBelief} <: AbstractRelativeRoots + x::T end -getSample(dpl::DevelopLikelihood, N::Int=1) = (reshape(rand(dpl.x, N),1,N), ) -function (vv::DevelopLikelihood)(res::AbstractArray{<:Real}, - userdata::FactorMetadata, - idx::Int, - meas::Tuple, - wXi::AbstractArray{<:Real,2}, - wXj::AbstractArray{<:Real,2} )::Nothing + +getSample(cf::CalcFactor{<:DevelopLikelihood}, N::Int=1) = (reshape(rand(cf.factor.x, N),1,N), ) +function (cf::CalcFactor{<:DevelopLikelihood})( res::AbstractVector{<:Real}, + meas, + wXi, + wXj ) # - res[1] = meas[1][idx] - (wXj[1,idx] - wXi[1,idx]) + res .= meas - (wXj - wXi) nothing end -global N = 100 -global fg = initfg() +N = 100 +fg = initfg() +## @testset "test populate factor graph with a multi-hypothesis factor..." begin -global v1 = addVariable!(fg, :x1, ContinuousScalar, N=N) +## -global pr = DevelopPrior(Normal(10.0,1.0)) -global f1 = addFactor!(fg,[:x1],pr) +v1 = addVariable!(fg, :x1, ContinuousScalar, N=N) +pr = DevelopPrior(Normal(10.0,1.0)) +f1 = addFactor!(fg,[:x1],pr) -ensureAllInitialized!(fg) -# Juno.breakpoint("/home/dehann/.julia/v0.5/IncrementalInference/src/ApproxConv.jl",121) +ensureAllInitialized!(fg) -global pts = evalFactor(fg, f1, v1.label, N=N) +pts = evalFactor(fg, f1, v1.label, N=N) @test sum(abs.(pts .- 1.0) .< 5) < 30 @test sum(abs.(pts .- 10.0) .< 5) > 30 - -global v2 = addVariable!(fg, :x2, ContinuousScalar, N=N) -global pp = DevelopLikelihood(Normal(100.0,1.0)) -global f2 = addFactor!(fg, [:x1;:x2], pp) +v2 = addVariable!(fg, :x2, ContinuousScalar, N=N) +pp = DevelopLikelihood(Normal(100.0,1.0)) +f2 = addFactor!(fg, [:x1;:x2], pp) ensureAllInitialized!(fg) @test abs(Statistics.mean(getVal(fg, :x2))-110.0) < 10.0 +v3 = addVariable!(fg, :x3, ContinuousScalar, N=N) +v4 = addVariable!(fg, :x4, ContinuousScalar, N=N) - -global v3 = addVariable!(fg, :x3, ContinuousScalar, N=N) -global v4 = addVariable!(fg, :x4, ContinuousScalar, N=N) - -global ppMH = DevelopLikelihood(Normal(90.0,1.0)) -global f3 = addFactor!(fg, [:x2;:x3;:x4], ppMH, multihypo=[1.0;0.5;0.5]) +ppMH = DevelopLikelihood(Normal(90.0,1.0)) +f3 = addFactor!(fg, [:x2;:x3;:x4], ppMH, multihypo=[1.0;0.5;0.5]) # @test IIIF._getCCW(f3).hypoverts == [:x3, :x4] @@ -82,27 +79,33 @@ initManual!(fg, :x2, 1*ones(1,100)) initManual!(fg, :x3, 2*ones(1,100)) initManual!(fg, :x4, 3*ones(1,100)) +## + end @testset "Test multi-hypothesis factor convolution exploration" begin -global pts = approxConv(fg, :x2x3x4f1, :x2, N=N) +## + +pts = approxConv(fg, :x2x3x4f1, :x2, N=N) @test 99 < sum(pts .<= -70.0) -global pts = approxConv(fg, :x2x3x4f1, :x3, N=N) +pts = approxConv(fg, :x2x3x4f1, :x3, N=N) @test 15 < sum(pts .== 3.0) < 75 -global pts = approxConv(fg, :x2x3x4f1, :x4, N=N) +pts = approxConv(fg, :x2x3x4f1, :x4, N=N) @test 15 < sum(pts .== 2.0) < 75 -end +## +end +## println("Packing converters") @@ -113,10 +116,10 @@ mutable struct PackedDevelopPrior <: PackedInferenceType PackedDevelopPrior(x) = new(x) end function convert(::Type{PackedDevelopPrior}, d::DevelopPrior) - PackedDevelopPrior(string(d.x)) + PackedDevelopPrior(convert(PackedSamplableBelief, d.x)) end function convert(::Type{DevelopPrior}, d::PackedDevelopPrior) - DevelopPrior(IncrementalInference.normalfromstring(d.x)) + DevelopPrior(convert(SamplableBelief, d.x)) end mutable struct PackedDevelopLikelihood <: PackedInferenceType @@ -125,56 +128,65 @@ mutable struct PackedDevelopLikelihood <: PackedInferenceType PackedDevelopLikelihood(x) = new(x) end function convert(::Type{PackedDevelopLikelihood}, d::DevelopLikelihood) - PackedDevelopLikelihood(string(d.x)) + PackedDevelopLikelihood(convert(PackedSamplableBelief, d.x)) end function convert(::Type{DevelopLikelihood}, d::PackedDevelopLikelihood) - DevelopLikelihood(IncrementalInference.extractdistribution(d.x)) + DevelopLikelihood(convert(SamplableBelief, d.x)) end +## + @testset "test packing and unpacking the data structure" begin - global topack = getSolverData(f1) - global dd = convert(PackedFunctionNodeData{PackedDevelopPrior},topack) - global unpacked = convert(FunctionNodeData{CommonConvWrapper{DevelopPrior}},dd) +## - @test abs(IIF._getCCW(unpacked).usrfnc!.x.μ - 10.0) < 1e-10 - @test abs(IIF._getCCW(unpacked).usrfnc!.x.σ - 1.0) < 1e-10 +topack = getSolverData(getFactor(fg,:x1f1)) +dd = convert(PackedFunctionNodeData{PackedDevelopPrior},topack) +unpacked = convert(FunctionNodeData{CommonConvWrapper{DevelopPrior}},dd) +@test abs(IIF._getCCW(unpacked).usrfnc!.x.μ - 10.0) < 1e-10 +@test abs(IIF._getCCW(unpacked).usrfnc!.x.σ - 1.0) < 1e-10 - global topack = getSolverData(f3) - global dd = convert(PackedFunctionNodeData{PackedDevelopLikelihood},topack) - global unpacked = convert(FunctionNodeData{CommonConvWrapper{DevelopLikelihood}},dd) +fct = getFactor(fg, :x2x3x4f1) +@show typeof(fct) +topack = getSolverData(fct) # f3 +dd = convert(PackedFunctionNodeData{PackedDevelopLikelihood},topack) +unpacked = convert(FunctionNodeData{CommonConvWrapper{DevelopLikelihood}},dd) - # @test IIF._getCCW(unpacked).hypoverts == Symbol[:x3; :x4] - @test sum(abs.(IIF._getCCW(unpacked).hypotheses.p[1] .- 0.0)) < 0.1 - @test sum(abs.(IIF._getCCW(unpacked).hypotheses.p[2:3] .- 0.5)) < 0.1 - # str = "Symbol[:x3, :x4];[0.5, 0.5]" - # IncrementalInference.parsemultihypostr(str) +# @test IIF._getCCW(unpacked).hypoverts == Symbol[:x3; :x4] +@test sum(abs.(IIF._getCCW(unpacked).hypotheses.p[1] .- 0.0)) < 0.1 +@test sum(abs.(IIF._getCCW(unpacked).hypotheses.p[2:3] .- 0.5)) < 0.1 -end +## +end + +## # start a new factor graph -global N = 200 -global fg = initfg() +N = 200 +fg = initfg() + +## @testset "test tri-modal factor..." begin +## -global v1 = addVariable!(fg, :x1, ContinuousScalar, N=N) +v1 = addVariable!(fg, :x1, ContinuousScalar, N=N) -global pr = DevelopPrior(Normal(10.0,1.0)) -global f1 = addFactor!(fg,[:x1],pr) +pr = DevelopPrior(Normal(10.0,1.0)) +f1 = addFactor!(fg,[:x1],pr) ensureAllInitialized!(fg) # Juno.breakpoint("/home/dehann/.julia/v0.5/IncrementalInference/src/ApproxConv.jl",121) -global pts = approxConv(fg, Symbol(f1.label), :x1, N=N) +pts = approxConv(fg, Symbol(f1.label), :x1, N=N) @test sum(abs.(pts .- 1.0) .< 5) < 30 @@ -182,9 +194,9 @@ global pts = approxConv(fg, Symbol(f1.label), :x1, N=N) -global v2 = addVariable!(fg, :x2, ContinuousScalar, N=N) -global pp = DevelopLikelihood(Normal(100.0,1.0)) -global f2 = addFactor!(fg, [:x1;:x2], pp) +v2 = addVariable!(fg, :x2, ContinuousScalar, N=N) +pp = DevelopLikelihood(Normal(100.0,1.0)) +f2 = addFactor!(fg, [:x1;:x2], pp) ensureAllInitialized!(fg) @@ -192,13 +204,13 @@ ensureAllInitialized!(fg) -global v3 = addVariable!(fg, :x3, ContinuousScalar, N=N) -global v4 = addVariable!(fg, :x4, ContinuousScalar, N=N) -global v5 = addVariable!(fg, :x5, ContinuousScalar, N=N) +v3 = addVariable!(fg, :x3, ContinuousScalar, N=N) +v4 = addVariable!(fg, :x4, ContinuousScalar, N=N) +v5 = addVariable!(fg, :x5, ContinuousScalar, N=N) -global ppMH = DevelopLikelihood(Normal(90.0,1.0)) -global f3 = addFactor!(fg, [:x2;:x3;:x4;:x5], ppMH, multihypo=[1.0,0.333,0.333,0.334]) +ppMH = DevelopLikelihood(Normal(90.0,1.0)) +f3 = addFactor!(fg, [:x2;:x3;:x4;:x5], ppMH, multihypo=[1.0,0.333,0.333,0.334]) @@ -216,13 +228,13 @@ initManual!(fg, :x5 ,4*ones(1,100)) # solve for certain idx -global pts = approxConv(fg, :x2x3x4x5f1, :x2, N=N) +pts = approxConv(fg, :x2x3x4x5f1, :x2, N=N) @test 0.95*N < sum(pts .<= -70.0) # solve for one of uncertain variables -global pts = approxConv(fg, :x2x3x4x5f1, :x3, N=N) +pts = approxConv(fg, :x2x3x4x5f1, :x3, N=N) @test 0.1*N < sum(80 .< pts .< 100.0) < 0.5*N @test 0.1*N < sum(pts .== 3.0) < 0.5*N @@ -233,7 +245,7 @@ global pts = approxConv(fg, :x2x3x4x5f1, :x3, N=N) # solve for one of uncertain variables -global pts = approxConv(fg, :x2x3x4x5f1, :x4, N=N) +pts = approxConv(fg, :x2x3x4x5f1, :x4, N=N) @test 0.1*N < sum(80 .< pts .< 100.0) < 0.5*N @test 0.1*N < sum(pts .== 2.0) < 0.5*N @@ -243,7 +255,7 @@ global pts = approxConv(fg, :x2x3x4x5f1, :x4, N=N) # solve for one of uncertain variables -global pts = approxConv(fg, :x2x3x4x5f1, :x5, N=N) +pts = approxConv(fg, :x2x3x4x5f1, :x5, N=N) @test 0.1*N < sum(80 .< pts .< 100.0) < 0.5*N @test 0.1*N < sum(pts .== 2.0) < 0.5*N @@ -251,14 +263,15 @@ global pts = approxConv(fg, :x2x3x4x5f1, :x5, N=N) @test 0.5*N <= sum(80 .< pts .< 100.0) + sum(pts .== 2.0) + sum(pts .== 3.0) - +## end -## @testset "test multihypo api numerical tolerance, #1086" begin +## + fg = initfg() addVariable!(fg, :x0, ContinuousEuclid{1}) @@ -268,6 +281,8 @@ addFactor!(fg, [:x0;:x1a;:x1b], LinearRelative(Normal()), multihypo=[1; 0.5;0.49 addFactor!(fg, [:x0;:x1a;:x1b], LinearRelative(Normal()), multihypo=[1; 0.5;0.5000000000001]) +## + end