Merge pull request #665 from JuliaRobotics/feat/2Q20/pool624

fix #624, #83, add #236

src/ApproxConv.jl

@@ -279,14 +279,14 @@ function evalPotentialSpecific(Xi::Vector{DFGVariable},
                                solvefor::Symbol,
                                measurement::Tuple=(zeros(0,100),);
                                N::Int=size(measurement[1],2),
-                               spreadfactor::Real=10,
+                               spreadNH::Real=3.0,
                                dbg::Bool=false ) where {T <: FunctorPairwiseNH}
   #
   @warn "FunctorPairwiseNH will be deprecated in favor of common `nullhypo=` interface."
   # TODO -- could be constructed and maintained at addFactor! time
   sfidx, maxlen = prepareCommonConvWrapper!(ccwl, Xi, solvefor, N)
   # prepare nullhypothesis
-  allelements, nhc, ENT = assembleNullHypothesis(ccwl, maxlen, spreadfactor)
+  allelements, nhc, ENT = assembleNullHypothesis(ccwl, maxlen, spreadNH)
 
   # Compute across the true or null hypothesis
   computeAcrossNullHypothesis!(ccwl, allelements, nhc, ENT )
@@ -299,8 +299,8 @@ function evalPotentialSpecific(Xi::Vector{DFGVariable},
                                solvefor::Symbol,
                                measurement::Tuple=(zeros(0,100),);
                                N::Int=size(measurement[1],2),
-                               dbg::Bool=false,
-                               spreadNH::Float64=3.0 ) where {T <: Union{FunctorPairwise, FunctorPairwiseMinimize}}
+                               spreadNH::Real=3.0,
+                               dbg::Bool=false  ) where {T <: Union{FunctorPairwise, FunctorPairwiseMinimize}}
   #
 
   # Prep computation variables

src/CliqStateMachineUtils.jl

