diff --git a/Project.toml b/Project.toml index 0d42fcc7f..66dacf537 100644 --- a/Project.toml +++ b/Project.toml @@ -2,7 +2,7 @@ name = "Caesar" uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406" keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics", "ROS"] desc = "Non-Gaussian simultaneous localization and mapping" -version = "0.10.0" +version = "0.10.1" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89" @@ -42,19 +42,19 @@ Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02" YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6" [compat] -ApproxManifoldProducts = "0.2, 0.3" +ApproxManifoldProducts = "0.3" AprilTags = "0.8, 0.9" Combinatorics = "0.7, 0.8, 0.9, 1" CoordinateTransformations = "0.5, 0.6" DataStructures = "0.16, 0.17, 0.18" -DistributedFactorGraphs = "0.11, 0.12" +DistributedFactorGraphs = "0.12, 0.13" Distributions = "0.19, 0.20, 0.21, 0.22, 0.23, 0.24" DocStringExtensions = "0.7, 0.8" FFTW = "1" FileIO = "1" ImageCore = "0.7, 0.8, 0.9" ImageMagick = "0.7, 1.0, 1.1" -IncrementalInference = "0.21, 0.22" +IncrementalInference = "0.22, 0.23" JLD2 = "0.1, 0.2, 0.3, 0.4" JSON = "0.18, 0.19, 0.20, 0.21, 0.22, 0.23" JSON2 = "0.3, 0.4" @@ -64,7 +64,7 @@ Optim = "1" ProgressMeter = "0.9, 1" Reexport = "0.2, 1.0" Requires = "0.5, 0.6, 0.7, 0.8, 0.9, 0.10, 1" -RoME = "0.13, 0.14" +RoME = "0.14, 0.15" Rotations = "0.13, 1" TensorCast = "0.2, 0.3, 0.4" TimeZones = "1.3.1, 1.4" diff --git a/README.md b/README.md index b8c0f997f..8520b9533 100644 --- a/README.md +++ b/README.md @@ -8,9 +8,9 @@ A multimodal/non-Gaussian robotic toolkit for localization and mapping -- reduci Click on the badges to follow web url links: -| Stable v0.8 | Stable v0.9 | Dev | Documentation | Public Slack | +| Stable v0.9 | Stable v0.10 | Dev | Documentation | Public Slack | |:------:|:------:|:----------------:|:-------------:|:-----:| -| [![Build Status][build-v0.8]][build-url] | [![Build Status][build-v0.9]][build-url] | [![Build Status][build-img]][build-url]
[![ColPrac][colp-badge]][colprac] | [![docs][docs-shield]][caesar-docs] | [![][caesar-slack-badge]][caesar-slack] | +| [![Build Status][build-v0.9]][build-url] | [![Build Status][build-v0.10]][build-url] | [![Build Status][build-img]][build-url]
[![ColPrac][colp-badge]][colprac] | [![docs][docs-shield]][caesar-docs] | [![][caesar-slack-badge]][caesar-slack] | ## Get Involved and Code of Conduct @@ -49,7 +49,7 @@ Consider citing our work: } ``` -Administration of the Caesar/RoME/IncrementalInference/Arena packages is currently done by @dehann who can be contacted for more details. +Administration of the Caesar.jl ecosystem is done by [NavAbility](http://www.navability.io) who can be contacted at (info@navability.io) for more details. ## Stargazers over time @@ -66,10 +66,10 @@ Administration of the Caesar/RoME/IncrementalInference/Arena packages is current [cov-img]: https://codecov.io/github/JuliaRobotics/Caesar.jl/coverage.svg?branch=master [cov-url]: https://codecov.io/github/JuliaRobotics/Caesar.jl?branch=master [build-img]: https://travis-ci.org/JuliaRobotics/Caesar.jl.svg?branch=master -[build-v0.8]: https://travis-ci.org/JuliaRobotics/Caesar.jl.svg?branch=release/v0.8 +[build-v0.10]: https://travis-ci.org/JuliaRobotics/Caesar.jl.svg?branch=release/v0.10 [build-v0.9]: https://travis-ci.org/JuliaRobotics/Caesar.jl.svg?branch=release/v0.9 [build-url]: https://travis-ci.org/JuliaRobotics/Caesar.jl -[caesar-stable]: https://img.shields.io/badge/2021Q1-v0.8.x-green.svg +[caesar-stable]: https://img.shields.io/badge/2021Q1-v0.10.x-green.svg [caesar-slack-badge]: https://img.shields.io/badge/Caesarjl-Slack-green.svg?style=popout [caesar-slack]: https://caesarjl.slack.com [caesar-milestones]: https://github.com/JuliaRobotics/Caesar.jl/milestones @@ -79,19 +79,19 @@ Administration of the Caesar/RoME/IncrementalInference/Arena packages is current [r-cov-img]: https://codecov.io/github/JuliaRobotics/RoME.jl/coverage.svg?branch=master [r-cov-url]: https://codecov.io/github/JuliaRobotics/RoME.jl?branch=master [r-build-img]: https://travis-ci.org/JuliaRobotics/RoME.jl.svg?branch=master -[r-build-v05]: https://travis-ci.org/JuliaRobotics/RoME.jl.svg?branch=release%2Fv0.9 +[r-build-v05]: https://travis-ci.org/JuliaRobotics/RoME.jl.svg?branch=release%2Fv0.15 [r-build-url]: https://travis-ci.org/JuliaRobotics/RoME.jl -[r-stable]: https://img.shields.io/badge/2021Q1-v0.13.x-green.svg +[r-stable]: https://img.shields.io/badge/2021Q1-v0.15.x-green.svg [r-milestones]: https://github.com/JuliaRobotics/RoME.jl/milestones [r-releases]: https://github.com/JuliaRobotics/RoME.jl/releases [iif-cov-img]: https://codecov.io/github/JuliaRobotics/IncrementalInference.jl/coverage.svg?branch=master [iif-cov-url]: https://codecov.io/github/JuliaRobotics/IncrementalInference.jl?branch=master [iif-build-img]: https://travis-ci.org/JuliaRobotics/IncrementalInference.jl.svg?branch=master -[iif-build-v020]: https://travis-ci.org/JuliaRobotics/IncrementalInference.jl.svg?branch=release/v0.20 +[iif-build-v020]: https://travis-ci.org/JuliaRobotics/IncrementalInference.jl.svg?branch=release/v0.23 [iif-build-url]: https://travis-ci.org/JuliaRobotics/IncrementalInference.jl [iif-url]: http://www.github.com/JuliaRobotics/IncrementalInference.jl -[iif-stable]: https://img.shields.io/badge/2021Q1-v0.21.x-green.svg +[iif-stable]: https://img.shields.io/badge/2021Q1-v0.23.x-green.svg [iif-milestones]: https://github.com/JuliaRobotics/IncrementalInference.jl/milestones [iif-releases]: https://github.com/JuliaRobotics/IncrementalInference.jl/releases @@ -141,7 +141,7 @@ Administration of the Caesar/RoME/IncrementalInference/Arena packages is current [dfg-build-img]: https://travis-ci.org/JuliaRobotics/DistributedFactorGraphs.jl.svg?branch=master [dfg-build-url]: https://travis-ci.org/JuliaRobotics/DistributedFactorGraphs.jl [dfg-url]: http://www.github.com/JuliaRobotics/DistributedFactorGraphs.jl -[dfg-stable]: https://img.shields.io/badge/2020Q4-v0.11.x-green.svg +[dfg-stable]: https://img.shields.io/badge/2021Q1-v0.13.x-green.svg [dfg-milestones]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/milestones [dfg-releases]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/releases @@ -150,7 +150,7 @@ Administration of the Caesar/RoME/IncrementalInference/Arena packages is current [amp-build-img]: https://travis-ci.org/JuliaRobotics/ApproxManifoldProducts.jl.svg?branch=master [amp-build-url]: https://travis-ci.org/JuliaRobotics/ApproxManifoldProducts.jl [amp-url]: http://www.github.com/JuliaRobotics/ApproxManifoldProducts.jl -[amp-stable]: https://img.shields.io/badge/2021Q1-v0.2.x-green.svg +[amp-stable]: https://img.shields.io/badge/2021Q1-v0.3.x-green.svg [amp-milestones]: https://github.com/JuliaRobotics/ApproxManifoldProducts.jl/milestones [amp-releases]: https://github.com/JuliaRobotics/ApproxManifoldProducts.jl/releases @@ -168,6 +168,6 @@ Administration of the Caesar/RoME/IncrementalInference/Arena packages is current [rp-cov-url]: https://codecov.io/github/JuliaRobotics/RoMEPlotting.jl?branch=master [rp-build-img]: https://travis-ci.org/JuliaRobotics/RoMEPlotting.jl.svg?branch=master [rp-build-url]: https://travis-ci.org/JuliaRobotics/RoMEPlotting.jl -[rp-stable]: https://img.shields.io/badge/2021Q1-v0.6.x-green.svg +[rp-stable]: https://img.shields.io/badge/2021Q1-v0.7.x-green.svg [rp-milestones]: https://github.com/JuliaRobotics/RoMEPlotting.jl/milestones [rp-releases]: https://github.com/JuliaRobotics/RoMEPlotting.jl/releases diff --git a/docs/make.jl b/docs/make.jl index f4873a7ea..509053bfb 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -4,6 +4,7 @@ using RoME import IncrementalInference: fmcmc!, localProduct, prodmultiplefullpartials, prodmultipleonefullpartials, setfreeze! import IncrementalInference: cliqGibbs, packFromLocalPotentials!, treeProductDwn, updateFGBT!, upGibbsCliqueDensity import IncrementalInference: initfg, downGibbsCliqueDensity +import IncrementalInference: solveGraphParametric, solveGraphParametric! using KernelDensityEstimatePlotting # import KernelDensityEstimatePlotting: plotKDE diff --git a/docs/src/examples/parametric_solve.md b/docs/src/examples/parametric_solve.md index 3848eecf9..46ddeb6c8 100644 --- a/docs/src/examples/parametric_solve.md +++ b/docs/src/examples/parametric_solve.md @@ -47,7 +47,7 @@ end ``` ### The Factor -The factor is evaluated in a cost function using the Mahalanobis distance and the measurement should therefore match the residual returned. +The factor is evaluated in a cost function using the [Mahalanobis distance](https://en.wikipedia.org/wiki/Mahalanobis_distance) and the measurement should therefore match the residual returned. ### Optimization [`IncrementalInference.solveGraphParametric!`](@ref) uses Optim.jl. The factors that are supported should have a gradient and Hessian available/exists and therefore it makes use of `TwiceDifferentiable`. Full control of Optim's setup is possible with keyword arguments. diff --git a/examples/dev/ParamSweep.jl b/examples/dev/ParamSweep.jl new file mode 100644 index 000000000..a68577a32 --- /dev/null +++ b/examples/dev/ParamSweep.jl @@ -0,0 +1,97 @@ + +using ArgParse +function parse_commandline() + s = ArgParseSettings() + + @add_arg_table! s begin + "--spreadNH_start" + help = "spreadNH_start" + arg_type = Float64 + default = 3.0 + "--spreadNH_stop" + help = "spreadNH_stop" + arg_type = Float64 + default = 3.0 + "--spreadNH_step" + help = "spreadNH_step" + arg_type = Float64 + default = 1.0 + "--inflation_start" + help = "inflation_start" + arg_type = Float64 + default = 5.0 + "--inflation_stop" + help = "inflation_stop" + arg_type = Float64 + default = 5.0 + "--inflation_step" + help = "inflation_step" + arg_type = Float64 + default = 1.0 + "--useMsgLikelihoods" + help = "useMsgLikelihoods" + arg_type = Bool + default = true + "--anneal" + help = "anneal the solve value according to param settings iterations" + action = :store_true + "fgtargz" + help = "fgtargz, but leave out the extension .tar.gz" + required = true + end + + return parse_args(s) +end + +using RoME +using Distributed +addprocs(10) +using RoME +@everywhere using RoME + + +function main() + pargs = parse_commandline() + println("Parsed args:") + for (arg,val) in pargs + println(" $arg => $val") + end + + fname = replace(pargs["fgtargz"],".tar.gz"=>"") + fg = loadDFG(fname); + + getSolverParams(fg).useMsgLikelihoods = pargs["useMsgLikelihoods"] + + SNH = pargs["spreadNH_start"]:pargs["spreadNH_step"]:pargs["spreadNH_stop"] + IFL = pargs["inflation_start"]:pargs["inflation_step"]:pargs["inflation_stop"] + + if pargs["anneal"] + for i in 1:length(SNH) + getSolverParams(fg).spreadNH = SNH[i] + getSolverParams(fg).inflation = IFL[i] + # solve + solveTree!(fg, storeOld=true); + end + # store the final result + snhn = replace(string(SNH),'.'=>'_') + ifln = replace(string(IFL),'.'=>'_') + saveDFG(pargs["fgtargz"]*"S$snhn"*"I$ifln", fg) + else + for snh in SNH, ifl in IFL + getSolverParams(fg).spreadNH = snh + getSolverParams(fg).inflation = ifl + fg_ = deepcopy(fg) + solveTree!(fg_, storeOld=true); + # save the result + snhn = replace("$snh",'.'=>'_') + ifln = replace("$ifl",'.'=>'_') + saveDFG(pargs["fgtargz"]*"S$snhn"*"I$ifln", fg_) + end + end + + @info "done" +end + +main() + +# diff --git a/examples/dev/batch_small_example.jl b/examples/dev/batch_small_example.jl new file mode 100644 index 000000000..aee9319b6 --- /dev/null +++ b/examples/dev/batch_small_example.jl @@ -0,0 +1,305 @@ +## Contributed by @doublestrong + +## Need 6 npz file and timing + +using Caesar +# using RoME, Distributions +# using GraphPlot +using RoMEPlotting +Gadfly.set_default_plot_size(35cm,20cm) + +# using Rebugger +# using Gadfly +using NPZ +using MAT + +# # add multiproc support +using Distributed +addprocs(3) +using Caesar +@everywhere using Caesar, RoME + +# # 90% precompile +for i in 1:5 + warmUpSolverJIT() +end + + +## + +# # [OPTIONAL] add more julia processes to speed up inference +# using Distributed +# nprocs() < 3 ? addprocs(4-nprocs()) : nothing + +# @everywhere using RoME +N = 1000 +# start with an empty factor graph object +fg = initfg() + +# fieldnames(typeof(getSolverParams(fg))) +# getSolverParams(fg).graphinit = true # init on graph first then solve on tree (default) +# getSolverParams(fg).graphinit = false # init and solve on tree +getSolverParams(fg).useMsgLikelihoods = true +getSolverParams(fg).N = 150 + + +# getSolverParams(fg).N = N +# # Add the first pose :x0 +# addVariable!(fg, :x0, Pose2) +# # Add a few more poses +# for i in 1:5 +# addVariable!(fg, Symbol("x$(i)"), Pose2) +# end +# # Add landmarks +# for i in 1:2 +# addVariable!(fg, Symbol("l$(i)"), Point2) +# end + + + +timing = zeros(0) + + +## Step 0 + +# Add at a fixed location Prior to pin :x0 to a starting location (0,0,0) +s_time = time_ns() +addVariable!(fg, :x0, Pose2) +addVariable!(fg, :l1, Point2) +addFactor!(fg, [:x0], PriorPose2( MvNormal([0; 0; 0], diagm([0.3;0.3;0.3].^2)) )) +ppr = Pose2Point2Range(MvNormal([7.323699045815659], diagm([0.3].^2))) +addFactor!(fg, [:x0; :l1], ppr ) +# Gadfly.draw(PDF("fg.pdf", 10cm, 10cm),pl) # or PNG(...) +# ensureAllInitialized!(fg) +# dfgplot(fg) + +## Perform inference +tree, smt, hist = solveTree!(fg) +e_time = time_ns() +elapsed = (e_time - s_time)/10^9 +append!(timing, elapsed) +println("elapsed time: ", elapsed) + +# pl = plotKDE(fg, [:x0,:l1],dims=[1;2], levels=4) +pl = plotKDE(fg, [:x0,:l1],dims=[1;2], levels=4) + + +## + +Gadfly.draw(PDF("batch_1.pdf", 20cm, 10cm),pl) +X0 = rand(getBelief(fg, :x0) ,N) +L1 = rand(getBelief(fg, :l1) ,N) +batch1 = vcat(X0,L1) +npzwrite("batch1.npz",batch1) + +# plX1 |> PNG("/tmp/testX1.png") +# X2 = rand(getBelief(fg, :x2) ,N) +# X3 = rand(getBelief(fg, :x3) ,N) +# X4 = rand(getBelief(fg, :x4) ,N) +# X5 = rand(getBelief(fg, :x5) ,N) +# res = vcat(X0,X1,L1) +# using NPZ +# npzwrite("res.npz",res) +# Gadfly.draw(PDF("batch.pdf", 10cm, 10cm),pl) # or PNG(...) +# error("here's a breakpoint") + +## Step 1 +i = 1 +s_time = time_ns() +addVariable!(fg, :x1, Pose2) +pp = Pose2Pose2(MvNormal([9.82039106655709;-0.040893643507463134;0.7851856362659602], diagm([0.3;0.3;0.05].^2))) +ppr = Pose2Point2Range(MvNormal([6.783355788017825], diagm([0.3].^2))) +addFactor!(fg, [Symbol("x$(i-1)"); Symbol("x$(i)")], pp ) +addFactor!(fg, [Symbol("x$(i)"); :l1], ppr ) + + +## + +tree, smt, hist = solveTree!(fg, tree) + +e_time = time_ns() +elapsed = (e_time - s_time)/10^9 +append!(timing, elapsed) +println("elapsed time: ", elapsed) + +pl = plotKDE(fg, [:x0,:x1,:l1],dims=[1;2], levels=4) + +## + +Gadfly.draw(PDF("batch_2.pdf", 20cm, 10cm),pl) +X0 = rand(getBelief(fg, :x0) ,N) +L1 = rand(getBelief(fg, :l1) ,N) +X1 = rand(getBelief(fg, :x1) ,N) +batch2 = vcat(X0,X1,L1) +npzwrite("batch2.npz",batch2) + + +# error("here's a breakpoint") +## Step 2 + +i = i + 1 +s_time = time_ns() +addVariable!(fg, :x2, Pose2) +addVariable!(fg, :l2, Point2) +pp = Pose2Pose2(MvNormal([9.824301116341609;-0.1827443713759503;0.7586281983933238], diagm([0.3;0.3;0.05].^2))) +ppr = Pose2Point2Range(MvNormal([6.768864478545091], diagm([0.3].^2))) +addFactor!(fg, [Symbol("x$(i-1)"); Symbol("x$(i)")], pp ) +addFactor!(fg, [Symbol("x$(i)"); :l2], ppr ) + + +## + + +tree, smt, hist = solveTree!(fg, tree) +# tree, smt, hist = solveTree!(fg; smtasks, recordcliqs=ls(fg)) + +e_time = time_ns() +elapsed = (e_time - s_time)/10^9 +append!(timing, elapsed) +println("elapsed time: ", elapsed) + +pl = plotKDE(fg, [:x0,:x1,:x2,:l1,:l2],dims=[1;2], levels=4) + +## +Gadfly.draw(PDF("batch_3.pdf", 20cm, 10cm),pl) +X0 = rand(getBelief(fg, :x0) ,N) +L1 = rand(getBelief(fg, :l1) ,N) +L2 = rand(getBelief(fg, :l2) ,N) +X1 = rand(getBelief(fg, :x1) ,N) +X2 = rand(getBelief(fg, :x2) ,N) +batch3 = vcat(X0,X1,X2,L1,L2) +npzwrite("batch3.npz",batch3) + +# +## Step 3 + +i = i + 1 +s_time = time_ns() +addVariable!(fg, :x3, Pose2) +pp = Pose2Pose2(MvNormal([9.776502885351334; -0.010587078502017132; 1.5591793408311467], diagm([0.3;0.3;0.05].^2))) +ppr = Pose2Point2Range(MvNormal([7.401417053438512], diagm([0.3].^2))) +addFactor!(fg, [Symbol("x$(i-1)"); Symbol("x$(i)")], pp ) +addFactor!(fg, [Symbol("x$(i)"); :l2], ppr ) + +## +tree, smt, hist = solveTree!(fg, tree) +e_time = time_ns() +elapsed = (e_time - s_time)/10^9 +append!(timing, elapsed) +println("elapsed time: ", elapsed) + +pl = plotKDE(fg, [:x0,:x1,:x2,:x3,:l1,:l2],dims=[1;2], levels=4) + +## +Gadfly.draw(PDF("batch_4.pdf", 20cm, 10cm),pl) +X0 = rand(getBelief(fg, :x0) ,N) +L1 = rand(getBelief(fg, :l1) ,N) +L2 = rand(getBelief(fg, :l2) ,N) +X1 = rand(getBelief(fg, :x1) ,N) +X2 = rand(getBelief(fg, :x2) ,N) +X3 = rand(getBelief(fg, :x3) ,N) +batch4 = vcat(X0,X1,X2,X3,L1,L2) +npzwrite("batch4.npz",batch4) + +## Step 4 (:l2 has triple range measurements) +i = i + 1 +s_time = time_ns() +addVariable!(fg, Symbol("x4"), Pose2) +pp = Pose2Pose2(MvNormal([9.644657137571507; 0.5847494762836476; 0.7422440549101994], Matrix(Diagonal([0.3;0.3;0.05].^2)))) +ppr = Pose2Point2Range(MvNormal([7.331883435735532], Matrix(Diagonal([0.3].^2)))) +addFactor!(fg, [Symbol("x$(i-1)"); Symbol("x$(i)")], pp ) +addFactor!(fg, [Symbol("x$(i)"); Symbol("l2")], ppr ) + +## +tree, smt, hist = solveTree!(fg, tree) +e_time = time_ns() +elapsed = (e_time - s_time)/10^9 +append!(timing, elapsed) +println("elapsed time: ", elapsed) + +pl = plotKDE(fg, [:x0,:x1,:x2,:x3,:x4,:l1,:l2],dims=[1;2], levels=4) + +## +Gadfly.draw(PDF("batch_5.pdf", 20cm, 10cm),pl) +X0 = rand(getBelief(fg, :x0) ,N) +L1 = rand(getBelief(fg, :l1) ,N) +L2 = rand(getBelief(fg, :l2) ,N) +X1 = rand(getBelief(fg, :x1),N) +X2 = rand(getBelief(fg, :x2) ,N) +X3 = rand(getBelief(fg, :x3) ,N) +X4 = rand(getBelief(fg, :x4),N) +batch5 = vcat(X0,X1,X2,X3,X4,L1,L2) +npzwrite("batch5.npz",batch5) + +## Step 5 (triple range to :l1) +i = i + 1 +s_time = time_ns() +addVariable!(fg, Symbol("x5"), Pose2) +pp = Pose2Pose2(MvNormal([9.725096752125593; 0.6094800276622434; 0.7750400402527422], Matrix(Diagonal([0.3;0.3;0.05].^2)))) +ppr = Pose2Point2Range(MvNormal([6.476782344345081], Matrix(Diagonal([0.3].^2)))) +addFactor!(fg, [Symbol("x$(i-1)"); Symbol("x$(i)")], pp ) +addFactor!(fg, [Symbol("x$(i)"); Symbol("l1")], ppr ) + +## +tree, smt, hist = solveTree!(fg, tree) +e_time = time_ns() +elapsed = (e_time - s_time)/10^9 +append!(timing, elapsed) +println("elapsed time: ", elapsed) + +pl = plotKDE(fg, [:x0,:x1,:x2,:x3,:x4,:x5,:l1,:l2],dims=[1;2], levels=4) + +## +Gadfly.draw(PDF("batch_6.pdf", 20cm, 10cm),pl) +X0 = rand(getBelief(fg, :x0),N) +L1 = rand(getBelief(fg, :l1),N) +L2 = rand(getBelief(fg, :l2),N) +X1 = rand(getBelief(fg, :x1),N) +X2 = rand(getBelief(fg, :x2),N) +X3 = rand(getBelief(fg, :x3),N) +X4 = rand(getBelief(fg, :x4),N) +X5 = rand(getBelief(fg, :x5),N) +batch6 = vcat(X0,X1,X2,X3,X4,X5,L1,L2) +npzwrite("batch6.npz",batch6) +npzwrite("timing.npz",timing) + +# +# +# pl = plotSLAM2D(fg) +# +# # For scripting use-cases you can export the image +# Gadfly.draw(PDF("/tmp/test.pdf", 20cm, 10cm),pl) # or PNG(...) +# +# L1 = rand(getBelief(fg, :l1) ,N) +# # L2 = rand(getBelief(fg, :l2) ,N) +# X0 = rand(getBelief(fg, :x0) ,N) +# X1 = rand(getBelief(fg, :x1) ,N) +# # X2 = rand(getBelief(fg, :x2) ,N) +# # X3 = rand(getBelief(fg, :x3) ,N) +# # X4 = rand(getBelief(fg, :x4) ,N) +# # X5 = rand(getBelief(fg, :x5) ,N) +# res = vcat(X0,X1,L1) +# using NPZ +# npzwrite("res.npz",res) +# # pl2 = Gadfly.plot(x=X0[1,:],y=X0[2,:], Geom.hexbin); +# # p1H = hstack(pl2) +# # +# # # convert to file +# # p1H |> PNG("/home/chad/Pictures/julia_x0_step0.png") +# +# # using RoMEPlotting +# +# #drawPoses(fg) +# # If you have landmarks, you can instead call +# # drawPosesLandms(fg) +# # +# # # # Draw the KDE for x0 +# # # plotKDE(fg, :x0) +# # # # Draw the KDE's for x0 and x1 +# # plotKDE(fg, [:x1]) +# # plotKDE(fg, [:x2]) +# # plotKDE(fg, [:x3]) +# # plotKDE(fg, [:x4]) +# # plotKDE(fg, [:x5]) +# # plotKDE(fg, [:l1]) +# # plotKDE(fg, [:l2]) diff --git a/examples/dev/case21_RMS.jl b/examples/dev/case21_RMS.jl new file mode 100644 index 000000000..2b34e5321 --- /dev/null +++ b/examples/dev/case21_RMS.jl @@ -0,0 +1,88 @@ +using JLD2 +@load "data/groundref_b8acd9_wynglas.jld2" +ground_ref = deepcopy(d) +varIds = collect(keys(ground_ref)) +sort!(varIds, lt=DFG.natural_lt) +refx = [] +refy = [] +refθ = [] +for idx in varIds + push!(refx, ground_ref[idx].val[1]) + push!(refy, ground_ref[idx].val[2]) + push!(refθ, ground_ref[idx].val[3]) +end + +cd(@__DIR__) +using DistributedFactorGraphs +using RoME +## load FG +folder = "results/skuif_X_l7_8_9_14/mm/" +folder = "results/skuif_X_l7_8_init/mm/" +filels = readdir(pwd()*"/"*folder) +sort!(filels, lt=natural_lt) +filels = filels[2:2:end] +n = length(filels) +big_ppi_mat_x = zeros(68,n)#Matrix{Float64}(undef, 68, 20) +big_ppi_mat_y = zeros(68,n)#Matrix{Float64}(undef, 68, 20) +big_ppi_mat_θ = zeros(68,n)#Matrix{Float64}(undef, 68, 20) +for (i,filein) in enumerate(filels) + fg = initfg() + @info "loading: $(filename*filein)" + loadDFG!(fg, folder*filein) ## + varIds = ls(fg) + sort!(varIds, lt=DFG.natural_lt) + for (k, idx) in enumerate(varIds) + # segppe = getVariableSolverData(fg, idx, :parametric).val + # segppe = getPPE(fg, idx) |> getMeanPPE + segppe = getPPE(fg, idx) |> getMaxPPE + big_ppi_mat_x[k,i] = segppe[1] + big_ppi_mat_y[k,i] = segppe[2] + big_ppi_mat_θ[k,i] = segppe[3] + end +end##using Plotserr_mat_x = Matrix{Float64}(undef, 68, n) +err_mat_y = Matrix{Float64}(undef, 68, n) +err_mat_θ = Matrix{Float64}(undef, 68, n) +Xrefx = refx[9:end] +Xrefy = refy[9:end] +Xrefθ = refθ[9:end] +p = scatter(refx[1:8],refy[1:8]) +scatter!(p, big_ppi_mat_x[1:2:16,1], big_ppi_mat_y[1:2:16,1]) +scatter!(p, big_ppi_mat_x[2:2:16,1], big_ppi_mat_y[2:2:16,1]) +Xbig_ppi_mat_x = big_ppi_mat_x[17:end,:] +Xbig_ppi_mat_y = big_ppi_mat_y[17:end,:] +Xbig_ppi_mat_θ = big_ppi_mat_θ[17:end,:] +err_mat_x = Matrix{Float64}(undef, 52, n) +err_mat_y = Matrix{Float64}(undef, 52, n) +err_mat_θ = Matrix{Float64}(undef, 52, n) +for i = 1:52 + err_mat_x[i,:] = Xrefx[i] .- Xbig_ppi_mat_x[i,:] + err_mat_y[i,:] = Xrefy[i] .- Xbig_ppi_mat_y[i,:] + err_mat_θ[i,:] = Xrefθ[i] .- Xbig_ppi_mat_θ[i,:] +end +for i = 1:68 + err_mat_x[i,:] = big_ppi_mat_x[i,1] .- big_ppi_mat_x[i,:] + err_mat_y[i,:] = big_ppi_mat_y[i,1] .- big_ppi_mat_y[i,:] + err_mat_θ[i,:] = big_ppi_mat_θ[i,1] .- big_ppi_mat_θ[i,:] +end +Plots.plot(err_mat_x[9:end,:]') +Plots.plot(err_mat_x[1:9,:]') +Plots.plot(big_ppi_mat_x) +Plots.plot(big_ppi_mat_y) +Plots.plot(big_ppi_mat_θ) +noises = collect(noiselevels) +noises = collect(multihypos) +# mx = Plots.plot(noises, sqrt.(mean(err_mat_x[9:end,:].^2; dims=1))', labels = "Mean Error x") +mx = Plots.plot(noises, sqrt.(mean(err_mat_x[9:end,:].^2; dims=1))', labels = "mm-x") +# mx = Plots.plot(noises, mean(err_mat_x[9:end,:]; dims=1)', labels = "") +# mc = Plots.plot!(mx, noises, sqrt.(mean(err_mat_y[9:end,:].^2; dims=1))', labels="Mean Error y") +mc = Plots.plot!(mx, noises, sqrt.(mean(err_mat_y[9:end,:].^2; dims=1))', labels="par-x") +xlabel!("Offset on landmarks l7 and l8 [m]") +ylabel!("RMS perturbation error over all poses [m]") +ylims!(0,0.15) +xlims!(0,0.5) +savefig("plots/skuif_X_l7_8_init/mm.pdf") +Plots.plot!(mc, ) +Plots.plot(err_mat_θ) +ground_ref[:l10].val + + diff --git a/examples/dev/sweeps.sh b/examples/dev/sweeps.sh new file mode 100644 index 000000000..f58d4f566 --- /dev/null +++ b/examples/dev/sweeps.sh @@ -0,0 +1,29 @@ +wynglasAnneal_sweep() { + julia160rc1 -O3 ParamSweep.jl $1 --anneal --spreadNH_start $2 --spreadNH_step $3 --spreadNH_stop $4 --inflation_start $2 --inflation_step $3 --inflation_stop $4 +} + +wynglasAnneal_30_05_05() { + wynglasAnneal_sweep $1 3.0 -0.5 0.5 +} + +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-10" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-9" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-8" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-7" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-6" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-5" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-4" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-3" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-2" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_-1" & +sleep 10 +wynglasAnneal_30_05_05 "skuif_X_l7_8_init/fg_0"