Add ploting to VisualISAM2Example.py

python/gtsam/examples/VisualISAM2Example.py

@@ -1,5 +1,100 @@
 import gtsam
 from gtsam.examples.SFMdata 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
+    # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
+
+    # Declare an id for the figure
+    fignum = 0;
+
+    fig = plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+    plt.cla()
+
+    # Plot points
+    # Can't use data because current frame might not see all points
+    # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); # TODO - this is slow
+    # gtsam.plot3DPoints(result, [], marginals);
+    plot3DPoints(fignum, result, 'rx');
+
+    # Plot cameras
+    M = 0;
+    while result.exists(int(gtsam.Symbol('x',M))):
+        ii = int(gtsam.Symbol('x',M));
+        pose_i = result.pose3_at(ii);
+        plotPose3(fignum, pose_i, 10);
+        
+        M = M + 1;
+
+    # draw
+    ax.set_xlim3d(-40, 40)
+    ax.set_ylim3d(-40, 40)
+    ax.set_zlim3d(-40, 40)
+    plt.show(block=False)
+    plt.draw();
 
 def visual_ISAM2_example():
     # Define the camera calibration parameters
@@ -76,7 +171,8 @@ def visual_ISAM2_example():
                 print gtsam.Symbol('l',j)
                 print currentEstimate.point3_at(int(gtsam.Symbol('l',j)))
 
-            # TODO: Print to screen or plot using matplotlib
+            visual_ISAM2_plot(poses, points, currentEstimate);
+            time.sleep(1)
 
             # Clear the factor graph and values for the next iteration
             graph.resize(0);