Spike multihypo, brute force towards "hode branching" w rLOOGibbs

[dehann]

Comments on new beehive example, starting with brute force

pp @Affie

Each leg has a θ = +pi/3, -pi/3 mixture. So 64

Also mode search up to 256

[dehann]

PS @Affie , on the second plot. Don't go down the "force collapse to one mode", that is the wrong logic path. The answers are/should be, here are all the modes and their probabilistic weight (all summing to 1). The idea of picking the largest weight mode is a fallacy.

[Affie]

PS @Affie , on the second plot. Don't go down the "force collapse to one mode", that is the wrong logic path. The answers are/should be, here are all the modes and their probabilistic weight (all summing to 1). The idea of picking the largest weight mode is a fallacy.

Not in this case, only one mode is probable, it's weight is approx 1. (collapsed by "loop closure" factors on l1, note l1 position is not symmetrical as in the first plot)

[Affie]

using IncrementalInference
using DistributedFactorGraphs
using LieGroups
using LinearAlgebra
using StaticArrays
using Distributions
using Manopt

SE2 = SpecialEuclideanGroup(2; variant = :right)
PoseMani = TranslationGroup(2) × SpecialOrthogonalGroup(2)
@defStateType SpecialEuclidean2 PoseMani ArrayPartition(@SVector([0.0, 0.0]), @SMatrix([1.0 0.0; 0.0 1.0]))

DFG.@defObservationType(
    SpecialEuclidean2SpecialEuclidean2,
    RelativeObservation,
    SE2
)
function (cf::CalcFactor{<:SpecialEuclidean2SpecialEuclidean2})(X, p, q)
    M = getManifold(SpecialEuclidean2SpecialEuclidean2)
    X̂ = log(M, p, q)
    return vee(LieAlgebra(M), X - X̂)
end
##

fg = initfg()
fg.solverParams.useMsgLikelihoods = true
fg.solverParams.graphinit = false
addVariable!(fg, :x0, SpecialEuclidean2)

mp = ManifoldPrior(SE2, ArrayPartition(Vector([0.0, 0.0]), Matrix([1.0 0.0; 0.0 1.0])), MvNormal(diagm([0.05, 0.05, 0.005] .^ 2)))
addFactor!(fg, [:x0], mp)

for i in 0:5
    psym = Symbol("x$i")
    nsym = Symbol("x$(i+1)")
    addVariable!(fg, nsym, SpecialEuclidean2)
    mixodo = Mixture(
        [MvNormal([10.0, 0, pi/3], [0.1, 0.1, 0.01]), MvNormal([10.0, 0, -pi/3], [0.1, 0.1, 0.01])],
        [0.5, 0.5],
    )
    # mixodo = MvNormal([10.0, 0, pi/3], [0.1, 0.1, 0.01])
    f = addFactor!(fg, [psym;nsym], SpecialEuclidean2SpecialEuclidean2(mixodo))
end

# variations
# 1: 64 -> 2 modes
# 2: 64 -> 1 mode, colapse variation 1 to one mode with an additional factor
# 3: 256 -> 4 modes (overpapping 2 hexes, different l1 positions)
# 4: 256 -> 1 mode, collapse variation 3 to one mode with an additional factor
variation = 1

if variation < 3
    addVariable!(fg, :l1, SpecialEuclidean2, tags=[:LANDMARK;])
    mf = ManifoldFactor(PoseMani, MvNormal([0.0, 9.55, pi/2], [1.0, 1.0, 0.1]))
    # 2 modes for hex and one for l1 for just these 2 factors
    addFactor!(fg, [:x0; :l1], mf)
    addFactor!(fg, [:x6; :l1], mf)
    # collapse to one mode with this additional factor
    if variation == 2
        mf = ManifoldFactor(PoseMani, MvNormal([0, -9.55, -pi/2], [1.0, 1.0, 0.1]))
        addFactor!(fg, [:x3, :l1], mf)
    end
else
    addVariable!(fg, :l1, SpecialEuclidean2, tags=[:LANDMARK;])
    for v in [:x0, :x6]
        mix = Mixture(
            [MvNormal([0, 9.55, pi/2], [1.0, 1.0, 0.1]), MvNormal([0, -9.55, -pi/2], [1.0, 1.0, 0.1])],
            [0.5, 0.5],
        )
        # mix = MvNormal([0, 9.55, pi/2], [1.0, 1.0, 0.1])
        mf = ManifoldFactor(PoseMani, mix)
        addFactor!(fg, [v; :l1], mf)
    end

    if variation == 4
        mf = ManifoldFactor(PoseMani, MvNormal([0, -9.55, -pi/2], [1.0, 1.0, 0.1]))
        addFactor!(fg, [:x3, :l1], mf)
    end

end