@@ -986,7 +986,8 @@ function solveCliqDownFrontalProducts!(subfg::G,
     downresult = Dict{Symbol, Tuple{BallTreeDensity, Float64, Vector{Symbol}}}()
     @sync for i in 1:length(directs)
       @async begin
-        downresult[directs[i]] = remotecall_fetch(localProductAndUpdate!, upp2(), subfg, directs[i], false)
+        downresult[directs[i]] = remotecall_fetch(localProductAndUpdate!, getWorkerPool(), subfg, directs[i], false)
+        # downresult[directs[i]] = remotecall_fetch(localProductAndUpdate!, upp2(), subfg, directs[i], false)
       end
     end
     with_logger(logger) do
@@ -1000,7 +1001,8 @@ function solveCliqDownFrontalProducts!(subfg::G,
     end
     for mc in 1:MCIters, fr in iterFrtls
       try
-        result = remotecall_fetch(localProductAndUpdate!, upp2(), subfg, fr, false)
+        result = remotecall_fetch(localProductAndUpdate!, getWorkerPool(), subfg, fr, false)
+        # result = remotecall_fetch(localProductAndUpdate!, upp2(), subfg, fr, false)
         setValKDE!(subfg, fr, result[1], false, result[2])
         with_logger(logger) do
           @info "cliq $(cliq.index), solveCliqDownFrontalProducts!, iter key=$(fr), infdim=$(result[2]), lbls=$(result[3])"

src/FactorGraphTypes.jl

@@ -68,7 +68,7 @@ mutable struct SolverParams <: DFG.AbstractParams
                 async::Bool=false,
                 limititers::Int=500,
                 N::Int=100,
-                multiproc::Bool=true,
+                multiproc::Bool=1 < nprocs(),
                 logpath::String="/tmp/caesar/$(now())",
                 graphinit::Bool=true,
                 treeinit::Bool=false,

src/SolveTree01.jl

@@ -4,6 +4,8 @@ global pidl = 1
 global pidA = 1
 global thxl = nprocs() > 4 ? floor(Int,nprocs()*0.333333333) : 1
 
+global WORKERPOOL = WorkerPool()
+
 # upploc to control processes done local to this machine and separated from other
 # highly loaded processes. upploc() should be used for dispatching short and burst
 # of high bottle neck computations. Use upp2() for general multiple dispatch.
@@ -32,6 +34,15 @@ function uppA()
   return pidA
 end
 
+function setWorkerPool!(pool::Vector{Int}=setdiff(procs(), [1;]))
+  global WORKERPOOL
+  WORKERPOOL = WorkerPool(pool)
+end
+
+function getWorkerPool()
+  global WORKERPOOL
+  return WORKERPOOL
+end
 
 function packFromIncomingDensities!(dens::Vector{BallTreeDensity},
                                     wfac::Vector{Symbol},
@@ -1014,7 +1025,8 @@ function approxCliqMarginalUp!(fgl::AbstractDFG,
     ett.cliq = cliqc
     # TODO create new dedicate file for separate process to log with
     try
-      urt = remotecall_fetch(upGibbsCliqueDensity, upp2(), ett, N, dbg, iters)
+      urt = remotecall_fetch(upGibbsCliqueDensity, getWorkerPool(), ett, N, dbg, iters)
+      # urt = remotecall_fetch(upGibbsCliqueDensity, upp2(), ett, N, dbg, iters)
     catch ex
       with_logger(logger) do
         @info ex

src/SolverAPI.jl

@@ -31,6 +31,13 @@ function solveTree!(dfgl::G,
   # workaround in case isolated variables occur
   ensureSolvable!(dfgl)
 
+  # update worker pool incase there are more or less
+  setWorkerPool!()
+  if getSolverParams(dfgl).multiproc && nprocs() == 1
+    @warn "Cannot use multiproc with only one process, setting `.multiproc=false`."
+    getSolverParams(dfgl).multiproc = false
+  end
+
   if getSolverParams(dfgl).graphinit
     @info "ensure all initialized (using graphinit)"
     ensureAllInitialized!(dfgl)
@@ -148,6 +155,13 @@ function solveTreeParametric!(dfgl::DFG.AbstractDFG,
   hist = Dict{Int, Vector{Tuple{DateTime, Int, Function, CliqStateMachineContainer}}}()
   opt = DFG.getSolverParams(dfgl)
 
+  # update worker pool incase there are more or less
+  setWorkerPool!()
+  if getSolverParams(dfgl).multiproc && nprocs() == 1
+    @warn "Cannot use multiproc with only one process, setting `.multiproc=false`."
+    getSolverParams(dfgl).multiproc = false
+  end
+
   @info "Do tree based init-inference"
   # if opt.async
   smtasks, hist = taskSolveTreeParametric!(dfgl, tree, oldtree=tree, drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs )

test/runtests.jl

@@ -91,7 +91,7 @@ if Base.Sys.ARCH in [:x86_64;]
   include("testTexTreeIllustration.jl")
 end
 
-
+include("testMultiprocess.jl")
 
 
 

test/testExplicitMultihypo.jl

@@ -49,7 +49,7 @@ end
 # sfidx=1, mhidx=2:  ah = [1;2]
 # sfidx=1, mhidx=3:  ah = [1;3]
 s3_1_gt1 = [1]
-s3_1_gt2 = (0,4,4,40)
+s3_1_gt2 = (0,3,3,40)
 s3_1_gt3 = [(1,Int[]); (2,Int[1;2]); (3,Int[1;3])]
 s3_1_gt4 = 40
 
@@ -85,7 +85,7 @@ end
 # sfidx=2, mhidx=2:  ah = [2;3]
 # sfidx=2, mhidx=3:  [2;3], 2 should take a value from 3
 s3_2_gt1 = [1]
-s3_2_gt2 = (0,4,4,40)
+s3_2_gt2 = (0,3,3,40)
 s3_2_gt3 = [(0, Int[2]); (1,Int[1;2]); (2,Int[1;2]); (3,Int[2;3])]
 s3_2_gt4 = 40
 
@@ -124,7 +124,7 @@ end
 # sfidx=3, mhidx=2:  [2:3], 3 should take a value from 2
 # sfidx=3, mhidx=3:  ah = [1;3]
 s3_3_gt1 = [1]
-s3_3_gt2 = (0,4,4,40)
+s3_3_gt2 = (0,3,3,40)
 s3_3_gt3 = [(0, Int[3]); (1,Int[1;3]); (2,Int[2;3]); (3,Int[1;3])]
 s3_3_gt4 = 40
 
@@ -165,7 +165,7 @@ end
 # sfidx=1, mhidx=2:  ah = [1;2]
 # sfidx=1, mhidx=3:  ah = [1;3]
 # s3_1_gt1 = [1]
-# s3_1_gt2 = (0,4,4,20)
+# s3_1_gt2 = (0,3,3,20)
 # s3_1_gt3 = [(1,Int[]); (2,Int[1;2]); (3,Int[1;3])]
 # s3_1_gt4 = 20
 #
@@ -200,7 +200,7 @@ end
 
 N = 50
 s4_1_gt1 = [1]
-s4_1_gt2 = (0,4,4,4,N)
+s4_1_gt2 = (0,3,3,3,N)
 s4_1_gt3 = [(1,Int[]); (2,Int[1;2]); (3,Int[1;3]); (4,Int[1;4])]
 s4_1_gt4 = N
 
@@ -240,7 +240,7 @@ end
 
 N = 50
 s4_2_gt1 = [1]
-s4_2_gt2 = (0,4,4,4,N)
+s4_2_gt2 = (0,3,3,3,N)
 s4_2_gt3 = [(0,Int[2]); (1,Int[1;2]); (2,Int[1;2]); (3,Int[2;3;4]); (4,Int[2;3;4])]
 s4_2_gt4 = N
 
@@ -283,7 +283,7 @@ end
 
 N = 50
 s4_3_gt1 = [1]
-s4_3_gt2 = (0,4,4,4,N)
+s4_3_gt2 = (0,3,3,3,N)
 s4_3_gt3 = [(0,Int[3]); (1,Int[1;3]); (2,Int[2;3;4]); (3,Int[1;3]); (4,Int[2;3;4])]
 s4_3_gt4 = N
 
@@ -325,7 +325,7 @@ end
 
 N = 50
 s4_4_gt1 = [1]
-s4_4_gt2 = (0,4,4,4,N)
+s4_4_gt2 = (0,3,3,3,N)
 s4_4_gt3 = [(0,Int[4]); (1,Int[1;4]); (2,Int[2;3;4]); (3,Int[2;3;4]); (4,Int[1;4])]
 s4_4_gt4 = N
 

test/testMultiHypo2Door.jl

@@ -28,10 +28,10 @@ x3 = 40.0
 fg = initfg()
 fg.solverParams.N = n_samples
 # Place strong prior on locations of three "doors"
-addVariable!(fg, Symbol("l0"), ContinuousScalar, N=n_samples)
+addVariable!(fg, :l0, ContinuousScalar, N=n_samples)
 addFactor!(fg, [:l0], Prior(Normal(l0, lm_prior_noise)))
 
-addVariable!(fg, Symbol("l1"), ContinuousScalar, N=n_samples)
+addVariable!(fg, :l1, ContinuousScalar, N=n_samples)
 addFactor!(fg, [:l1], Prior(Normal(l1, lm_prior_noise)))
 
 
@@ -66,22 +66,93 @@ tree, smt, hist = solveTree!(fg)
 # drawTree(tree, show=true)
 
 
-@test abs(getKDEMean(getKDE(fg, :x0))[1]) < 2.0
-@test abs(getKDEMean(getKDE(fg, :x1))[1]-10) < 2.0
-@test abs(getKDEMean(getKDE(fg, :x2))[1]-20) < 2.0
+@test abs(getKDEMean(getKDE(fg, :x0))[1]-x0) < 2.0
+@test abs(getKDEMean(getKDE(fg, :x1))[1]-x1) < 2.0
+@test abs(getKDEMean(getKDE(fg, :x2))[1]-x2) < 2.0
 
-@test abs(getKDEMean(getKDE(fg, :l0))[1]) < 3.0
-@test abs(getKDEMean(getKDE(fg, :l1))[1]-10) < 3.0
+@test abs(getKDEMean(getKDE(fg, :l0))[1]-l0) < 3.0
+@test abs(getKDEMean(getKDE(fg, :l1))[1]-l1) < 3.0
 
 
 0
 end
 
 #
 # using RoMEPlotting
+# Gadfly.set_default_plot_size(35cm,25cm)
 #
 # plotKDE(fg, [:l0;:l1])
 # plotKDE(fg, [:x0;:x1;:x2])
 
 
+@testset "3door basic trinary multihypo test..." begin
+
+# Initialize empty factor graph
+fg = initfg()
+
+# Place strong prior on locations of three "doors"
+addVariable!(fg, :l0, ContinuousScalar, N=n_samples)
+addFactor!(fg, [:l0], IIF.Prior(Normal(l0, lm_prior_noise)))
+
+addVariable!(fg, :l1, ContinuousScalar, N=n_samples)
+addFactor!(fg, [:l1], IIF.Prior(Normal(l1, lm_prior_noise)))
+
+addVariable!(fg, :l2, ContinuousScalar, N=n_samples)
+addFactor!(fg, [:l2], IIF.Prior(Normal(l2, lm_prior_noise)))
+
+# Add first pose
+addVariable!(fg, :x0, ContinuousScalar, N=n_samples)
+
+# Make first "door" measurement
+addFactor!(fg, [:x0; :l0; :l1; :l2], LinearConditional(Normal(0, meas_noise)), multihypo=[1.0; 1.0/3.0; 1.0/3.0; 1.0/3.0])
+
+# Add second pose
+addVariable!(fg, :x1, ContinuousScalar, N=n_samples)
+
+# # Gaussian transition model
+addFactor!(fg, [:x0; :x1], LinearConditional(Normal(x1-x0, odom_noise)))
+
+# # Make second "door" measurement
+addFactor!(fg, [:x1; :l0; :l1; :l2], LinearConditional(Normal(0, meas_noise)), multihypo=[1.0; 1.0/3.0; 1.0/3.0; 1.0/3.0])
+
+# # Add third pose
+addVariable!(fg, :x2, ContinuousScalar, N=n_samples)
+
+# # Gaussian transition model
+addFactor!(fg, [:x1; :x2], LinearConditional(Normal(x2-x1, odom_noise)))
+
+# Add fourth pose
+addVariable!(fg, :x3, ContinuousScalar, N=n_samples)
+
+# Add odometry transition and new landmark sighting
+addFactor!(fg, [:x2, :x3], LinearConditional(Normal(x3-x2, odom_noise)))
+addFactor!(fg, [:x3; :l0; :l1; :l2], LinearConditional(Normal(0, meas_noise)), multihypo=[1.0; 1.0/3.0; 1.0/3.0; 1.0/3.0])
+
+
+ensureAllInitialized!(fg)
+
+# drawGraph(fg)
+solveTree!(fg)
+
+
+
+@test abs(getKDEMean(getKDE(fg, :x0))[1]-x0) < 2.0
+@test abs(getKDEMean(getKDE(fg, :x1))[1]-x1) < 2.0
+@test abs(getKDEMean(getKDE(fg, :x2))[1]-x2) < 2.0
+
+@test abs(getKDEMean(getKDE(fg, :l0))[1]-l0) < 3.0
+@test abs(getKDEMean(getKDE(fg, :l1))[1]-l1) < 3.0
+@test abs(getKDEMean(getKDE(fg, :l2))[1]-l2) < 3.0
+
+
+end
+
+
+# using RoMEPlotting
+# Gadfly.set_default_plot_size(35cm,25cm)
+#
+# plotKDE(fg, [:l0;:l1;:l2])
+# plotKDE(fg, [:x0;:x1;:x2])
+
+
 #

test/testMultiprocess.jl

@@ -0,0 +1,14 @@
+using Test
+using Distributed
+
+addprocs(2)
+using IncrementalInference
+@everywhere using IncrementalInference
+
+@testset "test multiprocess solveTree!" begin
+
+fg = generateCanonicalFG_Kaess()
+
+solveTree!(fg)
+
+end

test/testmultihypothesisapi.jl

@@ -228,7 +228,7 @@ global pts = approxConv(fg, :x2x3x4x5f1, :x3, N=N)
 @test 0.1*N < sum(pts .== 4.0) < 0.5*N
 
 # 0.7 to accomodate bad-init null hypo
-@test 0.7*N <= sum(70 .< pts .< 110.0) + sum(pts .== 3.0) + sum(pts .== 4.0)
+@test 0.5*N <= sum(70 .< pts .< 110.0) + sum(pts .== 3.0) + sum(pts .== 4.0)
 
 
 # solve for one of uncertain variables