diff --git a/examples/OutlierVsMultimodal.jl b/examples/OutlierVsMultimodal.jl new file mode 100644 index 00000000..29ee2071 --- /dev/null +++ b/examples/OutlierVsMultimodal.jl @@ -0,0 +1,291 @@ +# simulate circle multimodal vs outlier example + +# using Revise + +using ArgParse + + + +function parse_commandline() + s = ArgParseSettings() + + @add_arg_table! s begin + "--altmode" + help = "fraction for alternative mode" + arg_type = Float64 + default = 0.7 + "--resolve" + help = "should a repeat solve be performed in the loop" + action = :store_true + "--CYCLES" + help = "How many cycles to drive" + arg_type = Int + default = 10 + end + + return parse_args(s) +end + +pargs = parse_commandline() + + +using Distributed +# addprocs(8) # use -p8 instead + +using Random + +using RoME, DistributedFactorGraphs +using RoMEPlotting +using KernelDensityEstimatePlotting +@everywhere using RoME + +using FileIO +using JLD2 +using Gadfly +Gadfly.set_default_plot_size(35cm,25cm) + +include(joinpath(@__DIR__, "FixedLagCirclePlotting.jl")) + +## Landmark ground truth + +# assume all 10 landmarks changing position from rows 1:10 to 11:20, randomly after each circle +L = [0.0 20.0; # L1 + 30.0 30.0; # L2 + -10.0 20.0; # L3 + 20.0 30.0; # L4 + -20.0 30.0; # L5 + -20.0 10.0; # L6 + 20.0 -0.0; # L7 + 10.0 10.0; # L8 + 0.0 40.0; # L9 + 30.0 10.0; # L10 + 10.0 -10.0; # L1_0 + 10.0 40.0; # L2_0 + -10.0 -10.0; # L3_0 + -10.0 30.0; # L4_0 + 30.0 -0.0; # L5_0 + 20.0 20.0; # L6_0 + 0.0 10.0; # L7_0 + 30.0 20.0; # L8_0 + 20.0 -10.0; # L9_0 + -10.0 10.0] # L10_0 + +# working from previous save stored right here in the example folder +# @load string(joinpath(@__DIR__, "LassoBearingRange.jld2")) BR + +SIZE = 10 +CYCLES = pargs["CYCLES"] +LANDMARKS = 10 +altFrac = pargs["altmode"] +mainFrac = 1-pargs["altmode"] + +## divise landmark rotation schedule + + +movePerCycle = rand(Categorical([0.6;0.4]),CYCLES) +idx = sum(cumsum(movePerCycle) .< CYCLES) +movePerCycle = movePerCycle[1:idx] +push!(movePerCycle, CYCLES-cumsum(movePerCycle)[end]) +order = shuffle(1:CYCLES) +sequence = Vector{Vector{Int}}() +total = 1 +for n in 1:length(movePerCycle) + global total + push!(sequence, order[total:total+movePerCycle[n]-1] ) + total += movePerCycle[n] +end +@assert sum(union(sequence...)) == 55 + +sequence + +## convert sequence to landmark lookups + +lookup = Vector{Vector{Symbol}}() +push!(lookup, [Symbol("l$i") for i in 1:10]) + +for i in 1:9 + lu = deepcopy(lookup[i]) + # before each loop, change location of some landmarks + seq = i <= length(sequence) ? sequence[i] : Int[] + for s in seq + lu[s] = Symbol("l$(s)_0") + end + + # push new vector into lookup + push!(lookup, lu) +end + +# this is the sequence of landmark transitions +lookup + + +# BRdistr[:l1] +# BRdistr[:l1_0] + + +### SETUP first circle---------------------------------------------- + +# drive first circle +fg = generateCanonicalFG_Circle(SIZE, kappaOdo=0.1, loopClosure=false, landmark=false, cyclePoses=10) + + +ensureAllInitialized!(fg) + +## add initial landmarks + +BR = Dict{Symbol,Dict{Symbol, Tuple}}() +# add the landmarks +for i in 1:2*CYCLES + lmid = Symbol(i <= 10 ? "l$i" : "l$(i-10)_0") + # lmid = Symbol("l$i") + addVariable!(fg, lmid, RoME.Point2) + pts = rand(MvNormal(L[i,:],diagm([0.01;0.01].^2)),100) + initManual!(fg, lmid, manikde!(pts, RoME.Point2)) + setVariablePosteriorEstimates!(fg,lmid) + + BR[lmid] = Dict{Symbol,Tuple}() + varNear, varDist = findVariablesNear(fg, L[i,:], r"x\d", number=10) + for vsym in varNear[varDist .< 25] + # calculate bearing and range factor + bear, rang = predictBodyBR(fg, vsym, lmid) + if abs(bear) < pi/3 && vsym != :x10 + BR[lmid][vsym] = (bear, rang) + end + end +end +for i in CYCLES+1:2*CYCLES + lmid = Symbol(i <= 10 ? "l$i" : "l$(i-10)_0") + # vsym = Symbol("l$i") + setSolvable!(fg, lmid, 0) + # ( x->deleteFactor!(fg, x) ).( ls(fg, vsym) ) # no factors added yet + # deleteVariable!(fg, lmid) +end +# @save joinpath(@__DIR__, "LassoBearingRange.jld2") BR + + +## see what is going on + +plfl1 = drawPosesLandms(fg, spscale=1.0) + +## prepare factors to use + +BRdistr = Dict{Symbol,Dict{Symbol,Tuple}}() +for i in 1:2*CYCLES + lmid = Symbol(i <= CYCLES ? "l$i" : "l$(i-CYCLES)_0") + BRdistr[lmid] = Dict{Symbol,Tuple}() + for (vsym, br) in BR[lmid] + @show bear, rang = br + BRdistr[lmid][vsym] = (Normal(bear,0.05),Normal(rang,0.3)) + end +end + +BRdistr + + +## add factors to the landmarks on first cycle + +@assert ls(fg, :l1) |> length == 0 + +OFFSET=0 +for id in 1:CYCLES + lmid = Symbol("l$(id)") + lmid_0 = Symbol("l$(id)_0") + for (psid, ppbr) in BRdistr[lmid] + # global OFFSET + adjpsid = psid # add offset + addFactor!(fg, [adjpsid;lmid], Pose2Point2BearingRange(ppbr...)) + end +end + + +## + + +getSolverParams(fg).drawtree = true +tree, smt, hist = solveTree!(fg) + + +plfl1 = drawPosesLandms(fg, spscale=1.0) + + +# lookup +## change to alternate location, one at a time + + +# activate solvable on alternative landmark locations for remainer of test +for l in 1:10 + lmid_0 = Symbol("l$(l)_0") + setSolvable!(fg, lmid_0, 1) +end + +POSEOFFSET=0 + +## store + +@save joinLogPath(fg, "lookup.jld2") lookup + +plfl1 = drawPosesLandms(fg, spscale=1.0, landmsPPE=:max, contour=true) #, posesPPE=:max) +plfl1 |> PDF(joinLogPath(fg, "plot_x20_before.pdf"), 20cm, 17cm) + +## + +# Drive CYCLES-1 more loops +i = 2 +for i in 2:CYCLES-1 + +global fg +global lookup +global SIZE +global BRdistr +global pargs +global POSEOFFSET += 10 +# drive the new loop without landmark detections (dont solve yet) +fg = generateCanonicalFG_Circle(i*SIZE, fg=fg, kappaOdo=0.1, loopClosure=false, landmark=false, cyclePoses=10) + +# add modified landmark sighting measurements +for l in 1:LANDMARKS + lmidBr = lookup[i][l] + lmid = Symbol("l$(l)") + lmid_0 = Symbol("l$(l)_0") + for (psid, ppbr) in BRdistr[lmidBr] + # apply pose offset + psnum = parse(Int, string(psid)[2:end]) + POSEOFFSET + opsid = Symbol("x$psnum") + # add the few BR factors from this lmid accordingly + addFactor!(fg, [opsid;lmid;lmid_0], Pose2Point2BearingRange(ppbr...), multihypo=[1;mainFrac;altFrac]) + end +end + +getSolverParams(fg).dbg = true +saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)before")) +tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg)) +saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_solve")) + +plfl1 = drawPosesLandms(fg, spscale=1.0, landmsPPE=:max, contour=true) +plfl1 |> PDF(joinLogPath(fg, "plot_x$(POSEOFFSET+10)_solve.pdf"), 20cm, 17cm) + +if pargs["resolve"] + tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg)) + saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_resolve")) + plfl1 = drawPosesLandms(fg, spscale=1.0, landmsPPE=:max, contour=true) + plfl1 |> PDF(joinLogPath(fg, "plot_x$(POSEOFFSET+10)_resolve.pdf"), 20cm, 17cm) +end + +end + + + +## test solve + +getSolverParams(fg).dbg = true +tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg)) #[:l9_0;:x14]) +saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_final")) + + +## + +plfl1 = drawPosesLandms(fg, spscale=1.0, landmsPPE=:max) +plfl1 |> PDF(joinLogPath(fg, "plot_x$(POSEOFFSET+10)_final.pdf"), 20cm, 17cm) + + +# diff --git a/src/CanonicalGraphs.jl b/src/CanonicalGraphs.jl index 7b00d114..3756622b 100644 --- a/src/CanonicalGraphs.jl +++ b/src/CanonicalGraphs.jl @@ -34,7 +34,8 @@ function generateCanonicalFG_Circle(poses::Int=6; loopClosure::Bool=true, stopEarly::Int=9999999, biasTurn::Real=0.0, - kappaOdo::Real=1.0 ) + kappaOdo::Real=1.0, + cyclePoses::Int=poses ) # assume empty factor graph object fg @assert offsetPoses < poses "`offsetPoses` must be smaller than total number of `poses`" # IIF.getSolverParams(fg).drawtree = true @@ -56,7 +57,7 @@ function generateCanonicalFG_Circle(poses::Int=6; @show psym = Symbol("x$i") @show nsym = Symbol("x$(i+1)") addVariable!(fg, nsym, Pose2) - pp = Pose2Pose2(MvNormal([10.0;0;2pi/(poses)+biasTurn], Matrix(Diagonal((kappaOdo*[0.1;0.1;0.1]).^2)))) + pp = Pose2Pose2(MvNormal([10.0;0;2pi/(cyclePoses)+biasTurn], Matrix(Diagonal((kappaOdo*[0.1;0.1;0.1]).^2)))) addFactor!(fg, [psym;nsym], pp , autoinit=autoinit) end