Fix uncertainty ellipses in python helpers

[senselessDev]

Because #1067 was not merged into develop, the uncertainty ellipses are still plotted wrong on develop.
I'd like to have that fixed and updated the explanation in the script and tried to avoid the duplicate implementation.
I'm not sure if it was 100% in a way @dellaert wished for. So please have a look.

I set the scaling in such a way that the uncertainty ellipse would include 95% of samples as inliers.

It does not change the fixed bugs by @magicbycalvin, just reorganizes it.

In 2D it is doing the right thing:

Can be tested with

import gtsam
import gtsam.utils.plot
import numpy
numpy.random.seed(42)
import matplotlib
import matplotlib.pyplot

def plot_point2_on_axes(axes,
                        point,
                        linespec,
                        P= None) -> None:
    """
    Plot a 2D point on given axis `axes` with given `linespec`.
    Args:
        axes (matplotlib.axes.Axes): Matplotlib axes.
        point: The point to be plotted.
        linespec: String representing formatting options for Matplotlib.
        P: Marginal covariance matrix to plot the uncertainty of the estimation.
    """
    axes.plot([point[0]], [point[1]], linespec, marker='.', markersize=10)
    if P is not None:
        w, v = numpy.linalg.eig(P)

        # 5 sigma corresponds to 99.9996%, see note above
        k = 5.0

        angle = numpy.arctan2(v[1, 0], v[0, 0])
        e1 = matplotlib.patches.Ellipse(point,
                                        numpy.sqrt(w[0] * k),
                                        numpy.sqrt(w[1] * k),
                                        numpy.rad2deg(angle),
                                        fill=False,
                                        color='red',
                                        label='before')
        axes.add_patch(e1)

mean = numpy.zeros(2)
covariance = numpy.array([[0.2, 0.1], [0.1, 0.5]])

fig, ax = matplotlib.pyplot.subplots()

gtsam.utils.plot.plot_point2_on_axes(ax,
                                     gtsam.Point2(mean),
                                     '',
                                     covariance)

plot_point2_on_axes(ax,
                    gtsam.Point2(mean),
                    '',
                    covariance)

samples = numpy.random.multivariate_normal(mean, covariance, 200)
ax.scatter(samples[:, 0], samples[:, 1], zorder=-10)
ax.legend()
matplotlib.pyplot.show()

[senselessDev]

Actually, changing that to eigh is even better I think. This would prevent from getting complex results due to float precision issues. Comment?

[varunagrawal]

LGTM though I want @dellaert's approval since he has better context from #1067

[varunagrawal]

@senselessDev just noticed that your PR is targeting the fix/ellipses branch. Is that intentional? Your PR comment makes it seem like you want this merged into develop.

[senselessDev]

@senselessDev just noticed that your PR is targeting the fix/ellipses branch. Is that intentional? Your PR comment makes it seem like you want this merged into develop.

Yes, because I was not really sure if everything @dellaert wanted is adressed. Obviously, once it is on his branch, I hope he can add if something's still left to do and then merge to develop soon.

[dellaert]

Will do, sorry for delay - strapped for time

[dellaert]

OK, this is awesome! Sorry for the delay in reviewing. I will merge into my branch, try it out, and then create a PR to develop.