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