Merge pull request #241 from JuliaRobotics/master

v0.5.5-rc1

.github/workflows/TagBot.yml

@@ -0,0 +1,11 @@
+name: TagBot
+on:
+  schedule:
+    - cron: 0 * * * *
+jobs:
+  TagBot:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: JuliaRegistries/TagBot@v1
+        with:
+          token: ${{ secrets.GITHUB_TOKEN }}

.travis.yml

@@ -1,37 +1,49 @@
 language: julia
 
+arch:
+  - amd64
+
 os:
   - linux
 
-arch:
-  - amd64
-  - arm64
-
+addons:
+  apt:
+    packages:
+      - hdf5-tools
+
 julia:
   - 1.0
-  - 1.2
   - 1.3
+  - 1.4
   - nightly
 
-notifications:
-  email: false
+after_success:
+  - julia -e 'using Pkg; cd(Pkg.dir("RoME")); Pkg.add("Coverage"); using Coverage; Codecov.submit(process_folder())'
 
-matrix:
+jobs:
   allow_failures:
     - julia: nightly
     - arch: arm64
-
-jobs:
+    - env: CANARYTEST=true
+  include:
+    - name: "Canary"
+      env: CANARYTEST=true
+      julia: 1.3
+      script:
+        - if [[ -a .git/shallow ]]; then git fetch --unshallow; fi
+        - julia -e 'using Pkg; pkg"add DistributedFactorGraphs#master"; pkg"add IncrementalInference#master";'
+        - julia --check-bounds=yes -e 'using Pkg; Pkg.test("Caesar"; coverage=false)'
+      after_success: skip
+    - name: "ARM64"
+      arch: arm64
+      julia: 1.3
+      after_success: skip
   fast_finish: true
 
-addons:
-  apt:
-    packages:
-      - hdf5-tools
+notifications:
+  email: false
+
 
 # script:
 #   - if [[ -a .git/shallow ]]; then git fetch --unshallow; fi
 #   - julia --check-bounds=yes -e 'using Pkg; Pkg.clone(pwd()); Pkg.test("RoME"; coverage=true)'
-
-after_success:
-  - julia -e 'using Pkg; cd(Pkg.dir("RoME")); Pkg.add("Coverage"); using Coverage; Codecov.submit(process_folder())'

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.4"
+version = "0.5.5"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

examples/FixedLagCircles.jl

