Suboptimal result for range-only SLAM

[tipf]

Hi guys,
I implemented a small toy example for range-only SLAM. It is based on a 2D Manhattan-world trajectory surrounded by a polygon of 9 ranging beacons.
The graph consists of relative poses (odometry) and range measurements. All landmark positions are treated as unknown.

Unfortunately, the solver fails to find the optimal solution. As you can see in the attached plots, some of the landmarks (1, 2, and 3) are not at their actual positions, and the trajectory is distorted.
At the beginning (Pose 10), the landmark's uncertainty is high, and the trajectory looks correct - but later, the uncertainty shrinks while the position of the landmarks is still wrong.
I used an incremental estimation scheme but got a similar result by estimating the whole trajectory at once.
Plots for incremental and batch solutions (after timestep 1/10/100) are attached.
Result_0001.pdf
Result_0010.pdf
ResultFinal_Incremental.pdf
ResultFinal_Batch.pdf

To check my data, I created another scenario where a prior is added to each landmark based on its true position. This case converges successfully. I attached these plots also.
Result_GT_0001.pdf
Result_GT_0010.pdf
ResultFinal_GT.pdf

Finally, I threw the data (without the landmark priors!) into a parametric solver (ceres) and was astonished that it converges to the correct solution. A comparison plot is attached.
Result_Parametric.pdf

Maybe I was too optimistic about the capabilities of a nonparametric solver, but is this behavior expected? Shouldn't it be able to solve such a toy example?
And are there any specific settings or methods that might improve the result?

I furthermore attached my source code as well as the dataset file. You can switch between incremental and batch estimation by changing the "IncrementalEstimation" parameter.
W100_Input.txt

# configure script
const MaxPose = 100
const IncrementalEstimation = true
const InputPath = "./W100_Input.txt"
const ImagePath = "./Result"

# umbrella package
using RoME

# library used for graph visualization
using GraphPlot, Cairo, RoMEPlotting
Gadfly.set_default_plot_size(15cm,15cm)

# load data
using CSV
using DataFrames
Data = CSV.read(InputPath, DataFrame, header = 0, ignoreemptyrows = true, delim=' ', ignorerepeated=true, maxwarnings=1)
OdomData = Data[Data[!,1] .== "odom2",:]
RangeData = Data[Data[!,1] .== "range_lm2",:]

# get timestamps
Time = unique(RangeData[:,2])
const NumOdom = size(OdomData,1)
const NumPoses = min(size(Time,1),MaxPose)

# symbols
createSymbolPose(n) = Symbol("x",n)
createSymbolLM(n) = Symbol("l",n)

# functions
function addPriorFactor!(fg, SymbolPose)
    # add first pose with prior
    addVariable!(fg, SymbolPose, Pose2)

    PriorDensity = MvNormal([0, 0, pi], diagm([0.1,0.1,0.01].^2))
    addFactor!(fg, [SymbolPose], PriorPose2(PriorDensity)) 
end

function addOdomFactor!(fg, Symbol1, Symbol2, Odom, OldTime)
    DeltaTime = Odom[2] - OldTime;
    # add new pose
    addVariable!(fg, Symbol2, Pose2)

    # add between pose factor
    OdomGauss = MvNormal([Odom[3],Odom[4],Odom[5]] * DeltaTime,
                         diagm([Odom[6],Odom[7],Odom[8]] * DeltaTime^2))
    addFactor!(fg, [Symbol1, Symbol2], Pose2Pose2(OdomGauss))
end

function addRangeFactor!(fg, SymbolPose, Range)

    # create Symbol
    SymbolLM = createSymbolLM(Range[5])

    # add landmark variable if not exist
    if ~any(ls(fg) .== SymbolLM)
        addVariable!(fg, SymbolLM, Point2)
    end

    # add range factor
    RangeGauss = Normal(Range[3], Range[4])
    addFactor!(fg, [SymbolPose, SymbolLM], Pose2Point2Range(RangeGauss))
end

# Start with an empty factor graph
fg = initfg()

# loop over data
for n in 1:(NumPoses)

    if n == 1
        # add a simple prior
        addPriorFactor!(fg, createSymbolPose(n))
        # create a new Bayes tree
        global Tree = buildTreeReset!(fg)
    else
        # add a single odometry constraint
        addOdomFactor!(fg, createSymbolPose(n-1), createSymbolPose(n), OdomData[n-1,:], Time[n-1])
    end

    # add multiple range measurements
    CurrentRanges = RangeData[RangeData[!,2] .== Time[n],:]
    for nRange = 1:size(CurrentRanges,1)
        addRangeFactor!(fg, createSymbolPose(n), CurrentRanges[nRange,:])
    end

    if IncrementalEstimation == true
        # solve
        Tree = solveTree!(fg, Tree, multithread=true)

        # plot
        pl = plotSLAM2D(fg, drawContour=false, drawEllipse=true, drawPoints=false,
                        xmin=-20, xmax=20, ymin=-20, ymax=20)
        NumStr = lpad(n, 4, "0")
        pl |> PDF(ImagePath*"_$NumStr.pdf", 15cm, 15cm)
    end
