Merge pull request #1618 from JuliaRobotics/22Q3/backport/fix_entropy

rc v0.30.7 Backport entropy fix

Author: Affie

Project.toml

@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.30.6"
+version = "0.30.7"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

src/services/EvalFactor.jl

@@ -182,13 +182,13 @@ function computeAcrossHypothesis!(ccwl::Union{<:CommonConvWrapper{F},
       for i in 1:Threads.nthreads()  ccwl.cpt[i].activehypo = vars; end
 
       addEntr = view(ccwl.params[sfidx], allelements[count])
-      # dynamic estimate with user requested speadNH of how much noise to inject (inflation or nullhypo)
-      spreadDist = calcVariableDistanceExpectedFractional(ccwl, sfidx, certainidx, kappa=ccwl.inflation)
 
       # do proposal inflation step, see #1051
       # consider duplicate convolution approximations for inflation off-zero
       # ultimately set by dfg.params.inflateCycles
       for iflc in 1:inflateCycles
+        # dynamic estimate with user requested speadNH of how much noise to inject (inflation or nullhypo)
+        spreadDist = calcVariableDistanceExpectedFractional(ccwl, sfidx, certainidx, kappa=ccwl.inflation)
         addEntropyOnManifold!(mani, addEntr, 1:getDimension(mani), spreadDist, ccwl.partialDims)
         # no calculate new proposal belief on kernels `allelements[count]`
           _checkErrorCCWNumerics(ccwl, testshuffle)

test/testEuclidDistance.jl

@@ -136,6 +136,33 @@ end
 # at least one of the 3 solves should produce the right result
 
 
+##
+#test one clique as in RoME
+N=100
+points = [[100.0;0.0],[0.0;100.0]]
+fg = IIF.generateGraph_EuclidDistance(points)
+fg.solverParams.graphinit = false
+
+M = getManifold(fg, :l1)
+TP = false
+for i in 1:3
+  # global TP, N
+  tree = solveTree!(fg);
+
+  L1 = getBelief(fg, :l1) |> getPoints
+
+  # check that two modes exist
+  am1 = sum(isapprox.(Ref(M), L1, Ref([0.0,0.0]), atol=10))
+  am2 = sum(isapprox.(Ref(M), L1, Ref([100.0,100.0]), atol=10))
+
+  TP  = am1 > N*0.03
+  TP &= am2 > N*0.03
+  if TP 
+    @info "test passed in $i"
+    break
+  end
+end
+@test TP
 ##
 
 end