Suboptimal result for range-only SLAM about caesar.jl HOT 8 OPEN

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.

from caesar.jl.

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:

• 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:

• 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:

• JuliaRobotics/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:

• JuliaRobotics/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.

from caesar.jl.

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.

from caesar.jl.

Ground truth would be great to have for testing please!

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

from caesar.jl.

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

from caesar.jl.

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.

from caesar.jl.

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

from caesar.jl.

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.

from caesar.jl.