From 81b4765299b2f08e4dc9fb689607444d54579634 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 20 Mar 2020 12:12:06 -0400
Subject: [PATCH] modernized SFM example and added plotting of trajectory and
 landmarks

---
 cython/gtsam/examples/SFMExample.py | 23 +++++++++++++++--------
 cython/gtsam/examples/SFMdata.py    |  5 +++--
 matlab/+gtsam/covarianceEllipse3D.m |  4 ++--
 3 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py
index bf46f09d5..d4451d44e 100644
--- a/cython/gtsam/examples/SFMExample.py
+++ b/cython/gtsam/examples/SFMExample.py
@@ -10,13 +10,17 @@ A structure-from-motion problem on a simulated dataset
 """
 from __future__ import print_function
 
-import gtsam
+import matplotlib.pyplot as plt
 import numpy as np
+
+import gtsam
 from gtsam.examples import SFMdata
 from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
-                         GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
-                         Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
-                         Rot3, SimpleCamera, Values)
+                         GenericProjectionFactorCal3_S2, Marginals,
+                         NonlinearFactorGraph, Point3, Pose3,
+                         PriorFactorPoint3, PriorFactorPose3, Rot3,
+                         SimpleCamera, Values)
+from gtsam.utils import plot
 
 
 def symbol(name: str, index: int) -> int:
@@ -94,12 +98,10 @@ def main():
     # Intentionally initialize the variables off from the ground truth
     initial_estimate = Values()
     for i, pose in enumerate(poses):
-        r = Rot3.Rodrigues(-0.1, 0.2, 0.25)
-        t = Point3(0.05, -0.10, 0.20)
-        transformed_pose = pose.compose(Pose3(r, t))
+        transformed_pose = pose.retract(0.1*np.random.randn(6,1))
         initial_estimate.insert(symbol('x', i), transformed_pose)
     for j, point in enumerate(points):
-        transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15]))
+        transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
         initial_estimate.insert(symbol('l', j), transformed_point)
     initial_estimate.print_('Initial Estimates:\n')
 
@@ -113,6 +115,11 @@ def main():
     print('initial error = {}'.format(graph.error(initial_estimate)))
     print('final error = {}'.format(graph.error(result)))
 
+    marginals = Marginals(graph, result)
+    plot.plot_3d_points(1, result, marginals=marginals)
+    plot.plot_trajectory(1, result, marginals=marginals, scale=8)
+    plot.set_axes_equal(1)
+    plt.show()
 
 if __name__ == '__main__':
     main()
diff --git a/cython/gtsam/examples/SFMdata.py b/cython/gtsam/examples/SFMdata.py
index 1d5c499fa..c586f7e52 100644
--- a/cython/gtsam/examples/SFMdata.py
+++ b/cython/gtsam/examples/SFMdata.py
@@ -25,14 +25,15 @@ def createPoints():
 
 def createPoses(K):
     # Create the set of ground-truth poses
-    radius = 30.0
+    radius = 40.0
+    height = 10.0
     angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
     up = gtsam.Point3(0, 0, 1)
     target = gtsam.Point3(0, 0, 0)
     poses = []
     for theta in angles:
         position = gtsam.Point3(radius*np.cos(theta),
-                                radius*np.sin(theta), 0.0)
+                                radius*np.sin(theta), height)
         camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
         poses.append(camera.pose())
     return poses
diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m
index 4364e0fe4..51026f698 100644
--- a/matlab/+gtsam/covarianceEllipse3D.m
+++ b/matlab/+gtsam/covarianceEllipse3D.m
@@ -22,8 +22,8 @@ end
 % rotate data with orientation matrix U and center M
 data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc);
 n = size(data,2);
-x = data(1:n,:)+c(1); 
-y = data(n+1:2*n,:)+c(2); 
+x = data(1:n,:)+c(1);
+y = data(n+1:2*n,:)+c(2);
 z = data(2*n+1:end,:)+c(3);
 
 % now plot the rotated ellipse