Merge remote-tracking branch 'origin/develop' into fix/msvc2017

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.h

@@ -2693,7 +2693,7 @@ virtual class Scenario {
 virtual class ConstantTwistScenario : gtsam::Scenario {
   ConstantTwistScenario(Vector w, Vector v);
   ConstantTwistScenario(Vector w, Vector v,
-                        const Pose3& nTb0);
+                        const gtsam::Pose3& nTb0);
 };
 
 virtual class AcceleratingScenario : gtsam::Scenario {

gtsam/3rdparty/Eigen/Eigen/src/Core/AssignEvaluator.h

@@ -39,7 +39,7 @@ public:
   enum {
     DstAlignment = DstEvaluator::Alignment,
     SrcAlignment = SrcEvaluator::Alignment,
-    DstHasDirectAccess = DstFlags & DirectAccessBit,
+    DstHasDirectAccess = (DstFlags & DirectAccessBit) == DirectAccessBit,
     JointAlignment = EIGEN_PLAIN_ENUM_MIN(DstAlignment,SrcAlignment)
   };
 
@@ -83,7 +83,7 @@ private:
                        && int(OuterStride)!=Dynamic && int(OuterStride)%int(InnerPacketSize)==0
                        && (EIGEN_UNALIGNED_VECTORIZE  || int(JointAlignment)>=int(InnerRequiredAlignment)),
     MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit),
-    MayLinearVectorize = bool(MightVectorize) && MayLinearize && DstHasDirectAccess
+    MayLinearVectorize = bool(MightVectorize) && bool(MayLinearize) && bool(DstHasDirectAccess)
                        && (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)) || MaxSizeAtCompileTime == Dynamic),
       /* If the destination isn't aligned, we have to do runtime checks and we don't unroll,
          so it's only good for large enough sizes. */

gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h

@@ -183,8 +183,8 @@ EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,C
     alignmentPattern = AllAligned;
   }
 
-  const Index offset1 = (FirstAligned && alignmentStep==1)?3:1;
-  const Index offset3 = (FirstAligned && alignmentStep==1)?1:3;
+  const Index offset1 = (alignmentPattern==FirstAligned && alignmentStep==1)?3:1;
+  const Index offset3 = (alignmentPattern==FirstAligned && alignmentStep==1)?1:3;
 
   Index columnBound = ((cols-skipColumns)/columnsAtOnce)*columnsAtOnce + skipColumns;
   for (Index i=skipColumns; i<columnBound; i+=columnsAtOnce)
@@ -457,8 +457,8 @@ EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,R
     alignmentPattern = AllAligned;
   }
 
-  const Index offset1 = (FirstAligned && alignmentStep==1)?3:1;
-  const Index offset3 = (FirstAligned && alignmentStep==1)?1:3;
+  const Index offset1 = (alignmentPattern==FirstAligned && alignmentStep==1)?3:1;
+  const Index offset3 = (alignmentPattern==FirstAligned && alignmentStep==1)?1:3;
 
   Index rowBound = ((rows-skipRows)/rowsAtOnce)*rowsAtOnce + skipRows;
   for (Index i=skipRows; i<rowBound; i+=rowsAtOnce)

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:

wrap/tests/expected-cython/geometry.pyx

@@ -155,7 +155,7 @@ cdef class Test:
     def create_ptrs(self):
         cdef pair [shared_ptr[CTest],shared_ptr[CTest]] ret = self.CTest_.get().create_ptrs()
         return (Test.cyCreateFromShared(ret.first),Test.cyCreateFromShared(ret.second))
-    def __str__(self):
+    def __repr__(self):
         strBuf = RedirectCout()
         self.print_('')
         return strBuf.str()