Fix test for br solve for pose, PartialPriorYawPose2 test, and cleanu…

…p Pose2Point2Bearing (#621)

* Fix test for br solve for pose

* Fix PartialPriorYawPose2 test

* Update Bearing only and fix one test

* relax PartialPriorYawPose2 test

* fix trilateration retry

* Relax a Bearing2D test that was @test_broken

* Relax new test PartialPriorYawPose2 test with BR

src/factors/Bearing2D.jl

@@ -25,42 +25,14 @@ function getSample(cfo::CalcFactor{<:Pose2Point2Bearing})
 end
 
 function (cfo::CalcFactor{<:Pose2Point2Bearing})(Xc, p, l)
-  #embed l in SE2
-  M = SpecialEuclidean(2)
-  q = ArrayPartition(l, identity_element(SpecialOrthogonal(2), submanifold_component(p,2)))
-  x,y = submanifold_component(Manifolds.compose(M, inv(M, p), q),1)
-  # TODO Xc is a coordinate (ie angle), maybe change to X ϵ so2 
-  # m̂ = exp(so{N}(hat(SpecialOrthogonal(N), SO{N}()[], atan(y, x))))
-  # distance(m, m̂)/sqrt(2)
-
-  # FIXME, this should be a vee(log()), not linear - and add to test
-  return  Manifolds.sym_rem(Xc[1] - atan(y, x))
+  # wl = l
+  # wTp = p
+  # pl = pTw*wl
+  pl  =  transpose(p.x[2]) * (l - p.x[1])
+  # δθ = mθ - plθ
+  δθ = Manifolds.sym_rem(Xc[1] - atan(pl[2], pl[1]))
+  return [δθ]
 end
-# define the conditional probability constraint
-# function (cfo::CalcFactor{<:Pose2Point2Bearing})(meas, _xi, lm)
-#   #
-#   M = SpecialEuclidean(2)
-#   ϵ = identity_element(M)
-#   xi = vee(M, ϵ, log(M, ϵ, _xi))
-
-#   reuse = cfo.cache.reuse # FIRST IMPL CACHE NOT THREAD SAFE 22Q1 # [Threads.threadid()]
-#   reuse.measvec[1] = cos(meas[1] + xi[3])
-#   reuse.measvec[2] = sin(meas[1] + xi[3])
-
-#   # @simd for i in 1:2
-#     reuse.predvec[1:2] .= lm[1:2].-xi[1:2]
-#   # end
-#   normalize!(reuse.predvec)
-#   reuse.resid .= reuse.measvec
-#   reuse.resid .-= reuse.predvec
-
-#   # must return result of length zDim==1 in this case
-#   return sum(abs.(reuse.resid))
-#   # reuse.resid .^= 2
-#   # res[1] = reuse.resid[1]
-#   # res[1] += reuse.resid[2]
-
-# end
 
 
 # Packing and Unpacking

src/factors/PartialPriorPose2.jl

@@ -14,7 +14,7 @@ function getSample(cf::CalcFactor{<:PartialPriorYawPose2})
 
   Z = cf.factor.Z
   M = getManifold(cf.factor)
-  p = identity_element(M)
+  p = getPointIdentity(M)
 
   Xc = [0,0,rand(Z)]
 

test/testBearing2D.jl

@@ -42,7 +42,7 @@ for (i,p) in enumerate(ps)
     # @show p
     r = calcFactorResidualTemporary(f, (Pose2, Point2), m, (p, q))
     # @warn "comp: $(isapprox(r, rs[i], atol=0.001))" a=r b=rs[i]
-    @test isapprox(r, rs[i], atol=0.001) 
+    @test isapprox(r[1], rs[i], atol=0.001) 
 end
 
 @warn "Bearing2D, must still test factor gradients, which will also verify the sign on residual function calculations"
@@ -53,14 +53,14 @@ f = Pose2Point2Bearing(Normal(pi/2,0.001))
 xi = ArrayPartition([0.,0], [1. 0; 0 1])
 xj = [1.,1]
 res = calcFactorResidualTemporary(f, (Pose2, Point2), [], (xi, xj))
-@test isapprox(res, pi/4, atol=0.1) # FIXME, confirm the sign
+@test isapprox(res, [pi/4], atol=0.1) # FIXME, confirm the sign
 
 # test -pi +pi case
 f = Pose2Point2Bearing(Normal(pi,0.001))
 xi = ArrayPartition([0.,0], [1. 0; 0 1])
 xj = [-1, -0.001]
 res = calcFactorResidualTemporary(f, (Pose2, Point2), [pi], (xi, xj))
-@test isapprox(res, -0.001, atol=1e-3)
+@test isapprox(res, [-0.001], atol=1e-3)
 
 ##
 end
@@ -70,6 +70,7 @@ end
 ## new factor graph
 
 fg = initfg()
+fg.solverParams.inflation = 0.0
 
 addVariable!(fg, :x1, Pose2)
 addVariable!(fg, :x2, Pose2)
@@ -105,8 +106,7 @@ points = getPoints(getBelief(fg, :l1))
 @cast pts[j,i] := points[i][j]
 pts = collect(pts)
 #FIXME check test after Bearing2D is fixed
-@test_broken all([80,80] .< sum(abs.(pts) .< [10,10],dims=2) )
-
+@test all([60,60] .< sum(abs.(pts) .< [10,10],dims=2) )
 ##
 end
 

test/testBearingRange2D.jl

@@ -314,12 +314,12 @@ end
 ##
 
 # Start with an empty graph
-N = 1
+N = 75
 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
+addFactor!(fg, [:l1], PriorPoint2(MvNormal([20.0;0.0], Matrix(Diagonal([0.1;0.1].^2)))),  graphinit=false ) # could be IIF.Prior
 li = zeros(2); li[1] = 20.0;
 setVal!(fg, :l1, [li])
 
@@ -329,25 +329,32 @@ addVariable!(fg, :x0, Pose2)
 setVal!(fg, :x0, [getPointIdentity(Pose2)])
 
 # Add bearing range measurement between pose and landmark
-p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0))
+p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,0.1))
 addFactor!(fg, [:x0; :l1], p2br, graphinit=false)
 
 # there should be just one (the bearingrange) factor connected to :l1
 @test length(ls(fg, :x0)) == 1
 # writeGraphPdf(fg)
 
 # check the forward convolution is working properly
