Multihypo seems to be doing the wrong thing on 1D scalar example?

[keevindoherty]

It seems like the example below produces some funky marginals...
I'm on Julia 0.6.3

Package versions:

• IIF: 0.3.8+ master
• RoME 0.1.6+ master
• RoMEPlotting 0.0.1+ master
• Distributions 0.15.0
• KernelDensityEstimate 0.3.1+ master

using IncrementalInference, RoME, RoMEPlotting, Distributions, KernelDensityEstimate

lm_prior_noise = 0.01
meas_noise = 0.25
odom_noise = 0.1
n_samples = 200

# initialize mean landmark locations
l0 = 0.0
l1 = 1.0
l2 = 4.0

# "Ground-truth" robot poses
x0 = 0.0
x1 = 1.0
x2 = 2.0
x3 = 4.0

# Initialize empty factor graph
fg = initfg()

# Place strong prior on locations of three "doors"
addVariable!(fg, Symbol("l0"), ContinuousScalar, N=n_samples)
addFactor!(fg, [:l0], IIF.Prior(Normal(l0, lm_prior_noise)))

addVariable!(fg, Symbol("l1"), ContinuousScalar, N=n_samples)
addFactor!(fg, [:l1], IIF.Prior(Normal(l1, lm_prior_noise)))

addVariable!(fg, Symbol("l2"), ContinuousScalar, N=n_samples)
addFactor!(fg, [:l2], IIF.Prior(Normal(l2, lm_prior_noise)))

# Add first pose
addVariable!(fg, Symbol("x0"), ContinuousScalar, N=n_samples)

# Make first "door" measurement
addFactor!(fg, [:x0; :l0; :l1; :l2], LinearConditional(Normal(0, meas_noise)), multihypo=[1.0; 1.0/3.0; 1.0/3.0; 1.0/3.0])

# Add second pose
addVariable!(fg, Symbol("x1"), ContinuousScalar, N=n_samples)

# # Gaussian transition model
addFactor!(fg, [:x0; :x1], LinearConditional(Normal(1, odom_noise)))

# # Make second "door" measurement
addFactor!(fg, [:x1; :l0; :l1; :l2], LinearConditional(Normal(0, meas_noise)), multihypo=[1.0; 1.0/3.0; 1.0/3.0; 1.0/3.0])

# # Add third pose
addVariable!(fg, Symbol("x2"), ContinuousScalar, N=n_samples)

# # Gaussian transition model
addFactor!(fg, [:x1; :x2], LinearConditional(Normal(1, odom_noise)))

# Add fourth pose
addVariable!(fg, :x3, ContinuousScalar, N=n_samples)

# Add odometry transition and new landmark sighting
addFactor!(fg, [:x2, :x3], LinearConditional(Normal(2, odom_noise)))
addFactor!(fg, [:x3; :l0; :l1; :l2], LinearConditional(Normal(0, meas_noise)), multihypo=[1.0; 1.0/3.0; 1.0/3.0; 1.0/3.0])


ensureAllInitialized!(fg)
tree, smt, hist = solveTree!(fg)  # EDIT, new API

# tree = wipeBuildNewTree!(fg)
# inferOverTree!(fg, tree, N=n_samples)
# addNode -> addVariable!

[dehann]

Hi Kevin, got it and thanks for posting — let me dig in and will report here what I find.

[dehann]

possible connection with---pending cross-examination--- #162

[dehann]

Must also confirm this is not related to #57

[dehann]

influences #141

[dehann]

Just a heads up: I am making progress on another issue in IncrementalInference that may well influence the one described here. Will link updates as they become available.

[dehann]

Limited progress with partial fix with #223, but likely found a route more general solutions (ie fixing a new bug found)

[dehann]

FYI, wip https://github.com/JuliaRobotics/RoME.jl/blob/31cfc065480f0b4d7edb84298a3c5abd6e3526ef/test/testMultiHypo3Door.jl#L1

[dehann]

moving this issue to IncrementalInference.jl

[dehann]

reduced 2door example: https://github.com/JuliaRobotics/RoME.jl/blob/4649a235ce72a9c27c5e0c7c8b1b662193856be9/test/testMultiHypo2Door.jl#L1

[dehann]

correction, I think the 2 door example is in fact working properly. Not sure what was going on when we were looking at it...

[dehann]

Possible fix, early signs look good:
5ce0559

one of the major items could be the selection of which factors to put in which clique. Previous the code would select only factors associated with the frontal variables (starting from the leaves) and work up to the root. This new commit introduces "factors amongst" all variables in a clique. This means more factors appear lower down in the tree.

The two door example had the priors on landmark :l0 and :l1 split between the two resulting cliques in the Bayes (Junction) tree. The :x0=-5 or x1=5 results came from the the leaf clique permanently solving :l1=0 (incorrectly, with only a very small mode at correct 10). The root clique with the prior on :l1 would then land between 0 and 10, resulting in the observed :x1 or :l1 = 5.

Will test this new code more rigorously in the morning, but seems like a step forward at this point.

[dehann]

PS: Still need to fix the iteration order within cliques. Found a minor issue there during this debugging.

[dehann]

Okay, so update -- the iteration order is not fixed yet but I think there is also a third fringe case issue that this example is bringing out (if you solve the graph many times over). At this point it looks like multihypo=[...] for trinary hypothesis does make a mistake under certain conditions that only become apparent after repeatedly solving the same Bayes tree.

To summarize the fixes required:

• Clique should use factors amonsgt all clique variables (not just frontals as in the past).
  first, then progressing a non-inits projection (rarely needed), and finally the regular clique solution as before (5ce0559).

• Iterated variable ordering should finalize on variables with Singleton belief factors (04457c1)

  • Would better distribute the parallel workload earlier in the upward pass too.
  • The autoinit interface on the tree can benefit from priming variables from Singleton factors in a clique first.

• multihypo=[...] seems to have a corner case error that is difficult to provoke, but greater than binary case has potential for incorrect modeling and solution (not initially apparent). Transferring to new issue Generic N-ary multihypothesis (bad-init nullhypo addition)  #248.

[dehann]

FYI this was fixed in IIF v0.11.1

[keevindoherty]

@dehann this is fantastic news!