Problems using multihypo on chains of relative measurements

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

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

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

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

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.

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.

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

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

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_poses_landms 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_poses_landms 00150_fg.tar.gz

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.

One clique MWE for tests:

fg = initfg()

pRight = 0.9
pWrong = 0.1
pr_noise = [0.01, 0.01, 0.001]
od_noise = [0.2; 0.2; 0.2]
lm_noise = [0.01, 0.01, 0.001]

#x0 prior
addVariable!(fg, :x0, Pose2)
prpo = MvNormal([0,0,-pi], pr_noise)
addFactor!(fg, [:x0], PriorPose2(MvNormal(rand(prpo), pr_noise)))

#l1 and l2
addVariable!(fg, :l1, Pose2, tags=[:LANDMARK])
addVariable!(fg, :l2, Pose2, tags=[:LANDMARK])

#x0 to l1 or l2
p2ln = MvNormal([1, 1, pi], lm_noise)
p2p = Pose2Pose2(MvNormal(rand(p2ln), lm_noise))
addFactor!(fg, [:x0; :l1; :l2], p2p, multihypo = [1.0, pRight, pWrong])

#x0 to x1
addVariable!(fg, :x1, Pose2)
pp = MvNormal([1.0,0,0], od_noise)
addFactor!(fg, [:x0,:x1], Pose2Pose2(MvNormal(rand(pp), od_noise)))

#x1 to l1 or l2
p2ln = MvNormal([0, 1, pi], lm_noise)
p2p = Pose2Pose2(MvNormal(rand(p2ln), lm_noise))
addFactor!(fg, [:x1; :l1; :l2], p2p, multihypo = [1.0, pRight, pWrong])

#x1 to l2 or l1
p2ln = MvNormal([0, -1, pi], lm_noise)
p2p = Pose2Pose2(MvNormal(rand(p2ln), lm_noise))
addFactor!(fg, [:x1; :l2; :l1], p2p, multihypo = [1.0, pRight, pWrong])

#aslo good example without x2
#x1 to x2
addVariable!(fg, :x2, Pose2)
pp = MvNormal([1.0,0,0], od_noise)
addFactor!(fg, [:x1,:x2], Pose2Pose2(MvNormal(rand(pp), od_noise)))

#x2 to l1 or l2
p2ln = MvNormal([-1, 1, pi], lm_noise)
p2p = Pose2Pose2(MvNormal(rand(p2ln), lm_noise))
addFactor!(fg, [:x2; :l1; :l2], p2p, multihypo = [1.0, pRight, pWrong])

#x2 to l2 or l1
p2ln = MvNormal([-1, -1, pi], lm_noise)
p2p = Pose2Pose2(MvNormal(rand(p2ln), lm_noise))
addFactor!(fg, [:x2; :l2; :l1], p2p, multihypo = [1.0, pRight, pWrong])

Result from parametric max mixture as reference:

non-parametric graphinit:

solved:

EDIT: x2 can be taken out for smaller MWE

Hi @dehann, perhaps this is also related to:

JuliaRobotics/IncrementalInference.jl#1051
JuliaRobotics/IncrementalInference.jl#802

See MWE above

spreadNH=30.0

vs spreadNH=3.0

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.

Hi @Affie,

I often already have the REPL running with more processes (doing the same steps before running the script in VSCode). My CPU load also confirms that its doing what it should. Maybe adding something like

using Distributed
if nprocs() > 1
    @everywhere using RoME
    @info "running RoME on $(nprocs()) processes"
else
    @info "running RoME on single process"
end

is a good idea to not hard-code the number of processes used (I only have 4 logical cores on my machine, so adding the 3 instead of 7 is more suitable). If I remember correctly, its always done differently in the examples ;)

If the script is then run from the bash, handing the julia executable the -p auto flag distributes the load best, the user can even define how many cores it should use right there, in case he still needs some cores for other stuff.

Not solving incrementally would make it faster, but deviates from what I'm doing with GTSAM. I will keep it in mind, at least here in this example it's pretty easy just to solve every n-th iteration by

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

Sounds like you have multiprocs under control. I normally do addprocs(total_wanted_procs-nprocs())

Hi @lemauee ,

Just a heads up, after the fixes for RoME 378 (IIF v0.21.2), I'm troubleshooting a new bug trail here for multihypo= in the newly consolidated system (post IIF 467). I'm already getting better results here with local changes, but will take a few more hours (maybe days) to tie down these fixes properly. I'm going to push to get these fixes tagged in time for your final results too, and planning a rapid cycle on IIF v0.21.3 for that. This is unlikely to be a breaking change, so hopefully all you'd need to do is an update on IIF and recompute.

