Merged in feature/cython-plot2d-marginals (pull request #388)

wip - plotting covariances in 2d in cython examples

cython/gtsam/examples/OdometryExample.py

@@ -17,6 +17,9 @@ import numpy as np
 
 import gtsam
 
+import matplotlib.pyplot as plt
+import gtsam.utils.plot as gtsam_plot
+
 # Create noise models
 ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
 PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@@ -50,3 +53,17 @@ params = gtsam.LevenbergMarquardtParams()
 optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
 result = optimizer.optimize()
 print("\nFinal Result:\n{}".format(result))
+
+# 5. Calculate and print marginal covariances for all variables
+marginals = gtsam.Marginals(graph, result)
+for i in range(1, 4):
+    print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
+
+fig = plt.figure(0)
+for i in range(1, 4):
+    gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
+plt.axis('equal')
+plt.show()
+
+
+

cython/gtsam/examples/Pose2SLAMExample.py

@@ -19,6 +19,9 @@ import numpy as np
 
 import gtsam
 
+import matplotlib.pyplot as plt
+import gtsam.utils.plot as gtsam_plot
+
 
 def vector3(x, y, z):
     """Create 3d double numpy array."""
@@ -85,3 +88,10 @@ print("Final Result:\n{}".format(result))
 marginals = gtsam.Marginals(graph, result)
 for i in range(1, 6):
     print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
+
+fig = plt.figure(0)
+for i in range(1, 6):
+    gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
+
+plt.axis('equal')
+plt.show()

cython/gtsam/utils/plot.py

@@ -2,9 +2,10 @@
 
 import numpy as np
 import matplotlib.pyplot as plt
+from matplotlib import patches
 
 
-def plot_pose2_on_axes(axes, pose, axis_length=0.1):
+def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
     """Plot a 2D pose on given axis 'axes' with given 'axis_length'."""
     # get rotation and translation (center)
     gRp = pose.rotation().matrix()  # rotation from pose to global
@@ -20,13 +21,26 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1):
     line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
     axes.plot(line[:, 0], line[:, 1], 'g-')
 
+    if covariance is not None:
+        pPp = covariance[0:2, 0:2]
+        gPp = np.matmul(np.matmul(gRp, pPp), gRp.T)
 
-def plot_pose2(fignum, pose, axis_length=0.1):
+        w, v = np.linalg.eig(gPp)
+
+        # k = 2.296
+        k = 5.0
+
+        angle = np.arctan2(v[1, 0], v[0, 0])
+        e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k),
+                             np.rad2deg(angle), fill=False)
+        axes.add_patch(e1)
+
+def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
     """Plot a 2D pose on given figure with given 'axis_length'."""
     # get figure object
     fig = plt.figure(fignum)
     axes = fig.gca()
-    plot_pose2_on_axes(axes, pose, axis_length)
+    plot_pose2_on_axes(axes, pose, axis_length, covariance)
 
 
 def plot_point3_on_axes(axes, point, linespec):