diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py index 5d2190d56..e778e3f85 100644 --- a/cython/gtsam/examples/OdometryExample.py +++ b/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() + + + diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py index b15b90864..680f2209f 100644 --- a/cython/gtsam/examples/Pose2SLAMExample.py +++ b/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() diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py index 4cca1400b..d68566b25 100644 --- a/cython/gtsam/tests/test_Scenario.py +++ b/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__': diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 08e133840..0bb1e0806 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/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() diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 19402a080..3871fa18c 100644 --- a/cython/gtsam/utils/plot.py +++ b/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):