Skip to content

update multihypo for CalcFactor #1109

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
dehann merged 1 commit into from
Jan 5, 2021
Merged

update multihypo for CalcFactor #1109

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 25 additions & 11 deletions test/testMultihypoFMD.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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


##

Expand All @@ -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);

##
Expand Down
161 changes: 88 additions & 73 deletions test/testmultihypothesisapi.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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")

Expand All @@ -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
Expand All @@ -125,80 +128,89 @@ 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
@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



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])



Expand All @@ -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
Expand All @@ -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
Expand All @@ -243,22 +255,23 @@ 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
@test 0.1*N < sum(pts .== 3.0) < 0.5*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})
Expand All @@ -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


Expand Down