Problems using multihypo on chains of relative measurements

[lemauee]

Hi,

I am using the multihypo keyword for uncertain data association and cannot achieve the results I expect even on tiny examples.

I looked through most of Caesars, Romes and IIFs smaller examples where the multihypo keyword is used and mostly found it being used directly on prior measurements or a relative measurement directly attached to a variable that has a prior. Multihypo-localization on a known map is also working with the tiny example I came up with myself. But if I go to a more realistic SLAM-Scenario that is only anchored using one prior in the beginning, multihypo-measurements produce wrong and very indeterministic results (every run can yield totally different landmark positions even for the tiny example).

The tiny example I attached has 3 poses and 3 landmarks. Every landmark is measured twice and for every measurement has the right landmark and a wrong landmark (which is a different one for the two measurements) assigned. So theoretically the strongest mode in the result should originate from the right associations. Unfortunately this is not the case. I even tried to initialize the landmarks using the GT. Running the solver on a perfecly initialized example did not produce the results I expected.

using RoME, RoMEPlotting 

pRight = 0.9
pWrong = 0.1

ΣOdo = Diagonal([0.01;0.01;0.001].^2)
ΣLm = Diagonal([0.1;0.1;0.01].^2)

gtx1 = [0;0;0]
gtx2 = [1;0;0]
gtx3 = [1;1;0]
gtl1 = [0;1;-pi/2]
gtl2 = [2;0;0]
gtl3 = [2;1;pi/2]

fg = LightDFG{SolverParams}(solverParams=SolverParams(logpath="minimal_multihypo"))
getSolverParams(fg).useMsgLikelihoods = false


addVariable!(fg, :x1, Pose2)
pri1 = addFactor!(fg, [:x1], PriorPoint2(MvNormal(gtx1, Diagonal([0.01;0.01;0.001].^2))))
initManual!(fg, :x1, [pri1.label])

addVariable!(fg, :l1, Pose2)
addVariable!(fg, :l2, Pose2)
dx1l1 = MvNormal(gtl1-gtx1, ΣLm)
tfx1l1l2 = addFactor!(fg, [:x1,:l1,:l2], Pose2Pose2(dx1l1), multihypo = [1.0,pRight,pWrong], graphinit=false)


addVariable!(fg, :x2, Pose2)
odo12 = addFactor!(fg, [:x1,:x2], Pose2Pose2(MvNormal(gtx2-gtx1, ΣOdo)))
initManual!(fg, :x2, [odo12.label])

addVariable!(fg, :l3, Pose2)
dx2l2 = MvNormal(gtl2-gtx2, ΣLm)
tfx2l2l1 = addFactor!(fg, [:x2,:l2,:l1], Pose2Pose2(dx2l2), multihypo = [1.0,pRight,pWrong], graphinit=false)
dx2l3 = MvNormal(gtl3-gtx2, ΣLm) # Typo -gtl2 corrected to -gtx2
tfx2l3l2 = addFactor!(fg, [:x2,:l3,:l2], Pose2Pose2(dx2l3), multihypo = [1.0,pRight,pWrong], graphinit=false)


addVariable!(fg, :x3, Pose2)
odo23 = addFactor!(fg, [:x2,:x3], Pose2Pose2(MvNormal(gtx3-gtx2, ΣOdo)))
initManual!(fg, :x3, [odo23.label])

dx3l2 = MvNormal(gtl2-gtx3, ΣLm)
tfx3l2l3 = addFactor!(fg, [:x3,:l2,:l3], Pose2Pose2(dx3l2), multihypo = [1.0,pRight,pWrong], graphinit=false)
dx3l3 = MvNormal(gtl3-gtx3, ΣLm)
tfx3l3l1 = addFactor!(fg, [:x3,:l3,:l1], Pose2Pose2(dx3l3), multihypo = [1.0,pRight,pWrong], graphinit=false)
dx3l1 = MvNormal(gtl1-gtx3, ΣLm)
tfx3l1l3 = addFactor!(fg, [:x3,:l1,:l3], Pose2Pose2(dx3l1), multihypo = [1.0,pRight,pWrong], graphinit=false)

# initManual!(fg, :l1, [tfx1l1l2.label, tfx3l1l3.label])
# initManual!(fg, :l2, [tfx2l2l1.label, tfx3l2l3.label])
# initManual!(fg, :l3, [tfx2l3l2.label, tfx3l3l1.label])

