Merge pull request #416 from JuliaRobotics/maint/21Q1/consol_range

Consolidate Range Factors

Author: dehann

Project.toml

@@ -35,7 +35,7 @@ DistributedFactorGraphs = "0.12"
 Distributions = "0.21, 0.22, 0.23, 0.24"
 DocStringExtensions = "0.7, 0.8"
 FileIO = "1.0.2, 1.1, 1.2"
-IncrementalInference = "0.21.1"
+IncrementalInference = "0.21.2"
 JLD2 = "0.2, 0.3, 0.4"
 KernelDensityEstimate = "0.5.1, 0.6"
 Optim = "0.22, 1.0"

src/factors/Range2D.jl

@@ -6,22 +6,21 @@ $(TYPEDEF)
 """
 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)
+  # 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)
+# Point2Point2Range(d::D) where {D <: IIF.SamplableBelief} = Point2Point2Range{D}(d)
+
 function getSample(cfo::CalcFactor{<:Point2Point2Range}, N::Int=1)
-  return (reshape(rand(cfo.factor.Z,N),1,N),  2*pi*rand(N))
-end
-function (cfo::CalcFactor{<:Point2Point2Range})(rho, theta, xi, lm)
-  #
-  XX = lm[1] - (rho[1]*cos(theta[1]) + xi[1])
-  YY = lm[2] - (rho[1]*sin(theta[1]) + xi[2])
-  #TODO JT - Should this have a sqrt for parametric?
-  # return XX^2 + YY^2 
-  return sqrt(XX^2 + YY^2)
+  return (reshape(rand(cfo.factor.Z,N),1,N),)
 end
 
+function (cfo::CalcFactor{<:Point2Point2Range})(rho, xi, lm)
+  # Basically `EuclidDistance`
+  # must return all dimensions
+  return rho .- norm(lm[1:2] .- xi[1:2])
+  # return [rho[1] - norm(lm[1:2] .- xi[1:2])]
+end
 
 passTypeThrough(d::FunctionNodeData{Point2Point2Range}) = d
 
@@ -30,8 +29,8 @@ $(TYPEDEF)
 """
 mutable struct PackedPoint2Point2Range  <: IncrementalInference.PackedInferenceType
   str::String
-  PackedPoint2Point2Range() = new()
-  PackedPoint2Point2Range(s::AS) where {AS <: AbstractString} = new(s)
+  # PackedPoint2Point2Range() = new()
+  # PackedPoint2Point2Range(s::AS) where {AS <: AbstractString} = new(s)
 end
 function convert(::Type{PackedPoint2Point2Range}, d::Point2Point2Range)
   return PackedPoint2Point2Range(convert(PackedSamplableBelief, d.Z))