@@ -62,6 +62,8 @@ plotCirc10BA(fg_, fgI, filepath=joinLogPath(fg, "circ$(SIZE)IncrBA.pdf"), points
 saveDFG(fgI, joinLogPath(fgI,"fg_IncrA"))
 
 
+calcCliquesRecycled(tree5)
+calcCliquesRecycled(tree)
 
 
 ## SHOW MARGINALIZATION---------------------------------------------------------
@@ -92,6 +94,14 @@ pl5blue |> PDF(joinLogPath(fgM, "circ$(SIZE)MargBA_blue.pdf"), 10cm, 8cm)
 saveDFG(fgM, joinLogPath(fgM,"fg_MargA"))
 
 
+# need a fix
+calcCliquesRecycled(tree)
+
+
+### DEBUGGING
+
+cliq = getCliq(tree, :x1)
+data = getData(cliq).allmarginalized
 
 
 ## SHOW BOTH MARG AND INCR TOGETHER---------------------------------------------

examples/MITDatasetBatch.jl

@@ -0,0 +1,55 @@
+using Distributed
+using Dates
+using RoME
+using RoMEPlotting
+using Gadfly
+@everywhere using RoME, RoMEPlotting, Gadfly
+
+total_meas = parse(Int, ARGS[1])
+# Max number is 827.
+
+# Let's load the MIT scenario using the g2o file.
+file = (normpath(Base.find_package("RoME"), "../..", "examples", "MIT_incremental.g2o"))
+global instructions = importG2o(file)
+
+# Make sure plots look a bit nicer.
+latex_fonts = Theme(major_label_font="CMU Serif", major_label_font_size=16pt,
+                    minor_label_font="CMU Serif", minor_label_font_size=14pt,
+                    key_title_font="CMU Serif", key_title_font_size=12pt,
+                    key_label_font="CMU Serif", key_label_font_size=10pt)
+Gadfly.push_theme(latex_fonts)
+
+function solve_batch(total_meas::Integer)
+    # Choose where to save the data.
+    mit_total_meas = total_meas
+    data_logpath = ENV["HOME"]*"/Documents/wafr/mit-batch-$(total_meas)-$(now())"
+
+    # Create initial factor graph with specified logging path.
+    fg = LightDFG{SolverParams}(params=SolverParams(logpath=data_logpath))
+
+    # Add initial variable with a prior measurement to anchor the graph.
+    addVariable!(fg, :x0, Pose2)
+    initial_pose = MvNormal([0.0; 0.0; 0.0], Matrix(Diagonal([0.1;0.1;0.05].^2)))
+    addFactor!(fg, [:x0], PriorPose2(initial_pose))
+
+    # Add all variables and measurements.
+    for i in 1:mit_total_meas
+        parseG2oInstruction!(fg, instructions[i])
+    end
+
+    # Solve the graph, and save a copy of the tree.
+    saveDFG(fg, "$(getLogPath(fg))/fg-before-solve")
+    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    saveDFG(fg, "$(getLogPath(fg))/fg-after-solve")
+    saveTree(tree, "$(getLogPath(fg))/tree$(mit_total_meas).jld2")
+    drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt.pdf")
+
+    # Just store some quick plots.
+    pl1 = drawPoses(fg, spscale=0.6, lbls=false)
+    Gadfly.draw(PDF("$(getLogPath(fg))/poses$(mit_total_meas).pdf", 20cm, 10cm), pl1)
+
+    # Run the garbage collector.
+    GC.gc()
+end
+
+solve_batch(total_meas)

examples/MITDatasetFixedLag.jl

@@ -0,0 +1,124 @@
+using Distributed
+using Dates
+using RoME
+using RoMEPlotting
+using Gadfly
+@everywhere using RoME, RoMEPlotting, Gadfly
+
+# Parse the arguments.
+initial_offset = parse(Int, ARGS[1])
+final_timestep = parse(Int, ARGS[2])
+qfl_length = parse(Int, ARGS[3])
+solve_stride = parse(Int, ARGS[4])
+
+# Let's load the MIT scenario using the g2o file.
+file = (normpath(Base.find_package("RoME"), "../..", "examples", "MIT_incremental.g2o"))
+global instructions = importG2o(file)
+
+# Make sure plots look a bit nicer.
+latex_fonts = Theme(major_label_font="CMU Serif", major_label_font_size=16pt,
+                    minor_label_font="CMU Serif", minor_label_font_size=14pt,
+                    key_title_font="CMU Serif", key_title_font_size=12pt,
+                    key_label_font="CMU Serif", key_label_font_size=10pt)
+Gadfly.push_theme(latex_fonts)
+
+function go_fixedlag(initial_offset::Integer,
+                     final_timestep::Integer,
+                     qfl_length_arg::Integer,
+                     solve_stride::Integer=5)
+    # Choose where to save the step's data.
+    qfl_length = qfl_length_arg # Fixed lag window size.
+    data_logpath = ENV["HOME"]*"/Documents/wafr/mit-b$(qfl_length)-$(now())"
+
+    # Create initial factor graph with specified logging path.
+    fg = LightDFG{SolverParams}(params=SolverParams(logpath=data_logpath))
+    tree = emptyBayesTree()
+
+    # Set up the fixed lag smoothing.
+    getSolverParams(fg).isfixedlag = true
+    getSolverParams(fg).qfl = qfl_length
+    getSolverParams(fg).limitfixeddown = true
+    getSolverParams(fg).dbg = true
+
+    # Add initial variable with a prior measurement to anchor the graph.
+    addVariable!(fg, :x0, Pose2)
+    initial_pose = MvNormal([0.0; 0.0; 0.0], Matrix(Diagonal([0.1;0.1;0.05].^2)))
+    addFactor!(fg, [:x0], PriorPose2(initial_pose))
+
+    # Add the next---or initial offset of---measurements to the graph.
+    padded_step = lpad(1, 4, "0")
+    if initial_offset == 1
+        parseG2oInstruction!(fg, instructions[1])
+    else
+        for j in 1:initial_offset
+            parseG2oInstruction!(fg, instructions[j])
+        end
+        padded_step = lpad(initial_offset, 4, "0")
+    end
+
+    # Solve the graph, and save a copy of the tree.
+    saveDFG(fg, "$(getLogPath(fg))/fg-before-solve$(padded_step)")
+    tree, smt, hist = solveTree!(fg, maxparallel=1000)
+    saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
+    saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
+    drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")
+
+    # Analyze clique counts.
+    fid = open("$(getLogPath(fg))/clique-counts.txt", "w")
+    nCliqs, nMarg, nReused, nBoth = calcCliquesRecycled(tree)
+    println(fid, "$(padded_step), $(nCliqs), $(nMarg), $(nReused), $(nBoth)")
+
+    # Just store some quick plots.
+    pl1 = drawPoses(fg, spscale=0.6)
+    Gadfly.draw(PDF("$(getLogPath(fg))/poses$(padded_step).pdf", 20cm, 10cm), pl1)
+
+    # Solver stride.
+    solveStride = 0
+    # Run the loop for the remaining time steps.
+    for i in (initial_offset + 1):final_timestep
+        # Add the next measurement to the graph.
+        parseG2oInstruction!(fg, instructions[i])
+        padded_step = lpad(i, 4, "0")
+
+        # Store each graph.
+        saveDFG(fg, "$(getLogPath(fg))/fg-before-solve$(padded_step)")
+
+        # Just store some quick plots, on another process
+        remotecall((fgl, padded_stepl) -> begin
+          @info "drawPoses, $(padded_stepl), for fg num variables=$(length(ls(fgl)))."
+          pl1 = drawPoses(fgl, spscale=0.6, lbls=false)
+          pl1 |> PDF("$(getLogPath(fgl))/poses$(padded_stepl).pdf", 20cm, 10cm)
+        end, rand(Categorical(nprocs()-1))+1, fg, padded_step)
+
+        # Only solve every 10th instruction.
+        solveStride += 1
+        if solveStride % solve_stride != 0
+            @info "solveStride=$solveStride"
+            continue
+        end
+        @info "Going for solve"
+
+        # Solve the graph, and save a copy of the tree.
+        tree, smt, hist = solveTree!(fg, tree, maxparallel=1000)
+        saveDFG(fg, "$(getLogPath(fg))/fg-after-solve$(padded_step)")
+        saveTree(tree, "$(getLogPath(fg))/tree$(padded_step).jld2")
+        drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt$(padded_step).pdf")
+
+        # Analyze clique number.
+        nCliqs, nMarg, nReused, nBoth = calcCliquesRecycled(tree)
+        println(fid, "$(padded_step), $(nCliqs), $(nMarg), $(nReused), $(nBoth)")
+        flush(fid)
+
+        # Run the garbage collector.
+        GC.gc()
+    end
+    # Final plot.
+    padded_step = lpad(final_timestep+1, 4, "0")
+    pl1 = drawPoses(fg, spscale=0.6)
+    pl1 |> PDF("$(getLogPath(fg))/poses$(padded_step).pdf", 20cm, 10cm)
+
+    close(fid)
+end
+
+# Run within a function to avoid undefined variable errors.
+go_fixedlag(initial_offset, final_timestep, qfl_length, solve_stride)