-_pts, = predictbelief(fg, :x0, ls(fg, :x0), N=75)
+_pts, = predictbelief(fg, :x0, ls(fg, :x0); N)
 p_μ = mean(SpecialEuclidean(2), _pts)
 
 _pts = getCoordinates.(Pose2, _pts)
 @cast pts[j,i] := _pts[i][j]
 
-# 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; 2.0; 0.01] .< Statistics.std(pts,dims=2) .< [5.0; 10.0; 2.0]) == 3
+dists = norm.(eachcol(pts[1:2, :] .- [20,0]))
+@test sum(isapprox.(dists, 20, atol=3)) > N*0.9
+
+# check likelihood at 0,0,0
+#FIXME don't know how this works
+@test_broken getBelief(fg, :x0)([0.0;0.0;0.0;;])[1] < 0.03
+#just testing direction on its own
+pts0 = filter(eachcol(pts)) do p
+    isapprox(p[1:2],[0,0], atol=1)
+end
+theta = mean(getindex.(pts0,3))
+@test isapprox(theta, 0.0, atol=0.1)
 
 ##
 

test/testPartialPriorYawPose2.jl

@@ -24,9 +24,51 @@ tree = solveTree!(fg)
 
 M = getManifold(Pose2)
 me_ = mean(M, getVal(fg, :x))
-@test isapprox(M.manifold[1], submanifold_component(me_,1), [0,0], atol=0.2)
+#x should form a dounut around 1,1 with yaw close to zero
+@test isapprox(M.manifold[1], submanifold_component(me_,1), [1,1], atol=1.0)
 @test isapprox(M.manifold[2], submanifold_component(me_,2), [1 0; 0 1], atol=0.05) 
 @test isapprox(mean(getVal(fg, :l)), [1,1], atol = 0.05)
 
 ##
 end