@@ -47,35 +46,29 @@ end
 
 Range only measurement from Pose2 to Point2 variable.
 """
-mutable struct Pose2Point2Range{T} <: IIF.AbstractRelativeMinimize
+mutable struct Pose2Point2Range{T <: IIF.SamplableBelief} <: IIF.AbstractRelativeMinimize
   Z::T
   partial::Tuple{Int,Int}
-  Pose2Point2Range{T}() where T = new()
-  Pose2Point2Range{T}(Z::T) where {T <: IIF.SamplableBelief} = new{T}(Z, (1,2))
+  # Pose2Point2Range{T}() where T = new()
 end
-Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range{T}(Z)
+Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range{T}(Z, (1,2))
+# Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range{T}(Z)
 
 function getSample(cfo::CalcFactor{<:Pose2Point2Range}, N::Int=1)
-  return (reshape(rand(cfo.factor.Z,N),1,N) ,  2*pi*rand(N))
-end
-function (pp2r::CalcFactor{<:Pose2Point2Range})(rho,
-                                                theta,
-                                                xi,
-                                                lm  )
-  #
-  # DONE in IIF -- still need to add multi-hypotheses support here
-  # this is the noisy range
-  XX = lm[1] - (rho[1]*cos(theta[1]) + xi[1])
-  YY = lm[2] - (rho[1]*sin(theta[1]) + xi[2])
-  return sqrt(XX^2 + YY^2)  
+  return (reshape(rand(cfo.factor.Z,N),1,N), )
 end
 
+function (cfo::CalcFactor{<:Pose2Point2Range})(rho, xi, lm)
+  # Basically `EuclidDistance`
+  return rho .- norm(lm[1:2] .- xi[1:2])
+  # return [rho[1] - norm(lm[1:2] .- xi[1:2])]
+end
 
 
 mutable struct PackedPose2Point2Range  <: IncrementalInference.PackedInferenceType
   str::String
-  PackedPose2Point2Range() = new()
-  PackedPose2Point2Range(s::AS) where {AS <: AbstractString} = new(s)
+  # PackedPose2Point2Range() = new()
+  # PackedPose2Point2Range(s::AS) where {AS <: AbstractString} = new(s)
 end
 function convert(::Type{PackedPose2Point2Range}, d::Pose2Point2Range)
   return PackedPose2Point2Range(convert(PackedSamplableBelief, d.Z))

test/TestPoseAndPoint2Constraints.jl

@@ -5,9 +5,11 @@ using Test
 using RoME
 # , IncrementalInference, Distributions
 
+##
 
 @testset "test pose and point combinations..." begin
 
+##
 
 N = 100
 fg = initfg()
@@ -21,9 +23,9 @@ v1 = addVariable!(fg, :x0, Pose2, N=N)
 initPosePrior = PriorPose2(MvNormal(zeros(3), initCov))
 f1  = addFactor!(fg,[v1], initPosePrior)
 
-@test Pose2Pose2(MvNormal(randn(2), Matrix{Float64}(LinearAlgebra.I, 2,2))) != nothing
-@test Pose2Pose2(MvNormal(randn(2), Matrix{Float64}(LinearAlgebra.I, 2,2))) != nothing
-@test Pose2Pose2(MvNormal(randn(2), Matrix{Float64}(LinearAlgebra.I, 2,2))) != nothing
+@test Pose2Pose2(MvNormal(randn(2), Matrix{Float64}(LinearAlgebra.I, 2,2))) !== nothing
+@test Pose2Pose2(MvNormal(randn(2), Matrix{Float64}(LinearAlgebra.I, 2,2))) !== nothing
+@test Pose2Pose2(MvNormal(randn(2), Matrix{Float64}(LinearAlgebra.I, 2,2))) !== nothing
 
 # and a second pose
 v2 = addVariable!(fg, :x1, Pose2, N=N)
@@ -32,9 +34,13 @@ f2 = addFactor!(fg, [:x0;:x1], ppc)
 
 # test evaluation of pose pose constraint
 pts = approxConv(fg, :x0x1f1, :x1)
-# pts = evalFactor(fg, f2, v2.index)
-@test norm(Statistics.mean(pts,dims=2)[1:2] - [50.0;0.0]) < 10.0
-@test abs(Statistics.mean(pts,dims=2)[3] - pi/2) < 0.5
+
+pts[3,:] .= TU.wrapRad.(pts[3,:])
+@show mv = Statistics.mean(pts,dims=2)
+@test norm(mv[1:2] - [50.0;0.0]) < 10.0
+@test abs(mv[3] - pi/2) < 0.5
+
+##
 
 # @show ls(fg)
 tree, smt, hist = solveTree!(fg)
@@ -45,10 +51,14 @@ tree, smt, hist = solveTree!(fg)
 
 # test post evaluation values are correct
 pts = getVal(fg, :x0)
+pts[3,:] .= TU.wrapRad.(pts[3,:])
+
 @test norm(Statistics.mean(pts, dims=2)[1:2] - [0.0;0.0]) < 10.0
 @test abs(Statistics.mean(pts, dims=2)[3]) < 0.5
 
 pts = getVal(fg, :x1)
+pts[3,:] .= TU.wrapRad.(pts[3,:])
+
 @test norm(Statistics.mean(pts, dims=2)[1:2]-[50.0;0.0]) < 10.0
 @test abs(Statistics.mean(pts, dims=2)[3]-pi/2) < 0.5
 
@@ -64,14 +74,17 @@ solveTree!(fg)
 # test post evaluation values are correct
 pts = getVal(fg, :x0)
 @test norm(Statistics.mean(pts, dims=2)[1:2]-[0.0;0.0]) < 20.0
+pts[3,:] .= TU.wrapRad.(pts[3,:])
 @test abs(Statistics.mean(pts, dims=2)[3]) < 0.5
 
 pts = getVal(fg, :x1)
 @test norm(Statistics.mean(pts, dims=2)[1:2]-[50.0;0.0]) < 20.0
+pts[3,:] .= TU.wrapRad.(pts[3,:])
 @test abs(Statistics.mean(pts, dims=2)[3] - pi/2) < 0.5
 
 pts = getVal(fg, :x2)
 @test norm(Statistics.mean(pts, dims=2)[1:2]-[50.0;50.0]) < 20.0
+pts[3,:] .= TU.wrapRad.(pts[3,:])
 @test abs(Statistics.mean(pts, dims=2)[3]-pi/2) < 0.5
 
 println("test bearing range evaluations")

test/testBearingRange2D.jl

@@ -225,10 +225,11 @@ addFactor!(fg, [:x0; :l1], p2br, graphinit=false)
 # check the forward convolution is working properly
 pts, = predictbelief(fg, :x0, ls(fg, :x0), N=75)
 
+pts[3,:] .= TU.wrapRad.(pts[3,:])
 @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
+@test sum([0.1; 2.0; 0.01] .< Statistics.std(pts,dims=2) .< [5.0; 10.0; 2.0]) == 3
 
 ##
 

test/testInflation380.jl

@@ -9,6 +9,134 @@ using Statistics
 
 ##
 
+
+@testset "test inflation is working via distance" begin
+
+##
+
+fg = initfg()
+# getSolverParams(fg).inflation = 50.0
+
+N = 100
+
+addVariable!(fg, :x0, ContinuousEuclid{2})
+addVariable!(fg, :x1, ContinuousEuclid{2})
+
+X0_ = zeros(2,N)
+X0_[1,:] .+= 1000.0
+initManual!(fg, :x0, X0_)
+
+addFactor!(fg, [:x0;:x1], EuclidDistance(Normal(100.0, 1.0)))
+
+pts = approxConv(fg, :x0, :x1)
+
+
+##
+# does this give a "donut ring" at 1000?
+
+res = 99999*ones(100)
+
+for i in 1:N
+  res[i] = calcFactorResidual(fg, :x0x1f1, [100.0], [1000.0;0.0], pts[:,i])[1]
+end
+
+@test 0.9*N < sum(abs.(res) .< 5)
+
+## new test trying to force inflation error
+
+X1_ = randn(2,N)
+X1_[1,:] .+= 1100.0
+initManual!(fg, :x1, X1_)
+
+
+##
+
+IIF._getCCW(fg, :x0x1f1).inflation = 50.0
+pts = approxConv(fg, :x0x1f1, :x1)
+
+initManual!(fg, :x1, pts)
+
+pts = approxConv(fg, :x0x1f1, :x1)
+
+
+##
+# does this give a "donut ring" at 1000?
+
+res = 99999*ones(100)
+
+for i in 1:N
+  res[i] = calcFactorResidual(fg, :x0x1f1, [100.0], [1000.0;0.0], pts[:,i])[1]
+end
+
+@test 0.9*N < sum(abs.(res) .< 5)
+
+
+##
+
+# using RoMEPlotting
+# Gadfly.set_default_plot_size(25cm,20cm)
+
+# ##
+
+# # pts = getBelief(fg, :x1) |> getPoints
+# plotKDE(manikde!(pts, ContinuousEuclid{2}))
+
+
+##
+
+end
+
+
+@testset "test inflation on range solve" begin
+
+##
+
+N = 100
+fg = initfg()
+fg.solverParams.inflation = 10.0 # super size the inflation to force wide coverage
+
+addVariable!(fg, :x1, ContinuousEuclid{2})
+addVariable!(fg, :l1, ContinuousEuclid{2})
+
+addFactor!(fg, [:l1], Prior(MvNormal([-1000.0,0], [0.1, 0.1])))
+addFactor!(fg, [:x1; :l1], EuclidDistance(Normal(100.0, 1.0)))
+
+pts = zeros(2,100)
+pts[1,:] .-= 900
+initManual!(fg, :x1, pts)
+
+##
+
+tree, _, = solveGraph!(fg);
+
+##
+
+pts = getBelief(fg, :x1) |> getPoints
+
+@test 0.9*N < sum( -1150 .< pts[1,:] .< -850)
+@test 0.9*N < sum( -150 .< pts[2,:] .< 150)
+
+pts_ = [norm(pts[:,i] - [-1000;0]) for i in 1:N]
+
+@test 0.9*N < sum(80 .< pts_ .< 120)
+
+# must still test spread
+
+@test 0.2*N < sum(-1150 .< pts[1,:] .< -1000)
+@test 0.2*N < sum(-1000 .< pts[1,:] .< -850)
+@test 0.2*N < sum(-150 .< pts[2,:] .< 0)
+@test 0.2*N < sum(0 .< pts[2,:] .< 150)
+
+
+##
+
+# pl = plotKDE(fg, ls(fg))
+
+##
+
+end
+  
+
 @testset "test bearing range with inflation, #380, IIF #1051" begin
 
 ##

test/testParametric.jl

@@ -190,3 +190,32 @@ vardict, result, varIds, Σ = IIF.solveFactorGraphParametric!(fg)
 @test isapprox(vardict[:x1].val, [10, 0, 0, 10, 0], atol = 1e-3)
 
 end
+
+
+@testset "Test Parametric PriorPoint2 and Point2Point2Range" begin
+
+fg = LightDFG( solverParams=SolverParams(algorithms=[:default, :parametric]))
+
+addVariable!(fg, :x1, Point2)
+addVariable!(fg, :l1, Point2)
+addVariable!(fg, :l2, Point2)
+addVariable!(fg, :l3, Point2)
+
+addFactor!(fg, [:l1], PriorPoint2(MvNormal([0., 0], [0.01, 0.01])))
+addFactor!(fg, [:l2], PriorPoint2(MvNormal([1., 0], [0.01, 0.01])))
+addFactor!(fg, [:l3], PriorPoint2(MvNormal([0., 1], [0.01, 0.01])))
+
+addFactor!(fg, [:x1; :l1], Point2Point2Range(Normal(sqrt(2), 0.1)))
+addFactor!(fg, [:x1; :l2], Point2Point2Range(Normal(1.0, 0.1)))
+addFactor!(fg, [:x1; :l3], Point2Point2Range(Normal(1.0, 0.1)))
+
+# ensureAllInitialized!(fg)
+#FIXME needs initializaiton from non-parametric to converge
+solveTree!(fg)
+IIF.initParametricFrom!(fg)
+
+vardict, result, varIds, Σ = IIF.solveFactorGraphParametric(fg)
+
+@test isapprox(vardict[:x1].val, [1, 1], atol = 1e-3)
+
+end