From f4564f444e75bf7405ec666733200379aaff6b61 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Oct 2018 10:47:03 -0400 Subject: [PATCH 1/3] 2D pose plotting --- cython/gtsam/utils/plot.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 3b1897548..19402a080 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -4,6 +4,31 @@ import numpy as np import matplotlib.pyplot as plt +def plot_pose2_on_axes(axes, pose, axis_length=0.1): + """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 + t = pose.translation() + origin = np.array([t.x(), t.y()]) + + # draw the camera axes + x_axis = origin + gRp[:, 0] * axis_length + line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], 'r-') + + y_axis = origin + gRp[:, 1] * axis_length + line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) + axes.plot(line[:, 0], line[:, 1], 'g-') + + +def plot_pose2(fignum, pose, axis_length=0.1): + """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) + + def plot_point3_on_axes(axes, point, linespec): """Plot a 3D point on given axis 'axes' with given 'linespec'.""" axes.plot([point.x()], [point.y()], [point.z()], linespec) From 984028697979e37c7ed9b05c09f37334f442a827 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 22 Oct 2018 11:31:12 +0000 Subject: [PATCH 2/3] Fixed noise model, removed extraneous variable --- examples/ImuFactorExample2.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 1137ca214..b4ad5d574 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -57,9 +57,9 @@ int main(int argc, char* argv[]) { Values initialEstimate, totalEstimate, result; // Add a prior on pose x0. This indirectly specifies where the origin is. - // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z. auto noise = noiseModel::Diagonal::Sigmas( - (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); newgraph.push_back(PriorFactor(X(0), pose_0, noise)); // Add imu priors @@ -102,8 +102,6 @@ int main(int argc, char* argv[]) { Vector6 covvec; covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; auto cov = noiseModel::Diagonal::Variances(covvec); - Vector6 zerovec; - zerovec << 0, 0, 0, 0, 0, 0; auto f = boost::make_shared >( b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); From 2a0cee830d7278bb79aafc330f838f6c934e3878 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 23 Oct 2018 19:55:41 +0000 Subject: [PATCH 3/3] Updated outdated comment --- gtsam/inference/Symbol.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 2d7ede8e7..6b5bfcc03 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -26,10 +26,9 @@ namespace gtsam { /** - * Character and index key used in VectorValues, GaussianFactorGraph, - * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol - * keys when linearizing a nonlinear factor graph. This key is not type - * safe, so cannot be used with any Nonlinear* classes. + * Character and index key used to refer to variables. Will simply cast to a Key, + * i.e., a large integer. Keys are used to retrieve values from Values, + * specify what variables factors depend on, etc. */ class GTSAM_EXPORT Symbol { protected: