From 948582501d7a10294e84e5744cf069ff0574fec2 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 14 Feb 2019 12:59:55 -0500 Subject: [PATCH 1/2] updates and fixes --- src/RoME.jl | 1 + test/testManifoldsPose2Equivalent.jl | 1 + 2 files changed, 2 insertions(+) diff --git a/src/RoME.jl b/src/RoME.jl index a101edb8..80cc5d3f 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -243,6 +243,7 @@ export IIF, KDE, TU, + AMP, # Deprecated PartialPriorRollPitchZ, diff --git a/test/testManifoldsPose2Equivalent.jl b/test/testManifoldsPose2Equivalent.jl index e4d9ae2e..6565a221 100644 --- a/test/testManifoldsPose2Equivalent.jl +++ b/test/testManifoldsPose2Equivalent.jl @@ -195,6 +195,7 @@ end + @testset "Factor graph based test of SE(2) equivalent using Priors and Conditional Likelihoods..." begin N = 100 From 9c0d33072af5a5819d5eb4b333e914ef74f12db8 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 15 Feb 2019 02:57:34 -0500 Subject: [PATCH 2/2] api update and new Range2D Bearing2D files --- examples/KinematicJoints.jl | 14 ++-- examples/RangesExample.jl | 10 +-- examples/RangesExampleTwo.jl | 145 ++++++++++++++++++++-------------- src/Deprecated.jl | 46 +++++------ src/Point2D.jl | 46 ----------- src/RoME.jl | 36 ++++----- src/factors/Bearing2D.jl | 49 ++++++++++++ src/factors/BearingRange2D.jl | 82 ------------------- src/factors/Range2D.jl | 82 +++++++++++++++++++ test/testInertialPose3.jl | 2 +- 10 files changed, 269 insertions(+), 243 deletions(-) create mode 100644 src/factors/Bearing2D.jl create mode 100644 src/factors/Range2D.jl diff --git a/examples/KinematicJoints.jl b/examples/KinematicJoints.jl index aec66ed4..201c8bd3 100644 --- a/examples/KinematicJoints.jl +++ b/examples/KinematicJoints.jl @@ -66,14 +66,14 @@ fg = RoME.initfg() addVariable!(fg, :x1, dims=6) pos = PriorPose3(MvNormal(zeros(6),1e-6*Matrix{Float64}(LinearAlgebra.I, 6,6))) addFactor!(fg, [:x1], pos) # base -initializeNode!(fg, :x1) +initVariable!(fg, :x1) # torso addVariable!(fg, :x2, dims=6) hip = ZJoint(Normal(pi/3,0.1)) addFactor!(fg, [:x1, :x2], hip) # hio -initializeNode!(fg, :x2) +initVariable!(fg, :x2) @@ -114,7 +114,7 @@ plotPose3Pairs(fg, :x2) addVariable!(fg, :x3, dims=6) should = XJoint(Normal(pi/4,0.1)) addFactor!(fg, [:x2, :x3], should) # hio -initializeNode!(fg, :x3) +initVariable!(fg, :x3) @@ -132,7 +132,7 @@ solveandvisualize(fg, vis) addVariable!(fg, :x4, dims=6) should = XJoint(Normal(pi/4,0.1)) addFactor!(fg, [:x3, :x4], should) # hio -initializeNode!(fg, :x4) +initVariable!(fg, :x4) @@ -159,19 +159,19 @@ fg = RoME.initfg() addVariable!(fg, :x1, dims=6) pos = PriorPose3(MvNormal(zeros(6),1e-6*Matrix{Float64}(LinearAlgebra.I, 6,6))) addFactor!(fg, [:x1], pos) # base -initializeNode!(fg, :x1) +initVariable!(fg, :x1) addVariable!(fg, :x2, dims=6) hip = ZJoint(Uniform(-pi/3,pi/3)) addFactor!(fg, [:x1, :x2], hip) # hio -initializeNode!(fg, :x2) +initVariable!(fg, :x2) addVariable!(fg, :x3, dims=6) should = XJoint(Uniform(-pi/4,pi/4)) addFactor!(fg, [:x2, :x3], should) # hio -initializeNode!(fg, :x3) +initVariable!(fg, :x3) visualizeallposes!(vis, fg, drawtype=:max) diff --git a/examples/RangesExample.jl b/examples/RangesExample.jl index 1f604135..dda29003 100644 --- a/examples/RangesExample.jl +++ b/examples/RangesExample.jl @@ -48,17 +48,17 @@ addFactor!(fg, [:l2;], PriorPoint2(MvNormal(GTl[:l2], Matrix{Float64}(LinearAlge # first range measurement rhoZ1 = norm(GTl[:l1]-GTp[:l100]) -ppr = Point2DPoint2DRange([rhoZ1], 2.0, [1.0]) +ppr = Point2Point2Range(Normal(rhoZ1, 2.0)) addFactor!(fg, [:l100;:l1], ppr) # second range measurement rhoZ2 = norm(GTl[:l2]-GTp[:l100]) -ppr = Point2DPoint2DRange([rhoZ2], 3.0, [1.0]) +ppr = Point2Point2Range(Normal(rhoZ2, 3.0)) addFactor!(fg, [:l100; :l2], ppr) # second range measurement rhoZ3 = norm(GTl[:l3]-GTp[:l100]) -ppr = Point2DPoint2DRange([rhoZ3], 3.0, [1.0]) +ppr = Point2Point2Range(Normal(rhoZ3, 3.0)) addFactor!(fg, [:l100; :l3], ppr) @@ -100,7 +100,7 @@ function vehicle_drives_to!(fgl::FactorGraph, pos_sym::Symbol, GTp::Dict, GTl::D println("Adding variable vertex $pos_sym, not yet in fgl::FactorGraph.") addVariable!(fgl, pos_sym, Point2) @show rho = norm(GTp[prev_sym] - GTp[pos_sym]) - ppr = Point2DPoint2DRange([rho], 3.0, [1.0]) + ppr = Point2Point2Range(Normal(rho, 3.0)) addFactor!(fgl, [prev_sym;pos_sym], ppr) else @warn "Variable node $pos_sym already in the factor graph." @@ -110,7 +110,7 @@ function vehicle_drives_to!(fgl::FactorGraph, pos_sym::Symbol, GTp::Dict, GTl::D rho = norm(GTl[ll] - GTp[pos_sym]) # Check for feasible measurements: vehicle within 150 units from the beacons/landmarks if rho < measurelimit - ppr = Point2DPoint2DRange([rho], 3.0, [1.0]) + ppr = Point2Point2Range(Normal(rho, 3.0)) if !(ll in currvar) println("Adding variable vertex $ll, not yet in fgl::FactorGraph.") addVariable!(fgl, ll, Point2) diff --git a/examples/RangesExampleTwo.jl b/examples/RangesExampleTwo.jl index 6d02d511..a3f6d629 100644 --- a/examples/RangesExampleTwo.jl +++ b/examples/RangesExampleTwo.jl @@ -1,34 +1,26 @@ # video at: https://vimeo.com/190052649 +using Distributed + +addprocs(3) + +## + +using RoME +using Gadfly, Colors +using RoMEPlotting +using RoME, IncrementalInference, KernelDensityEstimate -addprocs(7) -using RoME, IncrementalInference, Gadfly, Colors, KernelDensityEstimate -# for p in procs() -# # @everywhere begin -# remotecall_fetch(p, ()->using RoME) -# remotecall_fetch(p, ()->using IncrementalInference) -# remotecall_fetch(p, ()->using Gadfly) -# remotecall_fetch(p, ()->using Colors) -# # end -# end -@everywhere using RoME, RoMEPlotting -@everywhere using IncrementalInference @everywhere using Gadfly -@everywhere using Colors @everywhere using KernelDensityEstimate -# end +@everywhere using RoME, RoMEPlotting +@everywhere using Colors + + +## -# , Gadfly -# using CloudGraphs, Neo4j -# include("BlandAuthDB.jl") -# -# configuration = CloudGraphs.CloudGraphConfiguration(dbaddress, 7474, dbusr, dbpwd, mongoaddress, 27017, false, "", ""); -# cloudGraph = connect(configuration); -# # register types of interest in CloudGraphs -# registerGeneralVariableTypes!(cloudGraph) -# IncrementalInference.setCloudDataLayerAPI!() # togglePrtStbLines() @@ -87,7 +79,7 @@ function addLandmsOnPose!(fgl::FactorGraph, pose::Graphs.ExVertex, GTl::Dict{Str println("addLandmsOnPose! -- adding $(gtl[1])") v = isInFG!(fgl, Symbol(gtl[1]), N=N,ready=ready) # add the constraint - ppr = Point2DPoint2DRange([gtl[2]], 2.0, [1.0]) + ppr = Point2Point2Range( Normal(gtl[2], 2.0) ) # [gtl[2]], 2.0, [1.0]) addFactor!(fgl, [pose;v], ppr, ready=ready) end nothing @@ -103,9 +95,9 @@ function addNewPose!(fgl::FactorGraph, init = 300*randn(2,N) v = addVariable!(fgl, lbl, Point2, N=N, ready=ready) rhoZ = norm(GTp[string(lbl)]-GTp[string(from)]) - ppr = Point2DPoint2DRange([rhoZ], 3.0, [1.0]) + ppr = Point2Point2Range( Normal(rhoZ, 3.0) ) f = addFactor!(fgl, [from,lbl], ppr, ready=ready) - initializeNode!(fgl,lbl) + initVariable!(fgl,lbl) getVert(fgl, lbl) end @@ -124,6 +116,8 @@ function batchsolve(fgl::FactorGraph; N::Int=100) nothing end +## + @everywhere begin function drawQuadLandms(fgl; file="", @@ -132,8 +126,8 @@ function drawQuadLandms(fgl; h11 = Gadfly.context(), h12 = Gadfly.context() ) # - h21 = haskey(fgl.IDs,"l3") ? drawMarginalContour(fgl,"l3") : Gadfly.context() - h22 = haskey(fgl.IDs,"l4") ? drawMarginalContour(fgl,"l4") : Gadfly.context() + h21 = haskey(fgl.IDs,"l3") ? RoMEPlotting.drawMarginalContour(fgl,"l3") : Gadfly.context() + h22 = haskey(fgl.IDs,"l4") ? RoMEPlotting.drawMarginalContour(fgl,"l4") : Gadfly.context() hh = vstack(hstack(h11,h12),hstack(h21,h22)) if length(file) > 0 Gadfly.draw(PNG(file,w,h),hh) @@ -149,7 +143,7 @@ function drawLandmMargOver(fgl::FactorGraph, lbl::String, hover = Gadfly.context() if haskey(fgl.IDs,lbl) - hover = drawMarginalContour(fgl,lbl) + hover = RoMEPlotting.drawMarginalContour(fgl,lbl) # for gtl in gtlayers # push!(hover.layers, gtl) # end @@ -213,13 +207,13 @@ end function drawConvenience1(fgl, ii, fr, folderloc, h12, h12a, interp) drawQuadLandms(fgl,file="$(folderloc)/overlms$(fr).png",h12=h12); - draw(PNG("$(folderloc)/gt$(fr).png",30cm,20cm),h12) + Gadfly.draw(PNG("$(folderloc)/gt$(fr).png",30cm,20cm),h12) ii -= interp ? 1 : 0 - lstPosePl = RoME.drawMarginalContour(fgl,"l$(100+ii)") + lstPosePl = RoMEPlotting.drawMarginalContour(fgl,"l$(100+ii)") ii += interp ? 1 : 0 - draw(PNG("$(folderloc)/lst$(fr).png",30cm,20cm),lstPosePl) + Gadfly.draw(PNG("$(folderloc)/lst$(fr).png",30cm,20cm),lstPosePl) push!(h12a.layers, lstPosePl.layers[1]) - draw(PNG("$(folderloc)/lstOver$(fr).png",30cm,20cm),h12a) + Gadfly.draw(PNG("$(folderloc)/lstOver$(fr).png",30cm,20cm),h12a) nothing end @@ -229,7 +223,7 @@ function drawConvenience2(fgl, fr, folderloc, h12b) @sync begin allposesfile2 = "$(folderloc)/allPoses$(fr).png" - @async draw(PNG(allposesfile2,30cm,20cm),ppl) + @async Gadfly.draw(PNG(allposesfile2,30cm,20cm),ppl) # for l in ppl.layers templyrs = union(h12b.layers, ppl.layers) @@ -237,7 +231,7 @@ function drawConvenience2(fgl, fr, folderloc, h12b) # end allposesfile = "$(folderloc)/allPosesOver$(fr).png" - draw(PNG(allposesfile,30cm,20cm),h12b) + Gadfly.draw(PNG(allposesfile,30cm,20cm),h12b) end nothing end @@ -290,9 +284,9 @@ end function layerCircle(;cent=[0.0;0.0], radius=1.0, c="deepskyblue", N=200) - TH = range(0,stop=2pi,length=200) - X = real(radius*exp(TH*im))+cent[1] - Y = imag(radius*exp(TH*im))+cent[2] + TH = collect(range(0,stop=2pi,length=200)) + X = real(radius*exp.(TH*im)).+cent[1] + Y = imag(radius*exp.(TH*im)).+cent[2] layer(x=X,y=Y, Geom.path(), Theme(default_color=parse(Colorant,c))) end function plotCircle(;cent=[0.0;0.0], radius=1.0, c="deepskyblue", N=200, drawcent=false) @@ -366,20 +360,20 @@ end function drawIllustrations(GTl, GTp, folderloc) pl = drawGroundTruth(GTp, String[], GTl, ["l$(i)" for i in 1:4], drawranges=false, interp=false, t=0) filename = "$(folderloc)/gtPos0.png" - draw(PNG(filename,30cm,20cm),pl) + Gadfly.draw(PNG(filename,30cm,20cm),pl) # first pose location pl2 = drawFirstPoseIllustration(GTl, GTp) filename2 = "$(folderloc)/gtArc0.png" - draw(PNG(filename2,30cm,20cm),pl2) + Gadfly.draw(PNG(filename2,30cm,20cm),pl2) pl3 = drawSecondPoseIllustration(GTl, GTp) filename3 = "$(folderloc)/gtPose2Pred.png" - draw(PNG(filename3,30cm,20cm),pl3) + Gadfly.draw(PNG(filename3,30cm,20cm),pl3) pl4 = drawFirstL3Illustration(GTl, GTp) filename4 = "$(folderloc)/gtLm3Pred.png" - draw(PNG(filename4,30cm,20cm),pl4) + Gadfly.draw(PNG(filename4,30cm,20cm),pl4) nothing @@ -418,37 +412,66 @@ function evaluateAllAccuracy(fgl::FactorGraph, GTp, GTl) end -folderloc2 = "/home/dehann/irosVid/new" +## + + +folderloc2 = "/home/dehann/irosVid" drawIllustrations(GTl, GTp, folderloc2) + # N = 100 -fg = initfg() +# fg = initfg() -for N in collect(25:25:25) +N=100 +# for N in collect(25:25:25) fg = initfg() # fg.sessionname="SESSranges" # fg.cg = cloudGraph # Some starting position -init = 300*randn(2,N) -v1 = addVariable!(fg, :l100, Point2, N=N, ready=0) - -lmv1 = landmsInRange(GTl, GTp["l100"]) -addLandmsOnPose!(fg, v1, lmv1, N=N ) +# writeGraphPdf(fg) # must pin landmarks for guage +addVariable!(fg, :l1,Point2) pp2 = PriorPoint2(MvNormal(GTl["l1"], Matrix(Diagonal([1.0;1.0])))) -f = addFactor!(fg,[:l1], pp2) +f = addFactor!(fg,[:l1], pp2, autoinit=true) +# pts = approxConv(fg, :l1f1, :l1) +# pp = AMP.manikde!(pts, Point2().manifolds) +# setValKDE!(getVert(fg, :l1), deepcopy(pp)) + # f = addFactor!(fg,[getVert(fg,:l1)], pp2) +addVariable!(fg, :l2, Point2) pp2 = PriorPoint2(MvNormal(GTl["l2"], Matrix(Diagonal([1.0;1.0])))) -f = addFactor!(fg, [:l2], pp2) +f = addFactor!(fg, [:l2], pp2, autoinit=true) +# pts = approxConv(fg, :l2f1, :l2) +# pp = AMP.manikde!(pts, Point2().manifolds) +# setValKDE!(getVert(fg, :l2), deepcopy(pp)) + + +v1 = addVariable!(fg, :l100, Point2, N=N, ready=0) +lmv1 = landmsInRange(GTl, GTp["l100"]) +addLandmsOnPose!(fg, v1, lmv1, N=N ) + +doautoinit!(fg, :l100) + +# ls(fg, :l100) +# isInitialized(fg, :l3) +# getVal(fg, :l100) + +# ls(fg, :l100) +# pts = approxConv(fg, :l100l3f1, :l3) +# pp = AMP.manikde!(pts, Point2().manifolds) +# setValKDE!(getVert(fg, :l3), deepcopy(pp)) + + + +# writeGraphPdf(fg) -writeGraphPdf(fg) batchsolve(fg, N=N) lk, pe = evaluateAccuracy(fg, GTp) @@ -473,7 +496,7 @@ draw30AllFast(GTp, GTl, fg, 1, 0, 1, 29) addNewPose!(fg, :l100, :l101, GTp, N=N) -initializeNode!(fg,:l101) +initVariable!(fg,:l101) writeGraphPdf(fg) batchsolve(fg, N=N) lk, pe = evaluateAccuracy(fg, GTp) @@ -514,7 +537,7 @@ draw30AllFast(GTp, GTl, fg, 1, 45, 0, 14, interp=false) fgd = deepcopy(fg) drive(fg, GTp, GTl, :l101, :l102, N=N) -initializeNode!(fg,:l102) +initVariable!(fg,:l102) writeGraphPdf(fg) batchsolve(fg, N=N) @@ -532,7 +555,7 @@ draw30AllFast(GTp, GTl, fgd, 2, 60, 0, 29) -# initializeNode!(fg,:l102) +# initVariable!(fg,:l102) fgd = deepcopy(fg) @@ -548,11 +571,11 @@ close(fid) # drawLandms(fg,showmm=true) # h12 = drawGroundTruth(GTp, [:l$(00+i)" for i in 0:3], GTl, [:l$()" for i in 1:4]) # drawQuadLandms(fg,file="irosVid/test4.png",h12=h12); -# draw(PNG("irosVid/gt4.png",30cm,20cm),h12) -# lstPosePl = RoME.drawMarginalContour(fg,:l103) -# draw(PNG("irosVid/lst4.png",30cm,20cm),lstPosePl) +# Gadfly.draw(PNG("irosVid/gt4.png",30cm,20cm),h12) +# lstPosePl = RoMEPlotting.drawMarginalContour(fg,:l103) +# Gadfly.draw(PNG("irosVid/lst4.png",30cm,20cm),lstPosePl) # push!(h12.layers, lstPosePl.layers[1]) -# draw(PNG("irosVid/lstOver4.png",30cm,20cm),h12) +# Gadfly.draw(PNG("irosVid/lstOver4.png",30cm,20cm),h12) # drawAllVidImages(GTp, GTl, fg, 3, 4) draw30AllFast(GTp, GTl, fgd, 3, 90, 0, 29) @@ -795,7 +818,7 @@ draw30AllFast(GTp, GTl, fg, 12, 390, 0, 29, interp=false) # run(`convert -delay 100 irosVid/test*.png irosVid/anim.gif`) -run(`ffmpeg -y -i $(folderloc2)/lstOver%d.png -threads 8 -vcodec libx264 -s 1920x1080 -b:v 2M /home/dehann/Videos/slamedonutLastPose_$(N).mp4`) +# run(`ffmpeg -y -i $(folderloc2)/lstOver%d.png -threads 8 -vcodec libx264 -s 1920x1080 -b:v 2M /home/dehann/Videos/slamedonutLastPose_$(N).mp4`) # run(`ffmpeg -y -i $(folderloc2)/lm3Over%d.png -vcodec libx264 -s 1920x1080 -b:v 2M /home/dehann/Videos/slamedonutLm3_$(N).mp4`) # run(`ffmpeg -y -i $(folderloc2)/lm4Over%d.png -vcodec libx264 -s 1920x1080 -b:v 2M /home/dehann/Videos/slamedonutLm4_$(N).mp4`) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index d1b25eca..7b1c6e88 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -2,29 +2,29 @@ # old type interfaces -function Point2DPoint2DRange(mu,stdev,w) - @warn "Point2DPoint2DRange deprecated in favor of Point2Point2Range{<:IIF.SamplableBelief}." - Point2Point2Range{Normal}(Normal(mu,stdev)) -end -function Point2DPoint2D(d::D) where {D <: IIF.SamplableBelief} - @warn "Point2DPoint2D deprecated in favor of Point2Point2{<:Distribution}." - Point2Point2{D}(d) -end -function Point2DPoint2D(mu::Array{Float64}, cov::Array{Float64,2}, W::Array{Float64,1}) - @warn "Point2DPoint2D deprecated in favor of Point2Point2{<:Distribution}." - - Point2Point2{MvNormal}(MvNormal(mu[:], cov)) -end - -function Pose2DPoint2DBearing(x1::B) where {B <: Distributions.Distribution} - @warn "Pose2DPoint2DBearing deprecated in favor of Pose2Point2Bearing." - Pose2Point2Bearing(B) -end - -function Pose2DPoint2DRange(x1::T,x2::Vector{T},x3) where {T <: Real} - @warn "Pose2Point2Range(mu,cov,w) is being deprecated in favor of Pose2Point2Range(T(...)), such as Pose2Point2Range(MvNormal(mu, cov))" - Pose2Point2Range(Normal(x1, x2)) -end +# function Point2DPoint2DRange(mu,stdev,w) +# @warn "Point2DPoint2DRange deprecated in favor of Point2Point2Range{<:IIF.SamplableBelief}." +# Point2Point2Range{Normal}(Normal(mu,stdev)) +# end +# function Point2DPoint2D(d::D) where {D <: IIF.SamplableBelief} +# @warn "Point2DPoint2D deprecated in favor of Point2Point2{<:Distribution}." +# Point2Point2{D}(d) +# end +# function Point2DPoint2D(mu::Array{Float64}, cov::Array{Float64,2}, W::Array{Float64,1}) +# @warn "Point2DPoint2D deprecated in favor of Point2Point2{<:Distribution}." +# +# Point2Point2{MvNormal}(MvNormal(mu[:], cov)) +# end +# +# function Pose2DPoint2DBearing(x1::B) where {B <: Distributions.Distribution} +# @warn "Pose2DPoint2DBearing deprecated in favor of Pose2Point2Bearing." +# Pose2Point2Bearing(B) +# end +# +# function Pose2DPoint2DRange(x1::T,x2::Vector{T},x3) where {T <: Real} +# @warn "Pose2Point2Range(mu,cov,w) is being deprecated in favor of Pose2Point2Range(T(...)), such as Pose2Point2Range(MvNormal(mu, cov))" +# Pose2Point2Range(Normal(x1, x2)) +# end diff --git a/src/Point2D.jl b/src/Point2D.jl index 55c25cdd..8ce0321c 100644 --- a/src/Point2D.jl +++ b/src/Point2D.jl @@ -27,34 +27,6 @@ function getSample(p2::PriorPoint2, N::Int=1) return (rand(p2.Z, N),) end -""" -$(TYPEDEF) -""" -mutable struct Point2Point2Range{D <: IIF.SamplableBelief} <: IncrementalInference.FunctorPairwiseMinimize - Z::D - Point2Point2Range{D}() where {D} = new{D}() - Point2Point2Range{D}(d::D) where {D <: IIF.SamplableBelief} = new{D}(d) -end -Point2Point2Range(d::D) where {D <: IIF.SamplableBelief} = Point2Point2Range{D}(d) -function getSample(pp2::Point2Point2Range{T}, N::Int=1) where {T <: IIF.SamplableBelief} - return (reshape(rand(pp2.Z,N),1,N), 2*pi*rand(N)) -end -function (pp2r::Point2Point2Range{T})( - res::Array{Float64}, - userdata::FactorMetadata, - idx::Int, - meas::Tuple, - xi::Array{Float64,2}, - lm::Array{Float64,2} ) where {T <: IIF.SamplableBelief} - # - z = meas[1][1,idx] - XX = lm[1,idx] - (z*cos(meas[2][idx]) + xi[1,idx]) - YY = lm[2,idx] - (z*sin(meas[2][idx]) + xi[2,idx]) - res[1] = XX^2 + YY^2 - # @show "fnc", pointer(res), res - res[1] -end -# import RoME: Point2Point2Range """ $(TYPEDEF) @@ -173,7 +145,6 @@ mutable struct PackedPriorPoint2 <: IncrementalInference.PackedInferenceType PackedPriorPoint2(x::String) = new(x) end -passTypeThrough(d::FunctionNodeData{Point2Point2Range}) = d function convert(::Type{PriorPoint2}, d::PackedPriorPoint2) # Cov = reshapeVec2Mat(d.vecCov, d.dimc) @@ -188,23 +159,6 @@ end -""" -$(TYPEDEF) -""" -mutable struct PackedPoint2Point2Range <: IncrementalInference.PackedInferenceType - str::String - PackedPoint2Point2Range() = new() - PackedPoint2Point2Range(s::AS) where {AS <: AbstractString} = new(s) -end -function convert(::Type{PackedPoint2Point2Range}, d::Point2Point2Range) - return PackedPoint2Point2Range(string(d.Z)) -end -function convert(::Type{Point2Point2Range}, d::PackedPoint2Point2Range) - return Point2Point2Range(extractdistribution(d.str)) -end - - - """ diff --git a/src/RoME.jl b/src/RoME.jl index 80cc5d3f..38788958 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -26,6 +26,10 @@ import IncrementalInference: convert, getSample, reshapeVec2Mat, extractdistribu const AMP = ApproxManifoldProducts export + IIF, + KDE, + TU, + AMP, initfg, # RoME specific functions measureMeanDist, predictBodyBR, @@ -118,33 +122,22 @@ export # Point2D Point2, Point2Point2, - Point2DPoint2D, # deprecated PackedPoint2DPoint2D, Point2Point2WorldBearing, PackedPoint2Point2WorldBearing, Point2Point2Range, PackedPoint2Point2Range, - Point2DPoint2DRange, # deprecated - PackedPoint2DPoint2DRange, PriorPoint2, PackedPriorPoint2, - PriorPoint2D, # deprecated - PackedPriorPoint2D, Pose2Point2BearingRange, - Pose2DPoint2DBearingRange, # begin deprecated Pose2Point2BearingRangeMH, PackedPose2Point2BearingRange, PackedPose2Point2BearingRangeMH, Pose2Point2Bearing, PackedPose2Point2Bearing, - Pose2DPoint2DBearing, # deprecated Pose2Point2Range, - Point2DPoint2DRange, - PackedPoint2DPoint2DRange, PriorPoint2, - PriorPoint2D, # deprecated PackedPriorPoint2, - PackedPriorPoint2D, # deprecated` # Point2D with null hypotheses PriorPoint2DensityNH, PackedPriorPoint2DensityNH, @@ -169,7 +162,6 @@ export Pose2Point2BearingRangeDensity, PackedPose2Point2BearingRangeDensity, Pose2Point2RangeDensity, - Pose2DPoint2DRangeDensity, # to be deprecated PackedPose2Point2RangeDensity, # Pose2D @@ -240,11 +232,6 @@ export # jld required Features Type LaserFeatures, - IIF, - KDE, - TU, - AMP, - # Deprecated PartialPriorRollPitchZ, PackedPartialPriorRollPitchZ, @@ -252,6 +239,18 @@ export PackedPartialPose3XYYaw, PartialPose3XYYawNH, PackedPartialPose3XYYawNH + # Point2DPoint2DRange, + # PackedPoint2DPoint2DRange, + # PackedPoint2DPoint2DRange, + # PackedPriorPoint2D, + # Pose2DPoint2DRangeDensity, # to be deprecated + # Point2DPoint2D, # deprecated + # Point2DPoint2DRange, # deprecated + # PriorPoint2D, # deprecated + # Pose2DPoint2DBearingRange, # begin deprecated + # Pose2DPoint2DBearing, # deprecated + # PriorPoint2D, # deprecated + # PackedPriorPoint2D # deprecated` # doesnt seem to work @@ -281,9 +280,10 @@ include("DynPoint2D.jl") include("Pose2D.jl") include("DynPose2D.jl") include("Pose3D.jl") +include("factors/Bearing2D.jl") +include("factors/Range2D.jl") include("factors/BearingRange2D.jl") -# include("BearingRangeDensity2D.jl") include("factors/Pose3Pose3.jl") include("factors/PartialPose3.jl") diff --git a/src/factors/Bearing2D.jl b/src/factors/Bearing2D.jl new file mode 100644 index 00000000..99e2abf0 --- /dev/null +++ b/src/factors/Bearing2D.jl @@ -0,0 +1,49 @@ + +mutable struct P2P2BearingReuse + measvec::Vector{Float64} + predvec::Vector{Float64} + resid::Vector{Float64} + P2P2BearingReuse() = new(zeros(2),zeros(2),zeros(2)) +end + +mutable struct Pose2Point2Bearing{B <: IIF.SamplableBelief} <: IncrementalInference.FunctorPairwiseMinimize + bearing::B + reuse::Vector{P2P2BearingReuse} + Pose2Point2Bearing{B}() where B = new{B}() + Pose2Point2Bearing{B}(x1::B) where {B <: IIF.SamplableBelief} = new{B}(x1, [P2P2BearingReuse() for i in 1:Threads.nthreads()]) +end +Pose2Point2Bearing(x1::B) where {B <: Distributions.Distribution} = Pose2Point2Bearing{B}(x1) +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, + idx::Int, + meas::Tuple, + xi::Array{Float64,2}, + lm::Array{Float64,2} ) + # + # res[1] = ( meas[1][idx] - atan(lm[2,idx]-xi[2,idx], lm[1,idx]-xi[1,idx]) )^2 + reuse = pp2br.reuse[Threads.threadid()] + reuse.measvec[1] = cos(meas[1][idx] + xi[3,idx]) + reuse.measvec[2] = sin(meas[1][idx] + xi[3,idx]) + reuse.predvec[1] = lm[1,idx]-xi[1,idx] + reuse.predvec[2] = lm[2,idx]-xi[2,idx] + reuse.resid[1:2] = reuse.measvec - reuse.predvec + res[1] = reuse.resid'*reuse.resid + return res[1] +end + +# Packing and Unpacking +mutable struct PackedPose2Point2Bearing <: IncrementalInference.PackedInferenceType + bearstr::String + PackedPose2Point2Bearing() = new() + PackedPose2Point2Bearing(s1::AS) where {AS <: AbstractString} = new(string(s1)) +end +function convert(::Type{PackedPose2Point2Bearing}, d::Pose2Point2Bearing{B}) where {B <: IIF.SamplableBelief} + return PackedPose2Point2Bearing(string(d.bearing)) +end +function convert(::Type{Pose2Point2Bearing}, d::PackedPose2Point2Bearing) + Pose2Point2Bearing(extractdistribution(d.bearstr)) +end diff --git a/src/factors/BearingRange2D.jl b/src/factors/BearingRange2D.jl index 5d73444d..00ed738b 100644 --- a/src/factors/BearingRange2D.jl +++ b/src/factors/BearingRange2D.jl @@ -1,31 +1,3 @@ -# Bearing and Range constraints for 2D - -# better to use bearingrange with [uniform bearing], numerical solving issue on 1D -mutable struct Pose2Point2Range{T} <: IncrementalInference.FunctorPairwise - Z::T - Pose2Point2Range{T}() where T = new() - Pose2Point2Range{T}(Z::T) where {T <: IIF.SamplableBelief} = new{T}(Z) -end -Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range{T}(Z) - -function getSample(pp2::Pose2Point2Range, N::Int=1) - return (reshape(rand(pp2.Z,N),1,N) , 2*pi*rand(N)) -end -function (pp2r::Pose2Point2Range)(res::Array{Float64}, - userdata, - idx::Int, - meas::Tuple{Array{Float64,2}, Array{Float64,1}}, # from getSample - xi::Array{Float64,2}, - lm::Array{Float64,2} ) - # - # DONE in IIF -- still need to add multi-hypotheses support here - # this is the noisy range - z = meas[1][1,idx] - XX = lm[1,idx] - (z*cos(meas[2][idx]) + xi[1,idx]) - YY = lm[2,idx] - (z*sin(meas[2][idx]) + xi[2,idx]) - res[1] = XX^2 + YY^2 - nothing -end #------------------------------------------------------------------------------- # bearing and range available @@ -85,57 +57,3 @@ function convert(::Type{Pose2Point2BearingRange}, d::PackedPose2Point2BearingRan # where {B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} Pose2Point2BearingRange(extractdistribution(d.bearstr), extractdistribution(d.rangstr)) end - - - - - - - -#------------------------------------------------------------------------------- -# bearing only available - -# this factor type is still a work in progress -mutable struct Pose2Point2Bearing{B <: Distributions.Distribution} <: IncrementalInference.FunctorPairwiseMinimize - bearing::B - Pose2Point2Bearing{B}() where B = new{B}() - Pose2Point2Bearing{B}(x1::B) where {B <: Distributions.Distribution} = new{B}(x1) -end -Pose2Point2Bearing(x1::B) where {B <: Distributions.Distribution} = Pose2Point2Bearing{B}(x1) -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, - idx::Int, - meas::Tuple, - xi::Array{Float64,2}, - lm::Array{Float64,2} ) - # - res[1] = ( meas[1][idx] - atan(lm[2,idx]-xi[2,idx], lm[1,idx]-xi[1,idx]) )^2 - return res[1] -end - -# Packing and Unpacking -mutable struct PackedPose2Point2Bearing <: IncrementalInference.PackedInferenceType - bearstr::String - PackedPose2Point2Bearing() = new() - PackedPose2Point2Bearing(s1::AS) where {AS <: AbstractString} = new(string(s1)) -end -function convert(::Type{PackedPose2Point2Bearing}, d::Pose2Point2Bearing{B}) where {B <: IIF.SamplableBelief} - return PackedPose2Point2Bearing(string(d.bearing)) -end -function convert(::Type{Pose2Point2Bearing}, d::PackedPose2Point2Bearing) - Pose2Point2Bearing(extractdistribution(d.bearstr)) -end - - - - - - - - - -# ------------------------------------------------------ diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl new file mode 100644 index 00000000..7fe818e9 --- /dev/null +++ b/src/factors/Range2D.jl @@ -0,0 +1,82 @@ + + + +""" +$(TYPEDEF) +""" +mutable struct Point2Point2Range{D <: IIF.SamplableBelief} <: IncrementalInference.FunctorPairwiseMinimize + Z::D + Point2Point2Range{D}() where {D} = new{D}() + Point2Point2Range{D}(d::D) where {D <: IIF.SamplableBelief} = new{D}(d) +end +Point2Point2Range(d::D) where {D <: IIF.SamplableBelief} = Point2Point2Range{D}(d) +function getSample(pp2::Point2Point2Range{T}, N::Int=1) where {T <: IIF.SamplableBelief} + return (reshape(rand(pp2.Z,N),1,N), 2*pi*rand(N)) +end +function (pp2r::Point2Point2Range{T})( + res::Array{Float64}, + userdata::FactorMetadata, + idx::Int, + meas::Tuple, + xi::Array{Float64,2}, + lm::Array{Float64,2} ) where {T <: IIF.SamplableBelief} + # + z = meas[1][1,idx] + XX = lm[1,idx] - (z*cos(meas[2][idx]) + xi[1,idx]) + YY = lm[2,idx] - (z*sin(meas[2][idx]) + xi[2,idx]) + res[1] = XX^2 + YY^2 + # @show "fnc", pointer(res), res + res[1] +end +# import RoME: Point2Point2Range + + + + +# better to use bearingrange with [uniform bearing], numerical solving issue on 1D +mutable struct Pose2Point2Range{T} <: IncrementalInference.FunctorPairwise + Z::T + Pose2Point2Range{T}() where T = new() + Pose2Point2Range{T}(Z::T) where {T <: IIF.SamplableBelief} = new{T}(Z) +end +Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range{T}(Z) + +function getSample(pp2::Pose2Point2Range, N::Int=1) + return (reshape(rand(pp2.Z,N),1,N) , 2*pi*rand(N)) +end +function (pp2r::Pose2Point2Range)(res::Array{Float64}, + userdata, + idx::Int, + meas::Tuple{Array{Float64,2}, Array{Float64,1}}, # from getSample + xi::Array{Float64,2}, + lm::Array{Float64,2} ) + # + # DONE in IIF -- still need to add multi-hypotheses support here + # this is the noisy range + z = meas[1][1,idx] + XX = lm[1,idx] - (z*cos(meas[2][idx]) + xi[1,idx]) + YY = lm[2,idx] - (z*sin(meas[2][idx]) + xi[2,idx]) + res[1] = XX^2 + YY^2 + nothing +end + + + + + +passTypeThrough(d::FunctionNodeData{Point2Point2Range}) = d + +""" +$(TYPEDEF) +""" +mutable struct PackedPoint2Point2Range <: IncrementalInference.PackedInferenceType + str::String + PackedPoint2Point2Range() = new() + PackedPoint2Point2Range(s::AS) where {AS <: AbstractString} = new(s) +end +function convert(::Type{PackedPoint2Point2Range}, d::Point2Point2Range) + return PackedPoint2Point2Range(string(d.Z)) +end +function convert(::Type{Point2Point2Range}, d::PackedPoint2Point2Range) + return Point2Point2Range(extractdistribution(d.str)) +end diff --git a/test/testInertialPose3.jl b/test/testInertialPose3.jl index 5fdba2a2..361cfa1d 100644 --- a/test/testInertialPose3.jl +++ b/test/testInertialPose3.jl @@ -32,7 +32,7 @@ global inerodo = InertialPose3(noise,DATA[n][1],DATA[n][2]) global f2 = addFactor!(fg, [v1;v2], inerodo ) -initializeNode!(fg, :x2, N=N) +initVariable!(fg, :x2, N=N) # drawDensityMesh(fg, :x2)