Add example

examples/plot_trajectories.py

@@ -0,0 +1,203 @@
+"""
+Hai,
+
+On 14.05.20 12:20, Dennis Mronga wrote:
+>
+> - Ich bin mir nicht sicher ob die Schätzung der Varianz (in der Methode
+> predictVariance) so korrekt ist. Die Formel habe ich aus diesen Thread:
+> https://stats.stackexchange.com/questions/16608/what-is-the-variance-of-the-weighted-mixture-of-two-gaussians.
+> Es funktioinert jedenfalls für den 1. Fall sehr gut
+
+Sieht vernünftig aus. Die multivariate Variante kannst du hier
+nachlesen:
+https://ipvs.informatik.uni-stuttgart.de/mlr/marc/notes/gaussians.pdf
+(Gleichung 57, b ist der Mittelwert, B die Kovarianz).
+Eigentlich wäre es auch cool sowas in die Library zu integrieren, falls
+du Lust hast.
+
+> - Der Faktor hier:
+> https://github.com/AlexanderFabisch/gmr/blob/4831e67b11040b5c9dff8520c54c870d7b7b6751/gmr/mvn.py#L107
+> hat großen Einfluss auf die Lösung. Teilweise bekomme ich NaN i den
+> predictions, weil die Kovarianzen zu klein werden. Daher habe ich den
+> Wert dann auf 1e-3 vergrößert
+
+Jo, das sollte nur passieren, wenn eine Gaußverteilung *effektiv* mit
+weniger Beispielen als Dimensionen geschätzt wird. Das ist nicht so toll.
+
+Effektiv, weil eigentlich eine *gewichtete* Maximum-Likelihood-Schätzung
+der Gaußverteilungen mit allen Beispielen vorgenommen wird. Jedes
+Beispiel wird gewichtet mit den Werten der jeweiligen
+Wahrscheinlichkeitsdichtefunktionen der Gaußverteilungen. Davon werden
+die meisten Gewichte vermutlich 0 sein. Ein besserer Algorithmus würde
+das erkennen und die degenerierte Gaußverteilung neu initialisieren (in
+etwa hier:
+https://github.com/AlexanderFabisch/gmr/blob/4831e67b11040b5c9dff8520c54c870d7b7b6751/gmr/gmm.py#L108)
+- wahrscheinlich mit größerer Varianz, sodass in der nächsten Iteration
+mehr Beispiele dieser Gaußverteilung zugeordnet werden.
+
+Bei CMA-ES wird eine sogenannte variance effective selection mass (ich
+nenne das mal vesm) berechnet. Wenn alle Gewichte > 0 und zu 1
+aufsummieren (ist hier der Fall), dann
+
+    vesm = 1 / (summe aller quadrierten Gewichte).
+
+Beispiel:
+- alle Gewichte sind 0 außer eines, das 1 ist -> vesm = 1
+- zwei Gewichte sind 0.5 -> vesm = 2
+- drei Gewichte sind 1/3 -> vesm = 3
+Damit könnte man gucken, ob eine Gaußverteilung aus genügend Samples
+geschätzt wird, also wenn vesm >= Anzahl der Dimensionen, ist alles OK.
+
+CMA-ES operiert übrigens ständig mit einer schlechten Schätzung der
+Kovarianz. Die Lösung von CMA-ES ist nicht nur das gewichtete
+Maximum-Likelihood-Update zu nehmen, sondern das zu kombinieren mit der
+letzten Kovarianz.
+
+Man könnte auch sowas ähnliches wie Bessel's Correction für eine
+gewichtete Gaußverteilung berechnen. Siehe
+https://en.wikipedia.org/wiki/Weighted_arithmetic_mean#Weighted_sample_covariance,
+https://arxiv.org/pdf/1604.00772.pdf Abschnitt 3.2 oder
+https://core.ac.uk/download/pdf/84341151.pdf auf Seite 130-131: Gaussian
+Policy, Constant Mean.
+
+Ein Workaround wäre in einem Nachbearbeitungsschritt die "schlechten"
+Kovarianzen zu erkennen und wegzulassen. Du kannst auf den Attributen
+der GMM-Klasse direkt rumschreiben. NumPy hat eine Funktion
+np.linalg.matrix_rank(...). Ansonsten kann man auch die Werte der
+Singulärwerte prüfen. Sollten viele nahe 0 sein, ist das problematisch.
+"""
+import numpy as np
+
+
+def make_demonstrations(n_demonstrations, n_steps, sigma=0.25, mu=0.5,
+                        start=np.zeros(2), goal=np.ones(2), random_state=None):
+    """Generates demonstration that can be used to test imitation learning.
+
+    Parameters
+    ----------
+    n_demonstrations : int
+        Number of noisy demonstrations
+
+    n_steps : int
+        Number of time steps
+
+    sigma : float, optional (default: 0.25)
+        Standard deviation of noisy component
+
+    mu : float, optional (default: 0.5)
+        Mean of noisy component
+
+    start : array, shape (2,), optional (default: 0s)
+        Initial position
+
+    goal : array, shape (2,), optional (default: 1s)
+        Final position
+
+    random_state : int
+        Seed for random number generator
+
+    Returns
+    -------
+    X : array, shape (n_task_dims, n_steps, n_demonstrations)
+        Noisy demonstrated trajectories
+
+    ground_truth : array, shape (n_task_dims, n_steps)
+        Original trajectory
+    """
+    random_state = np.random.RandomState(random_state)
+
+    X = np.empty((2, n_steps, n_demonstrations))
+
+    # Generate ground-truth for plotting
+    ground_truth = np.empty((2, n_steps))
+    T = np.linspace(-0, 1, n_steps)
+    ground_truth[0] = T
+    ground_truth[1] = (T / 20 + 1 / (sigma * np.sqrt(2 * np.pi)) *
+                       np.exp(-0.5 * ((T - mu) / sigma) ** 2))
+
+    # Generate trajectories
+    for i in range(n_demonstrations):
+        noisy_sigma = sigma * random_state.normal(1.0, 0.1)
+        noisy_mu = mu * random_state.normal(1.0, 0.1)
+        X[0, :, i] = T
+        X[1, :, i] = T + (1 / (noisy_sigma * np.sqrt(2 * np.pi)) *
+                          np.exp(-0.5 * ((T - noisy_mu) /
+                                         noisy_sigma) ** 2))
+
+    # Spatial alignment
+    current_start = ground_truth[:, 0]
+    current_goal = ground_truth[:, -1]
+    current_amplitude = current_goal - current_start
+    amplitude = goal - start
+    ground_truth = ((ground_truth.T - current_start) * amplitude /
+                    current_amplitude + start).T
+
+    for demo_idx in range(n_demonstrations):
+        current_start = X[:, 0, demo_idx]
+        current_goal = X[:, -1, demo_idx]
+        current_amplitude = current_goal - current_start
+        X[:, :, demo_idx] = ((X[:, :, demo_idx].T - current_start) *
+                             amplitude / current_amplitude + start).T
+
+    return X, ground_truth
+
+
+if __name__ == "__main__":
+    import matplotlib.pyplot as plt
+    from gmr import GMM, plot_error_ellipses, kmeansplusplus_initialization, covariance_initialization, plot_error_ellipses
+    from gmr.utils import check_random_state
+
+    X, _ = make_demonstrations(
+        n_demonstrations=200, n_steps=100, goal=np.array([1., 2.]),
+        random_state=0)
+    X = X.transpose(2, 1, 0)
+    n_demonstrations, n_steps, n_task_dims = X.shape
+    X_train = np.empty((n_demonstrations, n_steps, n_task_dims + 1))
+    X_train[:, :, 1:] = X
+    t = np.linspace(0, 1, n_steps)
+    X_train[:, :, 0] = t
+    X_train = X_train.reshape(n_demonstrations * n_steps, n_task_dims + 1)
+
+    random_state = check_random_state(0)
+    n_components = 5
+    initial_means = kmeansplusplus_initialization(X_train, n_components, random_state)
+    initial_covs = covariance_initialization(X_train, n_components)
+    gmm = GMM(n_components=n_components,
+            priors=np.ones(n_components, dtype=np.float) / n_components,
+            means=np.copy(initial_means),
+            covariances=initial_covs,
+            verbose=2,
+            random_state=random_state)
+    gmm.from_samples(X_train, n_iter=200, max_eff_sample=0.5)
+
+    plt.figure()
+    plt.subplot(111)
+
+    for step in t:
+        conditional_gmm = gmm.condition([0], np.array([step]))
+        samples = conditional_gmm.sample(100)
+        #plot_error_ellipses(plt.gca(), conditional_gmm, colors=["r", "g", "b"])
+        #print(conditional_gmm.priors)
+        #print(conditional_gmm.means)
+        #print(conditional_gmm.covariances)
+        plt.scatter(samples[:, 0], samples[:, 1], s=10)
+
+    from matplotlib.patches import Ellipse
+    from itertools import cycle
+    colors = cycle(["r", "g", "b"])
+    for factor in np.linspace(0.5, 4.0, 8):
+        new_gmm = GMM(n_components=len(gmm.means), priors=gmm.priors, means=gmm.means[:, 1:], covariances=gmm.covariances[:, 1:, 1:], random_state=gmm.random_state)
+        #k = 0
+        for mean, (angle, width, height) in new_gmm.to_ellipses(factor):
+            ell = Ellipse(xy=mean, width=width, height=height,
+                          angle=np.degrees(angle))
+            ell.set_alpha(0.2)
+            #ell.set_alpha(new_gmm.priors[k])
+            #k += 1
+            ell.set_color(next(colors))
+            plt.gca().add_artist(ell)
+
+    plt.plot(X[:, :, 0].T, X[:, :, 1].T, alpha=0.2)
+    plt.xlabel("$x_1$")
+    plt.ylabel("$x_2$")
+    plt.show()