From 8fa1acc5535e29b81d2d00efecbf5b6e16d1ed2f Mon Sep 17 00:00:00 2001
From: Ellon Mendes <ellon.mendes@laas.fr>
Date: Tue, 24 Nov 2015 18:06:44 +0100
Subject: [PATCH] Move plot functions to a submodule utils

---
 python/gtsam/__init__.py                    |  1 +
 python/gtsam/examples/VisualISAM2Example.py | 59 +--------------------
 python/gtsam/utils/__init__.py              |  1 +
 python/gtsam/utils/_plot.py                 | 59 +++++++++++++++++++++
 4 files changed, 62 insertions(+), 58 deletions(-)
 create mode 100644 python/gtsam/utils/__init__.py
 create mode 100644 python/gtsam/utils/_plot.py

diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py
index f1b07905b..9ac4cc939 100644
--- a/python/gtsam/__init__.py
+++ b/python/gtsam/__init__.py
@@ -1 +1,2 @@
 from libgtsam_python import *
+import utils
diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py
index bd797d271..06221a303 100644
--- a/python/gtsam/examples/VisualISAM2Example.py
+++ b/python/gtsam/examples/VisualISAM2Example.py
@@ -1,67 +1,10 @@
 import gtsam
 from gtsam.examples.SFMdata import *
+from gtsam.utils import *
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
 import time # for sleep()
 
-def plotPoint3(fignum, point, linespec):
-    fig = plt.figure(fignum)
-    ax = fig.gca(projection='3d')
-    ax.plot([point.x()],[point.y()],[point.z()], linespec)
-
-
-def plot3DPoints(fignum, values, linespec, marginals=None):
-    # PLOT3DPOINTS Plots the Point3's in a 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 key in keys:
-        try:
-            p = values.point3_at(key);
-            # if haveMarginals
-            #     P = marginals.marginalCovariance(key);
-            #     gtsam.plotPoint3(p, linespec, P);
-            # else
-            plotPoint3(fignum, p, linespec);
-        except RuntimeError:
-            continue
-            #I guess it's not a Point3
-
-
-def plotPose3(fignum, pose, axisLength=0.1):
-    # get figure object
-    fig = plt.figure(fignum)
-    ax = fig.gca(projection='3d')
-
-    # get rotation and translation (center)
-    gRp = pose.rotation().matrix()  # rotation from pose to global
-    C = pose.translation().vector()
-
-    # draw the camera axes
-    xAxis = C+gRp[:,0]*axisLength
-    L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0)
-    ax.plot(L[:,0],L[:,1],L[:,2],'r-')
-    
-    yAxis = C+gRp[:,1]*axisLength
-    L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0)
-    ax.plot(L[:,0],L[:,1],L[:,2],'g-')
-    
-    zAxis = C+gRp[:,2]*axisLength
-    L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0)
-    ax.plot(L[:,0],L[:,1],L[:,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(C,gPp);  
-    # end
-        
-
 def visual_ISAM2_plot(poses, points, result):
     # VisualISAMPlot plots current state of ISAM2 object
     # Author: Ellon Paiva
diff --git a/python/gtsam/utils/__init__.py b/python/gtsam/utils/__init__.py
new file mode 100644
index 000000000..eedd7484e
--- /dev/null
+++ b/python/gtsam/utils/__init__.py
@@ -0,0 +1 @@
+from _plot import *
diff --git a/python/gtsam/utils/_plot.py b/python/gtsam/utils/_plot.py
new file mode 100644
index 000000000..f1603bbf5
--- /dev/null
+++ b/python/gtsam/utils/_plot.py
@@ -0,0 +1,59 @@
+import numpy as _np
+import matplotlib.pyplot as _plt
+from mpl_toolkits.mplot3d import Axes3D as _Axes3D
+
+def plotPoint3(fignum, point, linespec):
+    fig = _plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+    ax.plot([point.x()],[point.y()],[point.z()], linespec)
+
+
+def plot3DPoints(fignum, values, linespec, marginals=None):
+    # PLOT3DPOINTS Plots the Point3's in a 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 key in keys:
+        try:
+            p = values.point3_at(key);
+            # if haveMarginals
+            #     P = marginals.marginalCovariance(key);
+            #     gtsam.plotPoint3(p, linespec, P);
+            # else
+            plotPoint3(fignum, p, linespec);
+        except RuntimeError:
+            continue
+            #I guess it's not a Point3
+
+def plotPose3(fignum, pose, axisLength=0.1):
+    # get figure object
+    fig = _plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+
+    # get rotation and translation (center)
+    gRp = pose.rotation().matrix()  # rotation from pose to global
+    C = pose.translation().vector()
+
+    # draw the camera axes
+    xAxis = C+gRp[:,0]*axisLength
+    L = _np.append(C[_np.newaxis], xAxis[_np.newaxis], axis=0)
+    ax.plot(L[:,0],L[:,1],L[:,2],'r-')
+    
+    yAxis = C+gRp[:,1]*axisLength
+    L = _np.append(C[_np.newaxis], yAxis[_np.newaxis], axis=0)
+    ax.plot(L[:,0],L[:,1],L[:,2],'g-')
+    
+    zAxis = C+gRp[:,2]*axisLength
+    L = _np.append(C[_np.newaxis], zAxis[_np.newaxis], axis=0)
+    ax.plot(L[:,0],L[:,1],L[:,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(C,gPp);  
+    # end
\ No newline at end of file