From 71c2f21e93d5bf9d66626ffd112c8a97b8257f2d Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 25 Dec 2020 18:41:08 -0500 Subject: [PATCH 1/6] general updates, IIF 19 --- Project.toml | 2 +- src/factors/PriorPose2.jl | 4 +- src/factors/Range2D.jl | 2 +- test/testDynPose2D.jl | 172 +++++++++++++++++++++++++++++--------- 4 files changed, 136 insertions(+), 44 deletions(-) diff --git a/Project.toml b/Project.toml index bb12561c..48cec87a 100644 --- a/Project.toml +++ b/Project.toml @@ -35,7 +35,7 @@ DistributedFactorGraphs = "0.10.2, 0.11" Distributions = "0.21, 0.22, 0.23, 0.24" DocStringExtensions = "0.7, 0.8" FileIO = "1.0.2, 1.1, 1.2" -IncrementalInference = "0.17, 0.18" +IncrementalInference = "0.17, 0.18, 0.19" JLD2 = "0.2, 0.3" KernelDensityEstimate = "0.5.1, 0.6" Optim = "0.22, 1.0" diff --git a/src/factors/PriorPose2.jl b/src/factors/PriorPose2.jl index ed10a481..16ce0838 100644 --- a/src/factors/PriorPose2.jl +++ b/src/factors/PriorPose2.jl @@ -52,10 +52,10 @@ mutable struct PackedPriorPose2 <: IncrementalInference.PackedInferenceType PackedPriorPose2(x::String) = new(x) end function convert(::Type{PackedPriorPose2}, d::PriorPose2) - return PackedPriorPose2(string(d.Z)) + return PackedPriorPose2(convert(PackedSamplableBelief, d.Z)) end function convert(::Type{PriorPose2}, d::PackedPriorPose2) - distr = extractdistribution(d.str) + distr = convert(SamplableBelief, d.str) return PriorPose2{typeof(distr)}(distr) end diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl index 92cadea4..eb9c5125 100644 --- a/src/factors/Range2D.jl +++ b/src/factors/Range2D.jl @@ -4,7 +4,7 @@ """ $(TYPEDEF) """ -mutable struct Point2Point2Range{D <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeFactorMinimize +mutable struct Point2Point2Range{D <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeMinimize Z::D Point2Point2Range{D}() where {D} = new{D}() Point2Point2Range{D}(d::D) where {D <: IIF.SamplableBelief} = new{D}(d) diff --git a/test/testDynPose2D.jl b/test/testDynPose2D.jl index 784f45a0..b3bcf373 100644 --- a/test/testDynPose2D.jl +++ b/test/testDynPose2D.jl @@ -1,18 +1,20 @@ using RoME using Test - +## @testset "test DynPose2 and velocity..." begin -global N = 100 -global fg = initfg() +## + +N = 100 +fg = initfg() # add first pose locations addVariable!(fg, :x0, DynPose2; nanosecondtime=0) # Prior factor as boundary condition -global pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), Matrix(Diagonal([0.01; 0.01; 0.001].^2))), +pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), Matrix(Diagonal([0.01; 0.01; 0.001].^2))), MvNormal([10.0;0], Matrix(Diagonal([0.1; 0.1].^2)))) addFactor!(fg, [:x0;], pp0) @@ -22,19 +24,17 @@ IncrementalInference.doautoinit!(fg, [getVariable(fg,:x0);]) addVariable!(fg, :x1, DynPose2; nanosecondtime=1000_000_000) # conditional likelihood between Dynamic Point2 -global dp2dp2 = VelPose2VelPose2(MvNormal([10.0;0;0], Matrix(Diagonal([0.01;0.01;0.001].^2))), +dp2dp2 = VelPose2VelPose2(MvNormal([10.0;0;0], Matrix(Diagonal([0.01;0.01;0.001].^2))), MvNormal([0.0;0], Matrix(Diagonal([0.1; 0.1].^2)))) addFactor!(fg, [:x0;:x1], dp2dp2) -# getVal(fg,:x0) -global pts = approxConv(fg, :x0x1f1, :x1) +pts = approxConv(fg, :x0x1f1, :x1) -# Graphs.plot(fg.g) # ensureAllInitialized!(fg) -tree, smt, hist = solveTree!(fg) +tree, smt, hist = solveTree!(fg); -global X1 = getVal(fg, :x1) +X1 = getVal(fg, :x1) @test 0.9*N <= sum(abs.(X1[1,:] .- 10.0) .< 0.75) @test 0.9*N <= sum(abs.(X1[2,:] .- 0.0) .< 0.75) @@ -46,7 +46,7 @@ global X1 = getVal(fg, :x1) # using RoMEPlotting # # plotLocalProduct(fg, :x10, dims=[1;2]) -# # drawPoses(fg) +# # plotSLAM2DPoses(fg) # xx1 = marginal(getKDE(fg, :x1),[1;2;3]) # plotPose(Pose2(), [xx1]) # plotPose(fg, [:x1], levels=1, show=false) @@ -54,74 +54,86 @@ global X1 = getVal(fg, :x1) # plotKDE(marginal(getKDE(fg, :x1),[4;5]), levels=5) +## + end @testset "test distribution compare functions..." begin -global mu = randn(6) -global mv1 = MvNormal(deepcopy(mu), Matrix{Float64}(LinearAlgebra.I, 6,6)) -global mv2 = MvNormal(deepcopy(mu), Matrix{Float64}(LinearAlgebra.I, 6,6)) -global mv3 = MvNormal(randn(6), Matrix{Float64}(LinearAlgebra.I, 6,6)) +## + +mu = randn(6) +mv1 = MvNormal(deepcopy(mu), Matrix{Float64}(LinearAlgebra.I, 6,6)) +mv2 = MvNormal(deepcopy(mu), Matrix{Float64}(LinearAlgebra.I, 6,6)) +mv3 = MvNormal(randn(6), Matrix{Float64}(LinearAlgebra.I, 6,6)) @test RoME.compareDensity(mv1, mv2) @test !RoME.compareDensity(mv1, mv3) @test !RoME.compareDensity(mv2, mv3) +## + end @testset "test DynPose2 packing converters..." begin -global pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), Matrix(Diagonal([0.01; 0.01; 0.001].^2))), +## + +pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), Matrix(Diagonal([0.01; 0.01; 0.001].^2))), MvNormal([10.0;0], Matrix(Diagonal([0.1; 0.1].^2)))) -global pp = convert(PackedDynPose2VelocityPrior, pp0) -global ppu = convert(DynPose2VelocityPrior, pp) +pp = convert(PackedDynPose2VelocityPrior, pp0) +ppu = convert(DynPose2VelocityPrior, pp) @test RoME.compare(pp0, ppu) -global dp2dp2 = VelPose2VelPose2(MvNormal([10.0;0;0], Matrix(Diagonal([0.01;0.01;0.001].^2))), +dp2dp2 = VelPose2VelPose2(MvNormal([10.0;0;0], Matrix(Diagonal([0.01;0.01;0.001].^2))), MvNormal([0.0;0], Matrix(Diagonal([0.1; 0.1].^2)))) -global pp = convert(PackedVelPose2VelPose2, dp2dp2) -global ppu = convert(VelPose2VelPose2, pp) +pp = convert(PackedVelPose2VelPose2, dp2dp2) +ppu = convert(VelPose2VelPose2, pp) @test RoME.compare(dp2dp2, ppu) +## + end @testset "test many DynPose2 chain stationary and 'pulled'..." begin -global N = 100 -global fg = initfg() +## + +N = 100 +fg = initfg() # add first pose locations addVariable!(fg, :x0, DynPose2; nanosecondtime=0) # Prior factor as boundary condition -global pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), Matrix(Diagonal([0.01; 0.01; 0.001].^2))), +pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), Matrix(Diagonal([0.01; 0.01; 0.001].^2))), MvNormal([0.0;0], Matrix(Diagonal([0.1; 0.1].^2)))) addFactor!(fg, [:x0;], pp0) -global sym = :x0 -global k = 0 +sym = :x0 +k = 0 for sy in Symbol[Symbol("x$i") for i in 1:10] -global k+=1 +k+=1 addVariable!(fg, sy, DynPose2; nanosecondtime=1000_000_000*k) # conditional likelihood between Dynamic Point2 -global dp2dp2 = VelPose2VelPose2(MvNormal([0.0;0;0], Matrix(Diagonal([1.0;0.1;0.001].^2))), +dp2dp2 = VelPose2VelPose2(MvNormal([0.0;0;0], Matrix(Diagonal([1.0;0.1;0.001].^2))), MvNormal([0.0;0], Matrix(Diagonal([0.1; 0.1].^2)))) addFactor!(fg, [sym;sy], dp2dp2) -global sym =sy +sym =sy end # for -global x5 = KDE.getKDEMean(getKDE(getVariable(fg, :x5))) +x5 = KDE.getKDEMean(getKDE(getVariable(fg, :x5))) @test abs(x5[1]) < 1.25 @test abs(x5[2]) < 1.25 @@ -132,7 +144,7 @@ global x5 = KDE.getKDEMean(getKDE(getVariable(fg, :x5))) ensureAllInitialized!(fg) -global x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) +x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) @test abs(x10[1]) < 1.25 @test abs(x10[2]) < 1.25 @@ -142,20 +154,98 @@ global x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) # drawGraph(fg, show=true) -# tree = wipeBuildNewTree!(fg) +# tree = buildTreeReset!(fg) # drawTree(tree, show=true) # using RoMEPlotting # Gadfly.set_default_plot_size(35cm, 25cm) -# drawPoses(fg) +# plotSLAM2DPoses(fg) # plotPose(fg, [:x10]) -# solve after being (graph) initialized -tree, smt, hist = solveTree!(fg) +## + +@error ".useMsgLikelihoods = false required until IIF #1010 completed." +getSolverParams(fg).useMsgLikelihoods = false + +# solve +smtasks = Task[] +tree, smt, hist = solveTree!(fg, smtasks=smtasks); #, recordcliqs=ls(fg)); + +## + +# hists = IIF.fetchCliqHistoryAll!(smtasks) + +# IIF.printCSMHistorySequential(hists) +# IIF.printCSMHistorySequential(hists, 3=>1:50) +# IIF.printCSMHistoryLogical(hists) + +# ## + +# getLogPath(fg) + +# using Caesar, Images, Graphs +# tree = hists[1][1].csmc.tree +# csmAnimateSideBySide(tree, hists, encode=true, nvenc=true, show=true) -global x5 = KDE.getKDEMean(getKDE(getVariable(fg, :x5))) +# ## + +# sfg = hists[3][8].csmc.cliqSubFg +# drawGraph(sfg, show=true) +# getSolverParams(hists[3][8].csmc.cliqSubFg).dbg = true +# fnc_, csmc_ = IIF.repeatCSMStep!(hists, 3, 8); +# # sfg_= csmc_.cliqSubFg + +# sfg = hists[3][9].csmc.cliqSubFg +# drawGraph(sfg, show=true) + +# IIF.getMessageBuffer(hists[3][8].csmc.cliq).downRx.belief + +# getSolverParams(hists[3][9].csmc.cliqSubFg).dbg = true +# IIF.repeatCSMStep!(hists, 3, 9); + +# ## + +# tree_ = hists[3][9].csmc.tree +# drawTree(tree_, show=true) + +# ## whats up with down msgs + +# sfg9 = loadDFG(joinpath(getLogPath(fg), "logs/cliq3", "fg_beforedownsolve.tar.gz")) +# drawGraph(sfg9, show=true) + +# ls(sfg9, :x0) + +# getFactor(sfg9, :x0f2) + +# ## + +# sfg = hists[7][6].csmc.cliqSubFg +# drawGraph(sfg, show=true) +# IIF.repeatCSMStep!(hists, 7, 6) + + +# getSolverParams(hists[4][4].csmc.cliqSubFg).dbg = true +# IIF.repeatCSMStep!(hists, 4, 4); + +# IIF.getMessageBuffer(hists[4][4].csmc.cliq).upRx[7].belief +# IIF.getMessageBuffer(hists[4][4].csmc.cliq).upRx[7] +# IIF.getMessageBuffer(hists[4][4].csmc.cliq).upRx[7].jointmsg.priors + + +# IIF.repeatCSMStep!(hists, 4, 5) + + +## + +# tree = resetBuildTree!(fg); +# drawTree(tree, show=true); + +## + +x5 = getPPE(getVariable(fg, :x5)).suggested +# x5 = KDE.getKDEMean(getKDE(getVariable(fg, :x5))) @test abs(x5[1]) < 1.5 @test abs(x5[2]) < 1.5 @@ -163,7 +253,7 @@ global x5 = KDE.getKDEMean(getKDE(getVariable(fg, :x5))) @test abs(x5[4]) < 0.5 @test abs(x5[5]) < 0.5 -global x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) +x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) @test abs(x10[1]) < 2.75 @test abs(x10[2]) < 2.75 @@ -174,7 +264,7 @@ global x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) # pull the tail end out with position -global pp10 = DynPose2VelocityPrior(MvNormal([10.0;0;0], Matrix(Diagonal([0.01; 0.01; 0.001].^2))), +pp10 = DynPose2VelocityPrior(MvNormal([10.0;0;0], Matrix(Diagonal([0.01; 0.01; 0.001].^2))), MvNormal([0.0;0], Matrix(Diagonal([0.1; 0.1].^2)))) addFactor!(fg, [:x10;], pp10) @@ -184,7 +274,7 @@ fg2 = deepcopy(fg) tree, mst, hist = solveTree!(fg) # N=N -global x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) +x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) @test 5.0 < x10[1] @test abs(x10[2]) < 1.0 @@ -195,7 +285,7 @@ global x10 = KDE.getKDEMean(getKDE(getVariable(fg, :x10))) for sym in [Symbol("x$i") for i in 2:9] -global XX = KDE.getKDEMean(getKDE(getVariable(fg, sym))) +XX = KDE.getKDEMean(getKDE(getVariable(fg, sym))) @show sym, round.(XX,digits=5) @test -2.0 < XX[1] < 10.0 @@ -206,6 +296,8 @@ global XX = KDE.getKDEMean(getKDE(getVariable(fg, sym))) end +## + end From 405ceb20cf3739ea93dfa478886e15106ce575b5 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sat, 26 Dec 2020 01:33:34 +0000 Subject: [PATCH 2/6] CompatHelper: bump compat for "Reexport" to "1.0" --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 9fcc2bfe..0376e372 100644 --- a/Project.toml +++ b/Project.toml @@ -40,7 +40,7 @@ JLD2 = "0.2, 0.3" KernelDensityEstimate = "0.5.1, 0.6" Optim = "0.22, 1.0" ProgressMeter = "1" -Reexport = "0.2" +Reexport = "0.2, 1.0" Requires = "1.0" Rotations = "0.12.1, 0.13, 1.0" TensorCast = "0.2, 0.3" From a8ae7476c1a0f8b968714c5fdb7e3048b342417e Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 28 Dec 2020 18:10:19 -0500 Subject: [PATCH 3/6] restore IIF v0.19 tests --- src/factors/PartialPose3.jl | 27 +-- test/testBearingRange2D.jl | 109 +++++++---- test/testDidsonFunctions.jl | 74 +++++--- test/testPartialXYH.jl | 359 +++++++++++++++++++++--------------- test/testhigherdimroots.jl | 66 ++++--- test/testpartialpose3.jl | 95 +++++----- 6 files changed, 427 insertions(+), 303 deletions(-) diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index 311e4162..9b28798f 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -133,7 +133,7 @@ Partial factor between XY and Yaw of two Pose3 variables. To be deprecated: use Pose3Pose3XYYaw instead. """ -mutable struct PartialPose3XYYaw{T1,T2} <: AbstractRelativeFactor where {T1 <: SamplableBelief, T2 <: SamplableBelief} +mutable struct PartialPose3XYYaw{T1,T2} <: AbstractRelativeMinimize where {T1 <: SamplableBelief, T2 <: SamplableBelief} xy::T1 yaw::T2 partial::Tuple{Int,Int,Int} @@ -145,17 +145,17 @@ PartialPose3XYYaw(xy::T1, yaw::T2) where {T1 <: IIF.SamplableBelief, T2 <: IIF.S function getSample(pxyy::PartialPose3XYYaw, N::Int=1) return ([rand(pxyy.xy,N);rand(pxyy.yaw,N)[:]'], ) end -function (pxyy::PartialPose3XYYaw)(res::Array{Float64}, - userdata, - idx::Int, - meas::Tuple{Array{Float64,2}}, - wXi::Array{Float64,2}, - wXj::Array{Float64,2} ) +function (pxyy::PartialPose3XYYaw)( res::Array{Float64}, + userdata::FactorMetadata, + idx::Int, + meas::Tuple{Array{Float64,2}}, + wXi::Array{Float64,2}, + wXj::Array{Float64,2} ) # wXjhat = SE2(wXi[[1;2;6],idx]) * SE2(meas[1][1:3,idx]) jXjhat = SE2(wXj[[1;2;6],idx]) \ wXjhat se2vee!(res, jXjhat) - nothing + res'*res end """ @@ -163,7 +163,7 @@ end Partial factor between XY and Yaw of two Pose3 variables. """ -mutable struct Pose3Pose3XYYaw{T1,T2} <: AbstractRelativeFactor where {T1 <: SamplableBelief, T2 <: SamplableBelief} +mutable struct Pose3Pose3XYYaw{T1,T2} <: AbstractRelativeMinimize where {T1 <: SamplableBelief, T2 <: SamplableBelief} xy::T1 yaw::T2 partial::Tuple{Int,Int,Int} @@ -185,7 +185,7 @@ function (pxyy::Pose3Pose3XYYaw)(res::Array{Float64}, wXjhat = SE2(wXi[[1;2;6],idx]) * SE2(meas[1][1:3,idx]) jXjhat = SE2(wXj[[1;2;6],idx]) \ wXjhat se2vee!(res, jXjhat) - nothing + res'*res end """ @@ -200,10 +200,10 @@ mutable struct PackedPartialPose3XYYaw <: IncrementalInference.PackedInferenceTy PackedPartialPose3XYYaw(xy::String, yaw::String) = new(xy, yaw) end function convert(::Type{PartialPose3XYYaw}, d::PackedPartialPose3XYYaw) - return PartialPose3XYYaw( extractdistribution(d.xydata), extractdistribution(d.yawdata) ) + return PartialPose3XYYaw( convert(SamplableBelief, d.xydata), convert(SamplableBelief, d.yawdata) ) end function convert(::Type{PackedPartialPose3XYYaw}, d::PartialPose3XYYaw) - return PackedPartialPose3XYYaw( string(d.xy), string(d.yaw) ) + return PackedPartialPose3XYYaw( convert(PackedSamplableBelief, d.xy), convert(PackedSamplableBelief, d.yaw) ) end """ @@ -226,6 +226,9 @@ end $SIGNATURES Converter: PartialPose3XYYaw -> Dict{String, Any} + +DevNotes +- FIXME stop using _evalType, see DFG #590 """ function convert(::Type{RoME.PartialPose3XYYaw}, fact::Dict{String, Any}) xy = fact["measurement"][1] diff --git a/test/testBearingRange2D.jl b/test/testBearingRange2D.jl index 873942a0..a7e0e785 100644 --- a/test/testBearingRange2D.jl +++ b/test/testBearingRange2D.jl @@ -5,69 +5,90 @@ using Test import Base: convert +## + @testset "test sampling from BearingRange factor..." begin -global p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) +## + +p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) -global meas = getSample(p2br, 100) +meas = getSample(p2br, 100) @test abs(Statistics.mean(meas[1][1,:])) < 0.1 @test 0.05 < abs(Statistics.std(meas[1][1,:])) < 0.2 @test abs(Statistics.mean(meas[1][2,:]) - 20.0) < 1.0 @test 0.5 < abs(Statistics.std(meas[1][2,:])) < 1.5 +## + end @testset "test BearingRange factor residual function..." begin -global p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) +## + +# dummy variables +fg = initfg() +X0 = addVariable!(fg, :x0, Pose2) +X1 = addVariable!(fg, :x1, Point2) + + +## + +p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) + +xi = zeros(3,1) +li = zeros(2,1); li[1,1] = 20.0; +zi = (zeros(2,1),); zi[1][2,1] = 20.0 -global xi = zeros(3,1) -global li = zeros(2,1); li[1,1] = 20.0; -global zi = (zeros(2,1),); zi[1][2,1] = 20.0 +# dummy fmd during refactoring and consolidation work +fmd = IIF._defaultFactorMetadata([X0;X1]) -global idx = 1 -global res = zeros(2) -p2br(res, IncrementalInference.FactorMetadata(), idx, zi, xi, li) +idx = 1 +res = zeros(2) +p2br(res, fmd, idx, zi, xi, li) @show res @test norm(res) < 1e-14 -global xi = zeros(3,1) -global li = zeros(2,1); li[2,1] = 20.0; -global zi = (zeros(2,1),); zi[1][:,1] = [pi/2;20.0] +xi = zeros(3,1) +li = zeros(2,1); li[2,1] = 20.0; +zi = (zeros(2,1),); zi[1][:,1] = [pi/2;20.0] -global idx = 1 -global res = zeros(2) -p2br(res, IncrementalInference.FactorMetadata(), idx, zi, xi, li) +idx = 1 +res = zeros(2) +p2br(res, fmd, idx, zi, xi, li) @show res @test norm(res) < 1e-14 -global xi = zeros(3,1); xi[3,1] = pi/2 -global li = zeros(2,1); li[2,1] = 20.0; -global zi = (zeros(2,1),); zi[1][:,1] = [0.0;20.0] +xi = zeros(3,1); xi[3,1] = pi/2 +li = zeros(2,1); li[2,1] = 20.0; +zi = (zeros(2,1),); zi[1][:,1] = [0.0;20.0] -global idx = 1 -global res = zeros(2) -p2br(res, IncrementalInference.FactorMetadata(), idx, zi, xi, li) +idx = 1 +res = zeros(2) +p2br(res, fmd, idx, zi, xi, li) @show res @test norm(res) < 1e-14 -global xi = zeros(3,2); xi[3,2] = -pi/2 -global li = zeros(2,2); li[1,2] = 20.0; +xi = zeros(3,2); xi[3,2] = -pi/2 +li = zeros(2,2); li[1,2] = 20.0; # zi = ([0.0;pi/2],[0.0;20.0],) -global zi = (zeros(2,2),); zi[1][:,2] = [pi/2;20.0] +zi = (zeros(2,2),); zi[1][:,2] = [pi/2;20.0] -global idx = 2 -global res = zeros(2) -p2br(res, IncrementalInference.FactorMetadata(), idx, zi, xi, li) +idx = 2 +res = zeros(2) +p2br(res, fmd, idx, zi, xi, li) @show res @test norm(res) < 1e-14 +## + end @@ -75,9 +96,11 @@ end @testset "test unimodal bearing range factor, solve for landmark..." begin +## + # Start with an empty graph -global N = 1 -global fg = initfg() +N = 1 +fg = initfg() #add pose with partial constraint addVariable!(fg, :x0, Pose2) @@ -86,19 +109,19 @@ addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), 0.01*Matrix{Float64}(LinearA setVal!(fg, :x0, zeros(3,1)) ##----------- sanity check that predictbelief plumbing is doing the right thing -global pts, = predictbelief(fg, :x0, ls(fg, :x0), N=75) +pts, = predictbelief(fg, :x0, ls(fg, :x0), N=75) @test sum(abs.(Statistics.mean(pts,dims=2)) .< [0.1; 0.1; 0.1]) == 3 @test sum([0.05; 0.05; 0.05] .< Statistics.std(pts,dims=2) .< [0.15; 0.15; 0.15]) == 3 #------------ # Add landmark addVariable!(fg, :l1, Point2, tags=[:LANDMARK;]) -global li = zeros(2,1); li[1,1] = 20.0; +li = zeros(2,1); li[1,1] = 20.0; setVal!(fg, :l1, li) # Add bearing range measurement between pose and landmark -global p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) +p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) addFactor!(fg, [:x0; :l1], p2br, graphinit=false) # there should be just one (the bearingrange) factor connected to :l1 @@ -106,7 +129,7 @@ addFactor!(fg, [:x0; :l1], p2br, graphinit=false) # writeGraphPdf(fg) # check the forward convolution is working properly -global pts, = predictbelief(fg, :l1, ls(fg, :l1), N=75) +pts, = predictbelief(fg, :l1, ls(fg, :l1), N=75) @test sum(abs.(Statistics.mean(pts,dims=2) - [20.0; 0.0]) .< [2.0; 2.0]) == 2 @test sum([0.1; 0.1] .< Statistics.std(pts,dims=2) .< [3.0; 3.0]) == 2 @@ -116,19 +139,23 @@ global pts, = predictbelief(fg, :l1, ls(fg, :l1), N=75) # pl.coord = Coord.Cartesian(xmin=-5,xmax=25, ymin=-10.0,ymax=10) # pl +## + end @testset "test unimodal bearing range factor, solve for pose..." begin +## + # Start with an empty graph -global N = 1 -global fg = initfg() +N = 1 +fg = initfg() # Add landmark addVariable!(fg, :l1, Point2, tags=[:LANDMARK;]) addFactor!(fg, [:l1], PriorPoint2(MvNormal([20.0;0.0], Matrix(Diagonal([1.0;1.0].^2)))), graphinit=false ) # could be IIF.Prior -global li = zeros(2,1); li[1,1] = 20.0; +li = zeros(2,1); li[1,1] = 20.0; setVal!(fg, :l1, li) #add pose with partial constraint @@ -137,7 +164,7 @@ addVariable!(fg, :x0, Pose2) setVal!(fg, :x0, zeros(3,1)) # Add bearing range measurement between pose and landmark -global p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) +p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) addFactor!(fg, [:x0; :l1], p2br, graphinit=false) # there should be just one (the bearingrange) factor connected to :l1 @@ -145,21 +172,27 @@ addFactor!(fg, [:x0; :l1], p2br, graphinit=false) # writeGraphPdf(fg) # check the forward convolution is working properly -global pts, = predictbelief(fg, :x0, ls(fg, :x0), N=75) +pts, = predictbelief(fg, :x0, ls(fg, :x0), N=75) @show abs.(Statistics.mean(pts,dims=2)) @test sum(abs.(Statistics.mean(pts,dims=2)) .< [2.0; 2.0; 2.0]) == 3 @show Statistics.std(pts,dims=2) @test sum([0.1; 0.1; 0.01] .< Statistics.std(pts,dims=2) .< [5.0; 5.0; 2.0]) == 3 +## + end @testset "Testing Pose2Point2Bearing Initialization and Packing" begin +## + p2p2b = Pose2Point2Bearing( MvNormal([0.2,0.2,0.2], [1.0 0 0;0 1 0;0 0 1]) ) packed = convert(PackedPose2Point2Bearing, p2p2b) p2p2bTest = convert(Pose2Point2Bearing, packed) @test p2p2b.bearing.μ == p2p2bTest.bearing.μ @test p2p2b.bearing.Σ.mat == p2p2bTest.bearing.Σ.mat +## + end diff --git a/test/testDidsonFunctions.jl b/test/testDidsonFunctions.jl index 863961ac..9a8f2481 100644 --- a/test/testDidsonFunctions.jl +++ b/test/testDidsonFunctions.jl @@ -8,24 +8,33 @@ using RoME using Distributions +## +meas = LinearRangeBearingElevation((3.0,3e-4),(0.2,3e-4)) -global meas = LinearRangeBearingElevation((3.0,3e-4),(0.2,3e-4)) - -global N = 100 -global X, pts = 0.01*randn(6,N), zeros(3,N); -global t = Array{Array{Float64,2},1}() +N = 100 +X, pts = 0.01*randn(6,N), zeros(3,N); +t = Array{Array{Float64,2},1}() push!(t,X) push!(t,pts) # pre-emptively populate the measurements, kept separate since nlsolve calls fp(x, res) multiple times -global measurement = getSample(meas, N) +measurement = getSample(meas, N) @show zDim = size(measurement[1],1) -global ccw = CommonConvWrapper(meas, t[2], zDim, t, measurement=measurement, varidx=2) +fg = initfg() +X0 = addVariable!(fg, :x0, Pose3) +X1 = addVariable!(fg, :x1, Pose3) +addFactor!(fg, [:x0;:x1], meas) + +## + +fmd = IIF._defaultFactorMetadata([X0;X1], arrRef=t) + +ccw = CommonConvWrapper(meas, t[2], zDim, t, fmd, measurement=measurement, varidx=2) # TODO remove ccw.measurement = measurement @@ -45,24 +54,33 @@ end @warn "still need to insert kld(..) test to ensure this is working" -global p1 = kde!(pts); +p1 = kde!(pts); println("Test back projection from ") -global meas = LinearRangeBearingElevation((3.0,3e-4),(0.2,3e-4)) +meas = LinearRangeBearingElevation((3.0,3e-4),(0.2,3e-4)) -global N = 200 -global pts, L = 0.01*randn(6,N), zeros(3,N); +N = 200 +pts, L = 0.01*randn(6,N), zeros(3,N); L[1,:] .+= 3.0 L[2,:] .+= 0.65 -global t = Array{Array{Float64,2},1}() +t = Array{Array{Float64,2},1}() push!(t,pts) push!(t,L) -global measurement = getSample(meas, N) -global zDim = size(measurement,1) -global ccw = CommonConvWrapper(meas, t[1], zDim, t, varidx=1, measurement=measurement) +measurement = getSample(meas, N) +zDim = size(measurement,1) + + +fg = initfg() +X0 = addVariable!(fg, :x0, Pose3) +X1 = addVariable!(fg, :x1, Pose3) +addFactor!(fg, [:x0;:x1], meas) + +fmd = IIF._defaultFactorMetadata([X0;X1], arrRef=t) + +ccw = CommonConvWrapper(meas, t[1], zDim, t, fmd, varidx=1, measurement=measurement) # TODO remove ccw.measurement = measurement @@ -79,7 +97,7 @@ ccw.cpt[1].res = zeros(1) end -global p2 = kde!(pts); +p2 = kde!(pts); @@ -97,42 +115,42 @@ global p2 = kde!(pts); # do test directly in factor graph -global fg = initfg() +fg = initfg() # @show fg.registeredModuleFunctions -global N = 100 +N = 100 @warn "Breaks if not set to 100" -global initCov = Matrix{Float64}(LinearAlgebra.I, 6,6) +initCov = Matrix{Float64}(LinearAlgebra.I, 6,6) [initCov[i,i] = 0.01 for i in 4:6]; -global odoCov = deepcopy(initCov) +odoCov = deepcopy(initCov) println("Adding PriorPose3 to graph...") # X, pts = 0.01*randn(6,N), zeros(3,N); -global v1 = addVariable!(fg,:x1, Pose3, N=N) -global initPosePrior = PriorPose3( MvNormal(zeros(6), initCov) ) -global f1 = addFactor!(fg,[v1], initPosePrior) +v1 = addVariable!(fg,:x1, Pose3, N=N) +initPosePrior = PriorPose3( MvNormal(zeros(6), initCov) ) +f1 = addFactor!(fg,[v1], initPosePrior) println("Adding LinearRangeBearingElevation to graph...") # implemented in SensorModels -global meas = LinearRangeBearingElevation((3.0,3e-4),(0.2,3e-4)) +meas = LinearRangeBearingElevation((3.0,3e-4),(0.2,3e-4)) @time X = getVal(v1) # @time pts = approxConvBinary(X, meas, 3) # TODO add back when IIF v0.3.8+ is available # p1 = kde!(pts); # visual checking -global v2 = addVariable!(fg, :l1, Point3, N=N) -global f2 = addFactor!(fg, [:x1;:l1], meas) #, threadmodel=MultiThreaded) +v2 = addVariable!(fg, :l1, Point3, N=N) +f2 = addFactor!(fg, [:x1;:l1], meas) #, threadmodel=MultiThreaded) # ensureAllInitialized!(fg) # getVal(fg, :x1) -global L1pts = approxConv(fg, :x1l1f1, :l1) +L1pts = approxConv(fg, :x1l1f1, :l1) -global X1pts = approxConv(fg, :x1l1f1, :x1) +X1pts = approxConv(fg, :x1l1f1, :x1) # isInitialized(fg, :l1) # getVal(fg, :l1) diff --git a/test/testPartialXYH.jl b/test/testPartialXYH.jl index 5adce083..12c56334 100644 --- a/test/testPartialXYH.jl +++ b/test/testPartialXYH.jl @@ -8,42 +8,49 @@ using Test @testset "test x translation case" begin # trivial cases first, orientation based tests below -global wTx = Vector{AffineMap}(undef, 2) +wTx = Vector{AffineMap}(undef, 2) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(Quat(1.0, 0, 0, 0)) wTx[2] = Translation(10.0, 0, 0) ∘ LinearMap(Quat(1.0, 0, 0, 0)) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*diagm(ones(6)))) +res = zeros(6) + +# dummy value +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2], 0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -# global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2,0.001*Matrix{Float64}(LinearAlgebra.I, 3,3)) ) +# testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2,0.001*Matrix{Float64}(LinearAlgebra.I, 3,3)) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -55,38 +62,44 @@ end @testset "test z translation case" begin +# dummy value +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + # z translation only -global wTx = Vector{AffineMap}(undef,2) +wTx = Vector{AffineMap}(undef,2) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(Quat(1.0, 0, 0, 0)) wTx[2] = Translation(0, 0, 10.0) ∘ LinearMap(Quat(1.0, 0, 0, 0)) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -98,40 +111,46 @@ end @testset "test roll and translate case 1" begin +# dummy values +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + # different orientation, roll -global wTx = Vector{AffineMap}(undef, 2) -global sq2 = 1.0/sqrt(2) +wTx = Vector{AffineMap}(undef, 2) +sq2 = 1.0/sqrt(2) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(Quat(sq2, sq2, 0, 0)) wTx[2] = Translation(10.0, 0, 0) ∘ LinearMap(Quat(sq2, sq2, 0, 0)) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -144,40 +163,46 @@ end @testset "test roll and translate case 2" begin -global wTx = Vector{AffineMap}(undef, 2) +# dummy values +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +wTx = Vector{AffineMap}(undef, 2) # different orientation, roll -global sq2 = 1.0/sqrt(2) +sq2 = 1.0/sqrt(2) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(Quat(sq2, sq2, 0, 0)) wTx[2] = Translation(0, 10.0, 0) ∘ LinearMap(Quat(sq2, sq2, 0, 0)) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -192,40 +217,46 @@ end @testset "test pitch and translate case 1" begin -global wTx = Vector{AffineMap}(undef, 2) +# dummy value +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +wTx = Vector{AffineMap}(undef, 2) # different orientation, pitch -global qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) +qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(qq) wTx[2] = Translation(10.0, 0, 0) ∘ LinearMap(qq) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -239,40 +270,46 @@ end @testset "test pitch and translate case 2" begin -global wTx = Vector{AffineMap}(undef, 2) +# dummy value +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +wTx = Vector{AffineMap}(undef, 2) # different orientation, pitch -global qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) +qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(qq) wTx[2] = Translation(0, 10.0, 0) ∘ LinearMap(qq) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -288,40 +325,46 @@ end @testset "test pitch and translate case 3" begin -global wTx = Vector{AffineMap}(undef, 2) +# dummy values +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +wTx = Vector{AffineMap}(undef, 2) # different orientation, pitch -global qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) +qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(qq) wTx[2] = Translation(0, 0, 10.0) ∘ LinearMap(qq) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -338,40 +381,46 @@ end @testset "test pitch and translate case 4" begin -global wTx = Vector{AffineMap}(undef, 2) +# dummy values +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +wTx = Vector{AffineMap}(undef, 2) # different orientation, pitch -global qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) +qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/4,0,1.0,0)) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(qq) wTx[2] = Translation(0, 15.0, 10.0) ∘ LinearMap(qq) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 @@ -392,40 +441,46 @@ end @testset "test yaw and translate case 1" begin -global wTx = Vector{AffineMap}(undef, 2) +# dummy value +fg_ = initfg() +X0 = addVariable!(fg_, :x0, Pose3) +X1 = addVariable!(fg_, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +wTx = Vector{AffineMap}(undef, 2) # different orientation, yaw -global qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/2,0,0,1.0)) +qq = convert(Rotations.Quat, Rotations.AngleAxis(pi/2,0,0,1.0)) wTx[1] = Translation(0.0,0,0) ∘ LinearMap(qq) wTx[2] = Translation(10.0, 0, 0) ∘ LinearMap(qq) ## Recalculate XYH # change toolbox -global wTx1 = convert(SE3, wTx[1]) -global wTx2 = convert(SE3, wTx[2]) +wTx1 = convert(SE3, wTx[1]) +wTx2 = convert(SE3, wTx[2]) # get rotation to world frame for local level, free orientation -global wEx1 = convert(Euler, wTx1.R) +wEx1 = convert(Euler, wTx1.R) wEx1.Y = 0.0 -global wRlx1 = SE3(zeros(3), wEx1) +wRlx1 = SE3(zeros(3), wEx1) # wRx2 = deepcopy(wTx2); wRx2.t = zeros(3); # Odometries -global x1Tx2 = (wTx1\wTx2) -global wRlx1Tx2 = wRlx1 * x1Tx2 -global vEx1_2 = veeEuler(wRlx1Tx2) -global XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] -global prz2 = veeEuler(wTx1)[[3;4;5]] +x1Tx2 = (wTx1\wTx2) +wRlx1Tx2 = wRlx1 * x1Tx2 +vEx1_2 = veeEuler(wRlx1Tx2) +XYH1_2 = [vEx1_2[1], vEx1_2[2], vEx1_2[6]] +prz2 = veeEuler(wTx1)[[3;4;5]] # test with Pose3Pose3 -global testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) -global res = zeros(6) -testpp3(res, FactorMetadata(), 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testpp3 = Pose3Pose3(MvNormal(veeEuler(x1Tx2),0.001*Matrix{Float64}(LinearAlgebra.I, 6,6))) +res = zeros(6) +testpp3(res, fmd, 1, (veeEuler(x1Tx2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:3]) < 1e-10 @test norm(res[4:6]) < 1e-10 # test with PartialXYH -global testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) -global res = zeros(3) -testppxyh(res, FactorMetadata(), 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) +testppxyh = PartialPose3XYYaw( MvNormal(XYH1_2[1:2],0.001*Matrix{Float64}(LinearAlgebra.I, 2,2)), Normal(XYH1_2[3], 0.001) ) +res = zeros(3) +testppxyh(res, fmd, 1, (vectoarr2(XYH1_2),), vectoarr2(veeEuler(wTx1)), vectoarr2(veeEuler(wTx2))) @show res @test norm(res[1:2]) < 1e-10 @test abs(res[3]) < 1e-10 diff --git a/test/testhigherdimroots.jl b/test/testhigherdimroots.jl index 7130a4b9..ba2241dc 100644 --- a/test/testhigherdimroots.jl +++ b/test/testhigherdimroots.jl @@ -3,13 +3,15 @@ using LinearAlgebra using RoME # , IncrementalInference, TransformUtils, Distributions using Test -# import IncrementalInference: getSample +import IncrementalInference: getSample -mutable struct RotationTest <: IncrementalInference.AbstractRelativeFactor +mutable struct RotationTest <: IncrementalInference.AbstractRelativeRoots z::MvNormal end +getSample(rt::RotationTest, N::Int=1) = (reshape(rand(rt.z,N),1,:),) + # 3 dimensional line, z = [a b][x y]' + c function (rt::RotationTest)(res::Vector{Float64}, userdata, idx, meas, var1, var2) z = view(meas[1],:,idx) @@ -24,7 +26,7 @@ function (rt::RotationTest)(res::Vector{Float64}, userdata, idx, meas, var1, var nothing end -global rr = RotationTest(MvNormal(zeros(3), 0.001*Matrix{Float64}(LinearAlgebra.I, 3,3))) +rr = RotationTest(MvNormal(zeros(3), 0.001*Matrix{Float64}(LinearAlgebra.I, 3,3))) @@ -32,12 +34,12 @@ global rr = RotationTest(MvNormal(zeros(3), 0.001*Matrix{Float64}(LinearAlgebra. # known rotations -global eul = zeros(3,1) +eul = zeros(3,1) -global R1 = zeros(3,1) -global R2 = zeros(3,1) +R1 = zeros(3,1) +R2 = zeros(3,1) -global res = randn(3) +res = randn(3) rr(res, nothing, 1, (zeros(3,1),), R1, R2) @test norm(res) < 1e-10 @@ -45,12 +47,12 @@ rr(res, nothing, 1, (zeros(3,1),), R1, R2) # random rotations -global eul = 0.25*randn(3, 1) +eul = 0.25*randn(3, 1) -global R1 = rand(3,1) -global R2 = rand(3,1) +R1 = rand(3,1) +R2 = rand(3,1) -global res = zeros(3) +res = zeros(3) rr(res, nothing, 1, (zeros(3),), R1, R2) @@ -63,34 +65,41 @@ end @testset "test CommonConvWrapper functions" begin -global N = 10 +N = 10 -global i=1 +i=1 for i in 1:5 -global eul = 0.25*randn(3, N) +eul = 0.25*randn(3, N) # res = zeros(3) # @show rotationresidual!(res, eul, (zeros(0),x0)) # @show res # gg = (res, x) -> rotationresidual!(res, eul, (zeros(0),x)) -global x0 = 0.1*randn(3) -global res = zeros(3) +x0 = 0.1*randn(3) +res = zeros(3) # @show gg(res, x0) # @show res -global A = rand(3,N) -global B = rand(3,N) -global At = deepcopy(A) -global t = Array{Array{Float64,2},1}() +A = rand(3,N) +B = rand(3,N) +At = deepcopy(A) +t = Array{Array{Float64,2},1}() push!(t,A) push!(t,B) -global rr = RotationTest(MvNormal(zeros(3), 0.001*Matrix{Float64}(LinearAlgebra.I, 3,3))) +rr = RotationTest(MvNormal(zeros(3), 0.001*Matrix{Float64}(LinearAlgebra.I, 3,3))) + +zDim = 3 + +fg = initfg() +X0 = addVariable!(fg,:x0,Sphere1) +X1 = addVariable!(fg,:x1,Sphere1) +addFactor!(fg, [:x0;:x1], rr) -global zDim = 3 +fmd = IIF._defaultFactorMetadata([X0;X1], arrRef=t) -global ccw = CommonConvWrapper(rr, t[1], zDim, t, measurement=(eul,)) # old bug where measurement is not patched through fixed in IIF v0.3.9 +ccw = CommonConvWrapper(rr, t[1], zDim, t, fmd, measurement=(eul,)) # old bug where measurement is not patched through fixed in IIF v0.3.9 @test ccw.xDim == 3 @@ -100,17 +109,18 @@ ccw.measurement = (eul,) ccw(res, x0) # and return complete fr/gwp -global n = 1 +n = 1 for n in 1:N ccw.cpt[Threads.threadid()].particleidx = n numericSolutionCCW!( ccw ) # test the result -global qq = convert(Quaternion, Euler(eul[:,ccw.cpt[Threads.threadid()].particleidx]...)) -global q1 = convert(Quaternion, so3(ccw.cpt[Threads.threadid()].Y)) -global q2 = convert(Quaternion, so3(B[:,ccw.cpt[Threads.threadid()].particleidx])) -@test TransformUtils.compare(q1*q_conj(q2), qq, tol=1e-8) +qq = convert(Quaternion, Euler(eul[:,ccw.cpt[Threads.threadid()].particleidx]...)) +q2 = convert(Quaternion, so3(B[:,ccw.cpt[Threads.threadid()].particleidx])) +# removed as part of IIF #1025 push +# q1 = convert(Quaternion, so3(ccw.cpt[Threads.threadid()].Y)) +# @test TransformUtils.compare(q1*q_conj(q2), qq, tol=1e-8) end # particle for diff --git a/test/testpartialpose3.jl b/test/testpartialpose3.jl index 0ff881a7..22b52adb 100644 --- a/test/testpartialpose3.jl +++ b/test/testpartialpose3.jl @@ -7,23 +7,23 @@ using RoME using Test -global N=50 -global fg = initfg() +N=50 +fg = initfg() -global v1 = addVariable!(fg,:x1, Pose3, N=N) # 0.001*randn(6,N) -global f0 = addFactor!(fg, [:x1;], PriorPose3(MvNormal(zeros(6),1e-2*Matrix{Float64}(LinearAlgebra.I, 6,6)))) +v1 = addVariable!(fg,:x1, Pose3, N=N) # 0.001*randn(6,N) +f0 = addFactor!(fg, [:x1;], PriorPose3(MvNormal(zeros(6),1e-2*Matrix{Float64}(LinearAlgebra.I, 6,6)))) -global sigx = 0.01 -global sigy = 0.01 -global sigth = 0.0281 -global mu1 = [0.0;0.0; -10.0] -global prpz = PartialPriorRollPitchZ( +sigx = 0.01 +sigy = 0.01 +sigth = 0.0281 +mu1 = [0.0;0.0; -10.0] +prpz = PartialPriorRollPitchZ( MvNormal( mu1[1:2], [sigx 0.0; 0.0 sigy]^2 ), Normal( mu1[3], sigth ) ) -global mu2 = [20.0,5.0,pi/2] -global xyy = PartialPose3XYYaw( +mu2 = [20.0,5.0,pi/2] +xyy = PartialPose3XYYaw( MvNormal( mu2[1:2], [sigx 0.0; 0.0 sigy]^2 @@ -31,10 +31,10 @@ global xyy = PartialPose3XYYaw( Normal( mu2[3], sigth ) ) -global v2 = addVariable!(fg,:x2, Pose3, N=N) # randn(6,N) +v2 = addVariable!(fg,:x2, Pose3, N=N) # randn(6,N) -global f1 = addFactor!(fg, [:x2], prpz, graphinit=false) -global f2 = addFactor!(fg, [:x1;:x2], xyy, graphinit=false) +f1 = addFactor!(fg, [:x2], prpz, graphinit=false) +f2 = addFactor!(fg, [:x1;:x2], xyy, graphinit=false) # ls(fg, :x2) @@ -56,15 +56,15 @@ ensureAllInitialized!(fg) @test isInitialized(fg, :x2) # get values and ensure that a re-evaluation produces consistent results -global X2pts = getVal(fg, :x2) +X2pts = getVal(fg, :x2) @test sum(isnan.(X2pts)) == 0 # check that only partial states are updated -global pts = IIF.approxConv(fg, :x2f1, :x2, N=N) +pts = IIF.approxConv(fg, :x2f1, :x2, N=N) -global newdims = collect(DFG.getSolverData(f1).fnc.usrfnc!.partial) +newdims = collect(DFG.getSolverData(f1).fnc.usrfnc!.partial) -global olddims = setdiff(collect(1:6), newdims) +olddims = setdiff(collect(1:6), newdims) @test size(pts, 1) == 6 @test size(pts, 2) == N @@ -87,24 +87,29 @@ end @testset "test residual function of PartialPose3XYYaw" begin - global res = zeros(3) - global idx = 1 - global meas = getSample(xyy) - global xi = zeros(6,1) - global xja = zeros(6,1) - xyy(res, nothing, idx, meas, xi, xja) - @test abs(res[1]-mu2[1]) < 0.3 - @test abs(res[2]-mu2[2]) < 0.3 - @test abs(res[3]-mu2[3]) < 0.2 - - global xjb = zeros(6,1) - xjb[collect(xyy.partial),1] = mu2 - global res = zeros(3) - xyy(res, nothing, idx, meas, xi, xjb) - @test 0.0 < norm(res) < 0.3 - - global meas = getSample(xyy,100) - @test norm(Statistics.std(meas[1],dims=2) - [0.01;0.01;0.002]) < 0.05 +tfg = initfg() +X0 = addVariable!(tfg, :x0, Pose3) +X1 = addVariable!(tfg, :x1, Pose3) +fmd = IIF._defaultFactorMetadata([X0;X1]) + +res = zeros(3) +idx = 1 +meas = getSample(xyy) +xi = zeros(6,1) +xja = zeros(6,1) +xyy(res, fmd, idx, meas, xi, xja) +@test abs(res[1]-mu2[1]) < 0.3 +@test abs(res[2]-mu2[2]) < 0.3 +@test abs(res[3]-mu2[3]) < 0.2 + +xjb = zeros(6,1) +xjb[collect(xyy.partial),1] = mu2 +res = zeros(3) +xyy(res, fmd, idx, meas, xi, xjb) +@test 0.0 < norm(res) < 0.3 + +meas = getSample(xyy,100) +@test norm(Statistics.std(meas[1],dims=2) - [0.01;0.01;0.002]) < 0.05 end @@ -113,12 +118,12 @@ end @testset "test PartialPose3XYYaw evaluations" begin # get existing and predict new -global X2pts = getKDE(fg, :x2) |> getPoints -global pts = approxConv(fg, :x1x2f1, :x2, N=N) +X2pts = getBelief(fg, :x2) |> getPoints +pts = approxConv(fg, :x1x2f1, :x2, N=N) # find which dimensions are and and are not updated by XYYaw partial -global newdims = collect(DFG.getSolverData(f2).fnc.usrfnc!.partial) -global olddims = setdiff(collect(1:6), newdims) +newdims = collect(DFG.getSolverData(f2).fnc.usrfnc!.partial) +olddims = setdiff(collect(1:6), newdims) # check the number of points are correct @test size(pts, 1) == 6 @@ -141,7 +146,7 @@ end @test norm(X2pts[newdims,:] - pts[newdims,:]) < 1.0 # ensure that memory pointers are working correctly -global memcheck = getVal(v2) +memcheck = getVal(v2) @test norm(X2pts - memcheck) < 1e-10 end @@ -149,7 +154,7 @@ end @testset "test predictbelief with two functions" begin -global val, = predictbelief(fg, :x2, ls(fg, :x2), N=N) +val, = predictbelief(fg, :x2, ls(fg, :x2), N=N) for i in 1:N val[6,i] = wrapRad(val[6,i]) @@ -158,15 +163,15 @@ end @test size(val, 1) == 6 @test size(val, 2) == N -global estmu1mean = Statistics.mean(val[collect(DFG.getSolverData(f1).fnc.usrfnc!.partial),:],dims=2) -global estmu2mean = Statistics.mean(val[collect(DFG.getSolverData(f2).fnc.usrfnc!.partial),:],dims=2) +estmu1mean = Statistics.mean(val[collect(DFG.getSolverData(f1).fnc.usrfnc!.partial),:],dims=2) +estmu2mean = Statistics.mean(val[collect(DFG.getSolverData(f2).fnc.usrfnc!.partial),:],dims=2) @show estmu1mean @show estmu2mean @test sum(abs.(estmu1mean - mu1[[3;1;2]]) .< [0.7; 0.1; 0.1]) == 3 @test sum(abs.(estmu2mean - mu2) .< [0.7; 0.7; 0.15] ) == 3 -global memcheck = getVal(v2) +memcheck = getVal(v2) @test 1e-10 < norm(val - memcheck) From e74d3a22cc09f6556b8aaf34b149c21b07142ca3 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 29 Dec 2020 16:49:52 -0500 Subject: [PATCH 4/6] repace AbstractRelative Factor --- examples/KinematicJoints.jl | 4 ++-- src/SensorModels.jl | 2 +- src/factors/Bearing2D.jl | 14 +++++++------- src/factors/BearingRange2D.jl | 2 +- src/factors/DynPoint2D.jl | 4 ++-- src/factors/DynPose2D.jl | 17 ++++++++--------- src/factors/InertialPose3.jl | 2 +- src/factors/MultipleFeaturesConstraint.jl | 2 +- src/factors/MutablePose2Pose2.jl | 4 ++-- src/factors/Point2D.jl | 2 +- src/factors/Polar.jl | 2 +- src/factors/Pose2D.jl | 4 ++-- src/factors/Pose2Point2.jl | 2 +- src/factors/Pose3Pose3.jl | 2 +- src/factors/Range2D.jl | 2 +- src/factors/VelPoint2D.jl | 2 +- src/factors/VelPose2D.jl | 2 +- src/factors/flux/FluxModelsPose2Pose2.jl | 2 +- 18 files changed, 35 insertions(+), 36 deletions(-) diff --git a/examples/KinematicJoints.jl b/examples/KinematicJoints.jl index d198d3c6..23710443 100644 --- a/examples/KinematicJoints.jl +++ b/examples/KinematicJoints.jl @@ -6,7 +6,7 @@ import IncrementalInference: getSample using TransformUtils -mutable struct ZJoint <: AbstractRelativeFactor +mutable struct ZJoint <: AbstractRelativeRoots Zij::Distribution end function getSample(el::ZJoint, N=1) @@ -33,7 +33,7 @@ function (el::ZJoint)(res, userdata, idx, meas, xi, xj) nothing end -mutable struct XJoint <: AbstractRelativeFactor +mutable struct XJoint <: AbstractRelativeRoots Zij::Distribution end function getSample(el::XJoint, N=1) diff --git a/src/SensorModels.jl b/src/SensorModels.jl index 79cc11f9..8a30f2a2 100644 --- a/src/SensorModels.jl +++ b/src/SensorModels.jl @@ -19,7 +19,7 @@ end """ $(TYPEDEF) """ -mutable struct LinearRangeBearingElevation <: AbstractRelativeFactorMinimize +mutable struct LinearRangeBearingElevation <: AbstractRelativeMinimize range::Normal bearing::Normal elev::Uniform diff --git a/src/factors/Bearing2D.jl b/src/factors/Bearing2D.jl index 2d4eb2e9..1b40ea86 100644 --- a/src/factors/Bearing2D.jl +++ b/src/factors/Bearing2D.jl @@ -11,7 +11,7 @@ end Single dimension bearing constraint from Pose2 to Point2 variable. """ -struct Pose2Point2Bearing{B <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeFactorMinimize +struct Pose2Point2Bearing{B <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize bearing::B reuse::Vector{P2P2BearingReuse} Pose2Point2Bearing{B}() where B = new{B}() @@ -22,12 +22,12 @@ function getSample(pp2br::Pose2Point2Bearing, N::Int=1) return (reshape(rand(pp2br.bearing, N),1,N), ) end # define the conditional probability constraint -function (pp2br::Pose2Point2Bearing)(res::Array{Float64}, - userdata::FactorMetadata, - idx::Int, - meas::Tuple, - xi::Array{Float64,2}, - lm::Array{Float64,2} ) +function (pp2br::Pose2Point2Bearing)( res::Array{Float64}, + userdata::FactorMetadata, + idx::Int, + meas::Tuple, + xi::Array{Float64,2}, + lm::Array{Float64,2} ) # reuse = pp2br.reuse[Threads.threadid()] reuse.measvec[1] = cos(meas[1][idx] + xi[3,idx]) diff --git a/src/factors/BearingRange2D.jl b/src/factors/BearingRange2D.jl index 5979de24..8694604c 100644 --- a/src/factors/BearingRange2D.jl +++ b/src/factors/BearingRange2D.jl @@ -7,7 +7,7 @@ Bearing and Range constraint from a Pose2 to Point2 variable. """ -mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeFactorMinimize +mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize bearing::B range::R Pose2Point2BearingRange{B,R}() where {B,R} = new{B,R}() diff --git a/src/factors/DynPoint2D.jl b/src/factors/DynPoint2D.jl index 81260d8f..f828edcf 100644 --- a/src/factors/DynPoint2D.jl +++ b/src/factors/DynPoint2D.jl @@ -16,7 +16,7 @@ getSample(dp2v::DynPoint2VelocityPrior, N::Int=1) = (rand(dp2v.z,N), ) """ $(TYPEDEF) """ -mutable struct DynPoint2DynPoint2{T <: SamplableBelief} <: AbstractRelativeFactor +mutable struct DynPoint2DynPoint2{T <: SamplableBelief} <: AbstractRelativeRoots z::T DynPoint2DynPoint2{T}() where {T <: SamplableBelief} = new{T}() DynPoint2DynPoint2(z1::T) where {T <: SamplableBelief} = new{T}(z1) @@ -44,7 +44,7 @@ end """ $(TYPEDEF) """ -mutable struct Point2Point2Velocity{T} <: IncrementalInference.AbstractRelativeFactorMinimize where {T <: Distribution} +mutable struct Point2Point2Velocity{T} <: IncrementalInference.AbstractRelativeMinimize where {T <: Distribution} z::T Point2Point2Velocity{T}() where {T <: Distribution} = new{T}() Point2Point2Velocity(z1::T) where {T <: Distribution} = new{T}(z1) diff --git a/src/factors/DynPose2D.jl b/src/factors/DynPose2D.jl index 8d546a27..0a776cf4 100644 --- a/src/factors/DynPose2D.jl +++ b/src/factors/DynPose2D.jl @@ -22,7 +22,7 @@ getSample(dp2v::DynPose2VelocityPrior{T1,T2}, N::Int=1) where {T1 <: IIF.Samplab """ $(TYPEDEF) """ -mutable struct DynPose2Pose2{T} <: IncrementalInference.AbstractRelativeFactor where {T <: IIF.SamplableBelief} +mutable struct DynPose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeRoots Zpose::Pose2Pose2{T} #Zpose::T1 # reuseres::Vector{Float64} partial::Tuple{Int,Int,Int} @@ -32,13 +32,12 @@ end DynPose2Pose2(z1::T) where {T <: IIF.SamplableBelief} = DynPose2Pose2{T}(z1) getSample(vp2vp2::DynPose2Pose2, N::Int=1) = (rand(vp2vp2.Zpose.z,N), ) -function (vp2vp2::DynPose2Pose2{T})( - res::Array{Float64}, - userdata::FactorMetadata, - idx::Int, - meas::Tuple, - wXi::Array{Float64,2}, - wXj::Array{Float64,2} ) where {T <: IIF.SamplableBelief} +function (vp2vp2::DynPose2Pose2{T})(res::Array{Float64}, + userdata::FactorMetadata, + idx::Int, + meas::Tuple, + wXi::Array{Float64,2}, + wXj::Array{Float64,2} ) where {T <: IIF.SamplableBelief} # vp2vp2.Zpose(res, userdata, idx, meas, wXi, wXj) # wXjhat = SE2(wxi[1:3,idx])*SE2(meas[1][1:3,idx]) @@ -115,7 +114,7 @@ end """ $(TYPEDEF) """ -mutable struct DynPose2DynPose2{T <: IIF.SamplableBelief} <: AbstractRelativeFactor +mutable struct DynPose2DynPose2{T <: IIF.SamplableBelief} <: AbstractRelativeRoots Z::T reuseres::Vector{Vector{Float64}} DynPose2DynPose2{T}() where {T <: IIF.SamplableBelief} = new{T}() diff --git a/src/factors/InertialPose3.jl b/src/factors/InertialPose3.jl index 60c14610..7cb4fd61 100644 --- a/src/factors/InertialPose3.jl +++ b/src/factors/InertialPose3.jl @@ -146,7 +146,7 @@ $(TYPEDEF) Inertial Odometry version of preintegration procedure and used as a factor between InertialPose3 types for inertial navigation in factor graphs. """ -mutable struct InertialPose3 <: AbstractRelativeFactor #RoME.BetweenPoses +mutable struct InertialPose3 <: AbstractRelativeRoots # Zij is entropy of veeLie15, pioc is preintegral measurements, pido is compensation gradients. Zij::Distribution pioc::InertialPose3Container diff --git a/src/factors/MultipleFeaturesConstraint.jl b/src/factors/MultipleFeaturesConstraint.jl index 0f05853b..8cb36d69 100644 --- a/src/factors/MultipleFeaturesConstraint.jl +++ b/src/factors/MultipleFeaturesConstraint.jl @@ -34,7 +34,7 @@ $(TYPEDEF) two sets of three feature sightings and a body to camera lever arm transform """ -mutable struct MultipleFeatures2D <: IncrementalInference.AbstractRelativeFactorMinimize +mutable struct MultipleFeatures2D <: IIF.AbstractRelativeMinimize # treat these as angles positive X->Y, X forward, Y left and Z up xir1::Normal xir2::Normal diff --git a/src/factors/MutablePose2Pose2.jl b/src/factors/MutablePose2Pose2.jl index 73ceaabc..6262ad68 100644 --- a/src/factors/MutablePose2Pose2.jl +++ b/src/factors/MutablePose2Pose2.jl @@ -8,10 +8,10 @@ export MutablePose2Pose2Gaussian, PackedMutablePose2Pose2Gaussian Specialized Pose2Pose2 factor type (Gaussian), which allows for rapid accumulation of odometry information as a branch on the factor graph. """ -mutable struct MutablePose2Pose2Gaussian <: IIF.AbstractRelativeFactor +mutable struct MutablePose2Pose2Gaussian <: IIF.AbstractRelativeRoots Zij::MvNormal timestamp::DateTime - MutablePose2Pose2Gaussian(Zij::MvNormal=MvNormal(zeros(3),Matrix(Diagonal([0.01; 0.01; 0.001].^2))), timestamp::DateTime=now()) = new(Zij, timestamp) + MutablePose2Pose2Gaussian(Zij::MvNormal=MvNormal(zeros(3),diagm([0.01; 0.01; 0.001].^2)), timestamp::DateTime=now()) = new(Zij, timestamp) end function getSample(fct::MutablePose2Pose2Gaussian, N::Int=100) return (rand(fct.Zij, N), ) diff --git a/src/factors/Point2D.jl b/src/factors/Point2D.jl index f4710d97..d7f808a7 100644 --- a/src/factors/Point2D.jl +++ b/src/factors/Point2D.jl @@ -31,7 +31,7 @@ end """ $(TYPEDEF) """ -mutable struct Point2Point2{D <: IIF.SamplableBelief} <: AbstractRelativeFactor +mutable struct Point2Point2{D <: IIF.SamplableBelief} <: AbstractRelativeRoots Zij::D # empty constructor Point2Point2{T}() where T = new{T}() diff --git a/src/factors/Polar.jl b/src/factors/Polar.jl index 9dbee376..8d984c2c 100644 --- a/src/factors/Polar.jl +++ b/src/factors/Polar.jl @@ -38,7 +38,7 @@ end Linear offset factor of `IIF.SamplableBelief` between two `Polar` variables. """ -mutable struct PolarPolar{T1<:IIF.SamplableBelief, T2<:IIF.SamplableBelief} <: IIF.AbstractRelativeFactor +mutable struct PolarPolar{T1<:IIF.SamplableBelief, T2<:IIF.SamplableBelief} <: IIF.AbstractRelativeRoots Zrange::T1 Zangle::T2 # empty constructor diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index 03783450..b2fc9364 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -8,7 +8,7 @@ Related Pose3Pose3, Point2Point2, MutablePose2Pose2Gaussian, DynPose2, InertialPose3 """ -struct Pose2Pose2{T} <: IncrementalInference.AbstractRelativeFactor where {T <: IIF.SamplableBelief} +struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeRoots z::T # empty constructor Pose2Pose2{T}() where {T <: IIF.SamplableBelief} = new{T}() @@ -57,7 +57,7 @@ end """ $(TYPEDEF) """ -mutable struct PackedPose2Pose2 <: IncrementalInference.PackedInferenceType +mutable struct PackedPose2Pose2 <: IIF.PackedInferenceType datastr::String PackedPose2Pose2() = new() PackedPose2Pose2(x::String) = new(x) diff --git a/src/factors/Pose2Point2.jl b/src/factors/Pose2Point2.jl index 782c642f..cb1994fb 100644 --- a/src/factors/Pose2Point2.jl +++ b/src/factors/Pose2Point2.jl @@ -9,7 +9,7 @@ export Pose2Point2, PackedPose2Point2 Bearing and Range constraint from a Pose2 to Point2 variable. """ -struct Pose2Point2{T <: IIF.SamplableBelief} <: IncrementalInference.AbstractRelativeFactorMinimize +struct Pose2Point2{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize Zij::T # empty constructor Pose2Point2{T}() where {T} = new{T}() diff --git a/src/factors/Pose3Pose3.jl b/src/factors/Pose3Pose3.jl index 7187baa4..67e9f55b 100644 --- a/src/factors/Pose3Pose3.jl +++ b/src/factors/Pose3Pose3.jl @@ -39,7 +39,7 @@ $(TYPEDEF) Rigid transform factor between two Pose3 compliant variables. """ -mutable struct Pose3Pose3{T <: IIF.SamplableBelief} <: AbstractRelativeFactor +mutable struct Pose3Pose3{T <: IIF.SamplableBelief} <: AbstractRelativeRoots Zij::T reuse::Vector{PP3REUSE} Pose3Pose3{T}() where T = new{T}() diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl index eb9c5125..c74599ae 100644 --- a/src/factors/Range2D.jl +++ b/src/factors/Range2D.jl @@ -55,7 +55,7 @@ end Range only measurement from Pose2 to Point2 variable. """ -mutable struct Pose2Point2Range{T} <: IncrementalInference.AbstractRelativeFactorMinimize +mutable struct Pose2Point2Range{T} <: IIF.AbstractRelativeMinimize Z::T partial::Tuple{Int,Int} Pose2Point2Range{T}() where T = new() diff --git a/src/factors/VelPoint2D.jl b/src/factors/VelPoint2D.jl index c85a0ac1..d71bd521 100644 --- a/src/factors/VelPoint2D.jl +++ b/src/factors/VelPoint2D.jl @@ -6,7 +6,7 @@ """ $(TYPEDEF) """ -mutable struct VelPoint2VelPoint2{T} <: IncrementalInference.AbstractRelativeFactorMinimize where {T <: Distribution} +mutable struct VelPoint2VelPoint2{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize z::T VelPoint2VelPoint2{T}() where {T <: Distribution} = new{T}() VelPoint2VelPoint2{T}(z1::T) where {T <: Distribution} = new{T}(z1) diff --git a/src/factors/VelPose2D.jl b/src/factors/VelPose2D.jl index 4c13670b..439b6942 100644 --- a/src/factors/VelPose2D.jl +++ b/src/factors/VelPose2D.jl @@ -5,7 +5,7 @@ export VelPose2VelPose2, PackedVelPose2VelPose2 """ $(TYPEDEF) """ -mutable struct VelPose2VelPose2{T1 <: IIF.SamplableBelief,T2 <: IIF.SamplableBelief} <: AbstractRelativeFactorMinimize +mutable struct VelPose2VelPose2{T1 <: IIF.SamplableBelief,T2 <: IIF.SamplableBelief} <: AbstractRelativeMinimize Zpose::Pose2Pose2{T1} #Zpose::T1 Zvel::T2 reuseres::Vector{Vector{Float64}} diff --git a/src/factors/flux/FluxModelsPose2Pose2.jl b/src/factors/flux/FluxModelsPose2Pose2.jl index d2d19f79..8c148eb6 100644 --- a/src/factors/flux/FluxModelsPose2Pose2.jl +++ b/src/factors/flux/FluxModelsPose2Pose2.jl @@ -26,7 +26,7 @@ import Base: convert import IncrementalInference: getSample, calcZDim -struct FluxModelsPose2Pose2{P,D<:AbstractArray,M<:SamplableBelief} <: AbstractRelativeFactor +struct FluxModelsPose2Pose2{P,D<:AbstractArray,M<:SamplableBelief} <: AbstractRelativeRoots allPredModels::Vector{P} joyVelData::D naiveModel::M From 1463cbcc9d83a2169e19a9892621f7a918c15dd7 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 29 Dec 2020 16:58:14 -0500 Subject: [PATCH 5/6] more clean up --- Project.toml | 4 ++-- src/factors/DynPoint2D.jl | 8 ++++---- src/factors/Point2D.jl | 2 +- src/factors/flux/FluxModelsPose2Pose2.jl | 3 ++- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/Project.toml b/Project.toml index 0376e372..6c24be5a 100644 --- a/Project.toml +++ b/Project.toml @@ -1,8 +1,8 @@ name = "RoME" uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" -keywords = ["SLAM", "state-estimation", "MM-iSAM", "inference", "robotics"] +keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics"] desc = "Non-Gaussian simultaneous localization and mapping" -version = "0.11.1" +version = "0.11.2" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89" diff --git a/src/factors/DynPoint2D.jl b/src/factors/DynPoint2D.jl index f828edcf..2b6dd5a5 100644 --- a/src/factors/DynPoint2D.jl +++ b/src/factors/DynPoint2D.jl @@ -44,10 +44,10 @@ end """ $(TYPEDEF) """ -mutable struct Point2Point2Velocity{T} <: IncrementalInference.AbstractRelativeMinimize where {T <: Distribution} +mutable struct Point2Point2Velocity{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize z::T - Point2Point2Velocity{T}() where {T <: Distribution} = new{T}() - Point2Point2Velocity(z1::T) where {T <: Distribution} = new{T}(z1) + Point2Point2Velocity{T}() where {T <: IIF.SamplableBelief} = new{T}() + Point2Point2Velocity(z1::T) where {T <: IIF.SamplableBelief} = new{T}(z1) end getSample(p2p2v::Point2Point2Velocity, N::Int=1) = (rand(p2p2v.z,N), ) function (p2p2v::Point2Point2Velocity)( @@ -77,7 +77,7 @@ end """ $(TYPEDEF) """ -mutable struct PackedDynPoint2VelocityPrior <: IncrementalInference.PackedInferenceType +mutable struct PackedDynPoint2VelocityPrior <: IIF.PackedInferenceType str::String PackedDynPoint2VelocityPrior() = new() PackedDynPoint2VelocityPrior(z1::String) = new(z1) diff --git a/src/factors/Point2D.jl b/src/factors/Point2D.jl index d7f808a7..4c910fb7 100644 --- a/src/factors/Point2D.jl +++ b/src/factors/Point2D.jl @@ -4,7 +4,7 @@ $(TYPEDEF) Direction observation information of a `Point2` variable. """ -mutable struct PriorPoint2{T} <: IncrementalInference.AbstractPrior where {T <: IIF.SamplableBelief} +mutable struct PriorPoint2{T <: IIF.SamplableBelief} <: IncrementalInference.AbstractPrior Z::T # empty constructor PriorPoint2{T}() where T = new() diff --git a/src/factors/flux/FluxModelsPose2Pose2.jl b/src/factors/flux/FluxModelsPose2Pose2.jl index 8c148eb6..fd086a18 100644 --- a/src/factors/flux/FluxModelsPose2Pose2.jl +++ b/src/factors/flux/FluxModelsPose2Pose2.jl @@ -31,7 +31,8 @@ struct FluxModelsPose2Pose2{P,D<:AbstractArray,M<:SamplableBelief} <: AbstractRe joyVelData::D naiveModel::M naiveFrac::Ref{Float64} - Zij::Pose2Pose2 + # TODO type stability likely wants parameter Pose2Pose2{<:SamplableBelief} + Zij::Pose2Pose2{<:SamplableBelief} specialSampler::Function # special keyword field name used to invoke 'specialSampler' logic DT::Ref{Float64} shuffle::Ref{Bool} From fd64078374c018651841af86d96d412c14a3a640 Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 30 Dec 2020 12:31:03 -0500 Subject: [PATCH 6/6] depr maxparallel --- examples/MITDatasetBatch.jl | 3 ++- examples/MITDatasetFixedLag.jl | 6 ++++-- examples/MITDatasetIncremental.jl | 3 ++- examples/ManhattanDatasetBatch.jl | 3 ++- examples/ManhattanDatasetFixedLag-FromBatch.jl | 8 +++++--- examples/ManhattanDatasetFixedLag-MargOnly.jl | 8 +++++--- examples/ManhattanDatasetFixedLag.jl | 6 ++++-- examples/ManhattanDatasetIncremental.jl | 3 ++- examples/OutlierVsMultimodal.jl | 8 +++++--- src/Slam.jl | 2 +- test/testDynPose2D.jl | 2 +- 11 files changed, 33 insertions(+), 19 deletions(-) diff --git a/examples/MITDatasetBatch.jl b/examples/MITDatasetBatch.jl index 17c1ba36..05b5f7b0 100644 --- a/examples/MITDatasetBatch.jl +++ b/examples/MITDatasetBatch.jl @@ -39,7 +39,8 @@ function solve_batch(total_meas::Integer) # Solve the graph, and save a copy of the tree. saveDFG(fg, "$(getLogPath(fg))/fg-before-solve") - tree, smt, hist = solveTree!(fg, maxparallel=1000) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg) 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") diff --git a/examples/MITDatasetFixedLag.jl b/examples/MITDatasetFixedLag.jl index 0eb3cf8f..cc845b75 100644 --- a/examples/MITDatasetFixedLag.jl +++ b/examples/MITDatasetFixedLag.jl @@ -58,7 +58,8 @@ function go_fixedlag(initial_offset::Integer, # 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) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg) 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") @@ -99,7 +100,8 @@ function go_fixedlag(initial_offset::Integer, @info "Going for solve" # Solve the graph, and save a copy of the tree. - tree, smt, hist = solveTree!(fg, tree, maxparallel=1000) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg, tree) 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") diff --git a/examples/MITDatasetIncremental.jl b/examples/MITDatasetIncremental.jl index ce1b19b6..55d6d106 100644 --- a/examples/MITDatasetIncremental.jl +++ b/examples/MITDatasetIncremental.jl @@ -89,7 +89,8 @@ function go(initial_offset::Integer, final_timestep::Integer, solve_stride::Inte @info "Going for solve" # Solve the graph, and save a copy of the tree. - tree, smt, hist = solveTree!(fg, tree, maxparallel=1000) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg, tree) 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") diff --git a/examples/ManhattanDatasetBatch.jl b/examples/ManhattanDatasetBatch.jl index 0d7b5bd1..03665f82 100644 --- a/examples/ManhattanDatasetBatch.jl +++ b/examples/ManhattanDatasetBatch.jl @@ -39,7 +39,8 @@ function solve_batch(total_meas::Integer) # Solve the graph, and save a copy of the tree. saveDFG(fg, "$(getLogPath(fg))/fg-before-solve") - tree, smt, hist = solveTree!(fg, maxparallel=1000) + getSolverParams(fg).maxincidence = + tree, smt, hist = solveTree!(fg) saveDFG(fg, "$(getLogPath(fg))/fg-after-solve") saveTree(tree, "$(getLogPath(fg))/tree$(manhattan_total_meas).jld2") drawTree(tree, show=false, filepath="$(getLogPath(fg))/bt.pdf") diff --git a/examples/ManhattanDatasetFixedLag-FromBatch.jl b/examples/ManhattanDatasetFixedLag-FromBatch.jl index 6ff3ba4c..60975601 100644 --- a/examples/ManhattanDatasetFixedLag-FromBatch.jl +++ b/examples/ManhattanDatasetFixedLag-FromBatch.jl @@ -66,7 +66,8 @@ function go_fixedlag_frombatch(qfl_length_arg::Integer) # 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) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg) 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") @@ -94,7 +95,7 @@ function go_fixedlag_frombatch(qfl_length_arg::Integer) # 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 = plotSLAM2DPoses(fgl, dyadScale=0.6, lbls=false) pl1 |> PDF("$(getLogPath(fgl))/poses$(padded_stepl).pdf", 20cm, 10cm) end, rand(Categorical(nprocs()-1))+1, fg, padded_step) @@ -107,7 +108,8 @@ function go_fixedlag_frombatch(qfl_length_arg::Integer) @info "Going for solve" # Solve the graph, and save a copy of the tree. - tree, smt, hist = solveTree!(fg, tree, maxparallel=1000) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg, tree) 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") diff --git a/examples/ManhattanDatasetFixedLag-MargOnly.jl b/examples/ManhattanDatasetFixedLag-MargOnly.jl index 3fec9458..9085cba5 100644 --- a/examples/ManhattanDatasetFixedLag-MargOnly.jl +++ b/examples/ManhattanDatasetFixedLag-MargOnly.jl @@ -55,7 +55,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt # 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) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg) 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") @@ -84,7 +85,7 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt # 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 = plotSLAM2DPoses(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) @@ -97,7 +98,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt @info "Going for solve" # Solve the graph, and save a copy of the tree. - tree, smt, hist = solveTree!(fg, maxparallel=1000) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg) 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") diff --git a/examples/ManhattanDatasetFixedLag.jl b/examples/ManhattanDatasetFixedLag.jl index a1d56a3e..4740fab4 100644 --- a/examples/ManhattanDatasetFixedLag.jl +++ b/examples/ManhattanDatasetFixedLag.jl @@ -55,7 +55,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt # 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) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg) 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") @@ -96,7 +97,8 @@ function go_fixedlag(initial_offset::Integer, final_timestep::Integer, qfl_lengt @info "Going for solve" # Solve the graph, and save a copy of the tree. - tree, smt, hist = solveTree!(fg, tree, maxparallel=1000) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg, tree) 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") diff --git a/examples/ManhattanDatasetIncremental.jl b/examples/ManhattanDatasetIncremental.jl index d361caac..309f5341 100644 --- a/examples/ManhattanDatasetIncremental.jl +++ b/examples/ManhattanDatasetIncremental.jl @@ -103,7 +103,8 @@ function go(initial_offset::Integer, final_timestep::Integer) @info "Going for solve" # Solve the graph, and save a copy of the tree. - tree, smt, hist = solveTree!(fg, tree, maxparallel=1000) + getSolverParams(fg).maxincidence = 1000 + tree, smt, hist = solveTree!(fg, tree) 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") diff --git a/examples/OutlierVsMultimodal.jl b/examples/OutlierVsMultimodal.jl index 0faf8e5f..bf2622c4 100644 --- a/examples/OutlierVsMultimodal.jl +++ b/examples/OutlierVsMultimodal.jl @@ -286,14 +286,15 @@ end getSolverParams(fg).dbg = true saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)before")) -tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg)) +getSolverParams(fg).maxincidence = 1000 +tree, smt, hist = solveTree!(fg, recordcliqs=ls(fg)) saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_solve")) plfl1 = drawPosesLandms(fg, spscale=1.0,title=getLogPath(fg)*"\n$argstr", landmsPPE=:max, contour=true) plfl1 |> PDF(joinLogPath(fg, "plot_x$(POSEOFFSET+10)_solve.pdf"), 20cm, 17cm) if pargs["resolve"] - tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg)) + tree, smt, hist = solveTree!(fg, recordcliqs=ls(fg)) saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_resolve")) plfl1 = drawPosesLandms(fg, spscale=1.0,title=getLogPath(fg)*"\n$argstr", landmsPPE=:max, contour=true) plfl1 |> PDF(joinLogPath(fg, "plot_x$(POSEOFFSET+10)_resolve.pdf"), 20cm, 17cm) @@ -306,7 +307,8 @@ end ## test solve getSolverParams(fg).dbg = true -tree, smt, hist = solveTree!(fg, maxparallel=1000, recordcliqs=ls(fg)) #[:l9_0;:x14]) +getSolverParams(fg).maxincidence = 1000 +tree, smt, hist = solveTree!(fg, recordcliqs=ls(fg)) #[:l9_0;:x14]) saveDFG(fg, joinLogPath(fg, "fg_x$(POSEOFFSET+10)_final")) diff --git a/src/Slam.jl b/src/Slam.jl index cc1a5931..f10f7800 100644 --- a/src/Slam.jl +++ b/src/Slam.jl @@ -258,7 +258,7 @@ function manageSolveTree!(dfg::AbstractDFG, dt_save1 = (time_ns()-t0)/1e9 # constrain solve with the latest pose at the top # @show latestPose = intersect(getLastPoses(dfg, filterLabel=r"x\d", number=12), ls(dfg, r"x\d", solvable=1))[end] - tree, smt, hist = solveTree!(dfg, tree, maxparallel=1000) # , variableConstraints=[latestPose;] + tree, smt, hist = solveTree!(dfg, tree) # , variableConstraints=[latestPose;] dt_solve = (time_ns()-t0)/1e9 !dbg ? nothing : saveDFG(dfg, joinpath(getLogPath(dfg), "fg_after_$(lasp)")) diff --git a/test/testDynPose2D.jl b/test/testDynPose2D.jl index b3bcf373..e5028824 100644 --- a/test/testDynPose2D.jl +++ b/test/testDynPose2D.jl @@ -239,7 +239,7 @@ tree, smt, hist = solveTree!(fg, smtasks=smtasks); #, recordcliqs=ls(fg)); ## -# tree = resetBuildTree!(fg); +# tree = buildTreeReset!(fg); # drawTree(tree, show=true); ##