Merge pull request #1109 from JuliaRobotics/21Q1/maint/cf_derelative

update multihypo for CalcFactor

Author: dehann

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);
 
 ##

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,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])
 
 
 
@@ -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,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})
@@ -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