px1 = getPoints(getKDE(fg,:x1))
px1l1 = rand(dx1l1,100)
initManual!(fg,:l1,px1.+px1l1)

px2 = getPoints(getKDE(fg,:x2))
px2l2 = rand(dx2l2,100)
initManual!(fg,:l2,px2.+px2l2)

px3 = getPoints(getKDE(fg,:x3))
px3l3 = rand(dx3l3,100)
initManual!(fg,:l3,px3.+px3l3)


pl3i = plotSLAM2D(fg;contour=false)
pl3l1i = plotPose(fg,:l1)
pl3l2i = plotPose(fg,:l2)
pl3l3i = plotPose(fg,:l3)

tree, smt, hist = solveTree!(fg)

pl3 = plotSLAM2D(fg;contour=false)
pl3l1 = plotPose(fg,:l1)
pl3l2 = plotPose(fg,:l2)
pl3l3 = plotPose(fg,:l3)

Are there any examples I overlooked that test a similar case to mine?
Do you have any suggestions why my example is failing?

Best,
Leo

[GearsAD]

Hi Leo,

Thanks for reporting the issue and apologies on the late reply. Yes, as far as I can tell the strongest mode should bubble up. There have been quite a few changes to the core in the last few weeks so we may have introduced an issue.

We're working on some other changes but I'll work through your example ASAP and try get a fix into the next release.

Best,
Sam

[dehann]

Hi Leo, Sam,

Thanks, sorry I have been pretty busy with work to fix JuliaRobotics/IncrementalInference.jl#1010, hopefully not too much longer. Leo, just making sure you did see these comments in the documentation:
https://juliarobotics.org/Caesar.jl/latest/concepts/dataassociation/

What I can say without diving in too deep is that multihypo= does not make any permanent assignments about modes -- all modes are always proposed during inference. There is some parasitic locality in how the sampling works, but should be significantly better than naive MCMC or purely parametric methods. Quite a few reasons for that. Ironically the fix for IIF 1010 might actually help even further so best for me to keep pushing on that in the short term.

Getting out of the 'locality' becomes easier and easier if there is more data. I.e. if the graph gets more variables and factors. The solve should be able to move between hypotheses as new data indicates that other modes are more likely -- it's not bullet proof (yet), but again no 'baked in' internal assumptions are made about which modes are most likely. All options are evaluated each time.

Once 1010 is done I will work with the example you provided (thanks!) to clarify what the root cause issue is and then resolve as we continue to work. That may take some time though, so don't want to commit to a timeline for that just yet. Good news is the issue is triaged to a milestone so we will track, resolve, and report here as things progress.

Best,
Dehann

PS, just a heads up, we recently standardized a keyword on plotSLAM2D contour= --> drawContour=:
https://github.com/JuliaRobotics/RoMEPlotting.jl/pull/146/files

[lemauee]

Hi Sam, hi Dehann,

I also tried a larger example with 150 poses and 17 landmarks based on a real Apriltag-SLAM dataset, but with artificial ambiguation added afterwards. I used the same "only the right idx gets assigned to a landmark repeatedly" schematic for this, so mmiSAM should be able to figure out the right association through inference. The example is just the Minimal (Not) Working example, and I am aware that the "implicit pruning of wrong modes' (i know there is no real pruning, but the belief is limited by the number of kde-components) works better if there are more factors yielding the right hypothesis in combination.

What I will do in the meantime is test my tiny and the larger example using MH-iSAM2 (https://www.cs.cmu.edu/~kaess/pub/Hsiao19icra.pdf). If I can get a right solution using this framework we at least know for sure nothing is wrong with my examples. Sadly MH-iSAM2 has no Julia-, not even a MATLAB-Wrapper at the moment, doing this in C++ is not nearly as elegant as with RoME ;) I can supply the larger example if you want, but this still takes a considerable amount of time to compute , especilly if done in an incremental fashion (result obtained for every new robot pose, 4 hours in total).

Thanks also for the hint on the contour thing, trying to keep track of all things being standardized and deprecated, especially with plotting, can be hard sometimes ;)

Best, Leo

[lemauee]

While implementing the example in MH-iSAM I spotted a pretty bad typo in the example above and corrected it. Sadly it still does not solve as expected.

[lemauee]

