Update/test calcVariableDistanceExpectedFractional

Author: Affie

src/services/EvalFactor.jl

@@ -77,23 +77,18 @@ function calcVariableDistanceExpectedFractional(ccwl::CommonConvWrapper,
   # get mean of all fractional variables
   # ccwl.params::Vector{Vector{P}}
   uncertainidx = setdiff(1:length(ccwl.params), certainidx)
-  uncMeans = zeros(length(ccwl.params[sfidx][1]), length(uncertainidx))
   dists = zeros(length(uncertainidx)+length(certainidx))
-  dims = length(ccwl.params[sfidx][1])
+
+  dims = manifold_dimension(getManifold(ccwl.vartypes[sfidx]))
+
+  uncMeans = zeros(dims, length(uncertainidx))
   for (count,i) in enumerate(uncertainidx)
-    # uncMeans[:,count] = Statistics.mean(ccwl.params[i], dims=2)[:]
-    for pr in ccwl.params[i]
-      uncMeans[:,count] .+= pr
-    end
-    uncMeans[:,count] ./= length(ccwl.params[i])
+    u = mean(getManifold(ccwl.vartypes[i]), ccwl.params[i])
+    uncMeans[:,count] .= getCoordinates(ccwl.vartypes[i], u)
   end
   count = 0
-  # refMean = Statistics.mean(ccwl.params[sfidx], dims=2)[:]
-  refMean = zeros(length(ccwl.params[sfidx][1]))
-  for pr in ccwl.params[sfidx]
-    refMean .+= pr
-  end
-  refMean ./= length(ccwl.params[sfidx])
+
+  refMean = getCoordinates(ccwl.vartypes[sfidx], mean(getManifold(ccwl.vartypes[sfidx]), ccwl.params[sfidx]))
 
   # calc for uncertain and certain
   for i in uncertainidx

test/testSpecialEuclidean2Mani.jl

@@ -8,7 +8,7 @@ import IncrementalInference: HeatmapDensityRegular
 
 ## define new local variable types for testing
 
-@defVariable Point2 TranslationGroup(2) [0.0, 0.0]
+@defVariable TranslationGroup2 TranslationGroup(2) [0.0, 0.0]
 
 # @defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
 @defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
@@ -195,7 +195,7 @@ mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatri
 p = addFactor!(fg, [:x0], mp)
 
 ##
-v1 = addVariable!(fg, :x1, Point2)
+v1 = addVariable!(fg, :x1, TranslationGroup2)
 mf = ManiPose2Point2(MvNormal([1,2], [0.01,0.01]))
 f = addFactor!(fg, [:x0, :x1], mf)
 
@@ -376,4 +376,98 @@ initAll!(fg)
 end
 
 
+@testset "Test SpecialEuclidean(2) to TranslationGroup(2) multihypo" begin
+    
+##
+fg = initfg()
+# fg.solverParams.attemptGradients=false
+
+v0 = addVariable!(fg, :x0, SpecialEuclidean2)
+
+mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
+p = addFactor!(fg, [:x0], mp)
+
+##
+addVariable!(fg, :x1a, TranslationGroup2)
+addVariable!(fg, :x1b, TranslationGroup2)
+mf = ManiPose2Point2(MvNormal([1,2], [0.01,0.01]))
+f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
+
+solveTree!(fg)
+
+vnd = getVariableSolverData(fg, :x0)
+@test isapprox(SpecialEuclidean(2), mean(SpecialEuclidean(2), vnd.val), ProductRepr([0.0,0.0], [1.0 0; 0 1]), atol=0.1)
+
+#FIXME I would expect close to 50% of particles to land on the correct place
+pnt = getPoints(fg, :x1a)
+@test sum(isapprox.(pnt, Ref([1.0,2.0]), atol=0.1)) > 25
+
+#FIXME I would expect close to 50% of particles to land on the correct place
+pnt = getPoints(fg, :x1b)
+@test sum(isapprox.(pnt, Ref([1.0,2.0]), atol=0.1)) > 25
+
+
+## other way around
+
+fg = initfg()
+fg.solverParams.attemptGradients=false
+
+addVariable!(fg, :x0, SpecialEuclidean2)
+addVariable!(fg, :x1a, TranslationGroup2)
+addVariable!(fg, :x1b, TranslationGroup2)
+
+mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([10, 10, 0.01]))
+p = addFactor!(fg, [:x0], mp)
+mp = ManifoldPrior(TranslationGroup(2), [1.,1], MvNormal([0.01, 0.01]))
+p = addFactor!(fg, [:x1a], mp)
+mp = ManifoldPrior(TranslationGroup(2), [-1.,1], MvNormal([0.01, 0.01]))
+p = addFactor!(fg, [:x1b], mp)
+
+mf = ManiPose2Point2(MvNormal([0., 1], [0.01,0.01]))
+f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
+
+solveTree!(fg)
+
+# pnts = getPoints(fg, :x0)
+# c = getCoordinates.(SpecialEuclidean2, pnts)
+# @cast p[i,j] := c[i][j]
+# scatter(p[:,1], p[:,2])
+
+#FIXME
+@test 10 < sum(isapprox.(Ref(SpecialEuclidean(2)), pnts, Ref(ProductRepr([-1.0,0.0], [1.0 0; 0 1])), atol=0.5))
+@test 10 < sum(isapprox.(Ref(SpecialEuclidean(2)), pnts, Ref(ProductRepr([1.0,0.0], [1.0 0; 0 1])), atol=0.5))
+
+
+end
+
+@testset "Test SpecialEuclidean(2) to SpecialEuclidean(2) multihypo" begin
+##
+fg = initfg()
+# fg.solverParams.attemptGradients=false
+
+v0 = addVariable!(fg, :x0, SpecialEuclidean2)
+
+mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
+p = addFactor!(fg, [:x0], mp)
+
+##
+addVariable!(fg, :x1a, SpecialEuclidean2)
+addVariable!(fg, :x1b, SpecialEuclidean2)
+mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([1,2,pi/4], [0.01,0.01,0.01]))
+f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
+
+solveTree!(fg)
+
+vnd = getVariableSolverData(fg, :x0)
+@test isapprox(SpecialEuclidean(2), mean(SpecialEuclidean(2), vnd.val), ProductRepr([0.0,0.0], [1.0 0; 0 1]), atol=0.1)
+
+#FIXME I would expect close to 50% of particles to land on the correct place
+pnt = getPoints(fg, :x1a)
+@test sum(isapprox.(Ref(SpecialEuclidean(2)), pnt, Ref(ProductRepr([1.0,2.0], [0.7071 -0.7071; 0.7071 0.7071])), atol=0.1)) > 25
+
+#FIXME I would expect close to 50% of particles to land on the correct place
+pnt = getPoints(fg, :x1b)
+@test sum(isapprox.(Ref(SpecialEuclidean(2)), pnt, Ref(ProductRepr([1.0,2.0], [0.7071 -0.7071; 0.7071 0.7071])), atol=0.1)) > 25
+
+end
 #