Geodesics (and mean) of SpecialEuclidean Group/Manifold

[dehann]

Copy from Slack, per @Affie:

Here is the brute force mean and 2 options for geodesics:

Criteria:

• Explain what this figure shows with minimal code examples (then close this issue)

[Affie]

Pink (q) and green (p) shows the x,y components of 2 normal distributions on SE(2), blue is the brute force product. The orange line is the geodesic p->q on TranslationGroup(2) × SpecialOrthogonalGroup(2) while the pink line is the geodesic of p->q on SpecialEuclideanGroup(2).

The green distribution mode/mean m (gaussian) corresponds with the MAP estimate and not the exponential barycenter that will fall on the pink line. m will correspond to the Karcher mean when using TranslationGroup(2) × SpecialOrthogonalGroup(2). On SpecialEuclideanGroup(2), m is a left invariant mean only.
See "Xavier Pennec, Vincent Arsigny. Exponential Barycenters of the Canonical Cartan Connection and Invariant
Means on Lie Groups" for more detail

[Affie]

The exact plotting script does not exist any more, but the core is captured in a new test (PR soon)

Here are the plotting functions.

cols = Makie.wong_colors()
contour(xs, ys, sum(pdf_pqs; dims=3)[:, :, 1]; color = cols[1], linewidth = 2, levels = 15, axis =(aspect=DataAspect(),))
contour!(xs, ys, sum(pdf_ps; dims=3)[:, :, 1]; color = cols[2], alpha = 0.5)
contour!(xs, ys, sum(pdf_qs; dims=3)[:, :, 1]; color = cols[3], alpha = 0.5)

t_geo = range(0.0, 1.0; length = 100)
pq_geo = map(t_geo) do t
    exp(G, p, t * log(G, p, q))
end
pq_geo_x = [first(g.x)[1] for g in pq_geo]
pq_geo_y = [first(g.x)[2] for g in pq_geo]
lines!(pq_geo_x, pq_geo_y; color = cols[4], linewidth = 3) # magenta

#untested to plot the Riemannian geodesic (straight line), we use the base manifold
M = base_manifold(G)
t_geo = range(0.0, 1.0; length = 100)
pq_geo = map(t_geo) do t
    exp(M, p, t * log(M, p, q))
end
pq_geo_x = [first(g.x)[1] for g in pq_geo]
pq_geo_y = [first(g.x)[2] for g in pq_geo]
lines!(pq_geo_x, pq_geo_y; color = cols[5], linewidth = 3) 

[dehann]

Thanks, Alberto is going to help us review these