When changing my hypotheses probabilites to extreme values

  pRight = 0.999999
  pWrong = 0.000001

so that the wrong hypotheses are extremely unlikely, I can obtain something closer to the expected result, but still not quite there.

[GearsAD]

Hi Leo,

We're looking at your issues are we'd like to understand how we can help you work through these issues, as well as learn a bit more about the big picture of what you're setting up so we can help ensure that works. Would you be available for a short discussion sometime in the next week? If so could you send me a mail at sam@navability.io and we can figure out a time?

Thanks,
Sam

[lemauee]

Hi Sam,

I sent you a mail today from my university's adress, I hope it arrived alright, I'm looking forward to a talk :)

Best,
Leo

[lemauee]

Hi,

as @dehann suggested, here's a simplified, one-script version of the larger example I have been testing:

## Read Gt Input from files

using CSV

associationLm = CSV.File("association_lm.txt", header=["iPose", "jLm"])
gtLm = CSV.File("gt_lm.txt", header=["x", "y", "θ"])
gtTraj = CSV.File("gt_traj.txt", header=["x", "y", "θ"])

## Generate bearing range measurements from groundtruth

using CoordinateTransformations, TransformUtils, LinearAlgebra

Tinv(x,y,θ) = inv(Translation(x,y) ∘ LinearMap(R(θ)))

mTrajGt = [Vector{Float64}(undef,3) for _ = 1:length(gtTraj)]
mLmGt = [Dict{Int,Vector{Float64}}() for _ = 1:length(gtTraj)]

for kPose = 1:length(gtTraj)

    # odometry
    if kPose > 1 # no odemetry for first pose, prior instead
        trans = Tinv(gtTraj[kPose-1].x,gtTraj[kPose-1].y,gtTraj[kPose-1].θ)([gtTraj[kPose].x, gtTraj[kPose].y])
        rot = wrapRad(gtTraj[kPose].θ - gtTraj[kPose-1].θ)
        mTrajGt[kPose] = vcat(trans,rot)
    end

    # landmark measurements
    lmIdx = associationLm.jLm[findall(associationLm.iPose .== kPose)]
    for kLm = lmIdx
        trans = Tinv(gtTraj[kPose].x,gtTraj[kPose].y,gtTraj[kPose].θ)([gtLm[kLm].x, gtLm[kLm].y])
        rot = wrapRad(gtLm[kLm].θ - gtTraj[kPose].θ)
        mLmGt[kPose][kLm] = vcat(trans,rot)
    end
end

## Add noise to ground truth measurements

using Distributions, Random

Random.seed!(42); # reproducibility

ΣOdo = Diagonal([0.01,0.01,0.5*pi/180].^2)
ΣLm = Diagonal([0.01,0.01,0.5*pi/180].^2)
# ΣOdo = Diagonal([0.1,0.1,5*pi/180].^2)
# ΣLm = Diagonal([0.1,0.1,5*pi/180].^2)

noiseOdo = MvNormal([0,0,0],ΣOdo)
noiseLm = MvNormal([0,0,0],ΣLm)

mTrajNoisy = [Vector{Float64}(undef,3) for _ = 1:length(gtTraj)]
mLmNoisy = [Dict{Int,Vector{Float64}}() for _ = 1:length(gtTraj)]

for kPose = 1:length(gtTraj)

    # odometry
    if kPose > 1 # no odemetry for first pose, prior instead
        mTrajNoisy[kPose] = mTrajGt[kPose] + vec(rand(noiseOdo,1))
    end

    # landmark measurements
    for kLm = keys(mLmGt[kPose])
        mLmNoisy[kPose][kLm] = mLmGt[kPose][kLm] + vec(rand(noiseLm,1))
    end
end

## Ambiguate landmark idx (add a second one randomly)

jLm = [Dict{Int,Vector{Int}}() for _ = 1:length(gtTraj)]

possibleJLm = unique(associationLm.jLm)

for kPose = 1:length(gtTraj)
    for kLm = collect(keys(mLmGt[kPose]))
        possibleWrongJLm = possibleJLm[possibleJLm .!= kLm]
        jLm[kPose][kLm] = [kLm, rand(possibleWrongJLm)]
    end
end

# probabilites of associations
pRight = 0.9
pWrong = 0.1

## Build fg from measurements and solve incrementally

using Distributed, RoME, RoMEPlotting
@everywhere using RoME

