Once the graph has been built, 2D plot visualizations are provided by RoMEPlotting.jl and KernelDensityEstimatePlotting.jl. These visualizations tools are readily modifiable to highlight various aspects of mobile platform navigation.
Plotting packages can be installed separately.
The major 2D plotting functions between
This simplest example for visualizing a 2D robot trajectory–-such as first running the Hexagonal 2D SLAM example–-
fg<:AbstractDFG has been loaded/constructed:
# load the plotting functionality using RoME, RoMEPlotting # generate some factor graph with numerical values fg = generateGraph_Hexagonal() solveTree!(fg) # or fg = loadDFG("somepath") # slam2D plot pl = plotSLAM2D(fg, drawhist=true, drawPoints=false)
plotSLAM2D(fgl; solveKey, from, to, minnei, meanmax, posesPPE, landmsPPE, recalcPPEs, lbls, scale, x_off, y_off, drawTriads, dyadScale, levels, drawhist, MM, xmin, xmax, ymin, ymax, showmm, window, point_size, line_width, regexLandmark, regexPoses, variableList, manualColor, drawPoints, pointsColor, drawContour, drawEllipse, ellipseColor, title, aspect_ratio)
2D plot of both poses and landmarks contained in factor graph. Assuming poses and landmarks are labeled
:x1, :x2, ... and
:l0, :l1, ..., respectively. The range of numbers to include can be controlled with
to along with other keyword functionality for manipulating the plot.
:l2, ... for landmarks –
- Can increase default Gadfly plot size (for JSSVG in browser):
- Enable or disable features such as the covariance ellipse with keyword
- TODO update to use e.g.
- TODO fix
- TODO deprecate,
fg = generateGraph_Hexagonal() plotSLAM2D(fg) plotSLAM2D(fg, drawPoints=false) plotSLAM2D(fg, contour=false, drawEllipse=true) plotSLAM2D(fg, contour=false, title="SLAM result 1") # or load a factor graph fg_ = loadDFG("somewhere.tar.gz") plotSLAM2D(fg_)
While the Caesar.jl framework is focussed on non-Gaussian inference, it is frequently desirable to relate the results to a more familiar covariance ellipse, and native support for this exists:
plotSLAM2D(fg, drawContour=false, drawEllipse=true, drawPoints=true)
Lower down utility functions are used to plot poses and landmarks separately before joining the Gadfly layers.
plotSLAM2DPoses(fg; solveKey, regexPoses, from, to, variableList, meanmax, ppe, recalcPPEs, lbls, scale, x_off, y_off, drawhist, spscale, dyadScale, drawTriads, drawContour, levels, contour, line_width, drawPoints, pointsColor, drawEllipse, ellipseColor, manualColor)
2D plot of all poses, assuming poses are labeled from `
:x0, :x1, ..., :xn. Use
from to limit the range of numbers
n to be drawn. The underlying histogram can be enabled or disabled, and the size of maximum-point belief estimate cursors can be controlled with
- Relax to user defined pose labeling scheme, for example
:p1, :p2, ...
plotSLAM2DLandmarks(fg; solveKey, regexLandmark, from, to, minnei, variableList, meanmax, ppe, recalcPPEs, lbls, showmm, scale, x_off, y_off, drawhist, drawContour, levels, contour, manualColor, c, MM, point_size, drawPoints, pointsColor, drawEllipse, ellipseColor, resampleGaussianFit)
2D plot of landmarks, assuming
:l1, :l2, ... :ln. Use
to to control the range of landmarks
n to include.
KernelDensityEstimatePlotting (as used in
RoMEPlotting) provides an interface to visualize belief densities as counter plots. Something basic might be to just show all plane pairs of this variable marginal belief:
# Draw the KDE for x0 plotBelief(fg, :x0)
Plotting the marginal density over say variables
(x,y) in a
Pose2 would be:
plotBelief(fg, :x1, dims=[1;2])
The following example better shows some of features (via Gadfly.jl):
# Draw the (x,y) marginal estimated belief contour for :x0, :x2, and Lx4 pl = plotBelief(fg, [:x0; :x2; :x4], c=["red";"green";"blue"], levels=2, dims=[1;2]) # add a few fun layers pl3 = plotSLAM2DPoses(fg, regexPoses=r"x\d", from=3, to=3, drawContour=false, drawEllipse=true) pl5 = plotSLAM2DPoses(fg, regexPoses=r"x\d", from=5, to=5, drawContour=false, drawEllipse=true, drawPoints=false) pl_ = plotSLAM2DPoses(fg, drawContour=false, drawPoints=false, dyadScale=0.001, to=5) union!(pl.layers, pl3.layers) union!(pl.layers, pl5.layers) union!(pl.layers, pl_.layers) # change the plotting coordinates pl.coord = Coord.Cartesian(xmin=-10,xmax=20, ymin=-1, ymax=25) # save the plot to SVG and giving dedicated (although optional) sizing pl |> SVG("/tmp/test.svg", 25cm, 15cm) # also display the plot live pl
See function documentation for more details on API features
plotBelief(fgl, sym; solveKey, dims, title, levels, fill, layers, c, overlay)
A peneric KDE plotting function that allows marginals of higher dimensional beliefs and various keyword options.
p = manikde!(Position2, [randn(2) for _ in 1:100]) q = manikde!(Position2, [randn(2).+[5;0] for _ in 1:100]) plotBelief(p) plotBelief(p, dims=[1;2], levels=3) plotBelief(p, dims=) plotBelief([p;q]) plotBelief([p;q], dims=[1;2], levels=3) plotBelief([p;q], dims=)
VSCode/Juno can set plot to be opened in a browser tab instead. For scripting use-cases you can also export the image:
using Gadfly # can change the default plot size # Gadfly.set_default_plot_size(35cm, 30cm) pl |> PDF("/tmp/test.pdf", 20cm, 10cm) # or PNG, SVG
It is also possible to store the whole plot container to file using
JLD2.@save "/tmp/myplot.jld2" pl # and loading elsewhere JLD2.@load "/tmp/myplot.jld2" pl
See the following two discussions on Interactive 2D plots:
Red and Green dyad lines represent the visualization-only assumption of X-forward and Y-left direction of
Pose2. The inference and manifold libraries surrounding Caesar.jl are agnostic to any particular choice of reference frame alignment, such as north east down (NED) or forward left up (common in mobile robotics).
Also see Gadfly.jl notes about
vstack to combine plots side by side or vertically.
It is also possible to plot the belief density of a
plotPose(fgl, syms; solveKey, levels, c, axis, scale, show, filepath, app, hdl)
Example: pl = plotPose(fg, [:x1; :x2; :x3])
One useful function is to check that data in the factor graph makes sense. While the full inference algorithm uses a Bayes (Junction) tree to assemble marginal belief estimates in an efficient manner, it is often useful for a straight forward graph based sanity check. The
plotLocalProduct projects through
approxConv each of the factors connected to the target variable and plots the result. This example looks at the loop-closure point around
:x0, which is also pinned down by the only prior in the canonical Hexagonal factor graph.
@show ls(fg, :x0); # ls(fg, :x0) = [:x0f1, :x0x1f1, :x0l1f1] pl = plotLocalProduct(fg, :x0, dims=[1;2], levels=1)
While perhaps a little cluttered to read at first, this figure shows that a new calculation local to only the factor graph
prod in greem matches well with the existing value
curr in red in the
fg from the earlier
solveTree! call. These values are close to the prior prediction
:x0f1 in blue (fairly trivial case), while the odometry
:x0x1f1 and landmark sighting projection
:x0l1f1 are also well in agreement.
plotLocalProduct(fgl, lbl; solveKey, N, dims, levels, show, dirpath, mimetype, sidelength, title, xmin, xmax, ymin, ymax)
Plot the proposal belief from neighboring factors to
lbl in the factor graph (ignoring Bayes tree representation), and show with new product approximation for reference.
- TODO, standardize around ::MIME="image/svg", see JuliaRobotics/DistributedFactorGraphs.jl#640
plotLocalProduct(fgl, lbl; N, dims)
Plot the proposal belief from neighboring factors to
lbl in the factor graph (ignoring Bayes tree representation), and show with new product approximation for reference. String version is obsolete and will be deprecated.
Multiple beliefs can be plotted at the same time, while setting
levels=4 rather than the default value:
plX1 = plotBelief(fg, [:x0; :x1], dims=[1;2], levels=4) # plX1 |> PNG("/tmp/testX1.png")
One dimensional (such as
Θ) or a stack of all plane projections is also available:
plTh = plotBelief(fg, [:x0; :x1], dims=, levels=4) # plTh |> PNG("/tmp/testTh.png")
plAll = plotBelief(fg, [:x0; :x1], levels=3) # plAll |> PNG("/tmp/testX1.png",20cm,15cm)
vstack is provided through the
Gadfly package and allows the user to build a near arbitrary composition of plots.
Please see KernelDensityEstimatePlotting package source for more features.