Merge pull request #233 from JuliaRobotics/master

v0.5.4-rc1

.github/workflows/CompatHelper.yml

@@ -0,0 +1,26 @@
+name: CompatHelper
+
+on:
+  schedule:
+    - cron: '20 00 * * *'
+  issues:
+    types: [opened, reopened]
+
+jobs:
+  CompatHelper:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        julia-version: [1.2.0]
+        julia-arch: [x86]
+        os: [ubuntu-latest]
+    steps:
+      - uses: julia-actions/setup-julia@latest
+        with:
+          version: ${{ matrix.julia-version }}
+      - name: Pkg.add("CompatHelper")
+        run: julia -e 'using Pkg; Pkg.add("CompatHelper")'
+      - name: CompatHelper.main()
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+        run: julia -e 'using CompatHelper; CompatHelper.main()'

.travis.yml

@@ -9,7 +9,6 @@ arch:
 
 julia:
   - 1.0
-  - 1.1
   - 1.2
   - 1.3
   - nightly

Project.toml

@@ -2,7 +2,7 @@ name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 keywords = ["SLAM", "state-estimation", "mm-iSAM", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.5.3"
+version = "0.5.4"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -36,7 +36,7 @@ Graphs = "0.10.2, 0.11, 1"
 IncrementalInference = "^0.8.2"
 JLD2 = "0.1, 0.2, 0.3, 1.0"
 KernelDensityEstimate = "0.5.1, 0.6"
-Optim = "0.16, 0.17, 0.18, 0.19, 0.20, 0.21, 0.22, 0.23 1"
+Optim = "0.16, 0.17, 0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 1"
 ProgressMeter = "0.6, 0.7, 0.8, 0.9, 1"
 Reexport = "0.2, 0.3, 0.4, 0.5, 1"
 Rotations = "0.11, 0.12, 0.13, 0.14, 0.15, 0.16, 1"

examples/FixedLagCirclePlotting.jl

@@ -0,0 +1,64 @@
+
+
+function plotCirc10BA(fg_::AbstractDFG,
+                      fg::AbstractDFG;
+                      pointsList_::Vector{Symbol}=Symbol[],
+                      pointsList::Vector{Symbol}=Symbol[],
+                      filepath::AbstractString="/tmp/test.pdf",
+                      lineColor_::String="gray40",
+                      lineColor::String="green",
+                      ellipseList_=ls(fg_),
+                      ellipseColor_::String="gray60",
+                      ellipseColor::String="gray20",
+                      drawEllipse::Bool=true,
+                      contourList_::Vector{Symbol}=Symbol[],
+                      contourColor_::String="red",
+                      contourLineWidth_=1pt,
+                      levels::Int=3,
+                      show::Bool=true,
+                      width=10cm, height=8cm)
+  #
+  plfl4 = drawPosesLandms(fg_, spscale=1.5, manualColor=lineColor_, point_size=4pt, drawhist=false, contour=false, levels=2, lbls=false)
+
+  for vsym in ellipseList_
+    PL = plotCovEllipseLayer(fg_, vsym, points=vsym in pointsList_ || vsym in pointsList, ellipseColor=ellipseColor_)
+    if vsym != :x10
+      for ll in PL
+        push!(plfl4.layers, ll)
+      end
+    end
+  end
+
+  if exists(fg, :x9)
+    PL = plotCovEllipseLayer(fg, :x9, points=:x9 in pointsList, ellipseColor=ellipseColor, drawEllipse=drawEllipse)
+    for ll in PL
+      push!(plfl4.layers, ll)
+    end
+  end
+
+  plfl5 = drawPosesLandms(fg, spscale=1.5, point_size=3pt, manualColor=lineColor, drawhist=false, contour=false, line_width=2pt)
+
+  for ll in plfl4.layers
+    push!(plfl5.layers, ll)
+  end
+
+  for conl in contourList_
+    X = getKDE(fg_, conl)
+    plcon = plotKDEContour(marginal(X,[1;2]), levels=levels, c=[contourColor_], line_width=contourLineWidth_)
+    for ll in plcon.layers
+      push!(plfl5.layers, ll)
+    end
+  end
+
+  plfl5.coord = Coord.Cartesian(xmin=-20,xmax=30,ymin=-5,ymax=40)
+
+  plfl5 |> PDF("$filepath", width, height);
+  if show
+    @async run(`evince $filepath`)
+  end
+  plfl5
+end
+
+
+
+#

examples/FixedLagCircles.jl

@@ -0,0 +1,219 @@
+
+# circle trajectories to show marginalization
+using Distributed
+addprocs(4)
+
+using RoME, RoMEPlotting, DistributedFactorGraphs, KernelDensityEstimatePlotting
+@everywhere using RoME
+
+Gadfly.set_default_plot_size(35cm,25cm)
+
+include(joinpath(@__DIR__, "FixedLagCirclePlotting.jl"))
+
+
+SIZE = 10
+
+### SETUP first half of circle----------------------------------------------
+
+# drive first half
+fg = generateCanonicalFG_Circle(SIZE, stopEarly=round(Int, SIZE/2), biasTurn=-0.05)
+
+getSolverParams(fg).drawtree = true
+getSolverParams(fg).showtree = true
+# getSolverParams(fg).N = 200
+# tree = wipeBuildNewTree!(fg)
+# drawTree(tree, filepath=joinLogPath(fg, "bt5.pdf"))
+
+
+tree, smt, hist = solveTree!(fg, recordcliqs=ls(fg))
+
+# plfl1 = drawPosesLandms(fg, spscale=1.0)
+fg5a = deepcopy(fg)
+tree5 = deepcopy(tree)
+
+plotCirc10BA(fg5a, fg5a, filepath=joinLogPath(fg, "circ5A.pdf"), pointsList_=[:l1])
+pl5blue = plotCirc10BA(fg5a, fg5a, filepath=joinLogPath(fg, "circ5A_blue.pdf"), lineColor="deepskyblue")
+
+saveDFG(fg5a, joinLogPath(fg5a,"fg_5A"))
+
+# drive second half
+
+generateCanonicalFG_Circle(SIZE, fg=fg, biasTurn=-0.05, loopClosure=true, kappaOdo=3)
+
+ensureAllInitialized!(fg)
+
+# keep copy for later
+fg_ = deepcopy(fg)
+
+saveDFG(fg_, joinLogPath(fg_,"fg_10B"))
+
+
+
+### SHOW Incremental Recycling--------------------------------------------------------
+
+fgI = deepcopy(fg_)
+
+tree, smt, hist = solveTree!(fgI, deepcopy(tree5), recordcliqs=ls(fgI))
+
+
+plotCirc10BA(fg_, fgI, filepath=joinLogPath(fg, "circ$(SIZE)IncrBA.pdf"), pointsList_=[:x8])
+
+
+saveDFG(fgI, joinLogPath(fgI,"fg_IncrA"))
+
+
+
+
+## SHOW MARGINALIZATION---------------------------------------------------------
+
+fgM = deepcopy(fg_)
+
+defaultFixedLagOnTree!(fgM, round(Int, SIZE/2))
+
+# use slightly special variable ordering
+vo = getEliminationOrder(fgM)
+filter!(x->!(x in [:x5;:x4]), vo)
+push!(vo, :x4)
+push!(vo, :x5)
+
+tree, smt, hist = solveTree!(fgM, recordcliqs=ls(fgM), variableOrder=vo)
+
+## Plot ellipses to illustrate covariance fit
+
+pl10marg = plotCirc10BA(fg_, fgM, filepath=joinLogPath(fgM, "circ$(SIZE)MargBA.pdf"), pointsList=[:x5])
+
+for ll in pl10marg.layers
+  push!(pl5blue.layers, ll)
+end
+pl5blue |> PDF(joinLogPath(fgM, "circ$(SIZE)MargBA_blue.pdf"), 10cm, 8cm)
+
+
+
+saveDFG(fgM, joinLogPath(fgM,"fg_MargA"))
+
+
+
+
+## SHOW BOTH MARG AND INCR TOGETHER---------------------------------------------
+
+fgR = deepcopy(fg_)
+
+defaultFixedLagOnTree!(fgR, round(Int, SIZE/2), limitfixeddown=false)
+
+
+tree, smt, hist = solveTree!(fgR, deepcopy(tree5), recordcliqs=ls(fgR))
+
+
+pl10recl = plotCirc10BA(fg_, fgR, filepath=joinLogPath(fg, "circ$(SIZE)ReclBA.pdf"), levels=3, drawEllipse=true, ellipseList_=[:x9])
+#, contourList_=[:x8], width=11cm, ellipseColor="gray20"
+
+
+pl5recl = drawPosesLandms(fgR, spscale=1.5, manualColor="cyan4", point_size=4pt, drawhist=false, contour=false, levels=2, lbls=false, to=5, line_width=2pt)
+
+for ll in pl10recl.layers
+  push!(pl5recl.layers, ll)
+end
+pl5recl |> PDF(joinLogPath(fg, "circ10Recl_cyan4.pdf"), 10cm, 8cm)
+
+
+
+saveDFG(fgR, joinLogPath(fgR,"fg_ReclA"))
+
+
+
+
+
+
+
+
+
+## draw arcs
+
+plotCirc10BA(fg5a, fg5a, filepath=joinLogPath(fg, "circ5A.pdf"))
+
+
+
+tfg = initfg()
+
+# add a starting point (skipping prior for brevity)
+addVariable!(tfg, :a, Pose2)
+manualinit!(tfg, :a, 0.01*randn(3,100))
+
+addVariable!(tfg, :a_drt, Pose2)
+
+drt = MutablePose2Pose2Gaussian(MvNormal(zeros(3),Matrix{Float64}(LinearAlgebra.I,3,3)))
+addFactor!(tfg, [:a; :a_drt], drt)
+
+Qc = 0.001*Matrix{Float64}(LinearAlgebra.I,3,3)
+
+N = 10
+XYT = zeros(N,3)
+
+for i in 1:N
+  accumulateDiscreteLocalFrame!(drt,[0.1;0;0.05],Qc)
+  drval = accumulateFactorMeans(tfg, [:aa_drtf1;])
+  XYT[i,:] = drval
+end
+
+XYT
+
+
+
+
+## debugging solves
+
+
+using Gadfly
+drawTree(tree, imgs=false, show=true)
+
+
+spyCliqMat(tree, :x10)
+
+
+getUpMsgs(tree, :x9)
+
+plotTreeProductUp(fg,tree,:x11)
+plotTreeProductUp(fg, tree, :x10, :x11)
+
+
+# look at plotting with Makie
+
+
+## new experimental Makie plotting features
+using Caesar
+
+include(joinpath((Base.pathof(Caesar) |> splitpath)[1:end-2]..., "examples","marine","auv","Sandshark", "MakiePlotsFG.jl") )
+
+
+plotVariableBeliefs(fg, r"x\d", resolution=(1920,1080), fade=5, sortVars=true)
+
+
+
+0
+
+
+
+## ideas for IIF
+
+
+## check clique messages separately
+# tree = wipeBuildNewTree!(fg, drawpdf=true, show=true)
+
+# getSolverParams(fg).downsolve = false
+# getSolverParams(fg).async = true
+# stuffx9 = solveCliq!(fg, tree, :x9, recordcliq=true)
+# drawTree(tree)
+
+# umx9 = getUpMsgs(tree,:x9)
+# plotKDE(getKDE(fg, :x10), dims=[1;2])
+# plotKDE(umx9[:x10][1], dims=[1;2])
+#
+# stuffx10 = solveCliq!(fg, tree, :x10, recordcliq=true)
+#
+# umx10 = getUpMsgs(tree,:x10)
+# plotKDE(getKDE(fg, :x11), dims=[1;2])
+# plotKDE(umx10[:x11][1], dims=[1;2])
+#
+
+
+#