endPose = 150;
fg = LightDFG{SolverParams}(solverParams=SolverParams(logpath="mh_issue"))
getSolverParams(fg).useMsgLikelihoods = false

function addVarIfMissing!(fg,symbol,type,tags)
    if (symbol in ls(fg)) == false
        @info "adding missing variable $(symbol)"
        addVariable!(fg, symbol, type, tags = tags)
    end
end

for kPose = 1:endPose
    
    global fg,tree,smt,hist

    # odometry
    if kPose > 1 # no odometry for first pose
        fromPose = Symbol("x", kPose-1)
        toPose = Symbol("x", kPose)
        addVarIfMissing!(fg, fromPose, Pose2, [:POSE])
        addVarIfMissing!(fg, toPose, Pose2, [:POSE])   
        addFactor!(fg, [fromPose, toPose], Pose2Pose2(MvNormal(mTrajNoisy[kPose], ΣOdo)), graphinit = false)
        initManual!(fg, toPose, [Symbol("x",kPose-1,"x",kPose,"f",1)]) # doing this because I initlialize the same way in GTSAM
    else # prior instead
        addVariable!(fg, :x1, Pose2)
        addFactor!(fg, [:x1], PriorPose2(MvNormal([gtTraj[1].x, gtTraj[1].y, gtTraj[1].θ], Diagonal([0.01,0.01,0.5*pi/180].^2))))
    end

    # landmark measurements
    fromPose = Symbol("x", kPose)
    for kLm = collect(keys(mLmGt[kPose]))
        toLmRight = Symbol("l", jLm[kPose][kLm][1])
        toLmWrong = Symbol("l", jLm[kPose][kLm][2])
        addVarIfMissing!(fg, fromPose, Pose2, [:POSE])
        addVarIfMissing!(fg, toLmRight, Pose2, [:LANDMARK])
        addVarIfMissing!(fg, toLmWrong, Pose2, [:LANDMARK])
        addFactor!(fg, [fromPose, toLmRight, toLmWrong], Pose2Pose2(MvNormal(mLmNoisy[kPose][kLm], ΣLm)), multihypo = [1.0, pRight, pWrong])
    end

    # solve
    if kPose > 1 # not on first solve
        tree, smt, hist = solveTree!(fg, tree)
    else # first solve
        tree, smt, hist = solveTree!(fg)
    end

    # save
    prefix = lpad(kPose,5,'0')
    saveDFG(fg, "$(getLogPath(fg))/$(prefix)_fg")

    # plot
    pl = plotSLAM2D(fg;contour=false)
    push!(pl, Coord.cartesian(fixed=true),style(background_color=RGB(1,1,1)))
    @async draw(PNG("$(getLogPath(fg))/$(prefix)_poses_landms.png", 20cm, 20cm), pl)

end

It needs these three data files again:
gt_traj.txt
gt_lm.txt
association_lm.txt

and currently solves to:

00150_fg.tar.gz

The reference without any ambiguation

## Read Gt Input from files

using CSV

associationLm = CSV.File("association_lm.txt", header=["iPose", "jLm"])
gtLm = CSV.File("gt_lm.txt", header=["x", "y", "θ"])
gtTraj = CSV.File("gt_traj.txt", header=["x", "y", "θ"])

## Generate bearing range measurements from groundtruth

using CoordinateTransformations, TransformUtils, LinearAlgebra

Tinv(x,y,θ) = inv(Translation(x,y) ∘ LinearMap(R(θ)))

mTrajGt = [Vector{Float64}(undef,3) for _ = 1:length(gtTraj)]
mLmGt = [Dict{Int,Vector{Float64}}() for _ = 1:length(gtTraj)]

for kPose = 1:length(gtTraj)

    # odometry
    if kPose > 1 # no odemetry for first pose, prior instead
        trans = Tinv(gtTraj[kPose-1].x,gtTraj[kPose-1].y,gtTraj[kPose-1].θ)([gtTraj[kPose].x, gtTraj[kPose].y])
        rot = wrapRad(gtTraj[kPose].θ - gtTraj[kPose-1].θ)
        mTrajGt[kPose] = vcat(trans,rot)
    end

    # landmark measurements
    lmIdx = associationLm.jLm[findall(associationLm.iPose .== kPose)]
    for kLm = lmIdx
        trans = Tinv(gtTraj[kPose].x,gtTraj[kPose].y,gtTraj[kPose].θ)([gtLm[kLm].x, gtLm[kLm].y])
        rot = wrapRad(gtLm[kLm].θ - gtTraj[kPose].θ)
        mLmGt[kPose][kLm] = vcat(trans,rot)
    end