+
+
+@testset "Basic PartialPriorYawPose2 test with Bearing Range" begin
+##
+
+fg = initfg()
+
+# fg.solverParams.graphinit=false
+addVariable!(fg, :x, Pose2)
+addVariable!(fg, :l, Point2)
+
+addFactor!(fg, [:x], PartialPriorYawPose2(Normal(0.0, 0.001)))
+addFactor!(fg, [:l], PriorPoint2(MvNormal([10.,10], [0.01, 0.01])))
+
+addFactor!(fg, [:x; :l], Pose2Point2BearingRange(Normal(0.0, 0.01),Normal(5.0, 0.1)))
+
+initAll!(fg)
+
+tree = solveTree!(fg)
+
+M = getManifold(Pose2)
+me_ = mean(M, getVal(fg, :x))
+@test isapprox(M.manifold[1], submanifold_component(me_,1), [5,10], atol=3.0)
+@test isapprox(M.manifold[2], submanifold_component(me_,2), [1 0; 0 1], atol=0.05) 
+@test isapprox(mean(getVal(fg, :l)), [10,10], atol = 0.05)
+
+# ##
+end
+
+# Debug plots using Makie
+# points = getPoints(fg, :x)
+# @cast pts[j,i] := getCoordinates.(Pose2,points)[i][j]
+# f = scatter(pts[1,:], pts[2,:]; rotations = pts[3,:], markersize=15, marker = '►')
+# xlims!(0,20)
+# ylims!(0,20)
+# f.axis.aspect[] =1.0
+# points = getPoints(fg, :l)
+# @cast pts[j,i] := getCoordinates.(Point2,points)[i][j]
+# scatter!(pts[1,:], pts[2,:])
+
+##

test/testPoint2Point2.jl

@@ -1,7 +1,7 @@
 using RoME
 using Statistics
 using JSON2
-
+using TensorCast
 using Test
 
 ##

test/testPoint2Point2Init.jl

@@ -59,23 +59,25 @@ while 0 < i
 ##
   try
     # 
-    @cast l1_vals[j,i] := getVal(fg, :l1)[i][j]
-
-    @test 5 < sum( 80 .< l1_vals[1,:] .< 120 )
-    @test 5 < sum( -20 .< l1_vals[2,:] .< 20 )
-
-    @test 5 < sum( -20 .< l1_vals[1,:] .< 20 )
-    @test 5 < sum( 80 .< l1_vals[2,:] .< 120 )
-
-    @cast x0_vals[j,i] := getVal(fg, :x0)[i][j]
-
-    @test 80 < sum( 80 .< x0_vals[1,:] .< 120 )
-    @test 80 < sum( -20 .< x0_vals[2,:] .< 20 )
-
-    @cast x1_vals[j,i] := getVal(fg, :x1)[i][j]
-
-    @test 80 < sum( -20 .< x1_vals[1,:] .< 20 )
-    @test 80 < sum( 80 .< x1_vals[2,:] .< 120 )
+    @testset "retry $i" begin
+      @cast l1_vals[j,i] := getVal(fg, :l1)[i][j]
+
+      @test 5 < sum( 80 .< l1_vals[1,:] .< 120 )
+      @test 5 < sum( -20 .< l1_vals[2,:] .< 20 )
+
+      @test 5 < sum( -20 .< l1_vals[1,:] .< 20 )
+      @test 5 < sum( 80 .< l1_vals[2,:] .< 120 )
+
+      @cast x0_vals[j,i] := getVal(fg, :x0)[i][j]
+
+      @test 80 < sum( 80 .< x0_vals[1,:] .< 120 )
+      @test 80 < sum( -20 .< x0_vals[2,:] .< 20 )
+
+      @cast x1_vals[j,i] := getVal(fg, :x1)[i][j]
+
+      @test 80 < sum( -20 .< x1_vals[1,:] .< 20 )
+      @test 80 < sum( 80 .< x1_vals[2,:] .< 120 )
+    end
 
     break
     #