Merge pull request #969 from JuliaRobotics/main/4Q20/general

improve testing and docs

Author: dehann

src/FactorGraphTypes.jl

@@ -11,7 +11,8 @@ $(TYPEDEF)
 Solver parameters for the DistributedFactoGraph.
 
 Dev Notes
-- TODO remove NothingUnion
+- # TODO remove NothingUnion
+- # TODO Upgrade to common @kwargs struct approach
 """
 mutable struct SolverParams <: DFG.AbstractParams
   dimID::Int
@@ -110,26 +111,26 @@ function initfg(dfg::T=InMemDFGType(solverParams=SolverParams());
                                     sessionname="NA",
                                     robotname="",
                                     username="",
-                                    cloudgraph=nothing)::T where T <: AbstractDFG
+                                    cloudgraph=nothing) where T <: AbstractDFG
   #
   return dfg
 end
 
 
 #init an empty fg with a provided type and SolverParams
-function initfg(::Type{T}; solverParams=SolverParams(),
-                           sessionname="NA",
-                           robotname="",
-                           username="",
-                           cloudgraph=nothing)::AbstractDFG where T <: AbstractDFG
+function initfg(::Type{T};solverParams=SolverParams(),
+                          sessionname="NA",
+                          robotname="",
+                          username="",
+                          cloudgraph=nothing) where T <: AbstractDFG
   return T(solverParams=solverParams)
 end
 
-function initfg(::Type{T}, solverParams::SolverParams;
-                           sessionname="NA",
-                           robotname="",
-                           username="",
-                           cloudgraph=nothing)::AbstractDFG where T <: AbstractDFG
+function initfg(::Type{T},solverParams::SolverParams;
+                          sessionname="NA",
+                          robotname="",
+                          username="",
+                          cloudgraph=nothing) where T <: AbstractDFG
   return T{SolverParams}(solverParams=solverParams)
 end
 
@@ -188,15 +189,15 @@ mutable struct ConvPerThread
   ConvPerThread() = new()
 end
 
-function ConvPerThread(X::Array{Float64,2},
-                       zDim::Int;
-                       factormetadata::FactorMetadata=FactorMetadata(),
-                       particleidx::Int=1,
-                       activehypo= 1:length(params),
-                       p=collect(1:size(X,1)),
-                       perturb=zeros(zDim),
-                       Y=zeros(size(X,1)),
-                       res=zeros(zDim)  )
+function ConvPerThread( X::Array{Float64,2},
+                        zDim::Int;
+                        factormetadata::FactorMetadata=FactorMetadata(),
+                        particleidx::Int=1,
+                        activehypo= 1:length(params),
+                        p=collect(1:size(X,1)),
+                        perturb=zeros(zDim),
+                        Y=zeros(size(X,1)),
+                        res=zeros(zDim)  )
   #
   cpt = ConvPerThread()
   cpt.thrid_ = 0
@@ -239,26 +240,26 @@ mutable struct CommonConvWrapper{T<:FunctorInferenceType} <: FactorOperationalMe
 end
 
 
-function CommonConvWrapper(fnc::T,
-                           X::Array{Float64,2},
-                           zDim::Int,
-                           params::Vector{Array{Float64,2}};
-                           factormetadata::FactorMetadata=FactorMetadata(),
-                           specialzDim::Bool=false,
-                           partial::Bool=false,
-                           hypotheses=nothing,
-                           certainhypo=nothing,
-                           activehypo= 1:length(params),
-                           nullhypo::Real=0,
-                           varidx::Int=1,
-                           measurement::Tuple=(zeros(0,1),),
-                           particleidx::Int=1,
-                           p=collect(1:size(X,1)),
-                           perturb=zeros(zDim),
-                           Y=zeros(size(X,1)),
-                           xDim=size(X,1),
-                           res=zeros(zDim),
-                           threadmodel=MultiThreaded  ) where {T<:FunctorInferenceType}
+function CommonConvWrapper( fnc::T,
+                            X::Array{Float64,2},
+                            zDim::Int,
+                            params::Vector{Array{Float64,2}};
+                            factormetadata::FactorMetadata=FactorMetadata(),
+                            specialzDim::Bool=false,
+                            partial::Bool=false,
+                            hypotheses=nothing,
+                            certainhypo=nothing,
+                            activehypo= 1:length(params),
+                            nullhypo::Real=0,
+                            varidx::Int=1,
+                            measurement::Tuple=(zeros(0,1),),
+                            particleidx::Int=1,
+                            p=collect(1:size(X,1)),
+                            perturb=zeros(zDim),
+                            Y=zeros(size(X,1)),
+                            xDim=size(X,1),
+                            res=zeros(zDim),
+                            threadmodel=MultiThreaded  ) where {T<:FunctorInferenceType}
   #
   ccw = CommonConvWrapper{T}()
 

test/testMixturePrior.jl

@@ -11,28 +11,40 @@ using Test
 # init graph
 fg = initfg()
 N = 100
-fg.solverParams.N = N;
+getSolverParams(fg).N = N;
 
 # add first variable
 addVariable!(fg, :x0, ContinuousScalar)
 
+# also test AliasingScalingSampler
+v = rand(50)
+v[20:29] .+= 5*rand(10)
+v ./= sum(v)
+bss = AliasingScalarSampler(collect(1:50), v)
+
+
 # add bi-modal mixture prior
-Prior0 = MixturePrior((a= Normal(-5.0,1.0), b=Uniform(0.0,1.0)), (0.5,0.5))
-Prior0 = MixturePrior((Normal(-5.0,1.0), Normal(0.0,1.0)), (0.5,0.5))
-Prior0 = MixturePrior([Normal(-5.0,1.0), Normal(0.0,1.0)], (0.5,0.5))
-Prior0 = MixturePrior((Normal(-5.0,1.0), Normal(0.0,1.0)), [0.5;0.5])
-Prior0 = MixturePrior([Normal(-5.0,1.0), Normal(0.0,1.0)], [0.5;0.5])
-Prior0 = MixturePrior([Normal(-5.0,1.0), Normal(0.0,1.0)], Categorical([0.5;0.5]))
-addFactor!(fg, [:x0], Prior0)
+Prior0 = Mixture(Prior,(a=Normal(-5.0,1.0), b=Uniform(0.0,1.0)), (0.5,0.5))
+Prior0 = Mixture(Prior,(Normal(-5.0,1.0), Normal(0.0,1.0)), (0.5,0.5))
+Prior0 = Mixture(Prior,[Normal(-5.0,1.0), Normal(0.0,1.0)], (0.5,0.5))
+Prior0 = Mixture(Prior,(Normal(-5.0,1.0), Normal(0.0,1.0)), [0.5;0.5])
+Prior0 = Mixture(Prior,[Normal(-5.0,1.0), Normal(0.0,1.0)], [0.5;0.5])
+Prior0 = Mixture(Prior,(Normal(-5.0,1.0), bss), Categorical([0.5;0.5]))
+f1 = addFactor!(fg, [:x0], Prior0)
+
+# also test serialization of AliasingScalarSampler
+saveDFG("/tmp/test_fg_bss", fg)
 
+
+# check numerics
 smpls, lb = getSample(Prior0, N)
 
 # should be a balance of particles
 @test sum(lb .== 1) - sum(lb .== 2) |> abs < 0.3*N
 @test sum(smpls .< -2.5) - sum(-2.5 .< smpls) |> abs < 0.3*N
 
 # solve
-solveTree!(fg)
+solveTree!(fg);
 
 marginalPts = getBelief(fg, :x0) |> getPoints
 
@@ -43,8 +55,33 @@ marginalPts = getBelief(fg, :x0) |> getPoints
 end
 
 
+
+@testset "Serialization of Mixture(Prior,..) including a AliasingScalarSampler" begin
+
+
+fg_ = loadDFG("/tmp/test_fg_bss")
+
+N = getSolverParams(fg_).N
+
+
+solveTree!(fg_);
+
+
+marginalPts = getBelief(fg_, :x0) |> getPoints
+
+# check solver solution consistent too
+@test sum(marginalPts .< -2.5) - sum(-2.5 .< marginalPts) |> abs < 0.25*N
+
+
+# cleanup
+Base.rm("/tmp/test_fg_bss.tar.gz")
+
+end
+
+
 # using RoMEPlotting
-#
+# Gadfly.set_default_plot_size(35cm,20cm)
+
 # # plot the results
 # plotKDE(fg, :x0)