end

## Add noise to ground truth measurements

using Distributions, Random

Random.seed!(42); # reproducibility

ΣOdo = Diagonal([0.01,0.01,0.5*pi/180].^2)
ΣLm = Diagonal([0.01,0.01,0.5*pi/180].^2)
# ΣOdo = Diagonal([0.1,0.1,5*pi/180].^2)
# ΣLm = Diagonal([0.1,0.1,5*pi/180].^2)

noiseOdo = MvNormal([0,0,0],ΣOdo)
noiseLm = MvNormal([0,0,0],ΣLm)

mTrajNoisy = [Vector{Float64}(undef,3) for _ = 1:length(gtTraj)]
mLmNoisy = [Dict{Int,Vector{Float64}}() for _ = 1:length(gtTraj)]

for kPose = 1:length(gtTraj)

    # odometry
    if kPose > 1 # no odemetry for first pose, prior instead
        mTrajNoisy[kPose] = mTrajGt[kPose] + vec(rand(noiseOdo,1))
    end

    # landmark measurements
    for kLm = keys(mLmGt[kPose])
        mLmNoisy[kPose][kLm] = mLmGt[kPose][kLm] + vec(rand(noiseLm,1))
    end
end

## Build fg from measurements and solve incrementally

using Distributed, RoME, RoMEPlotting
@everywhere using RoME

endPose = 150;
fg = LightDFG{SolverParams}(solverParams=SolverParams(logpath="mh_issue_reference"))
getSolverParams(fg).useMsgLikelihoods = false

function addVarIfMissing!(fg,symbol,type,tags)
    if (symbol in ls(fg)) == false
        @info "adding missing variable $(symbol)"
        addVariable!(fg, symbol, type, tags = tags)
    end
end

for kPose = 1:endPose
    
    global fg,tree,smt,hist

    # odometry
    if kPose > 1 # no odometry for first pose
        fromPose = Symbol("x", kPose-1)
        toPose = Symbol("x", kPose)
        addVarIfMissing!(fg, fromPose, Pose2, [:POSE])
        addVarIfMissing!(fg, toPose, Pose2, [:POSE])   
        addFactor!(fg, [fromPose, toPose], Pose2Pose2(MvNormal(mTrajNoisy[kPose], ΣOdo)), graphinit = false)
        initManual!(fg, toPose, [Symbol("x",kPose-1,"x",kPose,"f",1)]) # doing this because I initlialize the same way in GTSAM
    else # prior instead
        addVariable!(fg, :x1, Pose2)
        addFactor!(fg, [:x1], PriorPose2(MvNormal([gtTraj[1].x, gtTraj[1].y, gtTraj[1].θ], Diagonal([0.01,0.01,0.5*pi/180].^2))))
    end

    # landmark measurements
    fromPose = Symbol("x", kPose)
    for kLm = collect(keys(mLmGt[kPose]))
        toLm = Symbol("l", kLm)
        addVarIfMissing!(fg, fromPose, Pose2, [:POSE])
        addVarIfMissing!(fg, toLm, Pose2, [:LANDMARK])
        addFactor!(fg, [fromPose, toLm], Pose2Pose2(MvNormal(mLmNoisy[kPose][kLm], ΣLm)))
    end

    # solve
    if kPose > 1 # not on first solve
        tree, smt, hist = solveTree!(fg, tree)
    else # first solve
        tree, smt, hist = solveTree!(fg)
    end

    # save
    prefix = lpad(kPose,5,'0')
    saveDFG(fg, "$(getLogPath(fg))/$(prefix)_fg")

    # plot
    pl = plotSLAM2D(fg;contour=false)
    push!(pl, Coord.cartesian(fixed=true),style(background_color=RGB(1,1,1)))
    @async draw(PNG("$(getLogPath(fg))/$(prefix)_poses_landms.png", 20cm, 20cm), pl)

end

works fine:

00150_fg.tar.gz

[Affie]

I just want to make sure you know how to turn on multiprocess calculation to speed things up:

using Distributed
addprocs(7) 
@everywhere using RoME

I would also say not to solve incrementally with every iteration.