From 4ba7c59330cda33ec496ebcf1bdf6e7f97e13ffe Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 27 Sep 2018 22:45:14 -0400
Subject: [PATCH] Plotting utilities

---
 cython/gtsam/utils/plot.py | 76 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 76 insertions(+)
 create mode 100644 cython/gtsam/utils/plot.py

diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py
new file mode 100644
index 000000000..556e9d8d5
--- /dev/null
+++ b/cython/gtsam/utils/plot.py
@@ -0,0 +1,76 @@
+"""Various plotting utlities."""
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+def plot_point3_on_axes(axes, point, linespec):
+    """Plot a 3D point on given axis 'axes' with given 'linespec'."""
+    axes.plot([point.x()], [point.y()], [point.z()], linespec)
+
+
+def plot_point3(fignum, point, linespec):
+    """Plot a 3D point on given figure with given 'linespec'."""
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plot_point3_on_axes(axes, point, linespec)
+
+
+def plot_3d_points(fignum, values, linespec, marginals=None):
+    """
+    Plots the Point3s in 'values', with optional covariances.
+    Finds all the Point3 objects in the given Values object and plots them.
+    If a Marginals object is given, this function will also plot marginal
+    covariance ellipses for each point.
+    """
+
+    keys = values.keys()
+
+    # Plot points and covariance matrices
+    for i in range(keys.size()):
+        try:
+            p = values.atPoint3(keys.at(i))
+            # if haveMarginals
+            #     P = marginals.marginalCovariance(key);
+            #     gtsam.plot_point3(p, linespec, P);
+            # else
+            plot_point3(fignum, p, linespec)
+        except RuntimeError:
+            continue
+            # I guess it's not a Point3
+
+
+def plot_pose3_on_axes(axes, pose, axis_length=0.1):
+    """Plot a 3D pose on given axis 'axes' with given 'axis_length'."""
+    # get rotation and translation (center)
+    gRp = pose.rotation().matrix()  # rotation from pose to global
+    t = pose.translation()
+    origin = np.array([t.x(), t.y(), t.z()])
+
+    # draw the camera axes
+    x_axis = origin + gRp[:, 0] * axis_length
+    line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')
+
+    y_axis = origin + gRp[:, 1] * axis_length
+    line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')
+
+    z_axis = origin + gRp[:, 2] * axis_length
+    line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
+
+    # # plot the covariance
+    # if (nargin>2) && (~isempty(P))
+    #     pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
+    #     gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
+    #     gtsam.covarianceEllipse3D(origin,gPp);
+    # end
+
+
+def plot_pose3(fignum, pose, axis_length=0.1):
+    """Plot a 3D pose on given figure with given 'axis_length'."""
+    # get figure object
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plot_pose3_on_axes(axes, pose, axis_length)