Best, Dehann

Hi @lemauee, IncrementalInference v0.21.3 is registered and should solve this problem. Could you please update and run your tests again.

The MWE gives this now:

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_poses_landms 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_poses_landms 00150_fg.tar.gz

I am currently running this example again and it still seems to be failing: 00107_poses_landms I am only at pose 107 out of 150, but dont' think it will get much better. Also, It takes considerably more time to compute than the last time I tried (@dehann mentioned that the recent changes could slow things down, but it already ran the whole night). Are there any solver parameters parameters that I could try to change to get better results?

If I take a look at the results at pose 50, I see that (as expected) a lot of landmarks are initialized at wrong poses due to the ambiguation I do in my code, I'd say the later right measurements cannot recover the true position of the landmarks.

00050_poses_landms

Best, Leo

Hi Leo, I've also seen it's not performing well yet. Perhaps you could help break down the example into another MWE that shows a clear error for tests and to help fine-tune the parameters. From what I've seen, the default spreadNH and inflation are not very accurate in this kind of example.

TODO

I think we should double-check that multihypo initialization is still working correctly.
Check if mulithypo is handled correctly with useMsgLikelihoods (I know you have it off, but it might help if it does work)

A slow down is expected from changes needed for #416. We are busy building out new features and will soon go into a performance round, there are a few things that can be improved performance-wise.

Hi @Affie,

sure I'll try to break this down into a MWE, but partly I think the issue originates from this being a quite large example. But doing something more grid-like with at least a little less variables will help to get a better overview.

I should be able to come up with a MWE for the intialization easily.

Turning on the useMsgLikelihoods Parameter also should be no problem.

Best, Leo

Just for completeness, the fg after the whole 150 poses: 00150_fg.tar.gz 00150_poses_landms

This is a small example testing the multihypo Initialization:

## Input Gt

using CSV

# pose to landmark
associationLm = [1 1; 1 2]
# x y θ
gtLm = [-1.0 0.0 0.0; 1.0 0.0 0.0]
gtTraj = [0.0 0.0 0.0]

nPoses = size(gtTraj,1)

## Generate measurements from groundtruth

using CoordinateTransformations, TransformUtils, LinearAlgebra

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

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

for kPose = 1:nPoses

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

    # landmark measurements
    lmIdx = associationLm[findall(associationLm[:,1] .== kPose),2]
    for kLm = lmIdx
        trans = Tinv(gtTraj[kPose,1],gtTraj[kPose,2],gtTraj[kPose,3])([gtLm[kLm,1], gtLm[kLm,2]])
        rot = wrapRad(gtLm[kLm,3] - gtTraj[kPose,3])
        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:nPoses]
mLmNoisy = [Dict{Int,Vector{Float64}}() for _ = 1:nPoses]

for kPose = 1:nPoses

    # 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:nPoses]

possibleJLm = unique(associationLm[:,2])

for kPose = 1:nPoses
    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 = 1;
fg = LightDFG{SolverParams}(solverParams=SolverParams(logpath="mh_init_test_tiny"))
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,1], gtTraj[1,2], gtTraj[1,3]], 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

    prefix = lpad(kPose,5,'0')

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

    pll1init = plotKDE(fg, :l1)
    #push!(pll1init, Coord.cartesian(fixed=true),style(background_color=RGB(1,1,1)))
    draw(PNG("$(getLogPath(fg))/$(prefix)_l1_init.png", 20cm, 20cm), pll1init)

    pll2init = plotKDE(fg, :l2)
    #push!(pll2init, Coord.cartesian(fixed=true),style(background_color=RGB(1,1,1)))
    draw(PNG("$(getLogPath(fg))/$(prefix)_l2_init.png", 20cm, 20cm), pll2init)

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

    # save
    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)))
    draw(PNG("$(getLogPath(fg))/$(prefix)_poses_landms.png", 20cm, 20cm), pl)

    pll1 = plotKDE(fg, :l1)
    #push!(pll1, Coord.cartesian(fixed=true),style(background_color=RGB(1,1,1)))
    draw(PNG("$(getLogPath(fg))/$(prefix)_l1.png", 20cm, 20cm), pll1)

    pll2 = plotKDE(fg, :l2)
    #push!(pll2, Coord.cartesian(fixed=true),style(background_color=RGB(1,1,1)))
    draw(PNG("$(getLogPath(fg))/$(prefix)_l2.png", 20cm, 20cm), pll2)

end

The results (init and solve) look wrong to me: l1 init: 00001_l1_init l2 init: 00001_l2_init init general: 00001_poses_landms_init