Fixed-Lag Solving 2D

# Hexagonal 2D with Fixed-Lag Solving

NOTE: This is an experimental feature that is currently being developed. This example provides an overview of how to enable it and the benefits of using fixed-lag solving. The objective is to provide a near-constant solve time for ever-growing graphs by only recalculating the most recent portion. Think of this as a placeholder, as we develop the solution this tutorial will be updated to demonstrate how that is achieved.

## Example Code

The complete code for this example can be found in the fixed-lag branch of RoME: Hexagonal Fixed-Lag Example.

## Introduction

Fixed-lag solving is enabled when creating the factor-graph. Users provide a window - the quasi fixed-lag constant (QFL) - which defines how many of the most-recent variables should be calculated. Any other variables are 'frozen'. The objective of this example is to explore providing a near-constant solve time for ever-growing graphs by only recalculating the most recent portion.

## Example Overview

In the example, the basic Hexagonal 2D is grown to solve 200 variables. The original example is remains the same, i.e. a vehicle is driving around in a hexagon and seeing the same bearing+range landmark as it crosses the starting point. At every 20th variable, a solve is invoked. Rather than use `batchSolve()`, the solve is performed in parts (construction of Bayes tree, solving the graph) to get performance statistics as the graph grows.

``````numVariables = 200
solveEveryNVariables = 20
lagLength = 30

# Standard Hexagonal example for totalIterations - solve every iterationsPerSolve iterations.
function runHexagonalExample(fg::FactorGraph, totalIterations::Int, iterationsPerSolve::Int)::DataFrame
# Add the first pose :x0

# Add at a fixed location PriorPose2 to pin :x0 to a starting location

# Drive around in a hexagon a number of times
solveTimes = DataFrame(GraphSize = [], TimeBuildBayesTree = [], TimeSolveGraph = [])
for i in 0:totalIterations
psym = Symbol("x\$i")
nsym = Symbol("x\$(i+1)")
pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal( [0.1;0.1;0.1].^2 ) )))
@info "Adding odometry factor between \$psym -> \$nsym..."

if i % 6 == 0
@info "Creating factor between \$psym and l1..."
p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0))
end
if i % iterationsPerSolve == 0 && i != 0
@info "Performing inference!"
if fg.isfixedlag
@info "Quasi fixed-lag is enabled (a feature currently in testing)!"
fifoFreeze!(fg)
end
tBuild = @timed tree = wipeBuildNewTree!(fg)
tInfer = @timed inferOverTree!(fg, tree, N=100)
graphSize = length([ls(fg)[1]..., ls(fg)[2]...])
push!(solveTimes, (graphSize, tBuild[2], tInfer[2]))
end
end
return solveTimes
end``````

Two cases are set up:

• One solving the full graph every time a solve is performed:
``````# start with an empty factor graph object
fg = initfg()
# DO NOT enable fixed-lag operation
solverTimesForBatch = runHexagonalExample(fg, numVariables, solveEveryNVariables)``````
• The other enabling fixed-lag with a window of 20 variables:
``````fgFixedLag = initfg()
fgFixedLag.isfixedlag = true
fgFixedLag.qfl = lagLength

solverTimesFixedLag = runHexagonalExample(fgFixedLag, numVariables, solveEveryNVariables)``````

The resultant path of the robot can be seen by using RoMEPlotting and is drawn if the visualization lines are uncommented:

``````#### Visualization

# Plot the many iterations to see that it succeeded.
# Batch
# drawPosesLandms(fg)

# Fixed lag
# drawPosesLandms(fgFixedLag)``````

Lastly, the timing results of both scenarios are merged into a single DataFrame table, exported to CSV, and a summary graph is shown using GadFly.

``````using Gadfly
using Colors
using CSV

# Make a clean dataset
rename!(solverTimesForBatch, :TimeBuildBayesTree => :Batch_BayedBuild, :TimeSolveGraph => :Batch_SolveGraph);
rename!(solverTimesFixedLag, :TimeBuildBayesTree => :FixedLag_BayedBuild, :TimeSolveGraph => :FixedLag_SolveGraph);
timingMerged = DataFrames.join(solverTimesForBatch, solverTimesFixedLag, on=:GraphSize)
CSV.write("timing_comparison.csv", timingMerged)

PP = []

Guide.title("Solving Time vs. Iteration for Fixed-Lag Operation"),
Guide.xlabel("Solving Iteration"),
Guide.ylabel("Solving Time (seconds)"),
Guide.manual_color_key("Legend", ["fixed", "batch"], ["green", "magenta"]))