Merge branch 'develop' into feature/Eigen_3.3.4

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)

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<Pose3>(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<BetweenFactor<imuBias::ConstantBias> >(
             b1, b2, imuBias::ConstantBias(), cov);
         newgraph.add(f);

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: