getKDEfit for on-manifold KDEs (Pose2) / credibility of mmiSAM solutions

[lemauee]

Hi,

I want to have a look mmiSAMs credibility (for example using NEES) and therfore need to estimate the variance / standard deviation of a normal distribution fit to a KDE. For euclidean manifolds getKDEfit does this just fine, but as the current comment implies:

function getKDEfit(p::BallTreeDensity; distribution=MvNormal)
  # TODO: update for on-manifold operations
  fit(distribution, getPoints(p))
end

on (partly) circular manifolds with large spread crossing the -pi, pi boundaries this will lead wrong results.

I already came up with a solution that at least covers the diagonal of the covariance matrix:

function getKDEfitPose2(p::BallTreeDensity)
    pp = getPoints(p)
    ppEuclid = pp[1:2,:]
    ppCirc = pp[3,:]
    normalEuclid = fit(MvNormal,ppEuclid)
    μ = zeros(3)
    μ[1:2] = normalEuclid.μ
    μ[3] = cmean(ppCirc)
    Σ = zeros(3,3)
    Σ[1:2,1:2] = normalEuclid.Σ
    Σ[3,3] = cstd(ppCirc)^2
    MvNormal(μ,PDMat(Σ))
end

it uses the CircStats.jl packages functions cmean and cstd.

Calculating the mean of angles is almost straightforward (see https://en.wikipedia.org/wiki/Mean_of_circular_quantities). Even the implementation in CircStats.jl is only a line long:

cmean(a) = atan(sum(sin.(a)), sum(cos.(a)))

The variance is another story. In circular statistics the variance is NOT defined as the standard deviation squared as described in https://en.wikipedia.org/wiki/Directional_statistics#Measures_of_location_and_spread . But I chose this implementation to stay consistent with the current behavior of getKDEfit in case we have only a small variance and are far from -pi or pi. Wikipedia describes this more formally as "for a wrapped normal distribution, it is an estimator of the standard deviation of the underlying normal distribution". In the end this should be the right measure to calculate credibility on. CircStats implements the calculation of the resultant vector and then calculates the standard deviation based on this just like described in Wikipedia

cresultant(a, degrees::Bool=false) = degrees ?
    sqrt(sum(sin.(deg2rad.(a)))^2 + sum(cos.(deg2rad.(a)))^2)/length(a) :
    sqrt(sum(sin.(a))^2 + sum(cos.(a))^2)/length(a)
cstd(a, degrees::Bool=false) = sqrt(-2*log(cresultant(a, degrees)))

Concerning the covariance matrices off-diagonal entries I have not found any literature by now and just left them zero. Perhaps handing back two seperate distributions or putting NaNs in these places would be the better idea.

Last but not least, the Implementation as seperate function getKDEfitPose2 seems pretty un-julia-ish. Can you think of a better option?

I would be happy to file a pull-request (which i actually never did to this date ;)) if you think this function in its current state is of general interest :)

OT: Are there any other options you can think of concerning evaluating the credibility of an mmiSAM solution?

[lemauee]

I put some thought into it today and came up with a version that only works for small variances in theta but can calculate the off-diagonal elements.

function getKDEfitPose2(p::BallTreeDensity)
    pp = getPoints(p)
    n = size(pp, 2)
    μ = vec([mean(pp[1:2,:],dims=2); cmean(pp[3,:])])
    z = pp .- μ
    z[3,:] = wrapRad.(z[3,:])
    Σ = 1/n.*z*z'
    MvNormal(μ,PDMat(Σ))
end

This is the script I used to investigte how the different variants behave, if you want to have a look:

using Distributions, Random, Gadfly, PDMats, TransformUtils, CircStats

x = 0.0
y = 0.0
θ = π

μ = [x;y;θ]

display("μ input:")
display(μ)

xx = 1.0^2
yy = 1.0^2
θθ = 0.1^2
xy = 0.5*0.5
xθ = 0.5*0.05
yθ = -0.5*0.05

Σ = [xx xy xθ;
     xy yy yθ;
     xθ yθ θθ]

display("Σ input:")
display(Σ)

p = MvNormal(μ,PDMat(Σ))

seed = 0
rng = MersenneTwister(seed)

nSamples = 1000
pp = Matrix{Float64}(undef,3,nSamples)
rand!(rng,p,pp)

histx = plot(x=pp[1,:], Geom.histogram)
histy = plot(x=pp[2,:], Geom.histogram)
histθ = plot(x=pp[3,:], Geom.histogram)

display(histx)
display(histy)
display(histθ)

pp[3,:] = wrapRad.(pp[3,:])
histθw = plot(x=pp[3,:], Geom.histogram)
display(histθw)

μe = vec([mean(pp[1:2,:],dims=2); cmean(pp[3,:])])

display("μ estimated using cmean: $(μe)")

z = pp .- μe

Σe = 1/nSamples.*z*z' # should be wrong ...

display("Σ estimated without wrapping of z (should be wrong:)")
display(Σe)

zc = z
zc[3,:] = wrapRad.(zc[3,:])
Σec = 1/nSamples.*zc*zc'

display("Σ estimated with wrapping of z:")
display(Σec)

## Circstats just for comparison
display("θθ (Σ[3,3]) estimated using cstd^2: $(cstd(pp[3,:])^2)")
display("variance(θ) estimated using cvar (should be different from Σ[3,3]): $(cvariance(pp[3,:]))")

yielding the results

"μ input:"
3-element Array{Float64,1}:
 0.0
 0.0
 3.141592653589793
"Σ input:"
3×3 Array{Float64,2}:
 1.0     0.25    0.025
 0.25    1.0    -0.025
 0.025  -0.025   0.01
"μ estimated using cmean: [-0.0032596977249884037, -0.011324033106056706, 3.1370778841570455]"
"Σ estimated without wrapping of z (should be wrong:)"
3×3 Array{Float64,2}:
  0.988132  0.276439  -0.529729
  0.276439  1.00446    0.442129
 -0.529729  0.442129  18.0368
"Σ estimated with wrapping of z:"
3×3 Array{Float64,2}:
 0.988132    0.276439    0.0241361
 0.276439    1.00446    -0.0166571
 0.0241361  -0.0166571   0.00972262
"θθ (Σ[3,3]) estimated using cstd^2: 0.009722374947696686"
"variance(θ) estimated using cvar (should be different from Σ[3,3]): 0.004849391024678185"

The diagonal elements are totally okay, the off-diagonal elements seem to be in the right ballpark, better than zeroing them.

[dehann]

HI @lemauee , apologies for the slow reply. We busy with a major rewrite of some internal components and we actually planning to release MM-iSAMv2 around Spring 2021.

Last but not least, the Implementation as seperate function getKDEfitPose2 seems pretty un-julia-ish. Can you think of a better option?

So that function definitely needs to be replaced. We use PPE (before Covid hit) as Point Parametric Estimate. The idea of fit should definitely be on-manifold. When we build BallTreeDensitys in ApproxManifoldProducts.jl we already do the CircStats step you mention, but there is no easy API for extracting the mean yet. Basically, yes we should expose that properly.

Part of the v2 push includes a wide variety of fixes and improvements, so for this side its been worth it for us to instead sequence something like CircStats after JuliaRobotics/RoME.jl#244 is complete. From there we have been talking with the folks over by Manifolds.jl. The ultimate goal here is just to use something like ManOpt.jl.

In the mean time, if you can make do with the code snippet solution you suggest (thanks very much for posting) on THETA for Pose2, that is best short term solution. Once MM-iSAMv2 roles around there will be a much better Julian way to get the "PPE.fit" which is the actual expected value on all Product Manifolds.

the off-diagonal elements seem to be in the right ballpark, better than zeroing them

Yep, the best way to do that in the long run is use on-manifold optimization and this is exactly what ApproxManifoldProducts.jl sets out to do. There is quite a lot to say about MM-iSAMv2, but its roughly captured in the many issues all round. Instead I'd just try point to the right issues (or open new ones) and push to get v2 out as fast as possible.

OT: Are there any other options you can think of concerning evaluating the credibility of an mmiSAM solution?

v2 will have better numerical performance than v1 (which is around 4 years old at this point). The main performance speedups will be done once v2 fixes all the known numerical issues we have cataloged all round.

Testing Pose error based on either Mean or Max or (major mode means) as point values (definitely on-manifold) is probably the most sensible, so your direction sounds right to me. The next thing is to evaluate the covariance of all beliefs, but that basically impossible for large scale systems since Gaussian method covariance approximations are only valid in fully uncorrelated (max-entropy), linear, unimodal, situations.

Our claim is that Caesar.jl is one of the first methods to seriously address large scale non-Gaussian/multimodal SLAM use cases and we are working to make the marginal posterior belief estimates of each variable both as accurate and efficiently recoverable as possible. v2 has a long laundry list of issues, so I'd suggest evaluating v2 for a more representative answer regarding the accuracy of this code stack. We basically stopped pursuing accuracy on v1 quite some time ago, and hence the push to get v2 out ASAP. I will post here again when we do the v2 announcement.

PS, I transferred the issue here to AMP as better venue.

PSS, @lemauee we can add you to the slack channel for faster response on issues etc if you wanted. Apologies again for the delay.

We really need a Slack invite page and perhaps I could please ask for some help with that, mayve @jim-r-hill or @GearsAD has some bandwidth and experience available on that front? See Julia's own slack invite page as cool example: https://julialang.org/slack/

[lemauee]

Hi @dehann,

Thanks for the in-depth reply. Great to see all these improvements improvements on the horizon. For the meantime I will use the code I posted above. I am aware that Gaussian method covariance approximations are only valid for very specific applications. In the end I want to compare mmiSAMs solutions to other (robust gaussian) estimators solutions in terms of accuracy and credibility. I decided on ATE and RPE for accuracy comparison and ANEES for credibility comparison (see Li, X. Rong, Zhanlue Zhao, and Vesselin P. Jilkov. "Estimator’s credibility and its measures." Proc. IFAC 15th World Congress. 2002). In the end a generalization of the ANEES to KDEs would be the right measure to evaluate mmiSAMs Credibility, but this goes beyond my mathematical knowledge at this point ;)

Being part of the slack channel would be great, I really like the discussion on here!

[dehann]

Hi @lemauee , are you able to join via logging in here caesarjl.slack.com? Alternatively just point me to your email and I can send an invite from the Slack side.

[dehann]

HI @lemauee , i sent a slack invite to the email address and deleted it here in case you don't want it out in the open.

Also note ongoing progress e.g #51 and #52 towards fixing issues as you opened here.

[lemauee]

Thanks for adding and deleting, I successfully joined the slack channel :) Nice to see the work ongoing there!

[dehann]

This is now fixed in the development branches using JuliaManifolds/Manifolds.jl internally. This should become available via the combination AMP v0.3.1 and IIF v0.22.0