end

# final batch solution
if IncrementalEstimation == false
    solveTree!(fg, multithread=true)
end

# final trajectory plot
pl = plotSLAM2D(fg, drawContour=false, drawEllipse=true, drawPoints=true,
                xmin=-20, xmax=20, ymin=-20, ymax=20)
pl |> PDF(ImagePath*"Final.pdf", 15cm, 15cm)

[dehann]

Hi @tipf, thanks for posting. We were all out over the weekend, apologies for slow reply. We'll work through this to see what's going on and post back here.

[dehann]

Hi Tim, think we are seeing some of the same problems in nonparametric solve. We also noticed the parametric solve slowed down significantly with recent upstream changes.

TL;DR, we've decided to resolve two bigger specific issues (below). Would you mind if we use some of your code and comments above to build into unit testing for the various libraries as we fix this issue?

---

Okay so the road we're going to follow to fix this issue (told backwards): the main blocker at the moment is a consolidated code update being worked on at Manifolds.jl:

• Polishing SE(n) JuliaManifolds/Manifolds.jl#493

There has been a lot of discussion there, and we're close to clearing the many questions. Basically the effort on those threads are to make sure Caesar.jl and Manifolds.jl are 100% aligned on the math before we work the main fixes.

I think the course is mostly set with Manifolds.jl, the action right now is sorting out the precise API/syntax on the new trait based Manifolds.jl:

• Discussion on how to use Manopt.jl in IncrementalInference.jl/Caesar.jl JuliaManifolds/Manopt.jl#90 (comment)

Our plan then is to follow up with Caesar.jl fixes fairly quickly (few weeks of work). One known issue specifically seems to be the cause of sub-optimum results you are seeing too. This might also have some links work Leo was doing. There have been several smaller fixes since, but we're confident this is the main issue to fix in the short term:

• Issue with differential constraint from partial factors IncrementalInference.jl#1010

We're actually going to do a code-quality / future-roadmap fix first which will allow fully Julian consolidation between parametric and nonparametric solutions:

• Ball/Metric Tree using NearestNeighbors.jl / Manifolds.jl / Distances.jl / CoordinateTransformations.jl ApproxManifoldProducts.jl#41

Apologies for the delay in closing out this issue, but think this will result in the overall best solution vs. effort. Once IIF#1010 is fixed, I'd expect the quality of results on this issue to be much better.

[tipf]

Thanks for the detailed response Dehann and sorry for my late reply, I was out of the office for two weeks.
I already had the vague feeling that the 1010 issue might be the root cause for the suboptimal behavior -- fingers crossed that the fixes/refactoring of the manifold handling will resolve it.
It's still hard for me to distinguish between fundamental limitations of approximate inference and issues of the specific implementation for such "weakly-determined" cases.

Please feel free to use any part of the code to create the required tests.
Do you need the ground truth for that example? I cloud provide it as well.

[dehann]

Ground truth would be great to have for testing please!

Manifolds.jl v0.8.11 just landed, so think the API path is set.

[tipf]

It contains only the ego poses but for a test this might be enough: W100_GT.txt
The format is quite simple, each line is a pose and can be treated as a row with the following elements:

1 - "pose2" [string]
2 - time stamp [s]
3 - position x-axis [m]
4 - position x-axis [m]
5 - orientation [rad]
6-14 - covariance matrix 3x3 in row-major order, filled with zeros

[Affie]

Hi Tim, I used your dataset to test the batch parametric solver IIF.solveGraphParametric! and as a sanity check if it's correct. Here are the results:

It's sensitive to initialization, but I used mm-ISAM to initialize and it worked well enough.
Unfortunately, it's still under development and still slow, it took about 10 minutes to converge.

I'll have a look at the non-parametric solution also, but I also suspect the path set out by Dehann should fix this issue. In the meantime differing the initialization of the landmarks might help with the batch solution addFactor!(fg, [SymbolPose, SymbolLM], Pose2Point2Range(RangeGauss), graphinit=false)
After initialization, it looks like this (I haven't solved it yet):

The landmarks look to be initialized wrong and I'll investigate further.

[Affie]

In the above case, initalization (and possibly solving) can be improved by tuning 2 solver parameters.

fg = initfg()
fg.solverParams.inflateCycles = ... #default 3
fg.solverParams.inflation = ... #default 5.0

Sorry I can't be of more help currently, but we will defenetely look more into this example in the future as its quite a nice case.
I suspect the incremental solution can only be improved by fixing IIF#1010

[tipf]

Thanks for the pointer to the inflation parameter Johannes, I will give it a try in combination with graphinit=false .

The result of the parametric solver looks promising, but 10 minutes is quite a lot. For comparison, ceres needs 45 milliseconds for the batch problem and about 350 milliseconds for an incremental solution.