Merge with upstream

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/tests/test_Scenario.py

@@ -1,5 +1,20 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for IMU testing scenarios.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
 import math
 import unittest
+
 import numpy as np
 
 import gtsam
@@ -29,7 +44,8 @@ class TestScenario(unittest.TestCase):
         T30 = scenario.pose(T)
         np.testing.assert_almost_equal(
             np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
-        self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
+        self.assertTrue(gtsam.Point3(
+            0, 0, 2.0 * R).equals(T30.translation(), 1e-9))
 
 
 if __name__ == '__main__':

cython/gtsam/tests/test_Values.py

@@ -1,8 +1,21 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for IMU testing scenarios.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
 import unittest
+
 import numpy as np
 
-from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3
-from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias
+from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
+                   Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
+                   imuBias_ConstantBias)
 
 
 class TestValues(unittest.TestCase):
@@ -12,8 +25,8 @@ class TestValues(unittest.TestCase):
         E = EssentialMatrix(Rot3(), Unit3())
         tol = 1e-9
 
-        values.insert(0, Point2(0,0))
-        values.insert(1, Point3(0,0,0))
+        values.insert(0, Point2(0, 0))
+        values.insert(1, Point3(0, 0, 0))
         values.insert(2, Rot2())
         values.insert(3, Pose2())
         values.insert(4, Rot3())
@@ -34,18 +47,19 @@ class TestValues(unittest.TestCase):
         # The wrapper will automatically fix the type and storage order for you,
         # but for performance reasons, it's recommended to specify the correct
         # type and storage order.
-        vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is
+        # for vectors, the order is not important, but dtype still is
+        vec = np.array([1., 2., 3.])
         values.insert(11, vec)
         mat = np.array([[1., 2.], [3., 4.]], order='F')
         values.insert(12, mat)
         # Test with dtype int and the default order='C'
         # This still works as the wrapper converts to the correct type and order for you
         # but is nornally not recommended!
-        mat2 = np.array([[1,2,],[3,5]])
+        mat2 = np.array([[1, 2, ], [3, 5]])
         values.insert(13, mat2)
 
-        self.assertTrue(values.atPoint2(0).equals(Point2(), tol))
-        self.assertTrue(values.atPoint3(1).equals(Point3(), tol))
+        self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol))
+        self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol))
         self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
         self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
         self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
@@ -65,5 +79,6 @@ class TestValues(unittest.TestCase):
         actualMatrix2 = values.atMatrix(13)
         self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
 
+
 if __name__ == "__main__":
     unittest.main()

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):