Bug in getKDERange/getKDEMax

[Affie]

Originally logged as: "Possible issue with Sphere1"

---

I see a lot of failures on the last element of this test:

IncrementalInference.jl/test/testSphere1.jl

Line 27 in 733a605

@test all(isapprox.(sppes, gt, atol=0.35))

It fails only on the value where a pi wrap happens

Here are 7 runs:
0.04340732121322821
0.04892840689558042
-0.03620065175907605
-0.5407379392747547
-0.0035459366733765485
-0.03978010457882153
0.07482259301990224
It does not look like an accuracy failure.

[dehann]

Hi, yes these kind of errors follows from the new inflate feature in v0.21.2. The right way to fix this problem is via Manifolds.jl and using the exp and log maps properly. Perhaps we can come back to this issue after JuliaRobotics/ApproxManifoldProducts.jl#41 and JuliaRobotics/RoME.jl#244...

Is there an immediate show stopping issue for which we could try a workaround?

BTW Pose3Pose3 has the same issue since Euler rotations [0,0,0] is "equivalent" (not really) to [pi,pi,pi]. Once transitioned to Manifolds.jl as standard then I expect these issues to go away too.

[Affie]

I wonder if a quick fix can be to update the optim step to use a circular manifold as with Parametric. But RoME 244 is a way better long-term solution, so don't know if it would be worth it.

[Affie]

A quick experiment yielded the optimization results:

10-element Vector{Float64}:
 -2.2719662326628667
  4.011868856571512
  3.9943516192010318
 10.285537838761103
 10.28135289821674
 -2.2962440138113673
 -8.561626846138694
 -2.27838647031575
  4.000384566665238
 -2.2994814458418107

manikde! on the results works as expected.

[Affie]

It looks like the issue has more to do with PPE.
x4 on a failed test:

and calcPPE

calcPPE(fg, :x4)
DistributedFactorGraphs.MeanMaxPPE(:default, [-1.5571493167163937], [-1.5571493167163937], [-2.274934952301959], Dates.DateTime("2021-02-26T08:23:07.773"))

[Affie]

The bug is in
getKDERange(mm, addop=addop, diffop=diffop ) / getKDEMax

It returns the wrong part of the manifold.

julia>     rangeV = getKDERange(mm, addop=addop, diffop=diffop )
1×2 Matrix{Float64}:
 3.04679  -1.55715

[Affie]

MWE

n = Normal(-pi, 0.1)
N=100
fg = initfg()
v = addVariable!(fg, :x, Circular)
addFactor!(fg, [:x], PriorCircular(n))
ensureAllInitialized!(fg)

P = getBelief(v)
ops = buildHybridManifoldCallbacks((:Circular,))
rangeV = getKDERange(P; addop=ops[1], diffop=ops[2])


# julia> rangeV = getKDERange(P; addop=ops[1], diffop=ops[2])
# 1×2 Matrix{Float64}:
#  -3.1398  3.13912


# julia> getPPE(fg, :x).suggested
# 1-element Vector{Float64}:
#  -3.138374303631888

[dehann]

Trying to fix this via #1197 and related.

[dehann]

See updated MWE above.

• PPE.suggested now works as expected using Circular (Sphere1).
• The range calculation is still not correct since a basic Euclidean assumption is made, and can only be fixed by completely changing how KDERange works. This will be done as part of AMP Manifolds upgrade instead. Transferred to Ensure getBeliefRange works as necessary ApproxManifoldProducts.jl#71