From 9f30d225fe65c63136e3f6d17e82714cb59fe259 Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Tue, 16 May 2017 14:15:01 -0700
Subject: [PATCH 001/281] fixing compile issues on vc++14

---
 CMakeLists.txt                         |  2 +-
 gtsam/discrete/DiscreteConditional.cpp | 24 +++++++++++++-----------
 2 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 77434d135..9aa8b701a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -116,7 +116,7 @@ if(MSVC)
 	endif()
 endif()
 
-find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono)
+find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
 
 # Required components
 if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 4a918ef02..f12cd2567 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -87,17 +87,19 @@ bool DiscreteConditional::equals(const DiscreteFactor& other,
 Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
   ADT pFS(*this);
   Key j; size_t value;
-  for(Key key: parents())
-  try {
-    j = (key);
-    value = parentsValues.at(j);
-    pFS = pFS.choose(j, value);
-  } catch (exception&) {
-    cout << "Key: " << j << "  Value: " << value << endl;
-    parentsValues.print("parentsValues: ");
-//    pFS.print("pFS: ");
-    throw runtime_error("DiscreteConditional::choose: parent value missing");
-  };
+  for(Key key: parents()) {
+    try {
+      j = (key);
+      value = parentsValues.at(j);
+      pFS = pFS.choose(j, value);
+    } catch (exception&) {
+      cout << "Key: " << j << "  Value: " << value << endl;
+      parentsValues.print("parentsValues: ");
+  //    pFS.print("pFS: ");
+      throw runtime_error("DiscreteConditional::choose: parent value missing");
+    };
+  }
+
   return pFS;
 }
 

From 6a5beab4d48f3bf479dfab5291433d100033ae1a Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Tue, 16 May 2017 17:42:30 -0700
Subject: [PATCH 002/281] fixing compile issue on vc++14

---
 gtsam/base/GenericValue.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 5b59f4872..bf186470e 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -167,10 +167,10 @@ public:
     // implicit assignment operator for (const GenericValue& rhs) works fine here
     /// Assignment operator, protected because only the Value or DERIVED
     /// assignment operators should be used.
-    //  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
-    //    // Nothing to do, do not call base class assignment operator
-    //    return *this;
-    //  }
+    GenericValue<T>& operator=(const GenericValue<T>& rhs) {
+      // Nothing to do, do not call base class assignment operator
+      return *this;
+    }
 
   private:
 

From 3f98942e9a5cf4dbefd358dd6ee7191d79e92f6c Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Wed, 17 May 2017 14:30:17 -0700
Subject: [PATCH 003/281] a few tmp fix to bypass eigen errors, should not be
 permanent solutions

---
 CMakeLists.txt                                        | 3 +++
 gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h | 2 +-
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9aa8b701a..6db1f6681 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -251,6 +251,9 @@ else()
 
 endif()
 
+# tmp mute eigen static assert to avoid errors in shared lib
+add_definitions(-DEIGEN_NO_STATIC_ASSERT)
+
 ###############################################################################
 # Global compile options
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
index 9f71956ff..be2c67c5f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
@@ -380,7 +380,7 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
       */
     EIGEN_STRONG_INLINE void conservativeResize(Index size)
     {
-      internal::conservative_resize_like_impl<Derived>::run(*this, size);
+      internal::conservative_resize_like_impl<Derived>::run(*this, size, 1);
     }
 
     /** Resizes the matrix to \a rows x \a cols of \c other, while leaving old values untouched.

From 89ca6fb92542a92e1d50d4ac5cb5b1b159c8fba8 Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Thu, 18 May 2017 11:39:51 -0700
Subject: [PATCH 004/281] fixing windows compile issues

---
 gtsam/geometry/SimpleCamera.h             | 2 +-
 gtsam/navigation/ImuBias.h                | 2 +-
 gtsam/navigation/ImuFactor.h              | 6 +++---
 gtsam/navigation/ManifoldPreintegration.h | 2 +-
 gtsam/navigation/NavState.h               | 2 +-
 gtsam/navigation/PreintegratedRotation.h  | 4 ++--
 gtsam/navigation/PreintegrationBase.h     | 2 +-
 gtsam/navigation/PreintegrationParams.h   | 2 +-
 gtsam/navigation/TangentPreintegration.h  | 2 +-
 9 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h
index fe493c075..a7e021394 100644
--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@@ -31,7 +31,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
  * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
  * Use PinholeCameraCal3_S2 instead
  */
-class SimpleCamera : public PinholeCameraCal3_S2 {
+class GTSAM_EXPORT SimpleCamera : public PinholeCameraCal3_S2 {
 
   typedef PinholeCamera<Cal3_S2> Base;
   typedef boost::shared_ptr<SimpleCamera> shared_ptr;
diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index 2ad3c17dd..84458f521 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -27,7 +27,7 @@ namespace gtsam {
 /// All bias models live in the imuBias namespace
 namespace imuBias {
 
-class ConstantBias {
+class GTSAM_EXPORT ConstantBias {
 private:
   Vector3 biasAcc_;
   Vector3 biasGyro_;
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 532abdac0..e713fcb99 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -68,7 +68,7 @@ typedef ManifoldPreintegration PreintegrationType;
  *
  * @addtogroup SLAM
  */
-class PreintegratedImuMeasurements: public PreintegrationType {
+class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
 
   friend class ImuFactor;
   friend class ImuFactor2;
@@ -176,7 +176,7 @@ private:
  *
  * @addtogroup SLAM
  */
-class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
+class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
     imuBias::ConstantBias> {
 private:
 
@@ -285,7 +285,7 @@ private:
  * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
  * @addtogroup SLAM
  */
-class ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
+class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3<NavState, NavState, imuBias::ConstantBias> {
 private:
 
   typedef ImuFactor2 This;
diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h
index 92d3f4814..22897b9d4 100644
--- a/gtsam/navigation/ManifoldPreintegration.h
+++ b/gtsam/navigation/ManifoldPreintegration.h
@@ -30,7 +30,7 @@ namespace gtsam {
  * IMU pre-integration on NavSatet manifold.
  * This corresponds to the original RSS paper (with one difference: V is rotated)
  */
-class ManifoldPreintegration : public PreintegrationBase {
+class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
  protected:
 
   /**
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index a1544735d..e9afcd3ac 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -31,7 +31,7 @@ typedef Vector3 Velocity3;
  * Navigation state: Pose (rotation, translation) + velocity
  * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
  */
-class NavState {
+class GTSAM_EXPORT NavState {
 private:
 
   // TODO(frank):
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 88d9c6437..c03cab88a 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -28,7 +28,7 @@ namespace gtsam {
 
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
-struct PreintegratedRotationParams {
+struct GTSAM_EXPORT PreintegratedRotationParams {
   Matrix3 gyroscopeCovariance;  ///< continuous-time "Covariance" of gyroscope measurements
   boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
   boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
@@ -63,7 +63,7 @@ struct PreintegratedRotationParams {
  * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
  * It includes the definitions of the preintegrated rotation.
  */
-class PreintegratedRotation {
+class GTSAM_EXPORT PreintegratedRotation {
  public:
   typedef PreintegratedRotationParams Params;
 
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 06be4642d..cf5465c05 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -55,7 +55,7 @@ struct PoseVelocityBias {
  * It includes the definitions of the preintegrated variables and the methods
  * to access, print, and compare them.
  */
-class PreintegrationBase {
+class GTSAM_EXPORT PreintegrationBase {
  public:
   typedef imuBias::ConstantBias Bias;
   typedef PreintegrationParams Params;
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index f2b2c0fef..831da0a12 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -23,7 +23,7 @@ namespace gtsam {
 
 /// Parameters for pre-integration:
 /// Usage: Create just a single Params and pass a shared pointer to the constructor
-struct PreintegrationParams: PreintegratedRotationParams {
+struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
   Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
   Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
   bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h
index dddafad7a..f5e3f7960 100644
--- a/gtsam/navigation/TangentPreintegration.h
+++ b/gtsam/navigation/TangentPreintegration.h
@@ -25,7 +25,7 @@ namespace gtsam {
  * Integrate on the 9D tangent space of the NavState manifold.
  * See extensive discussion in ImuFactor.lyx
  */
-class TangentPreintegration : public PreintegrationBase {
+class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase {
  protected:
 
   /**

From ed4a99f6209efe58accd8ba2139cc9145ceb1143 Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Thu, 18 May 2017 11:56:52 -0700
Subject: [PATCH 005/281] fixing vc++14 compile issues

---
 gtsam/geometry/PinholeCamera.h | 2 +-
 gtsam/geometry/PinholePose.h   | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index a5707ce89..e36630104 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -30,7 +30,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename Calibration>
-class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
+class PinholeCamera: public PinholeBaseK<Calibration> {
 
 public:
 
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index 43ba78ea2..4db1ee8c3 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -31,7 +31,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename CALIBRATION>
-class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
+class PinholeBaseK: public PinholeBase {
 
 private:
 
@@ -222,7 +222,7 @@ private:
  * \nosubgrouping
  */
 template<typename CALIBRATION>
-class GTSAM_EXPORT PinholePose: public PinholeBaseK<CALIBRATION> {
+class PinholePose: public PinholeBaseK<CALIBRATION> {
 
 private:
 

From 6b3608cf9ae954d71c2af349bc080239a674b5a7 Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Thu, 18 May 2017 12:32:29 -0700
Subject: [PATCH 006/281] fixing vc++14 compile issues

---
 gtsam/geometry/CameraSet.h     | 12 ++++++------
 gtsam/geometry/Point3.h        | 24 ++++++++++++------------
 gtsam/geometry/triangulation.h |  8 ++++----
 gtsam/slam/SmartFactorParams.h |  2 +-
 4 files changed, 23 insertions(+), 23 deletions(-)

diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 18f311a23..026bcdd9e 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -209,11 +209,11 @@ public:
       bool diagonalDamping = false) {
     if (E.cols() == 2) {
       Matrix2 P2;
-      ComputePointCovariance(P2, E, lambda, diagonalDamping);
+      ComputePointCovariance<2>(P2, E, lambda, diagonalDamping);
       return P2;
     } else {
       Matrix3 P3;
-      ComputePointCovariance(P3, E, lambda, diagonalDamping);
+      ComputePointCovariance<3>(P3, E, lambda, diagonalDamping);
       return P3;
     }
   }
@@ -227,12 +227,12 @@ public:
       bool diagonalDamping = false) {
     if (E.cols() == 2) {
       Matrix2 P;
-      ComputePointCovariance(P, E, lambda, diagonalDamping);
-      return SchurComplement(Fblocks, E, P, b);
+      ComputePointCovariance<2>(P, E, lambda, diagonalDamping);
+      return SchurComplement<2>(Fblocks, E, P, b);
     } else {
       Matrix3 P;
-      ComputePointCovariance(P, E, lambda, diagonalDamping);
-      return SchurComplement(Fblocks, E, P, b);
+      ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
+      return SchurComplement<3>(Fblocks, E, P, b);
     }
   }
 
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 99cb6c2e7..215161b3a 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -157,28 +157,28 @@ struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 
 // Convenience typedef
 typedef std::pair<Point3, Point3> Point3Pair;
-std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
+GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
 
 /// distance between two points
-double distance3(const Point3& p1, const Point3& q,
-                 OptionalJacobian<1, 3> H1 = boost::none,
-                 OptionalJacobian<1, 3> H2 = boost::none);
+GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
+	                          OptionalJacobian<1, 3> H1 = boost::none,
+                              OptionalJacobian<1, 3> H2 = boost::none);
 
 /// Distance of the point from the origin, with Jacobian
-double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
+GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
 
 /// normalize, with optional Jacobian
-Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
+GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
 
 /// cross product @return this x q
-Point3 cross(const Point3& p, const Point3& q,
-             OptionalJacobian<3, 3> H_p = boost::none,
-             OptionalJacobian<3, 3> H_q = boost::none);
+GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
+                          OptionalJacobian<3, 3> H_p = boost::none,
+                          OptionalJacobian<3, 3> H_q = boost::none);
 
 /// dot product
-double dot(const Point3& p, const Point3& q,
-           OptionalJacobian<1, 3> H_p = boost::none,
-           OptionalJacobian<1, 3> H_q = boost::none);
+GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
+                        OptionalJacobian<1, 3> H_p = boost::none,
+                        OptionalJacobian<1, 3> H_q = boost::none);
 
 template <typename A1, typename A2>
 struct Range;
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index fdfe32e8b..e49e93d5a 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -28,7 +28,7 @@
 namespace gtsam {
 
 /// Exception thrown by triangulateDLT when SVD returns rank < 3
-class TriangulationUnderconstrainedException: public std::runtime_error {
+class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
 public:
   TriangulationUnderconstrainedException() :
       std::runtime_error("Triangulation Underconstrained Exception.") {
@@ -36,7 +36,7 @@ public:
 };
 
 /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
-class TriangulationCheiralityException: public std::runtime_error {
+class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
 public:
   TriangulationCheiralityException() :
       std::runtime_error(
@@ -319,7 +319,7 @@ Point3 triangulatePoint3(
   (cameras, measurements, rank_tol, optimize);
 }
 
-struct TriangulationParameters {
+struct GTSAM_EXPORT TriangulationParameters {
 
   double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
   ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance)
@@ -382,7 +382,7 @@ private:
 /**
  * TriangulationResult is an optional point, along with the reasons why it is invalid.
  */
-class TriangulationResult: public boost::optional<Point3> {
+class GTSAM_EXPORT TriangulationResult: public boost::optional<Point3> {
   enum Status {
     VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
   };
diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h
index 761703f8b..e8a1fa7ab 100644
--- a/gtsam/slam/SmartFactorParams.h
+++ b/gtsam/slam/SmartFactorParams.h
@@ -39,7 +39,7 @@ enum DegeneracyMode {
 /*
  *  Parameters for the smart (stereo) projection factors
  */
-struct GTSAM_EXPORT SmartProjectionParams {
+struct SmartProjectionParams {
 
   LinearizationMode linearizationMode; ///< How to linearize the factor
   DegeneracyMode degeneracyMode; ///< How to linearize the factor

From eb1e75fd27c72cc2ed42ae64c8fa1e05194ac20a Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Thu, 18 May 2017 14:46:46 -0700
Subject: [PATCH 007/281] fixing vc++14 compile issues

---
 gtsam/base/deprecated/LieMatrix.h           |  4 ++--
 gtsam/base/deprecated/LieVector.h           |  6 +++---
 gtsam/geometry/Cal3_S2Stereo.h              |  2 +-
 gtsam/geometry/Point2.h                     | 14 +++++++-------
 gtsam/geometry/SO3.h                        |  4 ++--
 gtsam/geometry/tests/testCameraSet.cpp      |  6 +++---
 gtsam/geometry/tests/testOrientedPlane3.cpp |  2 --
 7 files changed, 18 insertions(+), 20 deletions(-)

diff --git a/gtsam/base/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h
index e26f45511..9ee20a596 100644
--- a/gtsam/base/deprecated/LieMatrix.h
+++ b/gtsam/base/deprecated/LieMatrix.h
@@ -29,7 +29,7 @@ namespace gtsam {
  * we can directly add double, Vector, and Matrix into values now, because of
  * gtsam::traits.
  */
-struct LieMatrix : public Matrix {
+struct GTSAM_EXPORT LieMatrix : public Matrix {
 
   /// @name Constructors
   /// @{
@@ -60,7 +60,7 @@ struct LieMatrix : public Matrix {
   /// @{
 
   /** print @param s optional string naming the object */
-  GTSAM_EXPORT void print(const std::string& name = "") const {
+  void print(const std::string& name = "") const {
     gtsam::print(matrix(), name);
   }
   /** equality up to tolerance */
diff --git a/gtsam/base/deprecated/LieVector.h b/gtsam/base/deprecated/LieVector.h
index 4a85036e0..b271275c3 100644
--- a/gtsam/base/deprecated/LieVector.h
+++ b/gtsam/base/deprecated/LieVector.h
@@ -27,7 +27,7 @@ namespace gtsam {
  * we can directly add double, Vector, and Matrix into values now, because of
  * gtsam::traits.
  */
-struct LieVector : public Vector {
+struct GTSAM_EXPORT LieVector : public Vector {
 
   enum { dimension = Eigen::Dynamic };
 
@@ -51,13 +51,13 @@ struct LieVector : public Vector {
   LieVector(double d) : Vector((Vector(1) << d).finished()) {}
 
   /** constructor with size and initial data, row order ! */
-  GTSAM_EXPORT LieVector(size_t m, const double* const data) : Vector(m) {
+  LieVector(size_t m, const double* const data) : Vector(m) {
     for (size_t i = 0; i < m; i++) (*this)(i) = data[i];
   }
 
   /// @name Testable
   /// @{
-  GTSAM_EXPORT void print(const std::string& name="") const {
+  void print(const std::string& name="") const {
     gtsam::print(vector(), name);
   }
   bool equals(const LieVector& expected, double tol=1e-5) const {
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index db49448ec..3a0f56c30 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -27,7 +27,7 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class Cal3_S2Stereo {
+  class GTSAM_EXPORT Cal3_S2Stereo {
   private:
 
     Cal3_S2 K_;
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index fb250df6d..cfd7b4500 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -152,23 +152,23 @@ struct traits<Point2> : public internal::VectorSpace<Point2> {
 #endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
 
 /// Distance of the point from the origin, with Jacobian
-double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
+GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
 
 /// distance between two points
-double distance2(const Point2& p1, const Point2& q,
+GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
                  OptionalJacobian<1, 2> H1 = boost::none,
                  OptionalJacobian<1, 2> H2 = boost::none);
 
 // Convenience typedef
 typedef std::pair<Point2, Point2> Point2Pair;
-std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
+GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
 
 // For MATLAB wrapper
 typedef std::vector<Point2> Point2Vector;
 
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {
-return p * s;
+  return p * s;
 }
 
 /*
@@ -185,7 +185,7 @@ return p * s;
  * @param tol: absolute tolerance below which we consider touching circles
  * @return optional Point2 with f and h, boost::none if no solution.
  */
-boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
+GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
 
 /*
  * @brief Circle-circle intersection, from the normalized radii solution.
@@ -193,7 +193,7 @@ boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double
  * @param c2 center of second circle
  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
  */
-std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
+GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
 
 /**
  * @brief Intersect 2 circles
@@ -204,7 +204,7 @@ std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional
  * @param tol: absolute tolerance below which we consider touching circles
  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
  */
-std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
+GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
     Point2 c2, double r2, double tol = 1e-9);
 
 } // \ namespace gtsam
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index 53f2c2130..e9d257acb 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -135,7 +135,7 @@ public:
 namespace so3 {
 
 /// Functor implementing Exponential map
-class ExpmapFunctor {
+class GTSAM_EXPORT ExpmapFunctor {
  protected:
   const double theta2;
   Matrix3 W, K, KK;
@@ -156,7 +156,7 @@ class ExpmapFunctor {
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class DexpFunctor : public ExpmapFunctor {
+class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
   double a, b;
   Matrix3 dexp_;
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index 01f784ae0..de7fe4625 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -89,7 +89,7 @@ TEST(CameraSet, Pinhole) {
   Vector4 b = actualV;
   Vector v = Ft * (b - E * P * Et * b);
   schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30;
-  SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b);
+  SymmetricBlockMatrix actualReduced = Set::SchurComplement<3>(Fs, E, P, b);
   EXPECT(assert_equal(schur, actualReduced.selfadjointView()));
 
   // Check Schur complement update, same order, should just double
@@ -98,14 +98,14 @@ TEST(CameraSet, Pinhole) {
   allKeys.push_back(2);
   keys.push_back(1);
   keys.push_back(2);
-  Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced);
+  Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys, actualReduced);
   EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
 
   // Check Schur complement update, keys reversed
   FastVector<Key> keys2;
   keys2.push_back(2);
   keys2.push_back(1);
-  Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced);
+  Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys2, actualReduced);
   Vector4 reverse_b;
   reverse_b << b.tail<2>(), b.head<2>();
   Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b);
diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp
index b3d87f18c..a1531e07c 100644
--- a/gtsam/geometry/tests/testOrientedPlane3.cpp
+++ b/gtsam/geometry/tests/testOrientedPlane3.cpp
@@ -120,8 +120,6 @@ TEST(OrientedPlane3, localCoordinates_retract) {
   maxXiLimit << M_PI, M_PI, 10.0;
   for (size_t i = 0; i < numIterations; i++) {
 
-    sleep(0);
-
     // Create a Plane
     OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit));
     Vector v12 = randomVector(minXiLimit, maxXiLimit);

From 0f80f9bf41e30103907b012374305915eaedbce4 Mon Sep 17 00:00:00 2001
From: Jing Dong <thu.dongjing@gmail.com>
Date: Fri, 19 May 2017 18:51:14 -0700
Subject: [PATCH 008/281] static lib and examples compiles

---
 CMakeLists.txt       |  2 +-
 cmake/dllexport.h.in | 29 ++++++++++++++++++-----------
 gtsam/CMakeLists.txt |  4 ++--
 3 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6db1f6681..115c8bb49 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -128,7 +128,7 @@ option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
 # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
 set(GTSAM_BOOST_LIBRARIES
     ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
-	${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY})
+	${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
     add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
diff --git a/cmake/dllexport.h.in b/cmake/dllexport.h.in
index 023f06f57..6539e869e 100644
--- a/cmake/dllexport.h.in
+++ b/cmake/dllexport.h.in
@@ -26,21 +26,28 @@
 //   class __declspec(dllexport) MyClass { ... };
 // When included while compiling other code against GTSAM:
 //   class __declspec(dllimport) MyClass { ... };
+
+#pragma once
+
+// Whether GTSAM is compiled as static or DLL in windows. 
+// This will be used to decide whether include __declspec(dllimport) or not in headers
+// TODO: replace GTSAM by @library_name@
+#cmakedefine GTSAM_BUILD_STATIC_LIBRARY
+
 #ifdef _WIN32
-#  ifdef @library_name@_EXPORTS
-#    define @library_name@_EXPORT __declspec(dllexport)
-#    define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
-#  else
-#    ifndef @library_name@_IMPORT_STATIC
+#  ifdef @library_name@_BUILD_STATIC_LIBRARY
+#    define @library_name@_EXPORT
+#    define @library_name@_EXTERN_EXPORT extern
+#  else /* @library_name@_BUILD_STATIC_LIBRARY */
+#    ifdef @library_name@_EXPORTS
+#      define @library_name@_EXPORT __declspec(dllexport)
+#      define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
+#    else /* @library_name@_EXPORTS */
 #      define @library_name@_EXPORT __declspec(dllimport)
 #      define @library_name@_EXTERN_EXPORT __declspec(dllimport)
-#    else /* @library_name@_IMPORT_STATIC */
-#      define @library_name@_EXPORT
-#      define @library_name@_EXTERN_EXPORT extern
-#    endif /* @library_name@_IMPORT_STATIC */
-#  endif /* @library_name@_EXPORTS */
+#    endif /* @library_name@_EXPORTS */
+#  endif /* @library_name@_BUILD_STATIC_LIBRARY */
 #else /* _WIN32 */
 #  define @library_name@_EXPORT
 #  define @library_name@_EXTERN_EXPORT extern
 #endif
-
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 8c1d8bb43..7cf801e74 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -110,8 +110,8 @@ if (GTSAM_BUILD_STATIC_LIBRARY)
      	SOVERSION           ${gtsam_soversion})
 	if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
 		set_target_properties(gtsam PROPERTIES
-			PREFIX "lib"
-			COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
+			PREFIX "lib")
+			#COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
 	endif()
     install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
     list(APPEND GTSAM_EXPORTED_TARGETS gtsam)

From bf0c9dccaa739542d1ec25392d044579e755bd1c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Mon, 22 Oct 2018 19:10:52 -0400
Subject: [PATCH 009/281] cmake changes

---
 CMakeLists.txt                      | 3 +++
 gtsam/3rdparty/metis/CMakeLists.txt | 2 +-
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 115c8bb49..b3e038efe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -108,6 +108,7 @@ set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
 
 # If using Boost shared libs, disable auto linking
 if(MSVC)
+	set(Boost_USE_STATIC_LIBS        ON)  # only find static libs
 	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
 	# Disable autolinking
 	if(NOT Boost_USE_STATIC_LIBS)
@@ -319,6 +320,8 @@ include_directories(BEFORE
 if(MSVC)
 	add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
 	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
+	add_definitions(/wd4244) # Disable loss of precision which is thrown all over our Eigen
+	add_definitions(/bigobj) # Allow large object files for template-based code
 endif()
 
 # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt
index dd21338a4..f864e02ac 100644
--- a/gtsam/3rdparty/metis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/CMakeLists.txt
@@ -23,7 +23,7 @@ set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib")
 set(METIS_SHARED TRUE CACHE BOOL "build a shared library")
 
 if(MSVC)
-  set(METIS_INSTALL FALSE)
+  set(METIS_INSTALL TRUE)
 else()
   set(METIS_INSTALL FALSE)
 endif()

From 1c8578d52f6ebf7a50e79d253b4da6b1255acbd5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 23 Oct 2018 12:18:25 -0400
Subject: [PATCH 010/281] Ignore MSVC 2017 files

---
 .gitignore | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.gitignore b/.gitignore
index 2682a6748..42bd27466 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,3 +17,5 @@ cython/gtsam.so
 cython/gtsam_wrapper.pxd
 .vscode
 .env
+/.vs/
+/CMakeSettings.json

From 846a2619df1c3271bfab0afe996d479ae1fae296 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 23 Oct 2018 22:50:30 -0400
Subject: [PATCH 011/281] Don't export because has templated methods. Might
 need to revisit and export individual non-templated methods.

---
 gtsam/geometry/SO3.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index e9d257acb..3b27d45c5 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -33,7 +33,7 @@ namespace gtsam {
  *  We guarantee (all but first) constructors only generate from sub-manifold.
  *  However, round-off errors in repeated composition could move off it...
  */
-class GTSAM_EXPORT SO3: public Matrix3, public LieGroup<SO3, 3> {
+class SO3: public Matrix3, public LieGroup<SO3, 3> {
 
 protected:
 

From 9b7f80f25cf69cde11de6eeda8093f969bcc6247 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 23 Oct 2018 22:51:44 -0400
Subject: [PATCH 012/281] GenericValue stores a T. So, it potentially has to be
 aligned. We check this requirement and call Eigen's aligning operator if so.

---
 gtsam/base/GenericValue.h | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index f98b3669e..1b59b6c1d 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -187,7 +187,14 @@ public:
       ar & boost::serialization::make_nvp("GenericValue",
               boost::serialization::base_object<Value>(*this));
       ar & boost::serialization::make_nvp("value", value_);
-    }
+
+	 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	 enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+
+  public:
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+	}
 
 /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues
 #define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue<Type>)

From a7678698d37abe33ab83c842e3f1fa84d6fda209 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 23 Oct 2018 23:20:17 -0400
Subject: [PATCH 013/281] Fixed some warnings

---
 gtsam/linear/HessianFactor.cpp                     | 2 +-
 gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp    | 2 +-
 gtsam/nonlinear/internal/LevenbergMarquardtState.h | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 29b2b8591..428fb6cde 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -486,7 +486,7 @@ boost::shared_ptr<GaussianConditional> HessianFactor::eliminateCholesky(const Or
 
     // Erase the eliminated keys in this factor
     keys_.erase(begin(), begin() + nFrontals);
-  } catch (const CholeskyFailed& e) {
+  } catch (const CholeskyFailed&) {
     throw IndeterminantLinearSystemException(keys.front());
   }
 
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 5f29c3bdf..b9579661d 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -148,7 +148,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
     // ============ Solve is where most computation happens !! =================
     delta = solve(dampedSystem, params_);
     systemSolvedSuccessfully = true;
-  } catch (const IndeterminantLinearSystemException& e) {
+  } catch (const IndeterminantLinearSystemException&) {
     systemSolvedSuccessfully = false;
   }
 
diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h
index bd5465dda..8ab2e7466 100644
--- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h
+++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h
@@ -147,7 +147,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
         CachedModel* item = getCachedModel(dim);
         item->A.diagonal() = sqrtHessianDiagonal.at(key);  // use diag(hessian)
         damped += boost::make_shared<JacobianFactor>(key, item->A, item->b, item->model);
-      } catch (const std::out_of_range& e) {
+      } catch (const std::out_of_range&) {
         continue;  // Don't attempt any damping if no key found in diagonal
       }
     }

From 1c2cd7045e9852aadd5689354c4d8384fbf7f063 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 23 Oct 2018 23:37:36 -0400
Subject: [PATCH 014/281] Fixed alignment warnings on windows by adding
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

---
 gtsam/geometry/BearingRange.h          | 3 +++
 gtsam/geometry/OrientedPlane3.h        | 3 +++
 gtsam/navigation/AttitudeFactor.h      | 6 ++++++
 gtsam/slam/BetweenFactor.h             | 3 +++
 gtsam/slam/EssentialMatrixConstraint.h | 3 +++
 gtsam/slam/PriorFactor.h               | 3 +++
 6 files changed, 21 insertions(+)

diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index b1e003864..33a799c1a 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -94,6 +94,9 @@ struct BearingRange
   }
 
   friend class boost::serialization::access;
+
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // Declare this to be both Testable and a Manifold
diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h
index e425a4b81..4bca8d138 100644
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@@ -156,6 +156,9 @@ public:
   }
 
   /// @}
+
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<> struct traits<OrientedPlane3> : public internal::Manifold<
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 21f74ac06..4ae6078e9 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -129,6 +129,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(nZ_);
     ar & BOOST_SERIALIZATION_NVP(bRef_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
@@ -213,6 +216,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(nZ_);
     ar & BOOST_SERIALIZATION_NVP(bRef_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index f3fd49fa7..7af43e986 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -122,6 +122,9 @@ namespace gtsam {
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(measured_);
     }
+  
+  public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   }; // \class BetweenFactor
 
   /// traits
diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h
index 92a78279b..9aa8f8b83 100644
--- a/gtsam/slam/EssentialMatrixConstraint.h
+++ b/gtsam/slam/EssentialMatrixConstraint.h
@@ -103,6 +103,9 @@ private:
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measuredE_);
   }
+
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // \class EssentialMatrixConstraint
 
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 9a3a4a47a..c8ed378d8 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -104,6 +104,9 @@ namespace gtsam {
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(prior_);
     }
+  
+  public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   };
 
 } /// namespace gtsam

From e493fc0718c84546f78e804b20008bb57bcc57f5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 28 Oct 2018 17:38:22 -0400
Subject: [PATCH 015/281] Added an example to show how GTSAM can be used to
 model planar manipulator arms.

---
 .../examples/PlanarManipulatorExample.py      | 325 ++++++++++++++++++
 1 file changed, 325 insertions(+)
 create mode 100644 cython/gtsam/examples/PlanarManipulatorExample.py

diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py
new file mode 100644
index 000000000..af21e6ca5
--- /dev/null
+++ b/cython/gtsam/examples/PlanarManipulatorExample.py
@@ -0,0 +1,325 @@
+"""
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import math
+import unittest
+from functools import reduce
+
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
+
+import gtsam
+import gtsam.utils.plot as gtsam_plot
+from gtsam import Pose2
+
+
+def vector3(x, y, z):
+    """Create 3D double numpy array."""
+    return np.array([x, y, z], dtype=np.float)
+
+
+def compose(*poses):
+    """Compose all Pose2 transforms given as arguments from left to right."""
+    return reduce((lambda x, y: x.compose(y)), poses)
+
+
+def vee(M):
+    """Pose2 vee operator."""
+    return vector3(M[0, 2], M[1, 2], M[1, 0])
+
+
+def delta(g0, g1):
+    """Difference between x,y,,theta components of SE(2) poses."""
+    return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
+
+
+def trajectory(g0, g1, N=20):
+    """ Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
+        g0 and g1 are the initial and final pose, respectively.
+        N is the number of *intervals*
+        Returns N+1 poses
+    """
+    e = delta(g0, g1)
+    return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
+
+
+class ThreeLinkArm(object):
+    """Three-link arm class."""
+
+    def __init__(self):
+        self.L1 = 3.5
+        self.L2 = 3.5
+        self.L3 = 2.5
+        self.xi1 = vector3(0, 0, 1)
+        self.xi2 = vector3(self.L1, 0, 1)
+        self.xi3 = vector3(self.L1+self.L2, 0, 1)
+        self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
+
+    def fk(self, q):
+        """ Forward kinematics.
+            Takes numpy array of joint angles, in radians.
+        """
+        sXl1 = Pose2(0, 0, math.radians(90))
+        l1Zl1 = Pose2(0, 0, q[0])
+        l1Xl2 = Pose2(self.L1, 0, 0)
+        l2Zl2 = Pose2(0, 0, q[1])
+        l2Xl3 = Pose2(self.L2, 0, 0)
+        l3Zl3 = Pose2(0, 0, q[2])
+        l3Xt = Pose2(self.L3, 0, 0)
+        return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
+
+    def jacobian(self, q):
+        """ Calculate manipulator Jacobian.
+            Takes numpy array of joint angles, in radians.
+        """
+        a = q[0]+q[1]
+        b = a+q[2]
+        return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
+                          -self.L1*math.cos(a)-self.L3*math.cos(b),
+                          - self.L3*math.cos(b)],
+                         [-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
+                          -self.L1*math.sin(a)-self.L3*math.sin(b),
+                          - self.L3*math.sin(b)],
+                         [1, 1, 1]], np.float)
+
+    def poe(self, q):
+        """ Forward kinematics.
+            Takes numpy array of joint angles, in radians.
+        """
+        l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
+        l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
+        l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
+        return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
+
+    def con(self, q):
+        """ Forward kinematics, conjugation form.
+            Takes numpy array of joint angles, in radians.
+        """
+        def expmap(x, y, theta):
+            """Implement exponential map via conjugation with axis (x,y)."""
+            return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
+
+        l1Zl1 = expmap(0.0, 0.0, q[0])
+        l2Zl2 = expmap(0.0, self.L1, q[1])
+        l3Zl3 = expmap(0.0, 7.0, q[2])
+        return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
+
+    def ik(self, sTt_desired, e=1e-9):
+        """ Inverse kinematics.
+            Takes desired Pose2 of tool T with respect to base S.
+            Optional: mu, gradient descent rate; e: error norm threshold
+        """
+        q = np.radians(vector3(30, -30, 45))  # well within workspace
+        error = vector3(100, 100, 100)
+
+        while np.linalg.norm(error) > e:
+            error = delta(sTt_desired, self.fk(q))
+            J = self.jacobian(q)
+            q -= np.dot(np.linalg.pinv(J), error)
+
+        # return result in interval [-pi,pi)
+        return np.remainder(q+math.pi, 2*math.pi)-math.pi
+
+    def manipulator_jacobian(self, q):
+        """ Calculate manipulator Jacobian.
+            Takes numpy array of joint angles, in radians.
+            Returns the manipulator Jacobian of differential twists. When multiplied with
+            a vector of joint velocities, will yield a single differential twist which is
+            the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
+            Just like always, differential twists can be hatted and multiplied with spatial
+            coordinates of a point to give the spatial velocity of the point.
+        """
+        l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
+        l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
+        # l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
+
+        p1 = self.xi1
+        # p1 = Pose2().Adjoint(self.xi1)
+
+        sTl1 = l1Zl1
+        p2 = sTl1.Adjoint(self.xi2)
+
+        sTl2 = compose(l1Zl1, l2Zl2)
+        p3 = sTl2.Adjoint(self.xi3)
+
+        differential_twists = [p1, p2, p3]
+        return np.stack(differential_twists, axis=1)
+
+    def plot(self, fignum, q):
+        """ Plot arm.
+            Takes figure number, and numpy array of joint angles, in radians.
+        """
+        fig = plt.figure(fignum)
+        axes = fig.gca()
+
+        sXl1 = Pose2(0, 0, math.radians(90))
+        t = sXl1.translation()
+        p1 = np.array([t.x(), t.y()])
+        gtsam_plot.plot_pose2_on_axes(axes, sXl1)
+
+        def plot_line(p, g, color):
+            t = g.translation()
+            q = np.array([t.x(), t.y()])
+            line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
+            axes.plot(line[:, 0], line[:, 1], color)
+            return q
+
+        l1Zl1 = Pose2(0, 0, q[0])
+        l1Xl2 = Pose2(self.L1, 0, 0)
+        sTl2 = compose(sXl1, l1Zl1, l1Xl2)
+        p2 = plot_line(p1, sTl2, 'r-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTl2)
+
+        l2Zl2 = Pose2(0, 0, q[1])
+        l2Xl3 = Pose2(self.L2, 0, 0)
+        sTl3 = compose(sTl2, l2Zl2, l2Xl3)
+        p3 = plot_line(p2, sTl3, 'g-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTl3)
+
+        l3Zl3 = Pose2(0, 0, q[2])
+        l3Xt = Pose2(self.L3, 0, 0)
+        sTt = compose(sTl3, l3Zl3, l3Xt)
+        plot_line(p3, sTt, 'b-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTt)
+
+
+# Create common example configurations.
+Q0 = vector3(0, 0, 0)
+Q1 = np.radians(vector3(-30, -45, -90))
+Q2 = np.radians(vector3(-90, 90, 0))
+
+
+class TestPose2SLAMExample(unittest.TestCase):
+    """Unit tests for functions used below."""
+
+    def setUp(self):
+        self.arm = ThreeLinkArm()
+
+    def assertPose2Equals(self, actual, expected, tol=1e-2):
+        """Helper function that prints out actual and expected if not equal."""
+        equal = actual.equals(expected, tol)
+        if not equal:
+            raise self.failureException(
+                "Poses are not equal:\n{}!={}".format(actual, expected))
+
+    def test_fk_arm(self):
+        """Make sure forward kinematics is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.fk(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.fk(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_jacobian(self):
+        """Test Jacobian calculation."""
+        # at rest
+        expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.jacobian(Q0)
+        np.testing.assert_array_almost_equal(J, expected)
+
+        # at -90, 90, 0
+        expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.jacobian(Q2)
+        np.testing.assert_array_almost_equal(J, expected)
+
+    def test_con_arm(self):
+        """Make sure POE is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.con(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.con(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_poe_arm(self):
+        """Make sure POE is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.poe(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.poe(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_ik(self):
+        """Check iterative inverse kinematics function."""
+        # at rest
+        actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
+        np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
+
+        # -30, -45, -90
+        sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
+        actual = self.arm.ik(sTt_desired)
+        self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
+        np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
+
+    def test_manipulator_jacobian(self):
+        """Test Jacobian calculation."""
+        # at rest
+        expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.manipulator_jacobian(Q0)
+        np.testing.assert_array_almost_equal(J, expected)
+
+        # at -90, 90, 0
+        expected = np.array(
+            [[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
+        J = self.arm.manipulator_jacobian(Q2)
+        np.testing.assert_array_almost_equal(J, expected)
+
+
+def run_example():
+    """ Use trajectory interpolation and then trajectory tracking a la Murray
+        to move a 3-link arm on a straight line.
+    """
+    arm = ThreeLinkArm()
+    q = np.radians(vector3(30, -30, 45))
+    sTt_initial = arm.fk(q)
+    sTt_goal = Pose2(2.4, 4.3, math.radians(0))
+    poses = trajectory(sTt_initial, sTt_goal, 50)
+
+    fignum = 0
+    fig = plt.figure(fignum)
+    axes = fig.gca()
+    axes.set_xlim(-5, 5)
+    axes.set_ylim(0, 10)
+    gtsam_plot.plot_pose2(fignum, arm.fk(q))
+
+    for pose in poses:
+        sTt = arm.fk(q)
+        error = delta(sTt, pose)
+        J = arm.jacobian(q)
+        q += np.dot(np.linalg.inv(J), error)
+        arm.plot(fignum, q)
+        plt.pause(0.01)
+
+    plt.pause(10)
+
+
+if __name__ == "__main__":
+    run_example()
+    unittest.main()

From c93cd7e5f1d8a4999052383de07676aa4e28b84a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Fri, 2 Nov 2018 23:26:20 -0400
Subject: [PATCH 016/281] Removed unnecessary (I think) aligned new operators.

---
 gtsam/geometry/BearingRange.h     | 3 ---
 gtsam/geometry/CalibratedCamera.h | 7 -------
 gtsam/geometry/OrientedPlane3.h   | 3 ---
 gtsam/geometry/Pose3.h            | 4 ----
 4 files changed, 17 deletions(-)

diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 33a799c1a..b1e003864 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -94,9 +94,6 @@ struct BearingRange
   }
 
   friend class boost::serialization::access;
-
-public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // Declare this to be both Testable and a Manifold
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index db9caf13b..acb3cacaf 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -229,10 +229,6 @@ private:
   void serialize(Archive & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_NVP(pose_);
   }
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
 };
 // end of class PinholeBase
 
@@ -416,9 +412,6 @@ private:
   }
 
   /// @}
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits
diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h
index 4bca8d138..e425a4b81 100644
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@@ -156,9 +156,6 @@ public:
   }
 
   /// @}
-
-public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<> struct traits<OrientedPlane3> : public internal::Manifold<
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 3229cbbbe..d70295f58 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -326,10 +326,6 @@ public:
     ar & BOOST_SERIALIZATION_NVP(t_);
   }
   /// @}
-
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
 };
 // Pose3 class
 

From d7ca1862690a379668055f269c46a5e90a8f2743 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Fri, 2 Nov 2018 23:29:52 -0400
Subject: [PATCH 017/281] Straightforward conditional aligned new operator

---
 gtsam/base/GenericValue.h  | 16 +++++++---------
 gtsam/slam/BetweenFactor.h |  6 ++++--
 gtsam/slam/PriorFactor.h   |  4 +++-
 3 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 1b59b6c1d..229050448 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -187,20 +187,18 @@ public:
       ar & boost::serialization::make_nvp("GenericValue",
               boost::serialization::base_object<Value>(*this));
       ar & boost::serialization::make_nvp("value", value_);
-
-	 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-	 enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
-
-  public:
-
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 	}
 
+
+  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+};
+
 /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues
 #define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue<Type>)
 
-};
-
 // traits
 template <typename ValueType>
 struct traits<GenericValue<ValueType> >
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 7af43e986..89291fac5 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -123,8 +123,10 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(measured_);
     }
   
-  public:
-	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   }; // \class BetweenFactor
 
   /// traits
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index c8ed378d8..3c5c42ccc 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -105,8 +105,10 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(prior_);
     }
   
+	// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
   public:
-	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
 
 } /// namespace gtsam

From 2d0ba69fa489c00d5d80a21f7372c2024df60f1d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Fri, 2 Nov 2018 23:31:28 -0400
Subject: [PATCH 018/281] Config-dependent aligned new operator

---
 gtsam/geometry/Pose2.h | 6 ++++++
 gtsam/geometry/Rot3.h  | 6 ++++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 1ba384857..f03e0852e 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -278,6 +278,12 @@ private:
     ar & BOOST_SERIALIZATION_NVP(t_);
     ar & BOOST_SERIALIZATION_NVP(r_);
   }
+
+#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
+public:
+  // Make sure Pose2 is aligned if it contains a Vector2
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 }; // Pose2
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 263301122..985c521a2 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -508,9 +508,11 @@ namespace gtsam {
 #endif
     }
 
-   public:
+#ifdef GTSAM_USE_QUATERNIONS
+  // only align if quaternion, Matrix3 has no alignment requirements
+  public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
+#endif
   };
 
   /**

From 8900739bd39583c52c6e6786cb3114b8f1c83d79 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 3 Nov 2018 00:21:36 -0400
Subject: [PATCH 019/281] Conditional aligned new, but not sure it has effect
 since inherits from pair

---
 gtsam/base/Manifold.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index b30edb3df..f89680b7c 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -209,6 +209,12 @@ public:
     v << v1, v2;
     return v;
   }
+
+  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
+  };
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept

From 142797723067a765c53378b6440aa4bb6d04be1b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 3 Nov 2018 10:20:31 -0400
Subject: [PATCH 020/281] Undo Eigen change (needed to remove GTSAM_EXPORT in
 SO3 to do this)

---
 gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
index 9e4981586..77f4f6066 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
@@ -431,7 +431,7 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE void conservativeResize(Index size)
     {
-      internal::conservative_resize_like_impl<Derived>::run(*this, size, 1);
+      internal::conservative_resize_like_impl<Derived>::run(*this, size);
     }
 
     /** Resizes the matrix to \a rows x \a cols of \c other, while leaving old values untouched.

From e38a3156c34d31b5d1286095bef467dfa8054e4f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 4 Nov 2018 12:37:43 -0500
Subject: [PATCH 021/281] Adding a pre-compiled header for MSVC

---
 cmake/GtsamAddPch.cmake      | 27 +++++++++++++++++
 gtsam/CMakeLists.txt         | 28 ++++++++---------
 gtsam/precompiled_header.cpp | 19 ++++++++++++
 gtsam/precompiled_header.h   | 59 ++++++++++++++++++++++++++++++++++++
 4 files changed, 119 insertions(+), 14 deletions(-)
 create mode 100644 cmake/GtsamAddPch.cmake
 create mode 100644 gtsam/precompiled_header.cpp
 create mode 100644 gtsam/precompiled_header.h

diff --git a/cmake/GtsamAddPch.cmake b/cmake/GtsamAddPch.cmake
new file mode 100644
index 000000000..cb872e361
--- /dev/null
+++ b/cmake/GtsamAddPch.cmake
@@ -0,0 +1,27 @@
+###############################################################################
+# Macro:
+#
+# gtsamAddPch(precompiledHeader precompiledSource sources)
+# 
+# Adds a precompiled header to compile all sources with. Currently only on MSVC.
+# Inspired by https://stackoverflow.com/questions/148570/
+#
+# Arguments:
+#   precompiledHeader: the header file that includes headers to be precompiled.
+#   precompiledSource: the source file that simply includes that header above.
+#   sources: the list of source files to apply this to.
+#
+macro(gtsamAddPch precompiledHeader precompiledSource sources)
+    get_filename_component(pchBasename ${precompiledHeader} NAME_WE)
+    SET(precompiledBinary "${CMAKE_CURRENT_BINARY_DIR}/${pchBasename}.pch")
+	IF(MSVC)
+		message(STATUS "Adding precompiled header for MSVC")
+		set_source_files_properties(${precompiledSource}
+									PROPERTIES COMPILE_FLAGS "/Yc\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
+											   OBJECT_OUTPUTS "${precompiledBinary}")
+		set_source_files_properties(${sources}
+									PROPERTIES COMPILE_FLAGS "/Yu\"${precompiledHeader}\" /FI\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
+											   OBJECT_DEPENDS "${precompiledBinary}")  
+	ENDIF(MSVC)
+endmacro(gtsamAddPch)
+
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 10092f195..6a3d08eb8 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -65,20 +65,19 @@ foreach(subdir ${gtsam_subdirs})
 endforeach(subdir)
 
 # To add additional sources to gtsam when building the full library (static or shared)
-# Add the subfolder with _srcs appended to the end to this list
-set(gtsam_srcs
-    ${3rdparty_srcs}
-    ${base_srcs}
-    ${geometry_srcs}
-    ${inference_srcs}
-    ${symbolic_srcs}
-    ${discrete_srcs}
-    ${linear_srcs}
-    ${nonlinear_srcs}
-    ${slam_srcs}
-    ${navigation_srcs}
-	${gtsam_core_headers}
-)
+# append the subfolder with _srcs appended to the end to this list
+set(gtsam_srcs ${3rdparty_srcs})
+foreach(subdir ${gtsam_subdirs})
+	list(APPEND gtsam_srcs ${${subdir}_srcs})
+endforeach(subdir)
+list(APPEND gtsam_srcs ${gtsam_core_headers})
+
+IF(MSVC)
+	# Add precompiled header to sources
+	include(gtsamAddPch)
+	gtsamAddPch("precompiled_header.h" "precompiled_header.cpp" ${gtsam_srcs})
+	list(INSERT gtsam_srcs 0 "precompiled_header.cpp")
+ENDIF(MSVC)
 
 # Generate and install config and dllexport files
 configure_file(config.h.in config.h)
@@ -155,6 +154,7 @@ if(MSVC)
 		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
 endif()
 
+
 # Create the matlab toolbox for the gtsam library
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Set up codegen
diff --git a/gtsam/precompiled_header.cpp b/gtsam/precompiled_header.cpp
new file mode 100644
index 000000000..83bc6231a
--- /dev/null
+++ b/gtsam/precompiled_header.cpp
@@ -0,0 +1,19 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+ /**
+  * @file    precompiled_header.cpp
+  * @brief   We need exactly one compilation unit that includes the precompiled headers
+  * @author  Frank Dellaert
+  * @date    November 2018
+  */
+
+#include "precompiled_header.h"
\ No newline at end of file
diff --git a/gtsam/precompiled_header.h b/gtsam/precompiled_header.h
new file mode 100644
index 000000000..c1b159802
--- /dev/null
+++ b/gtsam/precompiled_header.h
@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+ /**
+  * @file    precompiled_header.h>
+  * @brief   Include headers that will be included nearly everywhere
+  * @author  Frank Dellaert
+  * @date    November 2018
+  */
+
+#pragma once
+
+// All headers in base:
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/chartTesting.h>
+#include <gtsam/base/cholesky.h>
+#include <gtsam/base/concepts.h>
+#include <gtsam/base/ConcurrentMap.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/DSFVector.h>
+#include <gtsam/base/FastDefaultAllocator.h>
+#include <gtsam/base/FastList.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/base/FastSet.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/GenericValue.h>
+#include <gtsam/base/Group.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/base/ProductLieGroup.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/base/SymmetricBlockMatrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/base/ThreadsafeException.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/base/types.h>
+#include <gtsam/base/Value.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/VectorSpace.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+
+

From b8f3cd0f137beefa53253ecbada5af560942cbe2 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Nov 2018 13:05:28 -0500
Subject: [PATCH 022/281] Merge in alignment-related changes from
 'origin/fix/msvc2017'

---
 gtsam/base/Manifold.h             |  6 ++++++
 gtsam/geometry/CalibratedCamera.h |  7 -------
 gtsam/geometry/Point3.h           | 24 ++++++++++++------------
 gtsam/geometry/Pose2.h            |  6 ++++++
 gtsam/geometry/Pose3.h            |  8 +++++---
 gtsam/geometry/Rot3.h             |  6 ++++--
 gtsam/navigation/AttitudeFactor.h |  6 ++++++
 gtsam/slam/BetweenFactor.h        |  5 +++++
 gtsam/slam/PriorFactor.h          |  5 +++++
 9 files changed, 49 insertions(+), 24 deletions(-)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index b30edb3df..f89680b7c 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -209,6 +209,12 @@ public:
     v << v1, v2;
     return v;
   }
+
+  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
+  };
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index db9caf13b..acb3cacaf 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -229,10 +229,6 @@ private:
   void serialize(Archive & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_NVP(pose_);
   }
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
 };
 // end of class PinholeBase
 
@@ -416,9 +412,6 @@ private:
   }
 
   /// @}
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 99cb6c2e7..215161b3a 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -157,28 +157,28 @@ struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 
 // Convenience typedef
 typedef std::pair<Point3, Point3> Point3Pair;
-std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
+GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
 
 /// distance between two points
-double distance3(const Point3& p1, const Point3& q,
-                 OptionalJacobian<1, 3> H1 = boost::none,
-                 OptionalJacobian<1, 3> H2 = boost::none);
+GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
+	                          OptionalJacobian<1, 3> H1 = boost::none,
+                              OptionalJacobian<1, 3> H2 = boost::none);
 
 /// Distance of the point from the origin, with Jacobian
-double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
+GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
 
 /// normalize, with optional Jacobian
-Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
+GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
 
 /// cross product @return this x q
-Point3 cross(const Point3& p, const Point3& q,
-             OptionalJacobian<3, 3> H_p = boost::none,
-             OptionalJacobian<3, 3> H_q = boost::none);
+GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
+                          OptionalJacobian<3, 3> H_p = boost::none,
+                          OptionalJacobian<3, 3> H_q = boost::none);
 
 /// dot product
-double dot(const Point3& p, const Point3& q,
-           OptionalJacobian<1, 3> H_p = boost::none,
-           OptionalJacobian<1, 3> H_q = boost::none);
+GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
+                        OptionalJacobian<1, 3> H_p = boost::none,
+                        OptionalJacobian<1, 3> H_q = boost::none);
 
 template <typename A1, typename A2>
 struct Range;
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 1ba384857..f03e0852e 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -278,6 +278,12 @@ private:
     ar & BOOST_SERIALIZATION_NVP(t_);
     ar & BOOST_SERIALIZATION_NVP(r_);
   }
+
+#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
+public:
+  // Make sure Pose2 is aligned if it contains a Vector2
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 }; // Pose2
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 3229cbbbe..ca0fdff10 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -327,9 +327,11 @@ public:
   }
   /// @}
 
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 // Pose3 class
 
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 263301122..985c521a2 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -508,9 +508,11 @@ namespace gtsam {
 #endif
     }
 
-   public:
+#ifdef GTSAM_USE_QUATERNIONS
+  // only align if quaternion, Matrix3 has no alignment requirements
+  public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
+#endif
   };
 
   /**
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 21f74ac06..4ae6078e9 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -129,6 +129,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(nZ_);
     ar & BOOST_SERIALIZATION_NVP(bRef_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
@@ -213,6 +216,9 @@ private:
     ar & BOOST_SERIALIZATION_NVP(nZ_);
     ar & BOOST_SERIALIZATION_NVP(bRef_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index f3fd49fa7..89291fac5 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -122,6 +122,11 @@ namespace gtsam {
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(measured_);
     }
+  
+	  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   }; // \class BetweenFactor
 
   /// traits
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 9a3a4a47a..3c5c42ccc 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -104,6 +104,11 @@ namespace gtsam {
           boost::serialization::base_object<Base>(*this));
       ar & BOOST_SERIALIZATION_NVP(prior_);
     }
+  
+	// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+	enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
 
 } /// namespace gtsam

From 6e1994abd3b509b3db3507d808900c7332b8c66c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 4 Nov 2018 22:41:15 -0500
Subject: [PATCH 023/281] No longer uses ProductManifold, compilation issue
 with aligned operator.

---
 gtsam/geometry/EssentialMatrix.h | 94 ++++++++++++++++----------------
 1 file changed, 48 insertions(+), 46 deletions(-)

diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index 9dec574eb..308250033 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -11,7 +11,9 @@
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/Manifold.h>
+
 #include <iosfwd>
+#include <string>
 
 namespace gtsam {
 
@@ -21,19 +23,13 @@ namespace gtsam {
  * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
  * We can then non-linearly optimize immediately on this 5-dimensional manifold.
  */
-class GTSAM_EXPORT EssentialMatrix : private ProductManifold<Rot3, Unit3> {
-
-private:
-  typedef ProductManifold<Rot3, Unit3> Base;
-  Matrix3 E_; ///< Essential matrix
-
-  /// Construct from Base
-  EssentialMatrix(const Base& base) :
-      Base(base), E_(direction().skew() * rotation().matrix()) {
-  }
-
-public:
+class GTSAM_EXPORT EssentialMatrix {
+ private:
+  Rot3 R_;     ///< Rotation
+  Unit3 t_;    ///< Translation
+  Matrix3 E_;  ///< Essential matrix
 
+ public:
   /// Static function to convert Point2 to homogeneous coordinates
   static Vector3 Homogeneous(const Point2& p) {
     return Vector3(p.x(), p.y(), 1);
@@ -43,13 +39,12 @@ public:
   /// @{
 
   /// Default constructor
-  EssentialMatrix() :
-      Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
+  EssentialMatrix() :E_(t_.skew()) {
   }
 
   /// Construct from rotation and translation
   EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
-      Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
+      R_(aRb), t_(aTb), E_(t_.skew() * R_.matrix()) {
   }
 
   /// Named constructor with derivatives
@@ -79,27 +74,32 @@ public:
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
-    return rotation().equals(other.rotation(), tol)
-        && direction().equals(other.direction(), tol);
+    return R_.equals(other.R_, tol)
+        && t_.equals(other.t_, tol);
   }
 
   /// @}
 
   /// @name Manifold
   /// @{
+  enum { dimension = 5 };
+  inline static size_t Dim() { return dimension;}
+  inline size_t dim() const { return dimension;}
 
-  using Base::dimension;
-  using Base::dim;
-  using Base::Dim;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
   /// Retract delta to manifold
-  EssentialMatrix retract(const TangentVector& v) const {
-    return Base::retract(v);
+  EssentialMatrix retract(const Vector5& xi) const {
+    return EssentialMatrix(R_.retract(xi.head<3>()), t_.retract(xi.tail<2>()));
   }
 
   /// Compute the coordinates in the tangent space
-  TangentVector localCoordinates(const EssentialMatrix& other) const {
-    return Base::localCoordinates(other);
+  Vector5 localCoordinates(const EssentialMatrix& other) const {
+    auto v1 = R_.localCoordinates(other.R_);
+    auto v2 = t_.localCoordinates(other.t_);
+    Vector5 v;
+    v << v1, v2;
+    return v;
   }
   /// @}
 
@@ -108,12 +108,12 @@ public:
 
   /// Rotation
   inline const Rot3& rotation() const {
-    return this->first;
+    return R_;
   }
 
   /// Direction
   inline const Unit3& direction() const {
-    return this->second;
+    return t_;
   }
 
   /// Return 3*3 matrix representation
@@ -123,12 +123,12 @@ public:
 
   /// Return epipole in image_a , as Unit3 to allow for infinity
   inline const Unit3& epipole_a() const {
-    return direction();
+    return t_;
   }
 
   /// Return epipole in image_b, as Unit3 to allow for infinity
   inline Unit3 epipole_b() const {
-    return rotation().unrotate(direction());
+    return R_.unrotate(t_);
   }
 
   /**
@@ -139,8 +139,8 @@ public:
    * @return point in pose coordinates
    */
   Point3 transform_to(const Point3& p,
-      OptionalJacobian<3,5> DE = boost::none,
-      OptionalJacobian<3,3> Dpoint = boost::none) const;
+      OptionalJacobian<3, 5> DE = boost::none,
+      OptionalJacobian<3, 3> Dpoint = boost::none) const;
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -160,8 +160,8 @@ public:
   }
 
   /// epipolar error, algebraic
-  double error(const Vector3& vA, const Vector3& vB, //
-      OptionalJacobian<1,5> H = boost::none) const;
+  double error(const Vector3& vA, const Vector3& vB,
+      OptionalJacobian<1, 5> H = boost::none) const;
 
   /// @}
 
@@ -176,8 +176,7 @@ public:
 
   /// @}
 
-private:
-
+ private:
   /// @name Advanced Interface
   /// @{
 
@@ -185,21 +184,24 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(first);
-      ar & BOOST_SERIALIZATION_NVP(second);
+      ar & BOOST_SERIALIZATION_NVP(R_);
+      ar & BOOST_SERIALIZATION_NVP(t_);
 
-      ar & boost::serialization::make_nvp("E11", E_(0,0));
-      ar & boost::serialization::make_nvp("E12", E_(0,1));
-      ar & boost::serialization::make_nvp("E13", E_(0,2));
-      ar & boost::serialization::make_nvp("E21", E_(1,0));
-      ar & boost::serialization::make_nvp("E22", E_(1,1));
-      ar & boost::serialization::make_nvp("E23", E_(1,2));
-      ar & boost::serialization::make_nvp("E31", E_(2,0));
-      ar & boost::serialization::make_nvp("E32", E_(2,1));
-      ar & boost::serialization::make_nvp("E33", E_(2,2));
+      ar & boost::serialization::make_nvp("E11", E_(0, 0));
+      ar & boost::serialization::make_nvp("E12", E_(0, 1));
+      ar & boost::serialization::make_nvp("E13", E_(0, 2));
+      ar & boost::serialization::make_nvp("E21", E_(1, 0));
+      ar & boost::serialization::make_nvp("E22", E_(1, 1));
+      ar & boost::serialization::make_nvp("E23", E_(1, 2));
+      ar & boost::serialization::make_nvp("E31", E_(2, 0));
+      ar & boost::serialization::make_nvp("E32", E_(2, 1));
+      ar & boost::serialization::make_nvp("E33", E_(2, 2));
     }
 
   /// @}
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<>
@@ -208,5 +210,5 @@ struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 template<>
 struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 
-} // gtsam
+}  // namespace gtsam
 

From 415e4b10b1a6bae2d1e19211a7df62c53f60b97a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 4 Nov 2018 12:37:43 -0500
Subject: [PATCH 024/281] Adding a pre-compiled header for MSVC

(cherry picked from commit e38a3156c34d31b5d1286095bef467dfa8054e4f)
---
 cmake/GtsamAddPch.cmake      | 27 +++++++++++++++++
 gtsam/CMakeLists.txt         | 28 ++++++++---------
 gtsam/precompiled_header.cpp | 19 ++++++++++++
 gtsam/precompiled_header.h   | 59 ++++++++++++++++++++++++++++++++++++
 4 files changed, 119 insertions(+), 14 deletions(-)
 create mode 100644 cmake/GtsamAddPch.cmake
 create mode 100644 gtsam/precompiled_header.cpp
 create mode 100644 gtsam/precompiled_header.h

diff --git a/cmake/GtsamAddPch.cmake b/cmake/GtsamAddPch.cmake
new file mode 100644
index 000000000..cb872e361
--- /dev/null
+++ b/cmake/GtsamAddPch.cmake
@@ -0,0 +1,27 @@
+###############################################################################
+# Macro:
+#
+# gtsamAddPch(precompiledHeader precompiledSource sources)
+# 
+# Adds a precompiled header to compile all sources with. Currently only on MSVC.
+# Inspired by https://stackoverflow.com/questions/148570/
+#
+# Arguments:
+#   precompiledHeader: the header file that includes headers to be precompiled.
+#   precompiledSource: the source file that simply includes that header above.
+#   sources: the list of source files to apply this to.
+#
+macro(gtsamAddPch precompiledHeader precompiledSource sources)
+    get_filename_component(pchBasename ${precompiledHeader} NAME_WE)
+    SET(precompiledBinary "${CMAKE_CURRENT_BINARY_DIR}/${pchBasename}.pch")
+	IF(MSVC)
+		message(STATUS "Adding precompiled header for MSVC")
+		set_source_files_properties(${precompiledSource}
+									PROPERTIES COMPILE_FLAGS "/Yc\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
+											   OBJECT_OUTPUTS "${precompiledBinary}")
+		set_source_files_properties(${sources}
+									PROPERTIES COMPILE_FLAGS "/Yu\"${precompiledHeader}\" /FI\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
+											   OBJECT_DEPENDS "${precompiledBinary}")  
+	ENDIF(MSVC)
+endmacro(gtsamAddPch)
+
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 3875b6a19..4d1007d19 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -65,20 +65,19 @@ foreach(subdir ${gtsam_subdirs})
 endforeach(subdir)
 
 # To add additional sources to gtsam when building the full library (static or shared)
-# Add the subfolder with _srcs appended to the end to this list
-set(gtsam_srcs
-    ${3rdparty_srcs}
-    ${base_srcs}
-    ${geometry_srcs}
-    ${inference_srcs}
-    ${symbolic_srcs}
-    ${discrete_srcs}
-    ${linear_srcs}
-    ${nonlinear_srcs}
-    ${slam_srcs}
-    ${navigation_srcs}
-	${gtsam_core_headers}
-)
+# append the subfolder with _srcs appended to the end to this list
+set(gtsam_srcs ${3rdparty_srcs})
+foreach(subdir ${gtsam_subdirs})
+	list(APPEND gtsam_srcs ${${subdir}_srcs})
+endforeach(subdir)
+list(APPEND gtsam_srcs ${gtsam_core_headers})
+
+IF(MSVC)
+	# Add precompiled header to sources
+	include(gtsamAddPch)
+	gtsamAddPch("precompiled_header.h" "precompiled_header.cpp" ${gtsam_srcs})
+	list(INSERT gtsam_srcs 0 "precompiled_header.cpp")
+ENDIF(MSVC)
 
 # Generate and install config and dllexport files
 configure_file(config.h.in config.h)
@@ -155,6 +154,7 @@ if(MSVC)
 		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
 endif()
 
+
 # Create the matlab toolbox for the gtsam library
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Set up codegen
diff --git a/gtsam/precompiled_header.cpp b/gtsam/precompiled_header.cpp
new file mode 100644
index 000000000..83bc6231a
--- /dev/null
+++ b/gtsam/precompiled_header.cpp
@@ -0,0 +1,19 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+ /**
+  * @file    precompiled_header.cpp
+  * @brief   We need exactly one compilation unit that includes the precompiled headers
+  * @author  Frank Dellaert
+  * @date    November 2018
+  */
+
+#include "precompiled_header.h"
\ No newline at end of file
diff --git a/gtsam/precompiled_header.h b/gtsam/precompiled_header.h
new file mode 100644
index 000000000..c1b159802
--- /dev/null
+++ b/gtsam/precompiled_header.h
@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+ /**
+  * @file    precompiled_header.h>
+  * @brief   Include headers that will be included nearly everywhere
+  * @author  Frank Dellaert
+  * @date    November 2018
+  */
+
+#pragma once
+
+// All headers in base:
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/chartTesting.h>
+#include <gtsam/base/cholesky.h>
+#include <gtsam/base/concepts.h>
+#include <gtsam/base/ConcurrentMap.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/DSFVector.h>
+#include <gtsam/base/FastDefaultAllocator.h>
+#include <gtsam/base/FastList.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/base/FastSet.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/GenericValue.h>
+#include <gtsam/base/Group.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/lieProxies.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/base/ProductLieGroup.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/base/SymmetricBlockMatrix.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/base/ThreadsafeException.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/base/types.h>
+#include <gtsam/base/Value.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/VectorSpace.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+
+

From 7e566aeb16479183bcc8476a65bf9467104732d4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 5 Nov 2018 00:15:21 -0500
Subject: [PATCH 025/281] Resolve alignment conflict

---
 gtsam/geometry/Pose3.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index d70295f58..725a888ca 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -326,6 +326,12 @@ public:
     ar & BOOST_SERIALIZATION_NVP(t_);
   }
   /// @}
+ 
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 // Pose3 class
 

From 5409477c45d7532fe7b1bd325e8b4551d1da4bca Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 5 Nov 2018 00:19:04 -0500
Subject: [PATCH 026/281] Add aligned new operator

---
 gtsam/slam/EssentialMatrixConstraint.h | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h
index 92a78279b..179200fe1 100644
--- a/gtsam/slam/EssentialMatrixConstraint.h
+++ b/gtsam/slam/EssentialMatrixConstraint.h
@@ -103,6 +103,9 @@ private:
             boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(measuredE_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // \class EssentialMatrixConstraint
 

From 68f1cbdb08af182b96a717b6aa15bd141e810201 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 00:18:45 -0500
Subject: [PATCH 027/281] Refactored templates so we can get rid of FactorGraph
 include for faster/tighter compile

---
 gtsam/inference/Ordering.h | 35 ++++++++++++++++++-----------------
 1 file changed, 18 insertions(+), 17 deletions(-)

diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index ae7a10f44..30cc54c06 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -11,8 +11,10 @@
 
 /**
  * @file    Ordering.h
+ * @brief   Variable ordering for the elimination algorithm
  * @author  Richard Roberts
  * @author  Andrew Melim
+ * @author  Frank Dellaert
  * @date    Sep 2, 2010
  */
 
@@ -21,7 +23,6 @@
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/MetisIndex.h>
-#include <gtsam/inference/FactorGraph.h>
 #include <gtsam/base/FastSet.h>
 
 #include <boost/assign/list_inserter.hpp>
@@ -77,8 +78,8 @@ public:
   /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on
   /// performance). This internally builds a VariableIndex so if you already have a VariableIndex,
   /// it is faster to use COLAMD(const VariableIndex&)
-  template<class FACTOR>
-  static Ordering Colamd(const FactorGraph<FACTOR>& graph) {
+  template<class FACTOR_GRAPH>
+  static Ordering Colamd(const FACTOR_GRAPH& graph) {
     if (graph.empty())
       return Ordering();
     else
@@ -96,8 +97,8 @@ public:
   /// constrainLast will be ordered in the same order specified in the vector<Key> \c
   /// constrainLast.   If \c forceOrder is false, the variables in \c constrainLast will be
   /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
-  template<class FACTOR>
-  static Ordering ColamdConstrainedLast(const FactorGraph<FACTOR>& graph,
+  template<class FACTOR_GRAPH>
+  static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph,
       const std::vector<Key>& constrainLast, bool forceOrder = false) {
     if (graph.empty())
       return Ordering();
@@ -123,8 +124,8 @@ public:
   /// constrainFirst will be ordered in the same order specified in the vector<Key> \c
   /// constrainFirst.   If \c forceOrder is false, the variables in \c constrainFirst will be
   /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
-  template<class FACTOR>
-  static Ordering ColamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
+  template<class FACTOR_GRAPH>
+  static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph,
       const std::vector<Key>& constrainFirst, bool forceOrder = false) {
     if (graph.empty())
       return Ordering();
@@ -152,8 +153,8 @@ public:
   /// arbitrary order.  Any variables not present in \c groups will be assigned to group 0.  This
   /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the
   /// CCOLAMD documentation for more information.
-  template<class FACTOR>
-  static Ordering ColamdConstrained(const FactorGraph<FACTOR>& graph,
+  template<class FACTOR_GRAPH>
+  static Ordering ColamdConstrained(const FACTOR_GRAPH& graph,
       const FastMap<Key, int>& groups) {
     if (graph.empty())
       return Ordering();
@@ -172,8 +173,8 @@ public:
       const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
 
   /// Return a natural Ordering. Typically used by iterative solvers
-  template<class FACTOR>
-  static Ordering Natural(const FactorGraph<FACTOR> &fg) {
+  template<class FACTOR_GRAPH>
+  static Ordering Natural(const FACTOR_GRAPH &fg) {
     KeySet src = fg.keys();
     std::vector<Key> keys(src.begin(), src.end());
     std::stable_sort(keys.begin(), keys.end());
@@ -181,15 +182,15 @@ public:
   }
 
   /// METIS Formatting function
-  template<class FACTOR>
+  template<class FACTOR_GRAPH>
   static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
-      std::vector<int>& adj, const FactorGraph<FACTOR>& graph);
+      std::vector<int>& adj, const FACTOR_GRAPH& graph);
 
   /// Compute an ordering determined by METIS from a VariableIndex
   static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
 
-  template<class FACTOR>
-  static Ordering Metis(const FactorGraph<FACTOR>& graph) {
+  template<class FACTOR_GRAPH>
+  static Ordering Metis(const FACTOR_GRAPH& graph) {
     return Metis(MetisIndex(graph));
   }
 
@@ -197,9 +198,9 @@ public:
 
   /// @name Named Constructors @{
 
-  template<class FACTOR>
+  template<class FACTOR_GRAPH>
   static Ordering Create(OrderingType orderingType,
-      const FactorGraph<FACTOR>& graph) {
+      const FACTOR_GRAPH& graph) {
     if (graph.empty())
       return Ordering();
 

From 7d4ec362792b647be35d4ce62c8fe1eeb8159309 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 00:19:54 -0500
Subject: [PATCH 028/281] Removed some offending headers in pch

---
 gtsam/base/testLie.h       | 4 ++--
 gtsam/precompiled_header.h | 8 ++++----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h
index 1906ec439..63eb8674d 100644
--- a/gtsam/base/testLie.h
+++ b/gtsam/base/testLie.h
@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /*
- * @file chartTesting.h
- * @brief
+ * @file testLie.h
+ * @brief Test utilities for Lie groups
  * @date November, 2014
  * @author Paul Furgale
  */
diff --git a/gtsam/precompiled_header.h b/gtsam/precompiled_header.h
index c1b159802..37ab01cf2 100644
--- a/gtsam/precompiled_header.h
+++ b/gtsam/precompiled_header.h
@@ -18,7 +18,10 @@
 
 #pragma once
 
-// All headers in base:
+// All headers in base, except:
+// treeTraversal-inst.h: very specific to only a few compilation units
+// numericalDerivative.h : includes things in linear, nonlinear :-(
+// testLie.h: includes numericalDerivative
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/chartTesting.h>
 #include <gtsam/base/cholesky.h>
@@ -38,7 +41,6 @@
 #include <gtsam/base/lieProxies.h>
 #include <gtsam/base/Manifold.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/ProductLieGroup.h>
 #include <gtsam/base/serialization.h>
@@ -46,10 +48,8 @@
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
-#include <gtsam/base/testLie.h>
 #include <gtsam/base/ThreadsafeException.h>
 #include <gtsam/base/timing.h>
-#include <gtsam/base/treeTraversal-inst.h>
 #include <gtsam/base/types.h>
 #include <gtsam/base/Value.h>
 #include <gtsam/base/Vector.h>

From 65c0515829dce54c53bd74904e4d158f1b5253bf Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 00:28:36 -0500
Subject: [PATCH 029/281] Ignore windows temps

---
 .gitignore | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.gitignore b/.gitignore
index 2682a6748..42bd27466 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,3 +17,5 @@ cython/gtsam.so
 cython/gtsam_wrapper.pxd
 .vscode
 .env
+/.vs/
+/CMakeSettings.json

From 2584d421b8b78c974a8f7f938ea749c5ee97ae96 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 00:29:02 -0500
Subject: [PATCH 030/281] Weird alignment issue -> changed to static

---
 gtsam/geometry/Rot2.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 65dd5f609..eaec62b48 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -59,7 +59,7 @@ namespace gtsam {
 
     /// Named constructor from angle in degrees
     static Rot2 fromDegrees(double theta) {
-      const double degree = M_PI / 180;
+      static const double degree = M_PI / 180;
       return fromAngle(theta * degree);
     }
 

From c58a78b60a8b15a73fa4b72020166c8a31b65cc8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 00:29:38 -0500
Subject: [PATCH 031/281] More aligned new operators

---
 gtsam/navigation/CombinedImuFactor.h     |  3 +++
 gtsam/navigation/PreintegratedRotation.h | 12 +++++++++---
 gtsam/navigation/PreintegrationBase.h    |  3 ++-
 gtsam/navigation/PreintegrationParams.h  |  5 ++++-
 gtsam/nonlinear/ExpressionFactor.h       |  5 +++++
 gtsam/slam/ProjectionFactor.h            |  5 ++++-
 6 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 6fd98bfcb..7ca7fe463 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -320,6 +320,9 @@ private:
          boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(_PIM_);
   }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 // class CombinedImuFactor
 
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 54320417d..bf2f5c0c8 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -59,8 +59,11 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
   }
 
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 /**
@@ -169,8 +172,11 @@ class GTSAM_EXPORT PreintegratedRotation {
     ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
   }
 
- public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
+  public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 template <>
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index cf5465c05..3c22a1d00 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -202,7 +202,8 @@ public:
   /// @}
 #endif
 
-  /// @}
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /// namespace gtsam
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index 7bff82365..3a2fb467a 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -75,8 +75,11 @@ protected:
     ar & BOOST_SERIALIZATION_NVP(n_gravity);
   }
 
+#ifdef GTSAM_USE_QUATERNIONS
+  // Align if we are using Quaternions
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#endif
 };
 
 } // namespace gtsam
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 64a8a6bb6..34ba8e1ff 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -205,6 +205,11 @@ private:
  BOOST_SERIALIZATION_SPLIT_MEMBER()
 
  friend class boost::serialization::access;
+
+ // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+ enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 // ExpressionFactor
 
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index d13c28e11..8332acabc 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -186,7 +186,10 @@ namespace gtsam {
       ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
       ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
     }
-  };
+
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
 
   /// traits
   template<class POSE, class LANDMARK, class CALIBRATION>

From 3c4aadc712d19acc7bb164c54c18099e6c0c565b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 00:30:18 -0500
Subject: [PATCH 032/281] Use allocate_shared rather than make_shared to make
 sure alignment is good

---
 gtsam/base/GenericValue.h              |  2 +-
 gtsam/inference/FactorGraph.h          | 14 ++++++++------
 gtsam/navigation/CombinedImuFactor.cpp |  4 ++--
 3 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 93a7d0db5..87ddf574c 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -110,7 +110,7 @@ public:
      * Clone this value (normal clone on the heap, delete with 'delete' operator)
      */
     virtual boost::shared_ptr<Value> clone() const {
-      return boost::make_shared<GenericValue>(*this);
+		return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
     }
 
     /// Generic Value interface version of retract
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 016488701..6be3b89c2 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -26,6 +26,8 @@
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Key.h>
 
+#include <Eigen/Core>  // for Eigen::aligned_allocator
+
 #include <boost/serialization/nvp.hpp>
 #include <boost/assign/list_inserter.hpp>
 #include <boost/bind.hpp>
@@ -161,11 +163,11 @@ namespace gtsam {
       factors_.push_back(factor); }
 
     /** Emplace a factor */
-    template<class DERIVEDFACTOR, class... Args>
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-    emplace_shared(Args&&... args) {
-        factors_.push_back(boost::make_shared<DERIVEDFACTOR>(std::forward<Args>(args)...));
-    }
+	template<class DERIVEDFACTOR, class... Args>
+	typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
+		emplace_shared(Args&&... args) {
+		factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), std::forward<Args>(args)...));
+	}
 
     /** push back many factors with an iterator over shared_ptr (factors are not copied) */
     template<typename ITERATOR>
@@ -194,7 +196,7 @@ namespace gtsam {
     template<class DERIVEDFACTOR>
     typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
       push_back(const DERIVEDFACTOR& factor) {
-        factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor));
+        factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
     }
 //#endif
 
diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp
index 21c4200a9..1967e4a56 100644
--- a/gtsam/navigation/CombinedImuFactor.cpp
+++ b/gtsam/navigation/CombinedImuFactor.cpp
@@ -252,8 +252,8 @@ CombinedImuFactor::CombinedImuFactor(
 : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
     pose_j, vel_j, bias_i, bias_j),
 _PIM_(pim) {
-  boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
-  boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
+  using P = CombinedPreintegratedMeasurements::Params;
+  auto p = boost::allocate_shared<P>(Eigen::aligned_allocator<P>(), pim.p());
   p->n_gravity = n_gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;

From d02b33af8819b77a2c7670b46f0d9af6bfe812fc Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 10:16:55 -0500
Subject: [PATCH 033/281] Deprecated ProductManifold as has alignment issues
 and is overly obfuscating.

---
 gtsam/base/Manifold.h         |  5 +-
 gtsam/geometry/BearingRange.h | 91 +++++++++++++++++++++++++++--------
 2 files changed, 75 insertions(+), 21 deletions(-)

diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index f89680b7c..7bfdd7ea5 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -168,9 +168,9 @@ struct FixedDimension {
       "FixedDimension instantiated for dymanically-sized type.");
 };
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 /// Helper class to construct the product manifold of two other manifolds, M1 and M2
-/// Assumes nothing except manifold structure for M1 and M2, and the existence
-/// of default constructor for those types
+/// Deprecated because of limited usefulness, maximum obfuscation 
 template<typename M1, typename M2>
 class ProductManifold: public std::pair<M1, M2> {
   BOOST_CONCEPT_ASSERT((IsManifold<M1>));
@@ -221,6 +221,7 @@ public:
 template<typename M1, typename M2>
 struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
 };
+#endif
 
 } // \ namespace gtsam
 
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index b1e003864..9de5a07d8 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -45,23 +45,33 @@ struct Range;
  * and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
  */
 template <typename A1, typename A2>
-struct BearingRange
-    : public ProductManifold<typename Bearing<A1, A2>::result_type,
-                             typename Range<A1, A2>::result_type> {
+struct BearingRange {
+private:
   typedef typename Bearing<A1, A2>::result_type B;
   typedef typename Range<A1, A2>::result_type R;
-  typedef ProductManifold<B, R> Base;
+  B bearing_;
+  R range_;
+
+public:
+  enum { dimB = traits<B>::dimension };
+  enum { dimR = traits<R>::dimension };
+  enum { dimension = dimB + dimR };
+
+  /// @name Standard Constructors
+  /// @{
 
   BearingRange() {}
-  BearingRange(const ProductManifold<B, R>& br) : Base(br) {}
-  BearingRange(const B& b, const R& r) : Base(b, r) {}
+  BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
 
   /// Prediction function that stacks measurements
   static BearingRange Measure(
-      const A1& a1, const A2& a2,
-      OptionalJacobian<Base::dimension, traits<A1>::dimension> H1 = boost::none,
-      OptionalJacobian<Base::dimension, traits<A2>::dimension> H2 =
-          boost::none) {
+    const A1& a1, const A2& a2,
+    OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
+    OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
     typename MakeJacobian<B, A1>::type HB1;
     typename MakeJacobian<B, A2>::type HB2;
     typename MakeJacobian<R, A1>::type HR1;
@@ -75,32 +85,75 @@ struct BearingRange
     return BearingRange(b, r);
   }
 
+  /// @}
+  /// @name Testable
+  /// @{
+
   void print(const std::string& str = "") const {
     std::cout << str;
-    traits<B>::Print(this->first, "bearing ");
-    traits<R>::Print(this->second, "range ");
+    traits<B>::Print(bearing_, "bearing ");
+    traits<R>::Print(range_, "range ");
   }
   bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
-    return traits<B>::Equals(this->first, m2.first, tol) &&
-           traits<R>::Equals(this->second, m2.second, tol);
+    return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
+      traits<R>::Equals(range_, m2.range_, tol);
   }
 
- private:
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  inline static size_t Dim() { return dimension; }
+  inline size_t dim() const { return dimension; }
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  /// Retract delta to manifold
+  BearingRange retract(const TangentVector& xi) const {
+    B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
+    R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
+    return BearingRange(m1, m2);
+  }
+
+  /// Compute the coordinates in the tangent space
+  TangentVector localCoordinates(const BearingRange& other) const {
+    typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
+    typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
+    TangentVector v;
+    v << v1, v2;
+    return v;
+  }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+private:
   /// Serialization function
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& boost::serialization::make_nvp("bearing", this->first);
-    ar& boost::serialization::make_nvp("range", this->second);
+    ar& boost::serialization::make_nvp("bearing", bearing_);
+    ar& boost::serialization::make_nvp("range", range_);
   }
 
   friend class boost::serialization::access;
+
+  /// @}
+
+  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+  enum {
+    NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
+  };
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Declare this to be both Testable and a Manifold
 template <typename A1, typename A2>
 struct traits<BearingRange<A1, A2> >
-    : Testable<BearingRange<A1, A2> >,
-      internal::ManifoldTraits<BearingRange<A1, A2> > {};
+  : Testable<BearingRange<A1, A2> >,
+  internal::ManifoldTraits<BearingRange<A1, A2> > {};
 
 // Helper class for to implement Range traits for classes with a bearing method
 // For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say

From 36f5dbf9d229f7723d885f8bb2baa034019bc1ad Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 10:17:13 -0500
Subject: [PATCH 034/281] make_shared -> allocate_shared, for alignment

---
 gtsam/nonlinear/ExpressionFactorGraph.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h
index 122bd429f..665f887e2 100644
--- a/gtsam/nonlinear/ExpressionFactorGraph.h
+++ b/gtsam/nonlinear/ExpressionFactorGraph.h
@@ -42,7 +42,8 @@ public:
   template<typename T>
   void addExpressionFactor(const Expression<T>& h, const T& z,
       const SharedNoiseModel& R) {
-    push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
+    using F = ExpressionFactor<T>;
+    push_back(boost::allocate_shared<F>(Eigen::aligned_allocator<F>(), R, z, h));
   }
 
   /// @}

From 4113d99e208e0cd1eb72ef135335c8e274ef02d0 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 10:17:41 -0500
Subject: [PATCH 035/281] On MSVC, use aligned malloc and free

---
 gtsam/nonlinear/Expression-inl.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h
index 22172e44f..0ba2ad446 100644
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@@ -200,7 +200,7 @@ T Expression<T>::valueAndJacobianMap(const Values& values,
   // allocated on Visual Studio. For more information see the issue below
   // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
 #ifdef _MSC_VER
-  internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size];
+  auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(_aligned_malloc(size, internal::TraceAlignment));
 #else
   internal::ExecutionTraceStorage traceStorage[size];
 #endif
@@ -210,7 +210,7 @@ T Expression<T>::valueAndJacobianMap(const Values& values,
   trace.startReverseAD1(jacobians);
 
 #ifdef _MSC_VER
-  delete[] traceStorage;
+  _aligned_free(traceStorage);
 #endif
 
   return value;

From 79d63010dda9080e83958a8d8e4b37c8d9762a35 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 13:28:47 -0500
Subject: [PATCH 036/281] Give access to bearing/range

---
 examples/SolverComparer.cpp   | 42 ++++++++++++++++++-----------------
 gtsam/geometry/BearingRange.h |  6 +++++
 2 files changed, 28 insertions(+), 20 deletions(-)

diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index 63c512edb..b63e5faeb 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -251,10 +251,10 @@ void runIncremental()
   Key firstPose;
   while(nextMeasurement < datasetMeasurements.size())
   {
-    if(BetweenFactor<Pose>::shared_ptr measurement =
+    if(BetweenFactor<Pose>::shared_ptr factor =
       boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
     {
-      Key key1 = measurement->key1(), key2 = measurement->key2();
+      Key key1 = factor->key1(), key2 = factor->key2();
       if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
         // We found an odometry starting at firstStep
         firstPose = std::min(key1, key2);
@@ -302,52 +302,53 @@ void runIncremental()
 
       NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
 
-      if(BetweenFactor<Pose>::shared_ptr measurement =
+      if(BetweenFactor<Pose>::shared_ptr factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
       {
         // Stop collecting measurements that are for future steps
-        if(measurement->key1() > step || measurement->key2() > step)
+        if(factor->key1() > step || factor->key2() > step)
           break;
 
         // Require that one of the nodes is the current one
-        if(measurement->key1() != step && measurement->key2() != step)
+        if(factor->key1() != step && factor->key2() != step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add a new factor
-        newFactors.push_back(measurement);
+        newFactors.push_back(factor);
+        const auto& measured = factor->measured();
 
         // Initialize the new variable
-        if(measurement->key1() > measurement->key2()) {
-          if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
+        if(factor->key1() > factor->key2()) {
+          if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(measurement->key1(), measurement->measured().inverse());
+              newVariables.insert(factor->key1(), measured.inverse());
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
-              newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
+              newVariables.insert(factor->key1(), prevPose * measured.inverse());
             }
           }
         } else {
-          if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(measurement->key2(), measurement->measured());
+              newVariables.insert(factor->key2(), measured);
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
-              newVariables.insert(measurement->key2(), prevPose * measurement->measured());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
+              newVariables.insert(factor->key2(), prevPose * measured);
             }
           }
         }
       }
-      else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
+      else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
         boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
       {
-        Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
+        Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
 
         // Stop collecting measurements that are for future steps
         if(poseKey > step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add new factor
-        newFactors.push_back(measurement);
+        newFactors.push_back(factor);
 
         // Initialize new landmark
         if(!isam2.getLinearizationPoint().exists(lmKey))
@@ -357,8 +358,9 @@ void runIncremental()
             pose = isam2.calculateEstimate<Pose>(poseKey);
           else
             pose = newVariables.at<Pose>(poseKey);
-          Rot2 measuredBearing = measurement->measured().first;
-          double measuredRange = measurement->measured().second;
+          const auto& measured = factor->measured();
+          Rot2 measuredBearing = measured.bearing();
+          double measuredRange = measured.range();
           newVariables.insert(lmKey, 
             pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
         }
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 9de5a07d8..ffa373027 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -67,6 +67,12 @@ public:
   /// @name Standard Interface
   /// @{
 
+  /// Return bearing measurement
+  const B& bearing() const { return bearing_; }
+
+  /// Return range measurement
+  const R& range() const { return range_; }
+
   /// Prediction function that stacks measurements
   static BearingRange Measure(
     const A1& a1, const A2& a2,

From 2ee72806abdc8ae5fbe69b5dcf7d9ba2c89c442a Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 17:32:05 -0500
Subject: [PATCH 037/281] Eigen warning on clang

---
 .../src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
index 41e18ff07..b91a50340 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
@@ -52,7 +52,7 @@ struct general_matrix_matrix_triangular_product<Index,Scalar,LhsStorageOrder,Con
   static EIGEN_STRONG_INLINE void run(Index size, Index depth,const Scalar* lhs, Index lhsStride, \
                           const Scalar* rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking<Scalar, Scalar>& blocking) \
   { \
-    if ( lhs==rhs && ((UpLo&(Lower|Upper)==UpLo)) ) { \
+    if ( lhs==rhs && ((UpLo&(Lower|Upper))==UpLo) ) { \
       general_matrix_matrix_rankupdate<Index,Scalar,LhsStorageOrder,ConjugateLhs,ColMajor,UpLo> \
       ::run(size,depth,lhs,lhsStride,rhs,rhsStride,res,resStride,alpha,blocking); \
     } else { \

From 8e7892875a91fbe8c8fd34ba1001e8a0fe1c0752 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 6 Nov 2018 17:32:31 -0500
Subject: [PATCH 038/281] Check GeographicLib version

---
 gtsam/navigation/tests/testGPSFactor.cpp     | 12 +++++++++---
 gtsam/navigation/tests/testGeographicLib.cpp | 17 +++++++++++------
 2 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index 293bffa00..9457f501d 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -22,19 +22,26 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
 
+#if GEOGRAPHICLIB_VERSION_MINOR<37
+static const auto& kWGS84 = Geocentric::WGS84;
+#else
+static const auto& kWGS84 = Geocentric::WGS84();
+#endif
+
 // *************************************************************************
 namespace example {
 // ENU Origin is where the plane was in hold next to runway
 const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
 
 // Convert from GPS to ENU
-LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84);
+LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
 
 // Dekalb-Peachtree Airport runway 2L
 const double lat = 33.87071, lon = -84.30482, h = 274;
@@ -107,8 +114,7 @@ TEST(GPSData, init) {
   // GPS Reading 1 will be ENU origin
   double t1 = 84831;
   Point3 NED1(0, 0, 0);
-  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54,
-      Geocentric::WGS84);
+  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
 
   // GPS Readin 2
   double t2 = 84831.5;
diff --git a/gtsam/navigation/tests/testGeographicLib.cpp b/gtsam/navigation/tests/testGeographicLib.cpp
index aaa01b54d..6c495faf5 100644
--- a/gtsam/navigation/tests/testGeographicLib.cpp
+++ b/gtsam/navigation/tests/testGeographicLib.cpp
@@ -15,6 +15,7 @@
  * @author  Frank Dellaert
  */
 
+#include <GeographicLib/Config.h>
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/UTMUPS.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
@@ -29,21 +30,27 @@ using namespace std;
 using namespace GeographicLib;
 
 // Dekalb-Peachtree Airport runway 2L
-const double lat = 33.87071, lon = -84.30482, h = 274;
+static const double lat = 33.87071, lon = -84.30482, h = 274;
+
+#if GEOGRAPHICLIB_VERSION_MINOR<37
+static const auto& kWGS84 = Geocentric::WGS84;
+#else
+static const auto& kWGS84 = Geocentric::WGS84();
+#endif
 
 //**************************************************************************
 TEST( GeographicLib, Geocentric) {
 
   // From lat-lon to geocentric
   double X, Y, Z;
-  Geocentric::WGS84.Forward(lat, lon, h, X, Y, Z);
+  kWGS84.Forward(lat, lon, h, X, Y, Z);
   EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
   EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
   EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
 
   // From geocentric to lat-lon
   double lat_, lon_, h_;
-  Geocentric::WGS84.Reverse(X, Y, Z, lat_, lon_, h_);
+  kWGS84.Reverse(X, Y, Z, lat_, lon_, h_);
   EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
   EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
   EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
@@ -69,11 +76,9 @@ TEST( GeographicLib, UTM) {
 //**************************************************************************
 TEST( GeographicLib, ENU) {
 
-  const Geocentric& earth = Geocentric::WGS84;
-
   // ENU Origin is where the plane was in hold next to runway
   const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
-  LocalCartesian enu(lat0, lon0, h0, earth);
+  LocalCartesian enu(lat0, lon0, h0, kWGS84);
 
   // From lat-lon to geocentric
   double E, N, U;

From 43bae4dc2f5994db2385bb8f6b796787876ccee9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 8 Nov 2018 00:58:30 -0500
Subject: [PATCH 039/281] Added explicit to constructors

---
 gtsam/base/FastVector.h | 14 +++++---------
 1 file changed, 5 insertions(+), 9 deletions(-)

diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 7d1cb970c..3616704f7 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -35,9 +35,7 @@ namespace gtsam {
 template<typename VALUE>
 class FastVector: public std::vector<VALUE,
     typename internal::FastDefaultVectorAllocator<VALUE>::type> {
-
-public:
-
+ public:
   typedef std::vector<VALUE,
       typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
 
@@ -66,13 +64,13 @@ public:
   }
 
   /** Copy constructor from the base class */
-  FastVector(const Base& x) :
+  explicit FastVector(const Base& x) :
       Base(x) {
   }
 
   /** Copy constructor from a standard STL container */
   template<typename ALLOCATOR>
-  FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
+  explicit FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
     // This if statement works around a bug in boost pool allocator and/or
     // STL vector where if the size is zero, the pool allocator will allocate
     // huge amounts of memory.
@@ -85,14 +83,12 @@ public:
     return std::vector<VALUE>(this->begin(), this->end());
   }
 
-private:
+ private:
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
   }
-
 };
-
-}
+}  // namespace gtsam

From c1840f3d245c65c47600a6600ca50111125bf055 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 09:51:01 -0500
Subject: [PATCH 040/281] Removed TODO, superfluous comments

---
 cmake/dllexport.h.in | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/cmake/dllexport.h.in b/cmake/dllexport.h.in
index 6539e869e..55683a496 100644
--- a/cmake/dllexport.h.in
+++ b/cmake/dllexport.h.in
@@ -31,23 +31,22 @@
 
 // Whether GTSAM is compiled as static or DLL in windows. 
 // This will be used to decide whether include __declspec(dllimport) or not in headers
-// TODO: replace GTSAM by @library_name@
 #cmakedefine GTSAM_BUILD_STATIC_LIBRARY
 
 #ifdef _WIN32
 #  ifdef @library_name@_BUILD_STATIC_LIBRARY
 #    define @library_name@_EXPORT
 #    define @library_name@_EXTERN_EXPORT extern
-#  else /* @library_name@_BUILD_STATIC_LIBRARY */
+#  else
 #    ifdef @library_name@_EXPORTS
 #      define @library_name@_EXPORT __declspec(dllexport)
 #      define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
-#    else /* @library_name@_EXPORTS */
+#    else
 #      define @library_name@_EXPORT __declspec(dllimport)
 #      define @library_name@_EXTERN_EXPORT __declspec(dllimport)
-#    endif /* @library_name@_EXPORTS */
-#  endif /* @library_name@_BUILD_STATIC_LIBRARY */
-#else /* _WIN32 */
+#    endif
+#  endif
+#else
 #  define @library_name@_EXPORT
 #  define @library_name@_EXTERN_EXPORT extern
 #endif

From 8d61e3a4d95b89b6b700cf29455926ecc86ca0bb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 09:51:23 -0500
Subject: [PATCH 041/281] Restored to (nonsensical?) state it was before

---
 gtsam/3rdparty/metis/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt
index f864e02ac..dd21338a4 100644
--- a/gtsam/3rdparty/metis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/CMakeLists.txt
@@ -23,7 +23,7 @@ set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib")
 set(METIS_SHARED TRUE CACHE BOOL "build a shared library")
 
 if(MSVC)
-  set(METIS_INSTALL TRUE)
+  set(METIS_INSTALL FALSE)
 else()
   set(METIS_INSTALL FALSE)
 endif()

From 85a2c8e5bf00db63a7fff05ac7937aebb077c169 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 09:51:46 -0500
Subject: [PATCH 042/281] Restored flag, builds fine

---
 gtsam/CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 6a3d08eb8..4d1007d19 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -108,8 +108,8 @@ if (GTSAM_BUILD_STATIC_LIBRARY)
      	SOVERSION           ${gtsam_soversion})
 	if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
 		set_target_properties(gtsam PROPERTIES
-			PREFIX "lib")
-			#COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
+			PREFIX "lib"
+			COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
 	endif()
     install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
     list(APPEND GTSAM_EXPORTED_TARGETS gtsam)

From 1becaab652a0a25c07f3455465129aef575ec5c1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 09:52:12 -0500
Subject: [PATCH 043/281] Added comment

---
 CMakeLists.txt | 19 ++++++++++++-------
 1 file changed, 12 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 740ef7416..45a57827d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -111,11 +111,12 @@ set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
 # BOOST_ROOT: path to install prefix for boost
 # Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
 
-# If using Boost shared libs, disable auto linking
 if(MSVC)
-	set(Boost_USE_STATIC_LIBS        ON)  # only find static libs
-	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
-	# Disable autolinking
+	# By default, boost only builds static libraries on windows
+	set(Boost_USE_STATIC_LIBS ON)  # only find static libs
+	# If we ever reset above on windows and, ...
+	# If we use Boost shared libs, disable auto linking.
+	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
 	if(NOT Boost_USE_STATIC_LIBS)
 		add_definitions(-DBOOST_ALL_NO_LIB)
 	  add_definitions(-DBOOST_ALL_DYN_LINK)
@@ -257,8 +258,13 @@ else()
 
 endif()
 
-# tmp mute eigen static assert to avoid errors in shared lib
-add_definitions(-DEIGEN_NO_STATIC_ASSERT)
+if (MSVC)
+	if (NOT GTSAM_BUILD_STATIC_LIBRARY)
+		# mute eigen static assert to avoid errors in shared lib
+		add_definitions(-DEIGEN_NO_STATIC_ASSERT)
+	endif()
+	add_definitions(/wd4244) # Disable loss of precision which is thrown all over our Eigen
+endif()
 
 ###############################################################################
 # Global compile options
@@ -325,7 +331,6 @@ include_directories(BEFORE
 if(MSVC)
 	add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
 	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
-	add_definitions(/wd4244) # Disable loss of precision which is thrown all over our Eigen
 	add_definitions(/bigobj) # Allow large object files for template-based code
 endif()
 

From 2aa43e11bd3b6468c5fa821b022f05f06829a500 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 8 Nov 2018 00:58:50 -0500
Subject: [PATCH 044/281] Use KeyVector everywhere to avoid conversions

---
 gtsam/discrete/DiscreteFactorGraph.h          |  2 +-
 gtsam/discrete/DiscreteKey.cpp                |  4 +-
 gtsam/discrete/DiscreteKey.h                  |  3 +-
 gtsam/discrete/Signature.cpp                  |  4 +-
 gtsam/discrete/Signature.h                    |  2 +-
 gtsam/geometry/CameraSet.h                    |  5 +-
 gtsam/geometry/tests/testCameraSet.cpp        |  4 +-
 gtsam/inference/BayesTree-inst.h              |  6 +-
 gtsam/inference/BayesTree.h                   |  2 +-
 gtsam/inference/BayesTreeCliqueBase-inst.h    | 16 ++---
 gtsam/inference/BayesTreeCliqueBase.h         |  4 +-
 .../inference/EliminateableFactorGraph-inst.h | 18 +++---
 gtsam/inference/EliminateableFactorGraph.h    | 14 ++---
 gtsam/inference/EliminationTree-inst.h        |  2 +-
 gtsam/inference/Factor.h                      | 10 ++--
 gtsam/inference/ISAM-inst.h                   |  4 +-
 gtsam/inference/Key.h                         |  2 +-
 gtsam/inference/Ordering.cpp                  |  8 +--
 gtsam/inference/Ordering.h                    | 22 +++----
 gtsam/linear/BinaryJacobianFactor.h           |  2 +-
 gtsam/linear/GaussianConditional.cpp          |  8 +--
 gtsam/linear/GaussianFactor.h                 |  2 +-
 gtsam/linear/HessianFactor.cpp                |  4 +-
 gtsam/linear/HessianFactor.h                  |  4 +-
 gtsam/linear/JacobianFactor.cpp               |  2 +-
 gtsam/linear/JacobianFactor.h                 |  2 +-
 gtsam/linear/RegularHessianFactor.h           |  2 +-
 gtsam/linear/SubgraphPreconditioner.cpp       | 14 ++---
 gtsam/linear/SubgraphPreconditioner.h         |  4 +-
 gtsam/linear/tests/testGaussianBayesNet.cpp   |  2 +-
 gtsam/linear/tests/testGaussianBayesTree.cpp  |  2 +-
 gtsam/linear/tests/testHessianFactor.cpp      |  6 +-
 gtsam/linear/tests/testJacobianFactor.cpp     |  2 +-
 .../linear/tests/testRegularHessianFactor.cpp |  2 +-
 gtsam/linear/tests/testVectorValues.cpp       |  8 +--
 gtsam/nonlinear/ISAM2-impl.cpp                |  4 +-
 gtsam/nonlinear/ISAM2.cpp                     | 10 ++--
 gtsam/nonlinear/ISAM2.h                       |  2 +-
 gtsam/nonlinear/Marginals.cpp                 |  6 +-
 gtsam/nonlinear/Marginals.h                   |  6 +-
 gtsam/nonlinear/NonlinearFactor.cpp           |  2 +-
 gtsam/nonlinear/NonlinearFactor.h             |  2 +-
 gtsam/nonlinear/NonlinearFactorGraph.cpp      |  8 +--
 .../tests/testLinearContainerFactor.cpp       |  4 +-
 gtsam/nonlinear/utilities.h                   | 12 ++--
 gtsam/slam/InitializePose3.cpp                |  2 +-
 gtsam/slam/JacobianFactorQ.h                  |  4 +-
 gtsam/slam/JacobianFactorQR.h                 |  4 +-
 gtsam/slam/JacobianFactorSVD.h                |  4 +-
 gtsam/slam/RegularImplicitSchurFactor.h       |  4 +-
 gtsam/slam/SmartFactorBase.h                  |  4 +-
 gtsam/slam/SmartProjectionFactor.h            |  2 +-
 gtsam/slam/lago.cpp                           |  4 +-
 .../tests/testRegularImplicitSchurFactor.cpp  |  4 +-
 .../tests/testSmartProjectionCameraFactor.cpp | 12 ++--
 .../tests/testSmartProjectionPoseFactor.cpp   | 40 ++++++-------
 gtsam/symbolic/SymbolicFactor-inst.h          |  2 +-
 gtsam/symbolic/tests/testSymbolicBayesNet.cpp |  2 +-
 gtsam/symbolic/tests/testSymbolicFactor.cpp   |  2 +-
 .../tests/testSymbolicFactorGraph.cpp         |  4 +-
 gtsam_unstable/discrete/CSP.h                 |  2 +-
 gtsam_unstable/discrete/Constraint.h          |  2 +-
 gtsam_unstable/discrete/Domain.cpp            |  2 +-
 gtsam_unstable/discrete/Domain.h              |  2 +-
 gtsam_unstable/linear/RawQP.cpp               |  6 +-
 gtsam_unstable/linear/RawQP.h                 |  2 +-
 .../nonlinear/ConcurrentBatchFilter.cpp       |  2 +-
 .../nonlinear/ConcurrentBatchSmoother.cpp     |  2 +-
 .../ConcurrentFilteringAndSmoothing.cpp       |  2 +-
 .../nonlinear/ConcurrentIncrementalFilter.cpp |  2 +-
 .../ConcurrentIncrementalSmoother.cpp         |  2 +-
 .../nonlinear/NonlinearClusterTree.h          |  2 +-
 .../tests/testConcurrentBatchFilter.cpp       |  8 +--
 .../tests/testConcurrentBatchSmoother.cpp     |  2 +-
 .../tests/testConcurrentIncrementalFilter.cpp | 14 ++---
 .../testConcurrentIncrementalSmootherDL.cpp   |  2 +-
 .../testConcurrentIncrementalSmootherGN.cpp   |  2 +-
 gtsam_unstable/slam/BetweenFactorEM.h         | 54 ++++++++---------
 .../slam/SmartStereoProjectionFactor.h        |  2 +-
 .../slam/SmartStereoProjectionPoseFactor.h    |  4 +-
 .../slam/TransformBtwRobotsUnaryFactorEM.h    | 60 +++++++++----------
 .../testSmartStereoProjectionPoseFactor.cpp   | 32 +++++-----
 tests/testExpressionFactor.cpp                |  2 +-
 tests/testMarginals.cpp                       |  4 +-
 timing/timeSchurFactors.cpp                   |  2 +-
 85 files changed, 283 insertions(+), 281 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index cdfd7d522..4c2607f1f 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -72,7 +72,7 @@ public:
   typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
   /** A map from keys to values */
-  typedef std::vector<Key> Indices;
+  typedef KeyVector Indices;
   typedef Assignment<Key> Values;
   typedef boost::shared_ptr<Values> sharedValues;
 
diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp
index b4fd2e5a1..5ddad22b0 100644
--- a/gtsam/discrete/DiscreteKey.cpp
+++ b/gtsam/discrete/DiscreteKey.cpp
@@ -31,8 +31,8 @@ namespace gtsam {
     }
   }
 
-  vector<Key> DiscreteKeys::indices() const {
-    vector < Key > js;
+  KeyVector DiscreteKeys::indices() const {
+    KeyVector js;
     for(const DiscreteKey& key: *this)
       js.push_back(key.first);
     return js;
diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h
index bc2bd862d..c041c7e8e 100644
--- a/gtsam/discrete/DiscreteKey.h
+++ b/gtsam/discrete/DiscreteKey.h
@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/global_includes.h>
+#include <gtsam/inference/Key.h>
 
 #include <map>
 #include <string>
@@ -53,7 +54,7 @@ namespace gtsam {
     GTSAM_EXPORT DiscreteKeys(const std::vector<int>& cs);
 
     /// Return a vector of indices
-    GTSAM_EXPORT std::vector<Key> indices() const;
+    GTSAM_EXPORT KeyVector indices() const;
 
     /// Return a map from index to cardinality
     GTSAM_EXPORT std::map<Key,size_t> cardinalities() const;
diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp
index 0ee5c63b8..89e763703 100644
--- a/gtsam/discrete/Signature.cpp
+++ b/gtsam/discrete/Signature.cpp
@@ -130,8 +130,8 @@ namespace gtsam {
     return keys;
   }
 
-  vector<Key> Signature::indices() const {
-    vector<Key> js;
+  KeyVector Signature::indices() const {
+    KeyVector js;
     js.push_back(key_.first);
     for(const DiscreteKey& key: parents_)
       js.push_back(key.first);
diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h
index 6d49c7e4c..587cd6b30 100644
--- a/gtsam/discrete/Signature.h
+++ b/gtsam/discrete/Signature.h
@@ -90,7 +90,7 @@ namespace gtsam {
     DiscreteKeys discreteKeysParentsFirst() const;
 
     /** All key indices, with variable key first */
-    std::vector<Key> indices() const;
+    KeyVector indices() const;
 
     // the CPT as parsed, if successful
     const boost::optional<Table>& table() const {
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 709172c5b..1f5d6f774 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -19,10 +19,11 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
+#include <gtsam/geometry/CalibratedCamera.h>  // for Cheirality exception
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/inference/Key.h>
 #include <vector>
 
 namespace gtsam {
@@ -244,7 +245,7 @@ public:
   template<int N> // N = 2 or 3
   static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
       const Eigen::Matrix<double, N, N>& P, const Vector& b,
-      const FastVector<Key>& allKeys, const FastVector<Key>& keys,
+      const KeyVector& allKeys, const KeyVector& keys,
       /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
 
     assert(keys.size()==Fs.size());
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index 560206b9f..ab9227a08 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -93,7 +93,7 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal(schur, actualReduced.selfadjointView()));
 
   // Check Schur complement update, same order, should just double
-  FastVector<Key> allKeys, keys;
+  KeyVector allKeys, keys;
   allKeys.push_back(1);
   allKeys.push_back(2);
   keys.push_back(1);
@@ -102,7 +102,7 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
 
   // Check Schur complement update, keys reversed
-  FastVector<Key> keys2;
+  KeyVector keys2;
   keys2.push_back(2);
   keys2.push_back(1);
   Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced);
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 96d191419..33a23056b 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -344,7 +344,7 @@ namespace gtsam {
       // Get the set of variables to eliminate, which is C1\B.
       gttic(Full_root_factoring);
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
-        FastVector<Key> C1_minus_B; {
+        KeyVector C1_minus_B; {
           KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
           for(const Key j: *B->conditional()) {
             C1_minus_B_set.erase(j); }
@@ -356,7 +356,7 @@ namespace gtsam {
           FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
       }
       boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
-        FastVector<Key> C2_minus_B; {
+        KeyVector C2_minus_B; {
           KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
           for(const Key j: *B->conditional()) {
             C2_minus_B_set.erase(j); }
@@ -460,7 +460,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CLIQUE>
-  void BayesTree<CLIQUE>::removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans)
+  void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans)
   {
     // process each key of the new factor
     for(const Key& j: keys)
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index c22a5e257..ee0f3c7b5 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -214,7 +214,7 @@ namespace gtsam {
      * Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
      * Factors and orphans are added to the in/out arguments.
      */
-    void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans);
+    void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans);
 
     /**
      * Remove the requested subtree. */
diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h
index 31a8c55b6..6bcfb434d 100644
--- a/gtsam/inference/BayesTreeCliqueBase-inst.h
+++ b/gtsam/inference/BayesTreeCliqueBase-inst.h
@@ -40,12 +40,12 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class DERIVED, class FACTORGRAPH>
-  FastVector<Key>
+  KeyVector
     BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
   {
     KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
     KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
-    FastVector<Key> S_setminus_B;
+    KeyVector S_setminus_B;
     std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
       indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
     return S_setminus_B;
@@ -53,14 +53,14 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class DERIVED, class FACTORGRAPH>
-  FastVector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
+  KeyVector BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
     const derived_ptr& B, const FactorGraphType& p_Cp_B) const
   {
     gttic(shortcut_indices);
     KeySet allKeys = p_Cp_B.keys();
     KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
-    FastVector<Key> S_setminus_B = separator_setminus_B(B);
-    FastVector<Key> keep;
+    KeyVector S_setminus_B = separator_setminus_B(B);
+    KeyVector keep;
     // keep = S\B intersect allKeys (S_setminus_B is already sorted)
     std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
       allKeys.begin(), allKeys.end(), back_inserter(keep));
@@ -113,7 +113,7 @@ namespace gtsam {
     gttic(BayesTreeCliqueBase_shortcut);
     // We only calculate the shortcut when this clique is not B
     // and when the S\B is not empty
-    FastVector<Key> S_setminus_B = separator_setminus_B(B);
+    KeyVector S_setminus_B = separator_setminus_B(B);
     if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
     {
       // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
@@ -124,7 +124,7 @@ namespace gtsam {
       p_Cp_B += parent->conditional_; // P(Fp|Sp)
 
       // Determine the variables we want to keepSet, S union B
-      FastVector<Key> keep = shortcut_indices(B, p_Cp_B);
+      KeyVector keep = shortcut_indices(B, p_Cp_B);
 
       // Marginalize out everything except S union B
       boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
@@ -170,7 +170,7 @@ namespace gtsam {
         p_Cp += parent->conditional_; // P(Fp|Sp)
 
         // The variables we want to keepSet are exactly the ones in S
-        FastVector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
+        KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
         cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
       }
     }
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 055a03939..317ba1c44 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -149,12 +149,12 @@ namespace gtsam {
   protected:
 
     /// Calculate set \f$ S \setminus B \f$ for shortcut calculations
-    FastVector<Key> separator_setminus_B(const derived_ptr& B) const;
+    KeyVector separator_setminus_B(const derived_ptr& B) const;
 
     /** Determine variable indices to keep in recursive separator shortcut calculation The factor
      *  graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables
      *  not in S union B. */
-    FastVector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
+    KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
 
     /** Non-recursive delete cached shortcuts and marginals - internal only. */
     void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h
index 43bcffb09..af2a91257 100644
--- a/gtsam/inference/EliminateableFactorGraph-inst.h
+++ b/gtsam/inference/EliminateableFactorGraph-inst.h
@@ -129,7 +129,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
     EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
-    const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
+    const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex) {
       gttic(eliminatePartialSequential);
@@ -169,7 +169,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
     EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
-    const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
+    const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex) {
       gttic(eliminatePartialMultifrontal);
@@ -190,7 +190,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
-    boost::variant<const Ordering&, const std::vector<Key>&> variables,
+    boost::variant<const Ordering&, const KeyVector&> variables,
     OptionalOrdering marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
@@ -223,9 +223,9 @@ namespace gtsam {
         // No ordering was provided for the marginalized variables, so order them using constrained
         // COLAMD.
         bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
-        const std::vector<Key>* variablesOrOrdering =
+        const KeyVector* variablesOrOrdering =
           unmarginalizedAreOrdered ?
-          boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
+          boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
 
         Ordering totalOrdering =
           Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@@ -249,7 +249,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
-    boost::variant<const Ordering&, const std::vector<Key>&> variables,
+    boost::variant<const Ordering&, const KeyVector&> variables,
     OptionalOrdering marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
@@ -282,9 +282,9 @@ namespace gtsam {
         // No ordering was provided for the marginalized variables, so order them using constrained
         // COLAMD.
         bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
-        const std::vector<Key>* variablesOrOrdering =
+        const KeyVector* variablesOrOrdering =
           unmarginalizedAreOrdered ?
-          boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
+          boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
 
         Ordering totalOrdering =
           Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@@ -308,7 +308,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   boost::shared_ptr<FACTORGRAPH>
     EliminateableFactorGraph<FACTORGRAPH>::marginal(
-    const std::vector<Key>& variables,
+    const KeyVector& variables,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex)
diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h
index 891f22e61..a8f68ca2e 100644
--- a/gtsam/inference/EliminateableFactorGraph.h
+++ b/gtsam/inference/EliminateableFactorGraph.h
@@ -170,7 +170,7 @@ namespace gtsam {
      *  factor graph, and \f$ B = X\backslash A \f$. */
     std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
       eliminatePartialSequential(
-      const std::vector<Key>& variables,
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
@@ -190,14 +190,14 @@ namespace gtsam {
      *  factor graph, and \f$ B = X\backslash A \f$. */
     std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
       eliminatePartialMultifrontal(
-      const std::vector<Key>& variables,
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
     /** Compute the marginal of the requested variables and return the result as a Bayes net.
      *  @param variables Determines the variables whose marginal to compute, if provided as an
      *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
-     *         as a vector<Key> they will be ordered using constrained COLAMD.
+     *         as a KeyVector they will be ordered using constrained COLAMD.
      *  @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
      *         out, i.e. all variables not in \c variables.  If this is boost::none, the ordering
      *         will be computed with COLAMD.
@@ -206,7 +206,7 @@ namespace gtsam {
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
-      boost::variant<const Ordering&, const std::vector<Key>&> variables,
+      boost::variant<const Ordering&, const KeyVector&> variables,
       OptionalOrdering marginalizedVariableOrdering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
@@ -214,7 +214,7 @@ namespace gtsam {
     /** Compute the marginal of the requested variables and return the result as a Bayes tree.
      *  @param variables Determines the variables whose marginal to compute, if provided as an
      *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
-     *         as a vector<Key> they will be ordered using constrained COLAMD.
+     *         as a KeyVector they will be ordered using constrained COLAMD.
      *  @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
      *         out, i.e. all variables not in \c variables.  If this is boost::none, the ordering
      *         will be computed with COLAMD.
@@ -223,14 +223,14 @@ namespace gtsam {
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
-      boost::variant<const Ordering&, const std::vector<Key>&> variables,
+      boost::variant<const Ordering&, const KeyVector&> variables,
       OptionalOrdering marginalizedVariableOrdering = boost::none,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
     /** Compute the marginal factor graph of the requested variables. */
     boost::shared_ptr<FactorGraphType> marginal(
-      const std::vector<Key>& variables,
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = boost::none) const;
 
diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h
index 90540aaa6..333f898b8 100644
--- a/gtsam/inference/EliminationTree-inst.h
+++ b/gtsam/inference/EliminationTree-inst.h
@@ -48,7 +48,7 @@ namespace gtsam {
     gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
 
     // Do dense elimination step
-    FastVector<Key> keyAsVector(1); keyAsVector[0] = key;
+    KeyVector keyAsVector(1); keyAsVector[0] = key;
     std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
       function(gatheredFactors, Ordering(keyAsVector));
 
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 3e41d7692..d44d82954 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -58,15 +58,15 @@ namespace gtsam {
 
   public:
     /// Iterator over keys
-    typedef FastVector<Key>::iterator iterator;
+    typedef KeyVector::iterator iterator;
 
     /// Const iterator over keys
-    typedef FastVector<Key>::const_iterator const_iterator;
+    typedef KeyVector::const_iterator const_iterator;
 
   protected:
 
     /// The keys involved in this factor
-    FastVector<Key> keys_;
+    KeyVector keys_;
 
     /// @name Standard Constructors
     /// @{
@@ -112,7 +112,7 @@ namespace gtsam {
     const_iterator find(Key key) const { return std::find(begin(), end(), key); }
 
     /// Access the factor's involved variable keys
-    const FastVector<Key>& keys() const { return keys_; }
+    const KeyVector& keys() const { return keys_; }
 
     /** Iterator at beginning of involved variable keys */
     const_iterator begin() const { return keys_.begin(); }
@@ -148,7 +148,7 @@ namespace gtsam {
     /// @{
 
     /** @return keys involved in this factor */
-    FastVector<Key>& keys() { return keys_; }
+    KeyVector& keys() { return keys_; }
 
     /** Iterator at beginning of involved variable keys */
     iterator begin() { return keys_.begin(); }
diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h
index 4471567fc..785b2507d 100644
--- a/gtsam/inference/ISAM-inst.h
+++ b/gtsam/inference/ISAM-inst.h
@@ -30,7 +30,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
   BayesNetType bn;
   const KeySet newFactorKeys = newFactors.keys();
   if (!this->empty()) {
-    std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
+    KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
     this->removeTop(keyVector, bn, orphans);
   }
 
@@ -46,7 +46,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
   // Get an ordering where the new keys are eliminated last
   const VariableIndex index(factors);
   const Ordering ordering = Ordering::ColamdConstrainedLast(index,
-      std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
+      KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
 
   // eliminate all factors (top, added, orphans) into a new Bayes tree
   auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h
index 2c3eb84c6..d400a33c0 100644
--- a/gtsam/inference/Key.h
+++ b/gtsam/inference/Key.h
@@ -52,7 +52,7 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
 static const gtsam::KeyFormatter MultiRobotKeyFormatter =
     &_multirobotKeyFormatter;
 
-/// Useful typedef for operations with Values - allows for matlab interface
+/// Define collection type once and for all - also used in wrappers
 typedef FastVector<Key> KeyVector;
 
 // TODO(frank): Nothing fast about these :-(
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index b94f01689..1165b4a0f 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -61,7 +61,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
 
   if (nVars == 1)
   {
-    return Ordering(std::vector<Key>(1, variableIndex.begin()->first));
+    return Ordering(KeyVector(1, variableIndex.begin()->first));
   }
 
   const size_t nEntries = variableIndex.nEntries(), nFactors =
@@ -75,7 +75,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
   // Fill in input data for COLAMD
   p[0] = 0;
   int count = 0;
-  vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
+  KeyVector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
   size_t index = 0;
   for (auto key_factors: variableIndex) {
     // Arrange factor indices into COLAMD format
@@ -127,7 +127,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
 
 /* ************************************************************************* */
 Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
-    const std::vector<Key>& constrainLast, bool forceOrder) {
+    const KeyVector& constrainLast, bool forceOrder) {
   gttic(Ordering_COLAMDConstrainedLast);
 
   size_t n = variableIndex.size();
@@ -154,7 +154,7 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
 
 /* ************************************************************************* */
 Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
-    const std::vector<Key>& constrainFirst, bool forceOrder) {
+    const KeyVector& constrainFirst, bool forceOrder) {
   gttic(Ordering_COLAMDConstrainedFirst);
 
   const int none = -1;
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index ae7a10f44..f37de12fe 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -30,9 +30,9 @@
 
 namespace gtsam {
 
-class Ordering: public std::vector<Key> {
+class Ordering: public KeyVector {
 protected:
-  typedef std::vector<Key> Base;
+  typedef KeyVector Base;
 
 public:
 
@@ -93,12 +93,12 @@ public:
   /// VariableIndex, it is faster to use COLAMD(const VariableIndex&).  This function constrains
   /// the variables in \c constrainLast to the end of the ordering, and orders all other variables
   /// before in a fill-reducing ordering.  If \c forceOrder is true, the variables in \c
-  /// constrainLast will be ordered in the same order specified in the vector<Key> \c
+  /// constrainLast will be ordered in the same order specified in the KeyVector \c
   /// constrainLast.   If \c forceOrder is false, the variables in \c constrainLast will be
   /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
   template<class FACTOR>
   static Ordering ColamdConstrainedLast(const FactorGraph<FACTOR>& graph,
-      const std::vector<Key>& constrainLast, bool forceOrder = false) {
+      const KeyVector& constrainLast, bool forceOrder = false) {
     if (graph.empty())
       return Ordering();
     else
@@ -108,11 +108,11 @@ public:
   /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex.  This
   /// function constrains the variables in \c constrainLast to the end of the ordering, and orders
   /// all other variables before in a fill-reducing ordering.  If \c forceOrder is true, the
-  /// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
+  /// variables in \c constrainLast will be ordered in the same order specified in the KeyVector
   /// \c constrainLast.   If \c forceOrder is false, the variables in \c constrainLast will be
   /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
   static GTSAM_EXPORT Ordering ColamdConstrainedLast(
-      const VariableIndex& variableIndex, const std::vector<Key>& constrainLast,
+      const VariableIndex& variableIndex, const KeyVector& constrainLast,
       bool forceOrder = false);
 
   /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
@@ -120,12 +120,12 @@ public:
   /// VariableIndex, it is faster to use COLAMD(const VariableIndex&).  This function constrains
   /// the variables in \c constrainLast to the end of the ordering, and orders all other variables
   /// before in a fill-reducing ordering.  If \c forceOrder is true, the variables in \c
-  /// constrainFirst will be ordered in the same order specified in the vector<Key> \c
+  /// constrainFirst will be ordered in the same order specified in the KeyVector \c
   /// constrainFirst.   If \c forceOrder is false, the variables in \c constrainFirst will be
   /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
   template<class FACTOR>
   static Ordering ColamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
-      const std::vector<Key>& constrainFirst, bool forceOrder = false) {
+      const KeyVector& constrainFirst, bool forceOrder = false) {
     if (graph.empty())
       return Ordering();
     else
@@ -136,12 +136,12 @@ public:
   /// function constrains the variables in \c constrainFirst to the front of the ordering, and
   /// orders all other variables after in a fill-reducing ordering.  If \c forceOrder is true, the
   /// variables in \c constrainFirst will be ordered in the same order specified in the
-  /// vector<Key> \c constrainFirst.   If \c forceOrder is false, the variables in \c
+  /// KeyVector \c constrainFirst.   If \c forceOrder is false, the variables in \c
   /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
   /// reduce fill-in as well.
   static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
       const VariableIndex& variableIndex,
-      const std::vector<Key>& constrainFirst, bool forceOrder = false);
+      const KeyVector& constrainFirst, bool forceOrder = false);
 
   /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
   /// for note on performance).  This internally builds a VariableIndex so if you already have a
@@ -175,7 +175,7 @@ public:
   template<class FACTOR>
   static Ordering Natural(const FactorGraph<FACTOR> &fg) {
     KeySet src = fg.keys();
-    std::vector<Key> keys(src.begin(), src.end());
+    KeyVector keys(src.begin(), src.end());
     std::stable_sort(keys.begin(), keys.end());
     return Ordering(keys);
   }
diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h
index 462258762..343548613 100644
--- a/gtsam/linear/BinaryJacobianFactor.h
+++ b/gtsam/linear/BinaryJacobianFactor.h
@@ -48,7 +48,7 @@ struct BinaryJacobianFactor: JacobianFactor {
   }
 
   // Fixed-size matrix update
-  void updateHessian(const FastVector<Key>& infoKeys,
+  void updateHessian(const KeyVector& infoKeys,
       SymmetricBlockMatrix* info) const {
     gttic(updateHessian_BinaryJacobianFactor);
     // Whiten the factor if it has a noise model
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 60187d129..93217c027 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -117,7 +117,7 @@ namespace gtsam {
   /* ************************************************************************* */
   VectorValues GaussianConditional::solve(const VectorValues& x) const {
     // Concatenate all vector values that correspond to parent variables
-    const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));
+    const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
 
     // Update right-hand-side
     const Vector rhs = get_d() - get_S() * xS;
@@ -145,10 +145,10 @@ namespace gtsam {
   VectorValues GaussianConditional::solveOtherRHS(
     const VectorValues& parents, const VectorValues& rhs) const {
     // Concatenate all vector values that correspond to parent variables
-    Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));
+    Vector xS = parents.vector(KeyVector(beginParents(), endParents()));
 
     // Instead of updating getb(), update the right-hand-side from the given rhs
-    const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
+    const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
     xS = rhsR - get_S() * xS;
 
     // Solve Matrix
@@ -171,7 +171,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
-    Vector frontalVec = gy.vector(FastVector<Key>(beginFrontals(), endFrontals()));
+    Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
     frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
 
     // Check for indeterminant solution
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index 7f9c5ea3c..8c72ee734 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -126,7 +126,7 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    virtual void updateHessian(const FastVector<Key>& keys,
+    virtual void updateHessian(const KeyVector& keys,
                            SymmetricBlockMatrix* info) const = 0;
 
     /// y += alpha * A'*A*x
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 29b2b8591..61ce84fe9 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -155,7 +155,7 @@ DenseIndex _getSizeHF(const Vector& m) {
 }
 
 /* ************************************************************************* */
-HessianFactor::HessianFactor(const std::vector<Key>& js,
+HessianFactor::HessianFactor(const KeyVector& js,
     const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
     GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
   // Get the number of variables
@@ -356,7 +356,7 @@ double HessianFactor::error(const VectorValues& c) const {
 }
 
 /* ************************************************************************* */
-void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
+void HessianFactor::updateHessian(const KeyVector& infoKeys,
                                   SymmetricBlockMatrix* info) const {
   gttic(updateHessian_HessianFactor);
   assert(info);
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index e28bcdd81..cb9da0f1a 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -159,7 +159,7 @@ namespace gtsam {
      * quadratic term (the Hessian matrix) provided in row-order, gs the pieces
      * of the linear vector term, and f the constant term.
      */
-    HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
+    HessianFactor(const KeyVector& js, const std::vector<Matrix>& Gs,
         const std::vector<Vector>& gs, double f);
 
     /** Constructor with an arbitrary number of keys and with the augmented information matrix
@@ -310,7 +310,7 @@ namespace gtsam {
      * @param keys THe ordered vector of keys for the information matrix to be updated
      * @param info The information matrix to be updated
      */
-    void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
+    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
 
     /** Update another Hessian factor
      * @param other the HessianFactor to be updated
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 56a5dc085..1f5e5557c 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -501,7 +501,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
 }
 
 /* ************************************************************************* */
-void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
+void JacobianFactor::updateHessian(const KeyVector& infoKeys,
                                    SymmetricBlockMatrix* info) const {
   gttic(updateHessian_JacobianFactor);
 
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index d7814f541..36d1e12da 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -283,7 +283,7 @@ namespace gtsam {
      * @param scatter A mapping from variable index to slot index in this HessianFactor
      * @param info The information matrix to be updated
      */
-    void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const;
+    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
 
     /** Return A*x */
     Vector operator*(const VectorValues& x) const;
diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h
index f4df9d96b..1fe7009bc 100644
--- a/gtsam/linear/RegularHessianFactor.h
+++ b/gtsam/linear/RegularHessianFactor.h
@@ -36,7 +36,7 @@ public:
    * quadratic term (the Hessian matrix) provided in row-order, gs the pieces
    * of the linear vector term, and f the constant term.
    */
-  RegularHessianFactor(const std::vector<Key>& js,
+  RegularHessianFactor(const KeyVector& js,
       const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
       HessianFactor(js, Gs, gs, f) {
     checkInvariants();
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index d51f365be..d796e28b7 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -571,14 +571,14 @@ void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
   /* in place back substitute */
   for (auto cg: boost::adaptors::reverse(*Rc1_)) {
     /* collect a subvector of x that consists of the parents of cg (S) */
-    const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
-    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
+    const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
 
     /* compute the solution for the current pivot */
     const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
   }
 }
 
@@ -590,7 +590,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
 
   /* in place back substitute */
   for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
-    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
 //    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
     const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
 
@@ -598,7 +598,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
     if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
 
     /* substract from parent variables */
     for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
@@ -626,7 +626,7 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
 }
 
 /*****************************************************************************/
-Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {
+Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
 
   /* a cache of starting index and dim */
   typedef vector<pair<size_t, size_t> > Cache;
@@ -652,7 +652,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
 }
 
 /*****************************************************************************/
-void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
+void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
   /* use the cache  */
   size_t idx = 0;
   for ( const Key &key: keys ) {
diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h
index e74b92df1..e440f32e4 100644
--- a/gtsam/linear/SubgraphPreconditioner.h
+++ b/gtsam/linear/SubgraphPreconditioner.h
@@ -298,10 +298,10 @@ namespace gtsam {
   };
 
   /* get subvectors */
-  Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys);
+  Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys);
 
   /* set subvectors */
-  void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst);
+  void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst);
 
 
   /* build a factor subgraph, which is defined as a set of weighted edges (factors) */
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 89b44f1f8..45545b8ea 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -229,7 +229,7 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
   VectorValues actual = gbn.optimizeGradientSearch();
 
   // Check that points agree
-  FastVector<Key> keys = list_of(0)(1)(2)(3)(4);
+  KeyVector keys = list_of(0)(1)(2)(3)(4);
   Vector actualAsVector = actual.vector(keys);
   EXPECT(assert_equal(expected, actualAsVector, 1e-5));
 
diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp
index e5634c357..97809bfbc 100644
--- a/gtsam/linear/tests/testGaussianBayesTree.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTree.cpp
@@ -287,7 +287,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
   VectorValues actual = bt.optimizeGradientSearch();
 
   // Check that points agree
-  FastVector<Key> keys = list_of(0)(1)(2)(3)(4);
+  KeyVector keys = list_of(0)(1)(2)(3)(4);
   EXPECT(assert_equal(expected, actual.vector(keys), 1e-5));
   EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
 
diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp
index 61fa8bd2c..31030451b 100644
--- a/gtsam/linear/tests/testHessianFactor.cpp
+++ b/gtsam/linear/tests/testHessianFactor.cpp
@@ -41,7 +41,7 @@ const double tol = 1e-5;
 /* ************************************************************************* */
 TEST(HessianFactor, Slot)
 {
-  FastVector<Key> keys = list_of(2)(4)(1);
+  KeyVector keys = list_of(2)(4)(1);
   EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
   EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
   EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
@@ -252,7 +252,7 @@ TEST(HessianFactor, ConstructorNWay)
     (1, dx1)
     (2, dx2);
 
-  std::vector<Key> js;
+  KeyVector js;
   js.push_back(0); js.push_back(1); js.push_back(2);
   std::vector<Matrix> Gs;
   Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
@@ -517,7 +517,7 @@ TEST(HessianFactor, gradientAtZero)
   // test gradient at zero
   VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2);
   Matrix A; Vector b; boost::tie(A,b) = factor.jacobian();
-  FastVector<Key> keys; keys += 0,1;
+  KeyVector keys; keys += 0,1;
   EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
   VectorValues actualG = factor.gradientAtZero();
   EXPECT(assert_equal(expectedG, actualG));
diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index 26f02923b..2403895c2 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -350,7 +350,7 @@ TEST(JacobianFactor, operators )
   VectorValues expectedG;
   expectedG.insert(1, Vector2(20,-10));
   expectedG.insert(2, Vector2(-20, 10));
-  FastVector<Key> keys; keys += 1,2;
+  KeyVector keys; keys += 1,2;
   EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
   VectorValues actualG = lf.gradientAtZero();
   EXPECT(assert_equal(expectedG, actualG));
diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp
index 1618451f3..c610255c0 100644
--- a/gtsam/linear/tests/testRegularHessianFactor.cpp
+++ b/gtsam/linear/tests/testRegularHessianFactor.cpp
@@ -64,7 +64,7 @@ TEST(RegularHessianFactor, Constructors)
   EXPECT(assert_equal(factor,factor2));
 
   // Test n-way constructor
-  vector<Key> keys; keys += 0, 1, 3;
+  KeyVector keys; keys += 0, 1, 3;
   vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
   vector<Vector> gs; gs += g1, g2, g3;
   RegularHessianFactor<2> factor3(keys, Gs, gs, f);
diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp
index 7e972903a..d5e5aee68 100644
--- a/gtsam/linear/tests/testVectorValues.cpp
+++ b/gtsam/linear/tests/testVectorValues.cpp
@@ -62,7 +62,7 @@ TEST(VectorValues, basics)
   EXPECT(assert_equal(Vector2(2, 3), actual[1]));
   EXPECT(assert_equal(Vector2(4, 5), actual[2]));
   EXPECT(assert_equal(Vector2(6, 7), actual[5]));
-  FastVector<Key> keys = list_of(0)(1)(2)(5);
+  KeyVector keys = list_of(0)(1)(2)(5);
   EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
 
   // Check exceptions
@@ -101,7 +101,7 @@ TEST(VectorValues, subvector)
   init.insert(12, Vector2(4, 5));
   init.insert(13, Vector2(6, 7));
 
-  std::vector<Key> keys;
+  KeyVector keys;
   keys += 10, 12, 13;
   Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
   EXPECT(assert_equal(expSubVector, init.vector(keys)));
@@ -197,7 +197,7 @@ TEST(VectorValues, convert)
   EXPECT(assert_equal(expected, actual2));
 
   // Test other direction, note vector() is not guaranteed to give right result
-  FastVector<Key> keys = list_of(0)(1)(2)(5);
+  KeyVector keys = list_of(0)(1)(2)(5);
   EXPECT(assert_equal(x, actual.vector(keys)));
 
   // Test version with dims argument
@@ -222,7 +222,7 @@ TEST(VectorValues, vector_sub)
   expected << 1, 6, 7;
 
   // Test FastVector version
-  FastVector<Key> keys = list_of(0)(5);
+  KeyVector keys = list_of(0)(5);
   EXPECT(assert_equal(expected, vv.vector(keys)));
 
   // Test version with dims argument
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 8a8813af5..2cedeea9f 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -239,10 +239,10 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
     // Update the current variable
     // Get VectorValues slice corresponding to current variables
     Vector gR =
-        grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(),
+        grad.vector(KeyVector(clique->conditional()->beginFrontals(),
                                     clique->conditional()->endFrontals()));
     Vector gS =
-        grad.vector(FastVector<Key>(clique->conditional()->beginParents(),
+        grad.vector(KeyVector(clique->conditional()->beginParents(),
                                     clique->conditional()->endParents()));
 
     // Compute R*g and S*g for this clique
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index df07050de..32a24b51d 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -195,7 +195,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) {
 /* ************************************************************************* */
 boost::shared_ptr<KeySet> ISAM2::recalculate(
     const KeySet& markedKeys, const KeySet& relinKeys,
-    const vector<Key>& observedKeys, const KeySet& unusedIndices,
+    const KeyVector& observedKeys, const KeySet& unusedIndices,
     const boost::optional<FastMap<Key, int> >& constrainKeys,
     ISAM2Result* result) {
   // TODO(dellaert):  new factors are linearized twice,
@@ -243,7 +243,7 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
   gttic(removetop);
   Cliques orphans;
   GaussianBayesNet affectedBayesNet;
-  this->removeTop(FastVector<Key>(markedKeys.begin(), markedKeys.end()),
+  this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()),
                   affectedBayesNet, orphans);
   gttoc(removetop);
 
@@ -667,7 +667,7 @@ ISAM2Result ISAM2::update(
   // NOTE: we use assign instead of the iterator constructor here because this
   // is a vector of size_t, so the constructor unintentionally resolves to
   // vector(size_t count, Key value) instead of the iterator constructor.
-  FastVector<Key> observedKeys;
+  KeyVector observedKeys;
   observedKeys.reserve(markedKeys.size());
   for (Key index : markedKeys) {
     if (unusedIndices.find(index) ==
@@ -945,7 +945,7 @@ void ISAM2::marginalizeLeaves(
         // conditional
         auto cg = clique->conditional();
         const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals());
-        FastVector<Key> cliqueFrontalsToEliminate;
+        KeyVector cliqueFrontalsToEliminate;
         std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
                               leafKeys.begin(), leafKeys.end(),
                               std::back_inserter(cliqueFrontalsToEliminate));
@@ -967,7 +967,7 @@ void ISAM2::marginalizeLeaves(
         cg->matrixObject().rowStart() = dimToRemove;
 
         // Change the keys in the clique
-        FastVector<Key> originalKeys;
+        KeyVector originalKeys;
         originalKeys.swap(cg->keys());
         cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
         cg->nrFrontals() -= nToRemove;
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 5d448d786..04bd3d3eb 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -299,7 +299,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
 
   virtual boost::shared_ptr<KeySet> recalculate(
       const KeySet& markedKeys, const KeySet& relinKeys,
-      const std::vector<Key>& observedKeys, const KeySet& unusedIndices,
+      const KeyVector& observedKeys, const KeySet& unusedIndices,
       const boost::optional<FastMap<Key, int> >& constrainKeys,
       ISAM2Result* result);
 
diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp
index 68da1250e..fcbbf2f44 100644
--- a/gtsam/nonlinear/Marginals.cpp
+++ b/gtsam/nonlinear/Marginals.cpp
@@ -80,14 +80,14 @@ Matrix Marginals::marginalCovariance(Key variable) const {
 }
 
 /* ************************************************************************* */
-JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const {
+JointMarginal Marginals::jointMarginalCovariance(const KeyVector& variables) const {
   JointMarginal info = jointMarginalInformation(variables);
   info.blockMatrix_.invertInPlace();
   return info;
 }
 
 /* ************************************************************************* */
-JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variables) const {
+JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) const {
 
   // If 2 variables, we can use the BayesTree::joint function, otherwise we
   // have to use sequential elimination.
@@ -119,7 +119,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
     Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
 
     // Information matrix will be returned with sorted keys
-    std::vector<Key> variablesSorted = variables;
+    KeyVector variablesSorted = variables;
     std::sort(variablesSorted.begin(), variablesSorted.end());
 
     // Get dimensions from factor graph
diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h
index 35b0770c2..e0a78042d 100644
--- a/gtsam/nonlinear/Marginals.h
+++ b/gtsam/nonlinear/Marginals.h
@@ -74,10 +74,10 @@ public:
   Matrix marginalCovariance(Key variable) const;
 
   /** Compute the joint marginal covariance of several variables */
-  JointMarginal jointMarginalCovariance(const std::vector<Key>& variables) const;
+  JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
 
   /** Compute the joint marginal information of several variables */
-  JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
+  JointMarginal jointMarginalInformation(const KeyVector& variables) const;
 
   /** Optimize the bayes tree */
   VectorValues optimize() const;
@@ -130,7 +130,7 @@ public:
   void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
 protected:
-  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const std::vector<Key>& keys) :
+  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
     blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
 
   friend class Marginals;
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 5ff022023..c705e3c8f 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -52,7 +52,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
 
 /* ************************************************************************* */
 NonlinearFactor::shared_ptr NonlinearFactor::rekey(
-    const std::vector<Key>& new_keys) const {
+    const KeyVector& new_keys) const {
   assert(new_keys.size() == keys().size());
   shared_ptr new_factor = clone();
   new_factor->keys() = new_keys;
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index dd4c9123a..4cddc7dda 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -141,7 +141,7 @@ public:
    * Clones a factor and fully replaces its keys
    * @param new_keys is the full replacement set of keys
    */
-  shared_ptr rekey(const std::vector<Key>& new_keys) const;
+  shared_ptr rekey(const KeyVector& new_keys) const;
 
 }; // \class NonlinearFactor
 
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 56234c13c..a4bdd83e3 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -165,10 +165,10 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
 
   if (formatting.mergeSimilarFactors) {
     // Remove duplicate factors
-    std::set<vector<Key> > structure;
+    std::set<KeyVector > structure;
     for (const sharedFactor& factor : factors_) {
       if (factor) {
-        vector<Key> factorKeys = factor->keys();
+        KeyVector factorKeys = factor->keys();
         std::sort(factorKeys.begin(), factorKeys.end());
         structure.insert(factorKeys);
       }
@@ -176,7 +176,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
 
     // Create factors and variable connections
     size_t i = 0;
-    for(const vector<Key>& factorKeys: structure){
+    for(const KeyVector& factorKeys: structure){
       // Make each factor a dot
       stm << "  factor" << i << "[label=\"\", shape=point";
       {
@@ -199,7 +199,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
     for(size_t i = 0; i < size(); ++i) {
       const NonlinearFactor::shared_ptr& factor = at(i);
       if(formatting.plotFactorPoints) {
-        const FastVector<Key>& keys = factor->keys();
+        const KeyVector& keys = factor->keys();
         if (formatting.binaryEdges && keys.size()==2) {
           stm << "  var" << keys[0] << "--" << "var" << keys[1] << ";\n";
         } else {
diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
index df1df0d20..5321eb2c4 100644
--- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
+++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
@@ -47,7 +47,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
   EXPECT(!actFactor.isHessian());
 
   // check keys
-  FastVector<Key> expKeys; expKeys += l1, l2;
+  KeyVector expKeys; expKeys += l1, l2;
   EXPECT(assert_container_equality(expKeys, actFactor.keys()));
 
   Values values;
@@ -246,7 +246,7 @@ TEST( testLinearContainerFactor, creation ) {
   LinearContainerFactor actual(linear_factor, full_values);
 
   // Verify the keys
-  FastVector<Key> expKeys;
+  KeyVector expKeys;
   expKeys += l3, l5;
   EXPECT(assert_container_equality(expKeys, actual.keys()));
 
diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h
index 3816f26f8..58e7e78af 100644
--- a/gtsam/nonlinear/utilities.h
+++ b/gtsam/nonlinear/utilities.h
@@ -54,16 +54,16 @@ FastList<Key> createKeyList(std::string s, const Vector& I) {
 }
 
 // Create a KeyVector from indices
-FastVector<Key> createKeyVector(const Vector& I) {
-  FastVector<Key> set;
+KeyVector createKeyVector(const Vector& I) {
+  KeyVector set;
   for (int i = 0; i < I.size(); i++)
     set.push_back(I[i]);
   return set;
 }
 
 // Create a KeyVector from indices using symbol
-FastVector<Key> createKeyVector(std::string s, const Vector& I) {
-  FastVector<Key> set;
+KeyVector createKeyVector(std::string s, const Vector& I) {
+  KeyVector set;
   char c = s[0];
   for (int i = 0; i < I.size(); i++)
     set.push_back(Symbol(c, I[i]));
@@ -222,12 +222,12 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
 
 /// Convert from local to world coordinates
 Values localToWorld(const Values& local, const Pose2& base,
-    const FastVector<Key> user_keys = FastVector<Key>()) {
+    const KeyVector user_keys = KeyVector()) {
 
   Values world;
 
   // if no keys given, get all keys from local values
-  FastVector<Key> keys(user_keys);
+  KeyVector keys(user_keys);
   if (keys.size()==0)
     keys = local.keys();
 
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 58408e7e3..3979996da 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -53,7 +53,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
     else
       std::cout << "Error in buildLinearOrientationGraph" << std::endl;
 
-    const FastVector<Key>& keys = factor->keys();
+    const KeyVector& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
     Matrix M9 = Z_9x9;
     M9.block(0,0,3,3) = Rij;
diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h
index 3f5290c58..23cadbee9 100644
--- a/gtsam/slam/JacobianFactorQ.h
+++ b/gtsam/slam/JacobianFactorQ.h
@@ -37,7 +37,7 @@ public:
   }
 
   /// Empty constructor with keys
-  JacobianFactorQ(const FastVector<Key>& keys, //
+  JacobianFactorQ(const KeyVector& keys, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
@@ -50,7 +50,7 @@ public:
   }
 
   /// Constructor
-  JacobianFactorQ(const FastVector<Key>& keys,
+  JacobianFactorQ(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
       const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h
index 9e61f5324..aef10eb86 100644
--- a/gtsam/slam/JacobianFactorQR.h
+++ b/gtsam/slam/JacobianFactorQR.h
@@ -28,7 +28,7 @@ public:
   /**
    * Constructor
    */
-  JacobianFactorQR(const FastVector<Key>& keys,
+  JacobianFactorQR(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
       const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
@@ -46,7 +46,7 @@ public:
     // eliminate the point
     boost::shared_ptr<GaussianBayesNet> bn;
     GaussianFactorGraph::shared_ptr fg;
-    std::vector<Key> variables;
+    KeyVector variables;
     variables.push_back(pointKey);
     boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
     //fg->print("fg");
diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h
index e7bc5864d..b94714dd6 100644
--- a/gtsam/slam/JacobianFactorSVD.h
+++ b/gtsam/slam/JacobianFactorSVD.h
@@ -38,7 +38,7 @@ public:
   }
 
   /// Empty constructor with keys
-  JacobianFactorSVD(const FastVector<Key>& keys, //
+  JacobianFactorSVD(const KeyVector& keys, //
       const SharedDiagonal& model = SharedDiagonal()) :
       Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
@@ -58,7 +58,7 @@ public:
    *
    * @Fblocks:
    */
-  JacobianFactorSVD(const FastVector<Key>& keys,
+  JacobianFactorSVD(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
       const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h
index 47c9a4c7b..07b749811 100644
--- a/gtsam/slam/RegularImplicitSchurFactor.h
+++ b/gtsam/slam/RegularImplicitSchurFactor.h
@@ -48,7 +48,7 @@ public:
   }
 
   /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
-  RegularImplicitSchurFactor(const FastVector<Key>& keys,
+  RegularImplicitSchurFactor(const KeyVector& keys,
       const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
       const Vector& b) :
       GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
@@ -108,7 +108,7 @@ public:
     return D;
   }
 
-  virtual void updateHessian(const FastVector<Key>& keys,
+  virtual void updateHessian(const KeyVector& keys,
                          SymmetricBlockMatrix* info) const {
     throw std::runtime_error(
         "RegularImplicitSchurFactor::updateHessian non implemented");
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 497ebbc5b..489a4adf4 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -131,7 +131,7 @@ public:
   /**
    * Add a bunch of measurements, together with the camera keys
    */
-  void add(ZVector& measurements, std::vector<Key>& cameraKeys) {
+  void add(ZVector& measurements, KeyVector& cameraKeys) {
     for (size_t i = 0; i < measurements.size(); i++) {
       this->measured_.push_back(measurements.at(i));
       this->keys_.push_back(cameraKeys.at(i));
@@ -310,7 +310,7 @@ public:
   void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
       const double lambda, bool diagonalDamping,
       SymmetricBlockMatrix& augmentedHessian,
-      const FastVector<Key> allKeys) const {
+      const KeyVector allKeys) const {
     Matrix E;
     Vector b;
     computeJacobians(Fs, E, b, cameras, point);
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index 0d9f90d22..e554e0c85 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -179,7 +179,7 @@ public:
 
     size_t numKeys = this->keys_.size();
     // Create structures for Hessian Factors
-    std::vector<Key> js;
+    KeyVector js;
     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
     std::vector<Vector> gs(numKeys);
 
diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp
index e1cf9cea2..34c8385e8 100644
--- a/gtsam/slam/lago.cpp
+++ b/gtsam/slam/lago.cpp
@@ -168,14 +168,14 @@ GaussianFactorGraph buildLinearOrientationGraph(
 
   // put original measurements in the spanning tree
   for(const size_t& factorId: spanningTreeIds) {
-    const FastVector<Key>& keys = g[factorId]->keys();
+    const KeyVector& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
     lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
   }
   // put regularized measurements in the chordsIds
   for(const size_t& factorId: chordsIds) {
-    const FastVector<Key>& keys = g[factorId]->keys();
+    const KeyVector& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
     double key1_DeltaTheta_key2 = deltaTheta(0);
diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
index 6f9371e68..02893e628 100644
--- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
+++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
@@ -42,7 +42,7 @@ const Matrix26 F0 = Matrix26::Ones();
 const Matrix26 F1 = 2 * Matrix26::Ones();
 const Matrix26 F3 = 3 * Matrix26::Ones();
 const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
-const FastVector<Key> keys = list_of<Key>(0)(1)(3);
+const KeyVector keys = list_of<Key>(0)(1)(3);
 // RHS and sigmas
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
 
@@ -86,7 +86,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
   F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
 
   // Calculate expected result F'*alpha*(I - E*P*E')*F*x
-  FastVector<Key> keys2;
+  KeyVector keys2;
   keys2 += 0,1,2,3;
   Vector x = xvalues.vector(keys2);
   Vector expected = Vector::Zero(24);
diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
index 17fec7b9f..be0a9df88 100644
--- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
@@ -170,7 +170,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(c1);
   views.push_back(c2);
 
@@ -195,7 +195,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
   SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
-  vector<Key> views;
+  KeyVector views;
   views.push_back(c1);
   views.push_back(c2);
   views.push_back(c3);
@@ -293,7 +293,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(c1);
   views.push_back(c2);
   views.push_back(c3);
@@ -370,7 +370,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(c1);
   views.push_back(c2);
   views.push_back(c3);
@@ -450,7 +450,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(c1);
   views.push_back(c2);
   views.push_back(c3);
@@ -526,7 +526,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(c1);
   views.push_back(c2);
   views.push_back(c3);
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index 7d3d9c63c..080046dd4 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -189,7 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -236,7 +236,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   // Create smart factors
-  std::vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -299,7 +299,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -370,7 +370,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
   measurements_cam1.push_back(cam2.project(landmark1));
 
   // Create smart factors
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -459,7 +459,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
     b.setZero();
 
     // Create smart factors
-    FastVector<Key> keys;
+    KeyVector keys;
     keys.push_back(x1);
     keys.push_back(x2);
 
@@ -520,7 +520,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -577,7 +577,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -638,7 +638,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
 
   double excludeLandmarksFutherThanDist = 2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -701,7 +701,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
   double excludeLandmarksFutherThanDist = 1e10;
   double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -767,7 +767,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -821,7 +821,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
 
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -869,7 +869,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, CheckHessian) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -955,7 +955,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
 TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1014,7 +1014,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   // this test considers a condition in which the cheirality constraint is triggered
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1099,7 +1099,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
 
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -1133,7 +1133,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
 
   using namespace vanillaPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1186,7 +1186,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
 
   using namespace vanillaPose2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1260,7 +1260,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1309,7 +1309,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
 
   using namespace bundlerPose;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1421,7 +1421,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
   SmartFactor factor(model, sharedK, bts, params);
 
   // insert some measurments
-  vector<Key> key_view;
+  KeyVector key_view;
   Point2Vector meas_view;
   key_view.push_back(Symbol('x', 1));
   meas_view.push_back(Point2(10, 10));
diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h
index 47bb06515..e3b4c3297 100644
--- a/gtsam/symbolic/SymbolicFactor-inst.h
+++ b/gtsam/symbolic/SymbolicFactor-inst.h
@@ -57,7 +57,7 @@ namespace gtsam
       const size_t nFrontals = keys.size();
 
       // Build a key vector with the frontals followed by the separator
-      FastVector<Key> orderedKeys(allKeys.size());
+      KeyVector orderedKeys(allKeys.size());
       std::copy(keys.begin(), keys.end(), orderedKeys.begin());
       std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
 
diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
index 99a09adc9..f7df47f5f 100644
--- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
+++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
@@ -81,7 +81,7 @@ TEST( SymbolicBayesNet, combine )
 TEST(SymbolicBayesNet, saveGraph) {
   SymbolicBayesNet bn;
   bn += SymbolicConditional(_A_, _B_);
-  std::vector<Key> keys;
+  KeyVector keys;
   keys.push_back(_B_);
   keys.push_back(_C_);
   keys.push_back(_D_);
diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp
index 57c3a2d5f..18f3b84cc 100644
--- a/gtsam/symbolic/tests/testSymbolicFactor.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp
@@ -33,7 +33,7 @@ using namespace boost::assign;
 /* ************************************************************************* */
 #ifdef TRACK_ELIMINATE
 TEST(SymbolicFactor, eliminate) {
-  vector<Key> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
+  KeyVector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
   IndexFactor actual(keys.begin(), keys.end());
   BayesNet<IndexConditional> fragment = *actual.eliminate(3);
 
diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
index a26d2f581..3fd318456 100644
--- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
@@ -85,7 +85,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
   SymbolicBayesNet::shared_ptr actualBayesNet2;
   SymbolicFactorGraph::shared_ptr actualSfg2;
   boost::tie(actualBayesNet2, actualSfg2) =
-    simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<vector<Key> >());
+    simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<KeyVector >());
 
   EXPECT(assert_equal(expectedSfg, *actualSfg2));
   EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
@@ -137,7 +137,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
   SymbolicBayesTree::shared_ptr actualBayesTree2;
   SymbolicFactorGraph::shared_ptr actualFactorGraph2;
   boost::tie(actualBayesTree2, actualFactorGraph2) =
-    simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<vector<Key> >());
+    simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<KeyVector >());
 
   EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
   EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h
index 93f5c5d7d..bbdadd3dc 100644
--- a/gtsam_unstable/discrete/CSP.h
+++ b/gtsam_unstable/discrete/CSP.h
@@ -22,7 +22,7 @@ namespace gtsam {
   public:
 
     /** A map from keys to values */
-    typedef std::vector<Key> Indices;
+    typedef KeyVector Indices;
     typedef Assignment<Key> Values;
     typedef boost::shared_ptr<Values> sharedValues;
 
diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h
index d8f4689a8..32fb6f1ce 100644
--- a/gtsam_unstable/discrete/Constraint.h
+++ b/gtsam_unstable/discrete/Constraint.h
@@ -38,7 +38,7 @@ namespace gtsam {
   protected:
 
     /// Construct n-way factor
-    Constraint(const std::vector<Key>& js) :
+    Constraint(const KeyVector& js) :
       DiscreteFactor(js) {
     }
 
diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp
index 72d424baf..740ef067c 100644
--- a/gtsam_unstable/discrete/Domain.cpp
+++ b/gtsam_unstable/discrete/Domain.cpp
@@ -57,7 +57,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  bool Domain::checkAllDiff(const vector<Key> keys, vector<Domain>& domains) {
+  bool Domain::checkAllDiff(const KeyVector keys, vector<Domain>& domains) {
     Key j = keys_[0];
     // for all values in this domain
     for(size_t value: values_) {
diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h
index 812da9932..5dd597e20 100644
--- a/gtsam_unstable/discrete/Domain.h
+++ b/gtsam_unstable/discrete/Domain.h
@@ -104,7 +104,7 @@ namespace gtsam {
      *  If found, we make this a singleton... Called in AllDiff::ensureArcConsistency
      *  @param keys connected domains through alldiff
      */
-    bool checkAllDiff(const std::vector<Key> keys, std::vector<Domain>& domains);
+    bool checkAllDiff(const KeyVector keys, std::vector<Domain>& domains);
 
     /// Partially apply known values
     virtual Constraint::shared_ptr partiallyApply(
diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp
index ec71cae5b..47672a947 100644
--- a/gtsam_unstable/linear/RawQP.cpp
+++ b/gtsam_unstable/linear/RawQP.cpp
@@ -203,9 +203,9 @@ void RawQP::addQuadTerm(
 }
 
 QP RawQP::makeQP() {
-  std::vector < Key > keys;
-  std::vector < Matrix > Gs;
-  std::vector < Vector > gs;
+  KeyVector keys;
+  std::vector<Matrix> Gs;
+  std::vector<Vector> gs;
   for (auto kv : varname_to_key) {
     keys.push_back(kv.second);
   }
diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h
index aadf11e50..70b2a9d27 100644
--- a/gtsam_unstable/linear/RawQP.h
+++ b/gtsam_unstable/linear/RawQP.h
@@ -49,7 +49,7 @@ private:
   std::string name_;
   std::unordered_map<Key, double> up;
   std::unordered_map<Key, double> lo;
-  std::vector<Key> Free;
+  KeyVector Free;
   const bool debug = false;
 
 public:
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index 884bba9a3..51a959075 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -362,7 +362,7 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
 
   // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
   if(keysToMove && keysToMove->size() > 0) {
-    ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
+    ordering_ = Ordering::ColamdConstrainedFirst(factors_, KeyVector(keysToMove->begin(), keysToMove->end()));
   }else{
     ordering_ = Ordering::Colamd(factors_);
   }
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
index d64b697b8..0fc975958 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
@@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
   variableIndex_ = VariableIndex(factors_);
 
   KeyVector separatorKeys = separatorValues_.keys();
-  ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
+  ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, KeyVector(separatorKeys.begin(), separatorKeys.end()));
 
 }
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
index 61f1c889f..70cb3e268 100644
--- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
@@ -72,7 +72,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph,
     GaussianFactorGraph linearFactorGraph = *graph.linearize(theta);
     // .first is the eliminated Bayes tree, while .second is the remaining factor graph
     GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(
-        std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;
+        KeyVector(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;
 
     // Wrap in nonlinear container factors
     NonlinearFactorGraph marginalFactors;
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
index 3913ba95c..e7c8229ef 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
@@ -353,7 +353,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
   }
 
   // Create the set of clique keys LC:
-  std::vector<Key> cliqueKeys;
+  KeyVector cliqueKeys;
   for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     for(Key key: clique->conditional()->frontals()) {
       cliqueKeys.push_back(key);
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
index e95c1c81d..41a94b876 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
@@ -194,7 +194,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
   }
 
   // Create the set of clique keys LC:
-  std::vector<Key> cliqueKeys;
+  KeyVector cliqueKeys;
   for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
     for(Key key: clique->conditional()->frontals()) {
       cliqueKeys.push_back(key);
diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h
index b1f463c26..a483c9521 100644
--- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h
+++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h
@@ -19,7 +19,7 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
     // Given graph, index, add factors with specified keys into
     // Factors are erased in the graph
     // TODO(frank): fairly hacky and inefficient. Think about iterating the graph once instead
-    NonlinearCluster(const VariableIndex& variableIndex, const std::vector<Key>& keys,
+    NonlinearCluster(const VariableIndex& variableIndex, const KeyVector& keys,
                      NonlinearFactorGraph* graph) {
       for (const Key key : keys) {
         std::vector<NonlinearFactor::shared_ptr> factors;
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
index 4aff1b465..7b683208c 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
@@ -81,7 +81,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
   GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
 
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph LinearContainerForGaussianMarginals;
   for(const GaussianFactor::shared_ptr& factor: marginal) {
@@ -419,7 +419,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
   ordering.push_back(3);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
+  KeyVector linearIndices;
   linearIndices.push_back(1);
   linearIndices.push_back(2);
 
@@ -1008,7 +1008,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
   GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
+  KeyVector linearIndices;
   linearIndices.push_back(1);
 
   GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
@@ -1062,7 +1062,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
   GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
+  KeyVector linearIndices;
   linearIndices.push_back(1);
   linearIndices.push_back(2);
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
index a0b6e2c1b..8ecd087c5 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
@@ -563,7 +563,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
   for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     eliminateKeys.erase(key_value.key);
   }
-  std::vector<Key> variables(eliminateKeys.begin(), eliminateKeys.end());
+  KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());
   GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second;
 
   expectedSmootherSummarization.resize(0);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
index d6b7ab851..3cd59efec 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
@@ -98,7 +98,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
 
   GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
 
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph LinearContainerForGaussianMarginals;
   for(const GaussianFactor::shared_ptr& factor: marginal) {
@@ -429,7 +429,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
 
   GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
 
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph expectedGraph;
 
@@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
 
   GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues);
 
-    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
+    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
 
     NonlinearFactorGraph expectedGraph;
 
@@ -1108,12 +1108,12 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
   newValues.insert(3, value3);
 
   // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
+  KeyVector linearIndices;
   linearIndices.push_back(1);
 
   GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
 
-    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
+    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
 
     NonlinearFactorGraph expectedMarginals;
     for(const GaussianFactor::shared_ptr& factor: marginal) {
@@ -1157,14 +1157,14 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
 
 
     // Create the set of marginalizable variables
-  std::vector<Key> linearIndices;
+  KeyVector linearIndices;
   linearIndices.push_back(1);
   linearIndices.push_back(2);
 
 
   GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
 
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
 
   NonlinearFactorGraph expectedMarginals;
   for(const GaussianFactor::shared_ptr& factor: marginal) {
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
index c265bf5d1..d7bddece2 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
@@ -583,7 +583,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
   for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
     allkeys.erase(key_value.key);
   }
-  std::vector<Key> variables(allkeys.begin(), allkeys.end());
+  KeyVector variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
 
   expectedSmootherSummarization.resize(0);
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
index fdf9f08b5..82d8f2153 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
@@ -584,7 +584,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
   KeySet allkeys = LinFactorGraph->keys();
   for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
     allkeys.erase(key_value.key);
-  std::vector<Key> variables(allkeys.begin(), allkeys.end());
+  KeyVector variables(allkeys.begin(), allkeys.end());
   std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
 
   expectedSmootherSummarization.resize(0);
diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index 56b1269f0..b009927d6 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -40,10 +40,10 @@ public:
 private:
 
   typedef BetweenFactorEM<VALUE> This;
-  typedef gtsam::NonlinearFactor Base;
+  typedef NonlinearFactor Base;
 
-  gtsam::Key key1_;
-  gtsam::Key key2_;
+  Key key1_;
+  Key key2_;
 
   VALUE measured_; /** The measurement */
 
@@ -114,38 +114,38 @@ public:
   /** implement functions needed to derive from Factor */
 
   /* ************************************************************************* */
-  virtual double error(const gtsam::Values& x) const {
+  virtual double error(const Values& x) const {
     return whitenedError(x).squaredNorm();
   }
 
   /* ************************************************************************* */
   /**
-   * Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
+   * Linearize a non-linearFactorN to get a GaussianFactor,
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
   /* This version of linearize recalculates the noise model each time */
-  virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
-      const gtsam::Values& x) const {
+  virtual boost::shared_ptr<GaussianFactor> linearize(
+      const Values& x) const {
     // Only linearize if the factor is active
     if (!this->active(x))
-      return boost::shared_ptr<gtsam::JacobianFactor>();
+      return boost::shared_ptr<JacobianFactor>();
 
     //std::cout<<"About to linearize"<<std::endl;
-    gtsam::Matrix A1, A2;
-    std::vector<gtsam::Matrix> A(this->size());
-    gtsam::Vector b = -whitenedError(x, A);
+    Matrix A1, A2;
+    std::vector<Matrix> A(this->size());
+    Vector b = -whitenedError(x, A);
     A1 = A[0];
     A2 = A[1];
 
-    return gtsam::GaussianFactor::shared_ptr(
-        new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
-            gtsam::noiseModel::Unit::Create(b.size())));
+    return GaussianFactor::shared_ptr(
+        new JacobianFactor(key1_, A1, key2_, A2, b,
+            noiseModel::Unit::Create(b.size())));
   }
 
   /* ************************************************************************* */
-  gtsam::Vector whitenedError(const gtsam::Values& x,
-      boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
+  Vector whitenedError(const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const {
 
     bool debug = true;
 
@@ -181,11 +181,11 @@ public:
 
       Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
       Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
-      Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier);
+      Matrix H1_aug = stack(2, &H1_inlier, &H1_outlier);
 
       Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
       Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
-      Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier);
+      Matrix H2_aug = stack(2, &H2_inlier, &H2_outlier);
 
       (*H)[0].resize(H1_aug.rows(), H1_aug.cols());
       (*H)[1].resize(H2_aug.rows(), H2_aug.cols());
@@ -229,7 +229,7 @@ public:
   }
 
   /* ************************************************************************* */
-  gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
+  Vector calcIndicatorProb(const Values& x) const {
 
     bool debug = false;
 
@@ -285,7 +285,7 @@ public:
   }
 
   /* ************************************************************************* */
-  gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
+  Vector unwhitenedError(const Values& x) const {
 
     const T& p1 = x.at<T>(key1_);
     const T& p2 = x.at<T>(key2_);
@@ -328,8 +328,8 @@ public:
   }
 
   /* ************************************************************************* */
-  void updateNoiseModels(const gtsam::Values& values,
-      const gtsam::NonlinearFactorGraph& graph) {
+  void updateNoiseModels(const Values& values,
+      const NonlinearFactorGraph& graph) {
     /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
      * (note these are given in the E step, where indicator probabilities are calculated).
      *
@@ -340,7 +340,7 @@ public:
      */
 
     // get joint covariance of the involved states
-    std::vector<gtsam::Key> Keys;
+    KeyVector Keys;
     Keys.push_back(key1_);
     Keys.push_back(key2_);
     Marginals marginals(graph, values, Marginals::QR);
@@ -353,7 +353,7 @@ public:
   }
 
   /* ************************************************************************* */
-  void updateNoiseModels_givenCovs(const gtsam::Values& values,
+  void updateNoiseModels_givenCovs(const Values& values,
       const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
     /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
      * (note these are given in the E step, where indicator probabilities are calculated).
@@ -385,12 +385,12 @@ public:
     // update inlier and outlier noise models
     Matrix covRinlier =
         (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
-    model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(
+    model_inlier_ = noiseModel::Gaussian::Covariance(
         covRinlier + cov_state);
 
     Matrix covRoutlier =
         (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
-    model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
+    model_outlier_ = noiseModel::Gaussian::Covariance(
         covRoutlier + cov_state);
 
     //       model_inlier_->print("after:");
@@ -426,4 +426,4 @@ private:
 };
 // \class BetweenFactorEM
 
-}/// namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index b7d06b268..f32dd3b3e 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -202,7 +202,7 @@ public:
 
     size_t numKeys = this->keys_.size();
     // Create structures for Hessian Factors
-    std::vector<Key> js;
+    KeyVector js;
     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
     std::vector<Vector> gs(numKeys);
 
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
index 3b4c5e0db..7ea5a4c2f 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
@@ -92,7 +92,7 @@ public:
    * @param poseKeys vector of keys corresponding to the camera observing the same landmark
    * @param Ks vector of calibration objects
    */
-  void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
+  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
       std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
     Base::add(measurements, poseKeys);
     for (size_t i = 0; i < measurements.size(); i++) {
@@ -106,7 +106,7 @@ public:
    * @param poseKeys vector of keys corresponding to the camera observing the same landmark
    * @param K the (known) camera calibration (same for all measurements)
    */
-  void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
+  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
       const boost::shared_ptr<Cal3_S2Stereo> K) {
     for (size_t i = 0; i < measurements.size(); i++) {
       Base::add(measurements.at(i), poseKeys.at(i));
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index bf10ae531..b6d906fd4 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -43,16 +43,16 @@ namespace gtsam {
   private:
 
     typedef TransformBtwRobotsUnaryFactorEM<VALUE> This;
-    typedef gtsam::NonlinearFactor Base;
+    typedef NonlinearFactor Base;
 
-    gtsam::Key key_;
+    Key key_;
 
     VALUE measured_; /** The measurement */
 
-    gtsam::Values valA_; // given values for robot A map\trajectory
-    gtsam::Values valB_; // given values for robot B map\trajectory
-    gtsam::Key keyA_;    // key of robot A to which the measurement refers
-    gtsam::Key keyB_;    // key of robot B to which the measurement refers
+    Values valA_; // given values for robot A map\trajectory
+    Values valB_; // given values for robot B map\trajectory
+    Key keyA_;    // key of robot A to which the measurement refers
+    Key keyB_;    // key of robot B to which the measurement refers
 
     // TODO: create function to update valA_ and valB_
 
@@ -79,7 +79,7 @@ namespace gtsam {
 
     /** Constructor */
     TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
-        const gtsam::Values& valA, const gtsam::Values& valB,
+        const Values& valA, const Values& valB,
         const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
         const double prior_inlier, const double prior_outlier,
         const bool flag_bump_up_near_zero_probs = false,
@@ -97,7 +97,7 @@ namespace gtsam {
 
 
     /** Clone */
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
+    virtual NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
 
 
     /** implement functions needed for Testable */
@@ -134,7 +134,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /* ************************************************************************* */
-    void setValAValB(const gtsam::Values& valA, const gtsam::Values& valB){
+    void setValAValB(const Values& valA, const Values& valB){
       if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
         throw("something is wrong!");
 
@@ -151,36 +151,36 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    virtual double error(const gtsam::Values& x) const {
+    virtual double error(const Values& x) const {
       return whitenedError(x).squaredNorm();
     }
 
     /* ************************************************************************* */
     /**
-     * Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
+     * Linearize a non-linearFactorN to get a GaussianFactor,
      * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
      * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
      */
     /* This version of linearize recalculates the noise model each time */
-    virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
+    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
       // Only linearize if the factor is active
       if (!this->active(x))
-        return boost::shared_ptr<gtsam::JacobianFactor>();
+        return boost::shared_ptr<JacobianFactor>();
 
       //std::cout<<"About to linearize"<<std::endl;
-      gtsam::Matrix A1;
-      std::vector<gtsam::Matrix> A(this->size());
-      gtsam::Vector b = -whitenedError(x, A);
+      Matrix A1;
+      std::vector<Matrix> A(this->size());
+      Vector b = -whitenedError(x, A);
       A1 = A[0];
 
-      return gtsam::GaussianFactor::shared_ptr(
-          new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size())));
+      return GaussianFactor::shared_ptr(
+          new JacobianFactor(key_, A1, b, noiseModel::Unit::Create(b.size())));
     }
 
 
     /* ************************************************************************* */
-    gtsam::Vector whitenedError(const gtsam::Values& x,
-        boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
+    Vector whitenedError(const Values& x,
+        boost::optional<std::vector<Matrix>&> H = boost::none) const {
 
       bool debug = true;
 
@@ -227,7 +227,7 @@ namespace gtsam {
 
         Matrix H_inlier  = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh);
         Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh);
-        Matrix H_aug = gtsam::stack(2, &H_inlier, &H_outlier);
+        Matrix H_aug = stack(2, &H_inlier, &H_outlier);
 
         (*H)[0].resize(H_aug.rows(),H_aug.cols());
         (*H)[0] = H_aug;
@@ -246,7 +246,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const {
+    Vector calcIndicatorProb(const Values& x) const {
 
       Vector err =  unwhitenedError(x);
 
@@ -254,7 +254,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    gtsam::Vector calcIndicatorProb(const gtsam::Values& x, const gtsam::Vector& err) const {
+    Vector calcIndicatorProb(const Values& x, const Vector& err) const {
 
       // Calculate indicator probabilities (inlier and outlier)
       Vector err_wh_inlier  = model_inlier_->whiten(err);
@@ -288,7 +288,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
+    Vector unwhitenedError(const Values& x) const {
 
       T orgA_T_currA = valA_.at<T>(keyA_);
       T orgB_T_currB = valB_.at<T>(keyB_);
@@ -325,10 +325,10 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) {
+    void updateNoiseModels(const Values& values, const Marginals& marginals) {
       /* given marginals version, don't need to marginal multiple times if update a lot */
 
-      std::vector<gtsam::Key> Keys;
+      KeyVector Keys;
       Keys.push_back(keyA_);
       Keys.push_back(keyB_);
       JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
@@ -340,7 +340,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
+    void updateNoiseModels(const Values& values, const NonlinearFactorGraph& graph){
       /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
        * (note these are given in the E step, where indicator probabilities are calculated).
        *
@@ -358,7 +358,7 @@ namespace gtsam {
     }
 
     /* ************************************************************************* */
-    void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
+    void updateNoiseModels_givenCovs(const Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
       /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
        * (note these are given in the E step, where indicator probabilities are calculated).
        *
@@ -389,10 +389,10 @@ namespace gtsam {
 
       // update inlier and outlier noise models
       Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
-      model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
+      model_inlier_ = noiseModel::Gaussian::Covariance(covRinlier + cov_state);
 
       Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
-      model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
+      model_outlier_ = noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
 
       //       model_inlier_->print("after:");
       //       std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
index 229a49e3f..0ac2c24ee 100644
--- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
@@ -274,7 +274,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
   Ks.push_back(K);
   Ks.push_back(K);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
 
@@ -313,7 +313,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
   vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -455,7 +455,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
   vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
       cam2, cam3, landmark3);
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   // Create smart factors
-  std::vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -614,7 +614,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -680,7 +680,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 
 //  double excludeLandmarksFutherThanDist = 2;
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -822,7 +822,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -919,7 +919,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -980,7 +980,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -1040,7 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1128,7 +1128,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -1194,7 +1194,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
@@ -1268,7 +1268,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, Hessian ){
 //
-//  vector<Key> views;
+//  KeyVector views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //
@@ -1309,7 +1309,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -1379,7 +1379,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
 /* *************************************************************************/
 TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
 
-  vector<Key> views;
+  KeyVector views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index b6b196acc..769b458e4 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -231,7 +231,7 @@ TEST(ExpressionFactor, Shallow) {
   Point2_ expression = project(transform_to(x_, p_));
 
   // Get and check keys and dims
-  FastVector<Key> keys;
+  KeyVector keys;
   FastVector<int> dims;
   boost::tie(keys, dims) = expression.keysAndDims();
   LONGS_EQUAL(2,keys.size());
diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp
index 392b84deb..844510018 100644
--- a/tests/testMarginals.cpp
+++ b/tests/testMarginals.cpp
@@ -139,7 +139,7 @@ TEST(Marginals, planarSLAMmarginals) {
       0.151935669757191,   0.007741936219615,   0.090000180000270,  -0.000000000000000,   0.000000000000000,   0.160967924878730,   0.007741936219615,   0.004516127560770,
      -0.104516127560770,   0.351935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,   0.007741936219615,   0.351935664055174,   0.056129031890193,
      -0.050967744878460,   0.056129031890193,   0.000000000000000,   0.000000000000000,   0.010000000000000,   0.004516127560770,   0.056129031890193,   0.027741936219615;
-  vector<Key> variables(3);
+  KeyVector variables(3);
   variables[0] = x1;
   variables[1] = l2;
   variables[2] = x3;
@@ -227,7 +227,7 @@ TEST(Marginals, order) {
 
   Marginals marginals(fg, vals);
   KeySet set = fg.keys();
-  FastVector<Key> keys(set.begin(), set.end());
+  KeyVector keys(set.begin(), set.end());
   JointMarginal joint = marginals.jointMarginalCovariance(keys);
 
   LONGS_EQUAL(3, (long)joint(0,0).rows());
diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp
index e64c34340..f00e5d6f7 100644
--- a/timing/timeSchurFactors.cpp
+++ b/timing/timeSchurFactors.cpp
@@ -39,7 +39,7 @@ void timeAll(size_t m, size_t N) {
   // create F
   static const int D = CAMERA::dimension;
   typedef Eigen::Matrix<double, 2, D> Matrix2D;
-  FastVector<Key> keys;
+  KeyVector keys;
   vector <Matrix2D> Fblocks;
   for (size_t i = 0; i < m; i++) {
     keys.push_back(i);

From d86782eebcf730b743539b692f6ede96b1e8f856 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 12:25:21 -0500
Subject: [PATCH 045/281] Some remaining std::vector stragglers

---
 examples/SolverComparer.cpp   | 6 +++---
 tests/testGaussianISAM2.cpp   | 2 +-
 tests/testNonlinearFactor.cpp | 2 +-
 timing/timeIncremental.cpp    | 2 +-
 4 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index b63e5faeb..f3915ce22 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -429,7 +429,7 @@ void runIncremental()
   //    for (Key key12: boost::adaptors::reverse(values.keys())) {
   //      if(i != j) {
   //        gttic_(jointMarginalInformation);
-  //        std::vector<Key> keys(2);
+  //        KeyVector keys(2);
   //        keys[0] = key1;
   //        keys[1] = key2;
   //        JointMarginal info = marginals.jointMarginalInformation(keys);
@@ -524,7 +524,7 @@ void runCompare()
 
   // Check solution for equality
   cout << "Comparing solutions..." << endl;
-  vector<Key> missingKeys;
+  KeyVector missingKeys;
   br::set_symmetric_difference(soln1.keys(), soln2.keys(), std::back_inserter(missingKeys));
   if(!missingKeys.empty()) {
     cout << "  Keys unique to one solution file: ";
@@ -535,7 +535,7 @@ void runCompare()
     }
     cout << endl;
   }
-  vector<Key> commonKeys;
+  KeyVector commonKeys;
   br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
   double maxDiff = 0.0;
   for(Key j: commonKeys)
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index d6ca896dd..2b6809ada 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -648,7 +648,7 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
 namespace {
   bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
     Matrix expectedAugmentedHessian, expected3AugmentedHessian;
-    vector<Key> toKeep;
+    KeyVector toKeep;
     for(Key j: isam.getDelta() | br::map_keys)
       if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end())
         toKeep.push_back(j);
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index fb85c3742..2240539f4 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -401,7 +401,7 @@ TEST( NonlinearFactor, clone_rekey )
   EXPECT(assert_equal(*init, *actClone));
 
   // Re-key factor - clones with different keys
-  std::vector<Key> new_keys(4);
+  KeyVector new_keys(4);
   new_keys[0] = X(5);
   new_keys[1] = X(6);
   new_keys[2] = X(7);
diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp
index 61e129699..8f5be86d6 100644
--- a/timing/timeIncremental.cpp
+++ b/timing/timeIncremental.cpp
@@ -231,7 +231,7 @@ int main(int argc, char *argv[]) {
       for (Key key2: boost::adaptors::reverse(values.keys())) {
         if(i != j) {
           gttic_(jointMarginalInformation);
-          std::vector<Key> keys(2);
+          KeyVector keys(2);
           keys[0] = key1;
           keys[1] = key2;
           JointMarginal info = marginals.jointMarginalInformation(keys);

From b7f7e147cdb71923290d8dbe0e9fd2fe885bbfbb Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 13:39:03 -0500
Subject: [PATCH 046/281] Removed explicit again

---
 gtsam/base/FastVector.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 3616704f7..307a75c87 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -64,13 +64,13 @@ class FastVector: public std::vector<VALUE,
   }
 
   /** Copy constructor from the base class */
-  explicit FastVector(const Base& x) :
+  FastVector(const Base& x) :
       Base(x) {
   }
 
   /** Copy constructor from a standard STL container */
   template<typename ALLOCATOR>
-  explicit FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
+  FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
     // This if statement works around a bug in boost pool allocator and/or
     // STL vector where if the size is zero, the pool allocator will allocate
     // huge amounts of memory.

From 6fef22f1edc87a407adecc18013498c7677558b9 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 16:18:12 -0500
Subject: [PATCH 047/281] C++ typedef is all we need?

---
 gtsam/base/FastVector.h | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 307a75c87..46bc25642 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -19,12 +19,20 @@
 #pragma once
 
 #include <gtsam/base/FastDefaultAllocator.h>
-#include <boost/serialization/nvp.hpp>
-#include <boost/serialization/vector.hpp>
 #include <vector>
 
 namespace gtsam {
 
+#ifndef GTSAM_USE_OLD_FAST_VECTOR
+
+template <typename T>
+using FastVector = std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
+
+#else
+
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/vector.hpp>
+
 /**
  * FastVector is a thin wrapper around std::vector that uses the boost
  * pool_allocator instead of the default STL allocator.  This is just a
@@ -91,4 +99,6 @@ class FastVector: public std::vector<VALUE,
     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
   }
 };
+#endif
+
 }  // namespace gtsam

From 99e346d6d14d31ac2fde49c56da786eb84d57238 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 16:37:33 -0500
Subject: [PATCH 048/281] return const & to vectors and remove obsolete member
 variable

---
 gtsam/inference/MetisIndex.h | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index 22b03ee5d..943edf79d 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -51,7 +51,6 @@ public:
 private:
   FastVector<int32_t> xadj_; // Index of node's adjacency list in adj
   FastVector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
-  FastVector<int32_t> iadj_; // Integer keys for passing into metis. One to one mapping with adj_;
   boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship
   size_t nKeys_;
 
@@ -83,10 +82,10 @@ public:
   template<class FACTOR>
   void augment(const FactorGraph<FACTOR>& factors);
 
-  std::vector<int32_t> xadj() const {
+  const FastVector<int32_t>& xadj() const {
     return xadj_;
   }
-  std::vector<int32_t> adj() const {
+  const FastVector<int32_t>& adj() const {
     return adj_;
   }
   size_t nValues() const {

From 355bb2533932858725aa81321b362fc9edc8a051 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 16:53:08 -0500
Subject: [PATCH 049/281] Metis does not like TBB allocated vectors

---
 gtsam/inference/MetisIndex.h | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index 943edf79d..7ec435caa 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -19,7 +19,6 @@
 
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/FactorGraph.h>
-#include <gtsam/base/FastVector.h>
 #include <gtsam/base/types.h>
 #include <gtsam/base/timing.h>
 
@@ -49,8 +48,8 @@ public:
   typedef boost::bimap<Key, int32_t> bm_type;
 
 private:
-  FastVector<int32_t> xadj_; // Index of node's adjacency list in adj
-  FastVector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
+  std::vector<int32_t> xadj_; // Index of node's adjacency list in adj
+  std::vector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
   boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship
   size_t nKeys_;
 
@@ -82,10 +81,10 @@ public:
   template<class FACTOR>
   void augment(const FactorGraph<FACTOR>& factors);
 
-  const FastVector<int32_t>& xadj() const {
+  const std::vector<int32_t>& xadj() const {
     return xadj_;
   }
-  const FastVector<int32_t>& adj() const {
+  const std::vector<int32_t>& adj() const {
     return adj_;
   }
   size_t nValues() const {

From 8f83791bb68cd4ea3c1f5e6b2ed5105567ab96b8 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 17:17:13 -0500
Subject: [PATCH 050/281] Fixed 'make timing' compile errors

---
 timing/timeIncremental.cpp  | 4 ++--
 timing/timeSchurFactors.cpp | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp
index 8f5be86d6..e09b83232 100644
--- a/timing/timeIncremental.cpp
+++ b/timing/timeIncremental.cpp
@@ -153,8 +153,8 @@ int main(int argc, char *argv[]) {
         if(!isam2.getLinearizationPoint().exists(lmKey))
         {
           Pose pose = isam2.calculateEstimate<Pose>(poseKey);
-          Rot2 measuredBearing = measurement->measured().first;
-          double measuredRange = measurement->measured().second;
+          Rot2 measuredBearing = measurement->measured().bearing();
+          double measuredRange = measurement->measured().range();
           newVariables.insert(lmKey, 
             pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
         }
diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp
index f00e5d6f7..9e057f830 100644
--- a/timing/timeSchurFactors.cpp
+++ b/timing/timeSchurFactors.cpp
@@ -40,7 +40,7 @@ void timeAll(size_t m, size_t N) {
   static const int D = CAMERA::dimension;
   typedef Eigen::Matrix<double, 2, D> Matrix2D;
   KeyVector keys;
-  vector <Matrix2D> Fblocks;
+  vector <Matrix2D, Eigen::aligned_allocator<Matrix2D>> Fblocks;
   for (size_t i = 0; i < m; i++) {
     keys.push_back(i);
     Fblocks.push_back((i + 1) * Matrix::Ones(2, D));

From 79fe89eaa97bc7069d3729576c18b30345040902 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 8 Nov 2018 17:18:47 -0500
Subject: [PATCH 051/281] Switched to using c++11 braces-style initialization

---
 gtsam/base/tests/testFastContainers.cpp       |  3 +-
 gtsam/geometry/tests/testCameraSet.cpp        | 10 ++-----
 gtsam/linear/tests/testGaussianBayesNet.cpp   |  2 +-
 gtsam/linear/tests/testGaussianBayesTree.cpp  |  2 +-
 gtsam/linear/tests/testHessianFactor.cpp      |  7 ++---
 gtsam/linear/tests/testJacobianFactor.cpp     |  2 +-
 .../linear/tests/testRegularHessianFactor.cpp |  2 +-
 gtsam/linear/tests/testVectorValues.cpp       |  9 +++---
 gtsam/navigation/tests/testGeographicLib.cpp  |  6 +++-
 .../tests/testLinearContainerFactor.cpp       |  7 ++---
 gtsam/nonlinear/tests/testValues.cpp          |  5 ++--
 gtsam/sam/tests/testRangeFactor.cpp           |  4 +--
 .../tests/testRegularImplicitSchurFactor.cpp  |  5 ++--
 .../tests/testSmartProjectionCameraFactor.cpp | 29 ++++---------------
 gtsam/symbolic/tests/testSymbolicBayesNet.cpp |  5 +---
 gtsam/symbolic/tests/testSymbolicFactor.cpp   |  2 +-
 .../tests/testConcurrentBatchFilter.cpp       | 11 ++-----
 .../tests/testConcurrentIncrementalFilter.cpp | 16 +++-------
 tests/testMarginals.cpp                       |  5 +---
 tests/testNonlinearFactor.cpp                 |  6 +---
 20 files changed, 43 insertions(+), 95 deletions(-)

diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp
index 4909f4ecb..12dee036e 100644
--- a/gtsam/base/tests/testFastContainers.cpp
+++ b/gtsam/base/tests/testFastContainers.cpp
@@ -22,8 +22,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST( testFastContainers, KeySet ) {
 
-  KeyVector init_vector;
-  init_vector += 2, 3, 4, 5;
+  KeyVector init_vector {2, 3, 4, 5};
 
   KeySet actSet(init_vector);
   KeySet expSet; expSet += 2, 3, 4, 5;
diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp
index ab9227a08..f3feab741 100644
--- a/gtsam/geometry/tests/testCameraSet.cpp
+++ b/gtsam/geometry/tests/testCameraSet.cpp
@@ -93,18 +93,12 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal(schur, actualReduced.selfadjointView()));
 
   // Check Schur complement update, same order, should just double
-  KeyVector allKeys, keys;
-  allKeys.push_back(1);
-  allKeys.push_back(2);
-  keys.push_back(1);
-  keys.push_back(2);
+  KeyVector allKeys {1, 2}, keys {1, 2};
   Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced);
   EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
 
   // Check Schur complement update, keys reversed
-  KeyVector keys2;
-  keys2.push_back(2);
-  keys2.push_back(1);
+  KeyVector keys2 {2, 1};
   Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced);
   Vector4 reverse_b;
   reverse_b << b.tail<2>(), b.head<2>();
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 45545b8ea..13601844c 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -229,7 +229,7 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
   VectorValues actual = gbn.optimizeGradientSearch();
 
   // Check that points agree
-  KeyVector keys = list_of(0)(1)(2)(3)(4);
+  KeyVector keys {0, 1, 2, 3, 4};
   Vector actualAsVector = actual.vector(keys);
   EXPECT(assert_equal(expected, actualAsVector, 1e-5));
 
diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp
index 97809bfbc..2e8b60ac5 100644
--- a/gtsam/linear/tests/testGaussianBayesTree.cpp
+++ b/gtsam/linear/tests/testGaussianBayesTree.cpp
@@ -287,7 +287,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
   VectorValues actual = bt.optimizeGradientSearch();
 
   // Check that points agree
-  KeyVector keys = list_of(0)(1)(2)(3)(4);
+  KeyVector keys {0, 1, 2, 3, 4};
   EXPECT(assert_equal(expected, actual.vector(keys), 1e-5));
   EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
 
diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp
index 31030451b..3e13ecf10 100644
--- a/gtsam/linear/tests/testHessianFactor.cpp
+++ b/gtsam/linear/tests/testHessianFactor.cpp
@@ -41,7 +41,7 @@ const double tol = 1e-5;
 /* ************************************************************************* */
 TEST(HessianFactor, Slot)
 {
-  KeyVector keys = list_of(2)(4)(1);
+  KeyVector keys {2, 4, 1};
   EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
   EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
   EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
@@ -252,8 +252,7 @@ TEST(HessianFactor, ConstructorNWay)
     (1, dx1)
     (2, dx2);
 
-  KeyVector js;
-  js.push_back(0); js.push_back(1); js.push_back(2);
+  KeyVector js {0, 1, 2};
   std::vector<Matrix> Gs;
   Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
   std::vector<Vector> gs;
@@ -517,7 +516,7 @@ TEST(HessianFactor, gradientAtZero)
   // test gradient at zero
   VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2);
   Matrix A; Vector b; boost::tie(A,b) = factor.jacobian();
-  KeyVector keys; keys += 0,1;
+  KeyVector keys {0, 1};
   EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
   VectorValues actualG = factor.gradientAtZero();
   EXPECT(assert_equal(expectedG, actualG));
diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index 2403895c2..2ea1b15bd 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -350,7 +350,7 @@ TEST(JacobianFactor, operators )
   VectorValues expectedG;
   expectedG.insert(1, Vector2(20,-10));
   expectedG.insert(2, Vector2(-20, 10));
-  KeyVector keys; keys += 1,2;
+  KeyVector keys {1, 2};
   EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
   VectorValues actualG = lf.gradientAtZero();
   EXPECT(assert_equal(expectedG, actualG));
diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp
index c610255c0..3441c6cc2 100644
--- a/gtsam/linear/tests/testRegularHessianFactor.cpp
+++ b/gtsam/linear/tests/testRegularHessianFactor.cpp
@@ -64,7 +64,7 @@ TEST(RegularHessianFactor, Constructors)
   EXPECT(assert_equal(factor,factor2));
 
   // Test n-way constructor
-  KeyVector keys; keys += 0, 1, 3;
+  KeyVector keys {0, 1, 3};
   vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
   vector<Vector> gs; gs += g1, g2, g3;
   RegularHessianFactor<2> factor3(keys, Gs, gs, f);
diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp
index d5e5aee68..d1d9990b0 100644
--- a/gtsam/linear/tests/testVectorValues.cpp
+++ b/gtsam/linear/tests/testVectorValues.cpp
@@ -62,7 +62,7 @@ TEST(VectorValues, basics)
   EXPECT(assert_equal(Vector2(2, 3), actual[1]));
   EXPECT(assert_equal(Vector2(4, 5), actual[2]));
   EXPECT(assert_equal(Vector2(6, 7), actual[5]));
-  KeyVector keys = list_of(0)(1)(2)(5);
+  KeyVector keys {0, 1, 2, 5};
   EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
 
   // Check exceptions
@@ -101,8 +101,7 @@ TEST(VectorValues, subvector)
   init.insert(12, Vector2(4, 5));
   init.insert(13, Vector2(6, 7));
 
-  KeyVector keys;
-  keys += 10, 12, 13;
+  KeyVector keys {10, 12, 13};
   Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
   EXPECT(assert_equal(expSubVector, init.vector(keys)));
 }
@@ -197,7 +196,7 @@ TEST(VectorValues, convert)
   EXPECT(assert_equal(expected, actual2));
 
   // Test other direction, note vector() is not guaranteed to give right result
-  KeyVector keys = list_of(0)(1)(2)(5);
+  KeyVector keys {0, 1, 2, 5};
   EXPECT(assert_equal(x, actual.vector(keys)));
 
   // Test version with dims argument
@@ -222,7 +221,7 @@ TEST(VectorValues, vector_sub)
   expected << 1, 6, 7;
 
   // Test FastVector version
-  KeyVector keys = list_of(0)(5);
+  KeyVector keys {0, 5};
   EXPECT(assert_equal(expected, vv.vector(keys)));
 
   // Test version with dims argument
diff --git a/gtsam/navigation/tests/testGeographicLib.cpp b/gtsam/navigation/tests/testGeographicLib.cpp
index 6c495faf5..c568c7445 100644
--- a/gtsam/navigation/tests/testGeographicLib.cpp
+++ b/gtsam/navigation/tests/testGeographicLib.cpp
@@ -23,6 +23,8 @@
 #include <gtsam/base/types.h>
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/algorithm/string.hpp>
+#include <string>
 #include <iostream>
 
 using namespace std;
@@ -68,7 +70,9 @@ TEST( GeographicLib, UTM) {
   // UTM is 16N 749305.58 3751090.08
   // Obtained by
   // http://geographiclib.sourceforge.net/cgi-bin/GeoConvert?input=33.87071+-84.30482000000001&zone=-3&prec=2&option=Submit
-  EXPECT(UTMUPS::EncodeZone(zone, northp)=="16N");
+  auto actual = UTMUPS::EncodeZone(zone, northp);
+  boost::to_upper(actual);
+  EXPECT(actual=="16N");
   EXPECT_DOUBLES_EQUAL(749305.58, x, 1e-2);
   EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2);
 }
diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
index 5321eb2c4..9e5e08e92 100644
--- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
+++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp
@@ -47,8 +47,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
   EXPECT(!actFactor.isHessian());
 
   // check keys
-  KeyVector expKeys; expKeys += l1, l2;
-  EXPECT(assert_container_equality(expKeys, actFactor.keys()));
+  EXPECT(assert_container_equality({l1, l2}, actFactor.keys()));
 
   Values values;
   values.insert(l1, landmark1);
@@ -246,9 +245,7 @@ TEST( testLinearContainerFactor, creation ) {
   LinearContainerFactor actual(linear_factor, full_values);
 
   // Verify the keys
-  KeyVector expKeys;
-  expKeys += l3, l5;
-  EXPECT(assert_container_equality(expKeys, actual.keys()));
+  EXPECT(assert_container_equality({l3, l5}, actual.keys()));
 
   // Verify subset of linearization points
   EXPECT(assert_equal(exp_values, actual.linearizationPoint(), tol));
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index c82ba3391..bcf01eff5 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -279,9 +279,8 @@ TEST(Values, extract_keys)
   config.insert(key3, Pose2());
   config.insert(key4, Pose2());
 
-  KeyVector expected, actual;
-  expected += key1, key2, key3, key4;
-  actual = config.keys();
+  KeyVector expected {key1, key2, key3, key4};
+  KeyVector actual = config.keys();
 
   CHECK(actual.size() == expected.size());
   KeyVector::const_iterator itAct = actual.begin(), itExp = expected.begin();
diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp
index c7309786d..5d57886f5 100644
--- a/gtsam/sam/tests/testRangeFactor.cpp
+++ b/gtsam/sam/tests/testRangeFactor.cpp
@@ -102,9 +102,7 @@ TEST( RangeFactor, ConstructorWithTransform) {
 
   RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
       body_P_sensor_2D);
-  KeyVector expected;
-  expected.push_back(2);
-  expected.push_back(1);
+  KeyVector expected {2, 1};
   CHECK(factor2D.keys() == expected);
   RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
       body_P_sensor_3D);
diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
index 02893e628..b85dd891a 100644
--- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
+++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
@@ -42,7 +42,7 @@ const Matrix26 F0 = Matrix26::Ones();
 const Matrix26 F1 = 2 * Matrix26::Ones();
 const Matrix26 F3 = 3 * Matrix26::Ones();
 const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
-const KeyVector keys = list_of<Key>(0)(1)(3);
+const KeyVector keys {0, 1, 3};
 // RHS and sigmas
 const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
 
@@ -86,8 +86,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
   F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
 
   // Calculate expected result F'*alpha*(I - E*P*E')*F*x
-  KeyVector keys2;
-  keys2 += 0,1,2,3;
+  KeyVector keys2{0,1,2,3};
   Vector x = xvalues.vector(keys2);
   Vector expected = Vector::Zero(24);
   RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
index be0a9df88..aaffbf0e6 100644
--- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
@@ -170,9 +170,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  KeyVector views;
-  views.push_back(c1);
-  views.push_back(c2);
+  KeyVector views {c1, c2};
 
   factor2->add(measurements, views);
 
@@ -195,10 +193,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
   SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
-  KeyVector views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
   smartFactor1->add(measurements_cam1, views);
   smartFactor2->add(measurements_cam2, views);
   smartFactor3->add(measurements_cam3, views);
@@ -293,10 +288,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
-  KeyVector views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SfM_Track track1;
   for (size_t i = 0; i < 3; ++i) {
@@ -370,10 +362,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  KeyVector views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   smartFactor1->add(measurements_cam1, views);
@@ -450,10 +439,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  KeyVector views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   smartFactor1->add(measurements_cam1, views);
@@ -526,10 +512,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
   projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
 
-  KeyVector views;
-  views.push_back(c1);
-  views.push_back(c2);
-  views.push_back(c3);
+  KeyVector views {c1, c2, c3};
 
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
   smartFactor1->add(measurements_cam1, views);
diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
index f7df47f5f..309b8f22e 100644
--- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
+++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
@@ -81,10 +81,7 @@ TEST( SymbolicBayesNet, combine )
 TEST(SymbolicBayesNet, saveGraph) {
   SymbolicBayesNet bn;
   bn += SymbolicConditional(_A_, _B_);
-  KeyVector keys;
-  keys.push_back(_B_);
-  keys.push_back(_C_);
-  keys.push_back(_D_);
+  KeyVector keys {_B_, _C_, _D_};
   bn += SymbolicConditional::FromKeys(keys,2);
   bn += SymbolicConditional(_D_);
 
diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp
index 18f3b84cc..4e2a7b8c9 100644
--- a/gtsam/symbolic/tests/testSymbolicFactor.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp
@@ -33,7 +33,7 @@ using namespace boost::assign;
 /* ************************************************************************* */
 #ifdef TRACK_ELIMINATE
 TEST(SymbolicFactor, eliminate) {
-  KeyVector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
+  KeyVector keys {2, 3, 4, 6, 7, 9, 10, 11};
   IndexFactor actual(keys.begin(), keys.end());
   BayesNet<IndexConditional> fragment = *actual.eliminate(3);
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
index 7b683208c..54ad22bcb 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
@@ -419,9 +419,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
   ordering.push_back(3);
 
   // Create the set of marginalizable variables
-  KeyVector linearIndices;
-  linearIndices.push_back(1);
-  linearIndices.push_back(2);
+  KeyVector linearIndices {1, 2};
 
   GaussianFactorGraph linearPartialGraph = *partialGraph.linearize(partialValues);
   GaussianFactorGraph result = *linearPartialGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
@@ -1008,8 +1006,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
   GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
 
   // Create the set of marginalizable variables
-  KeyVector linearIndices;
-  linearIndices.push_back(1);
+  KeyVector linearIndices {1};
 
   GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
@@ -1062,9 +1059,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
   GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
 
   // Create the set of marginalizable variables
-  KeyVector linearIndices;
-  linearIndices.push_back(1);
-  linearIndices.push_back(2);
+  KeyVector linearIndices {1, 2};
 
   GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
index 3cd59efec..97b79394f 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
@@ -1108,12 +1108,9 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
   newValues.insert(3, value3);
 
   // Create the set of marginalizable variables
-  KeyVector linearIndices;
-  linearIndices.push_back(1);
-
   GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
 
-    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
+    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal({1}, EliminateCholesky).second;
 
     NonlinearFactorGraph expectedMarginals;
     for(const GaussianFactor::shared_ptr& factor: marginal) {
@@ -1156,15 +1153,10 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
   newValues.insert(3, value3);
 
 
-    // Create the set of marginalizable variables
-  KeyVector linearIndices;
-  linearIndices.push_back(1);
-  linearIndices.push_back(2);
-
-
+  // Create the set of marginalizable variables
+  KeyVector linearIndices {1, 2};
   GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
-
-  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
   NonlinearFactorGraph expectedMarginals;
   for(const GaussianFactor::shared_ptr& factor: marginal) {
diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp
index 844510018..f44496e27 100644
--- a/tests/testMarginals.cpp
+++ b/tests/testMarginals.cpp
@@ -139,10 +139,7 @@ TEST(Marginals, planarSLAMmarginals) {
       0.151935669757191,   0.007741936219615,   0.090000180000270,  -0.000000000000000,   0.000000000000000,   0.160967924878730,   0.007741936219615,   0.004516127560770,
      -0.104516127560770,   0.351935664055174,   0.000000000000000,   0.090000180000270,   0.040000000000000,   0.007741936219615,   0.351935664055174,   0.056129031890193,
      -0.050967744878460,   0.056129031890193,   0.000000000000000,   0.000000000000000,   0.010000000000000,   0.004516127560770,   0.056129031890193,   0.027741936219615;
-  KeyVector variables(3);
-  variables[0] = x1;
-  variables[1] = l2;
-  variables[2] = x3;
+  KeyVector variables {x1, l2, x3};
   JointMarginal joint_l2x1x3 = marginals.jointMarginalCovariance(variables);
   EXPECT(assert_equal(Matrix(expected_l2x1x3.block(0,0,2,2)), Matrix(joint_l2x1x3(l2,l2)), 1e-6));
   EXPECT(assert_equal(Matrix(expected_l2x1x3.block(2,0,3,2)), Matrix(joint_l2x1x3(x1,l2)), 1e-6));
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 2240539f4..6e49fa749 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -401,11 +401,7 @@ TEST( NonlinearFactor, clone_rekey )
   EXPECT(assert_equal(*init, *actClone));
 
   // Re-key factor - clones with different keys
-  KeyVector new_keys(4);
-  new_keys[0] = X(5);
-  new_keys[1] = X(6);
-  new_keys[2] = X(7);
-  new_keys[3] = X(8);
+  KeyVector new_keys {X(5),X(6),X(7),X(8)};
   shared_nlf actRekey = init->rekey(new_keys);
   EXPECT(actRekey.get() != init.get()); // Ensure different pointers
 

From 895da61f05598550d4797cd2d6a07f4a79119a02 Mon Sep 17 00:00:00 2001
From: cbeall <cbeall@nuro.ai>
Date: Thu, 8 Nov 2018 16:18:51 -0800
Subject: [PATCH 052/281] Fix remaining FastVector errors for gcc + tbb +
 Ubuntu in tests

---
 gtsam/base/tests/testTreeTraversal.cpp                |  4 ++--
 .../tests/testConcurrentIncrementalFilter.cpp         | 11 ++++++-----
 2 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp
index 8fe5e53ef..88e476cb9 100644
--- a/gtsam/base/tests/testTreeTraversal.cpp
+++ b/gtsam/base/tests/testTreeTraversal.cpp
@@ -40,8 +40,8 @@ struct TestNode {
 struct TestForest {
   typedef TestNode Node;
   typedef Node::shared_ptr sharedNode;
-  vector<sharedNode> roots_;
-  const vector<sharedNode>& roots() const { return roots_; }
+  FastVector<sharedNode> roots_;
+  const FastVector<sharedNode>& roots() const { return roots_; }
 };
 
 TestForest makeTestForest() {
diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
index 97b79394f..d6f6446e8 100644
--- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
@@ -1110,12 +1110,13 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
   // Create the set of marginalizable variables
   GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
 
-    GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal({1}, EliminateCholesky).second;
+  KeyVector linearIndices {1};
+  GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
 
-    NonlinearFactorGraph expectedMarginals;
-    for(const GaussianFactor::shared_ptr& factor: marginal) {
-      expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
-    }
+  NonlinearFactorGraph expectedMarginals;
+  for(const GaussianFactor::shared_ptr& factor: marginal) {
+    expectedMarginals.push_back(LinearContainerFactor(factor, newValues));
+  }
 
   FastList<Key> keysToMarginalize;
   keysToMarginalize.push_back(1);

From ea953a3d452eead04862cac64b94e5452f92b878 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 8 Nov 2018 22:32:39 -0500
Subject: [PATCH 053/281] Removed old FastVector code.

---
 gtsam/base/FastVector.h | 82 ++++-------------------------------------
 1 file changed, 7 insertions(+), 75 deletions(-)

diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index 46bc25642..e54b91126 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -11,8 +11,9 @@
 
 /**
  * @file    FastVector.h
- * @brief   A thin wrapper around std::vector that uses boost's pool_allocator.
+ * @brief   A thin wrapper around std::vector that uses a custom allocator.
  * @author  Richard Roberts
+ * @author  Frank Dellaert
  * @date    Feb 9, 2011
  */
 
@@ -23,82 +24,13 @@
 
 namespace gtsam {
 
-#ifndef GTSAM_USE_OLD_FAST_VECTOR
-
-template <typename T>
-using FastVector = std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
-
-#else
-
-#include <boost/serialization/nvp.hpp>
-#include <boost/serialization/vector.hpp>
-
 /**
- * FastVector is a thin wrapper around std::vector that uses the boost
- * pool_allocator instead of the default STL allocator.  This is just a
- * convenience to avoid having lengthy types in the code.  Through timing,
- * we've seen that the pool_allocator can lead to speedups of several %
+ * FastVector is a type alias to a std::vector with a custom memory allocator.
+ * The particular allocator depends on GTSAM's cmake configuration.
  * @addtogroup base
  */
-template<typename VALUE>
-class FastVector: public std::vector<VALUE,
-    typename internal::FastDefaultVectorAllocator<VALUE>::type> {
- public:
-  typedef std::vector<VALUE,
-      typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
-
-  /** Default constructor */
-  FastVector() {
-  }
-
-  /** Constructor from size */
-  explicit FastVector(size_t size) :
-      Base(size) {
-  }
-
-  /** Constructor from size and initial values */
-  explicit FastVector(size_t size, const VALUE& initial) :
-      Base(size, initial) {
-  }
-
-  /** Constructor from a range, passes through to base class */
-  template<typename INPUTITERATOR>
-  explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
-    // This if statement works around a bug in boost pool allocator and/or
-    // STL vector where if the size is zero, the pool allocator will allocate
-    // huge amounts of memory.
-    if (first != last)
-      Base::assign(first, last);
-  }
-
-  /** Copy constructor from the base class */
-  FastVector(const Base& x) :
-      Base(x) {
-  }
-
-  /** Copy constructor from a standard STL container */
-  template<typename ALLOCATOR>
-  FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
-    // This if statement works around a bug in boost pool allocator and/or
-    // STL vector where if the size is zero, the pool allocator will allocate
-    // huge amounts of memory.
-    if (x.size() > 0)
-      Base::assign(x.begin(), x.end());
-  }
-
-  /** Conversion to a standard STL container */
-  operator std::vector<VALUE>() const {
-    return std::vector<VALUE>(this->begin(), this->end());
-  }
-
- private:
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-  }
-};
-#endif
+template <typename T>
+using FastVector =
+    std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
 
 }  // namespace gtsam

From 48117d35ff3c46e45f8df68872ebfe590dab7a51 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 10 Nov 2018 01:18:27 -0500
Subject: [PATCH 054/281] Just making sure Adjoints are correct

---
 gtsam/geometry/tests/testPose2.cpp | 29 +++++++++++++++++++++++++++++
 gtsam/geometry/tests/testPose3.cpp | 18 ++++++++++++++++++
 2 files changed, 47 insertions(+)

diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index ca550cdc7..88976697a 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -172,6 +172,35 @@ TEST(Pose2, expmap_c_full)
   EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6));
 }
 
+/* ************************************************************************* */
+// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
+TEST(Pose2, Adjoint_full) {
+  Pose2 T(1, 2, 3);
+  Pose2 expected = T * Pose2::Expmap(screwPose2::xi) * T.inverse();
+  Vector xiprime = T.Adjoint(screwPose2::xi);
+  EXPECT(assert_equal(expected, Pose2::Expmap(xiprime), 1e-6));
+
+  Vector3 xi2(4, 5, 6);
+  Pose2 expected2 = T * Pose2::Expmap(xi2) * T.inverse();
+  Vector xiprime2 = T.Adjoint(xi2);
+  EXPECT(assert_equal(expected2, Pose2::Expmap(xiprime2), 1e-6));
+}
+
+/* ************************************************************************* */
+// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
+TEST(Pose2, Adjoint_hat) {
+  Pose2 T(1, 2, 3);
+  auto hat = [](const Vector& xi) { return ::wedge<Pose2>(xi); };
+  Matrix3 expected = T.matrix() * hat(screwPose2::xi) * T.matrix().inverse();
+  Matrix3 xiprime = hat(T.Adjoint(screwPose2::xi));
+  EXPECT(assert_equal(expected, xiprime, 1e-6));
+
+  Vector3 xi2(4, 5, 6);
+  Matrix3 expected2 = T.matrix() * hat(xi2) * T.matrix().inverse();
+  Matrix3 xiprime2 = hat(T.Adjoint(xi2));
+  EXPECT(assert_equal(expected2, xiprime2, 1e-6));
+}
+
 /* ************************************************************************* */
 TEST(Pose2, logmap) {
   Pose2 pose0(M_PI/2.0, Point2(1, 2));
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 98d3e11ee..d516ddc8b 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -143,6 +143,24 @@ TEST(Pose3, Adjoint_full)
   EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
 }
 
+/* ************************************************************************* */
+// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
+TEST(Pose3, Adjoint_hat)
+{
+  auto hat = [](const Vector& xi) { return ::wedge<Pose3>(xi); };
+  Matrix4 expected = T.matrix() * hat(screwPose3::xi) * T.matrix().inverse();
+  Matrix4 xiprime = hat(T.Adjoint(screwPose3::xi));
+  EXPECT(assert_equal(expected, xiprime, 1e-6));
+
+  Matrix4 expected2 = T2.matrix() * hat(screwPose3::xi) * T2.matrix().inverse();
+  Matrix4 xiprime2 = hat(T2.Adjoint(screwPose3::xi));
+  EXPECT(assert_equal(expected2, xiprime2, 1e-6));
+
+  Matrix4 expected3 = T3.matrix() * hat(screwPose3::xi) * T3.matrix().inverse();
+  Matrix4 xiprime3 = hat(T3.Adjoint(screwPose3::xi));
+  EXPECT(assert_equal(expected3, xiprime3, 1e-6));
+}
+
 /* ************************************************************************* */
 /** Agrawal06iros version of exponential map */
 Pose3 Agrawal06iros(const Vector& xi) {

From e5cd8c6dae83e5dfa29eb01b5f8d1b8ac68dbfea Mon Sep 17 00:00:00 2001
From: cbeall <cbeall@nuro.ai>
Date: Thu, 15 Nov 2018 11:06:32 -0800
Subject: [PATCH 055/281] Fix serialization of AttitudeFactor and add roundtrip
 test

---
 gtsam/navigation/AttitudeFactor.h             | 26 ++++++++++-------
 gtsam/navigation/tests/testAttitudeFactor.cpp | 29 +++++++++++++++++++
 2 files changed, 45 insertions(+), 10 deletions(-)

diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 4ae6078e9..23cd5c477 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -55,6 +55,14 @@ public:
   /** vector of errors */
   Vector attitudeError(const Rot3& p,
       OptionalJacobian<2,3> H = boost::none) const;
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
+    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
+  }
 };
 
 /**
@@ -123,11 +131,10 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar
-        & boost::serialization::make_nvp("NoiseModelFactor1",
-            boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(nZ_);
-    ar & BOOST_SERIALIZATION_NVP(bRef_);
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+        boost::serialization::base_object<Base>(*this));
+    ar & boost::serialization::make_nvp("AttitudeFactor",
+        boost::serialization::base_object<AttitudeFactor>(*this));
   }
 
 public:
@@ -210,11 +217,10 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar
-        & boost::serialization::make_nvp("NoiseModelFactor1",
-            boost::serialization::base_object<Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(nZ_);
-    ar & BOOST_SERIALIZATION_NVP(bRef_);
+    ar & boost::serialization::make_nvp("NoiseModelFactor1",
+        boost::serialization::base_object<Base>(*this));
+    ar & boost::serialization::make_nvp("AttitudeFactor",
+        boost::serialization::base_object<AttitudeFactor>(*this));
   }
 
 public:
diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index f7f0aa9ad..70b78c916 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -19,6 +19,8 @@
 #include <gtsam/navigation/AttitudeFactor.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -57,6 +59,22 @@ TEST( Rot3AttitudeFactor, Constructor ) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+/* ************************************************************************* */
+// Export Noisemodels
+// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
+
+/* ************************************************************************* */
+TEST(Rot3AttitudeFactor, Serialization) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Rot3AttitudeFactor factor(0, nDown, model);
+
+  EXPECT(serializationTestHelpers::equalsObj(factor));
+  EXPECT(serializationTestHelpers::equalsXML(factor));
+  EXPECT(serializationTestHelpers::equalsBinary(factor));
+}
+
 // *************************************************************************
 TEST( Pose3AttitudeFactor, Constructor ) {
 
@@ -90,6 +108,17 @@ TEST( Pose3AttitudeFactor, Constructor ) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+/* ************************************************************************* */
+TEST(Pose3AttitudeFactor, Serialization) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Pose3AttitudeFactor factor(0, nDown, model);
+
+  EXPECT(serializationTestHelpers::equalsObj(factor));
+  EXPECT(serializationTestHelpers::equalsXML(factor));
+  EXPECT(serializationTestHelpers::equalsBinary(factor));
+}
+
 // *************************************************************************
 int main() {
   TestResult tr;

From 6f8bfe0f0a10a94db26b7dda6b5e1d1e6b8e4b72 Mon Sep 17 00:00:00 2001
From: Mike Sheffler <msheffler@toyon.com>
Date: Fri, 16 Nov 2018 20:49:48 +0000
Subject: [PATCH 056/281] Merged in mikesheffler/gtsam (pull request #338)

Add <boost/serialization/vector.hpp> to fix a whole raft of compilation problems in gtsam_unstable_wrapper.cpp

* Add <boost/serialization/vector.hpp> to fix a whole raft of compilation problems in gtsam_unstable_wrapper.cpp

* Move the addition of the boost vector serialization include

* Moved include again. Frank pointed out that it really belongs in FactorGraph, not VectorValues. My bad.
---
 gtsam/inference/FactorGraph.h | 1 +
 gtsam/linear/VectorValues.h   | 1 +
 2 files changed, 2 insertions(+)

diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 6be3b89c2..0ecfd87f1 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -29,6 +29,7 @@
 #include <Eigen/Core>  // for Eigen::aligned_allocator
 
 #include <boost/serialization/nvp.hpp>
+#include <boost/serialization/vector.hpp>
 #include <boost/assign/list_inserter.hpp>
 #include <boost/bind.hpp>
 #include <boost/make_shared.hpp>
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index f187b56de..39abe1b56 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -26,6 +26,7 @@
 
 #include <boost/shared_ptr.hpp>
 
+
 #include <map>
 #include <string>
 

From 4b2070df2bbfee838579eee2b3e264e5b7c3927e Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Tue, 20 Nov 2018 15:02:15 -0800
Subject: [PATCH 057/281] Fix xml roundtrip serialization crash in boost
 1.66-1.68.

---
 gtsam/base/serialization.h | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h
index 89544cfd6..ebd893ad1 100644
--- a/gtsam/base/serialization.h
+++ b/gtsam/base/serialization.h
@@ -84,8 +84,13 @@ bool deserializeFromFile(const std::string& filename, T& output) {
 template<class T>
 std::string serializeXML(const T& input, const std::string& name="data") {
   std::ostringstream out_archive_stream;
-  boost::archive::xml_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);
+  // braces to flush out_archive as it goes out of scope before taking str()
+  // fixes crash with boost 1.66-1.68
+  // see https://github.com/boostorg/serialization/issues/82
+  {
+    boost::archive::xml_oarchive out_archive(out_archive_stream);
+    out_archive << boost::serialization::make_nvp(name.c_str(), input);
+  }
   return out_archive_stream.str();
 }
 

From 5dcd61ba90a28a334cfe83b22e53f06f0ec3a6f4 Mon Sep 17 00:00:00 2001
From: JOSE LUIS BLANCO CLARACO <jlblanco@ual.es>
Date: Sun, 25 Nov 2018 23:51:34 +0000
Subject: [PATCH 058/281] Fix PCH in MSVC

---
 gtsam/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 4d1007d19..8900ea86b 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -75,7 +75,7 @@ list(APPEND gtsam_srcs ${gtsam_core_headers})
 IF(MSVC)
 	# Add precompiled header to sources
 	include(gtsamAddPch)
-	gtsamAddPch("precompiled_header.h" "precompiled_header.cpp" ${gtsam_srcs})
+	gtsamAddPch("precompiled_header.h" "precompiled_header.cpp" "${gtsam_srcs}")
 	list(INSERT gtsam_srcs 0 "precompiled_header.cpp")
 ENDIF(MSVC)
 

From b15f50fb8ff5d6c613723380f24ca123ef71db4f Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <jlblanco@ual.es>
Date: Mon, 26 Nov 2018 10:43:53 +0100
Subject: [PATCH 059/281] fix ambiguous function call (MSVC2017)

---
 gtsam/base/Vector.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index 6dc9800ca..7caf490ef 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -125,8 +125,8 @@ bool assert_inequal(const Vector& expected, const Vector& actual, double tol) {
 bool assert_equal(const SubVector& expected, const SubVector& actual, double tol) {
   if (equal_with_abs_tol(expected,actual,tol)) return true;
   cout << "not equal:" << endl;
-  print(expected, "expected");
-  print(actual, "actual");
+  print(static_cast<Vector>(expected), "expected");
+  print(static_cast<Vector>(actual), "actual");
   return false;
 }
 

From 67d87cb2bd1844a1a8342f26b068fb60bdd4b4f9 Mon Sep 17 00:00:00 2001
From: JOSE LUIS BLANCO CLARACO <jlblanco@ual.es>
Date: Tue, 27 Nov 2018 15:30:29 +0000
Subject: [PATCH 060/281] Remove /bigobj flag here (it's already enabled in
 parent scope)

Already defined here: https://bitbucket.org/gtborg/gtsam/src/0e679b8b97a600c30debf64b1de20a9c47e06d20/CMakeLists.txt#lines-334

This was also causing build errors due to a missing whitespace between cl parameters.
---
 gtsam/CMakeLists.txt | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 8900ea86b..a6814a422 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -146,15 +146,6 @@ else()
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
 endif()
 
-# Special cases
-if(MSVC)
-	set_property(SOURCE
-		"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
-		"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ISAM2.cpp"
-		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
-endif()
-
-
 # Create the matlab toolbox for the gtsam library
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Set up codegen

From 8c221efda258a09b3c36b434c7b783d40b6f1dd5 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Tue, 27 Nov 2018 10:54:14 -0800
Subject: [PATCH 061/281] Fix build for GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF

---
 gtsam/navigation/tests/testScenario.cpp | 2 +-
 tests/testManifold.cpp                  | 2 ++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp
index 161b8841a..f814f1710 100644
--- a/gtsam/navigation/tests/testScenario.cpp
+++ b/gtsam/navigation/tests/testScenario.cpp
@@ -106,7 +106,7 @@ TEST(Scenario, LoopWithInitialPose) {
   // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
   const double v = 2, w = 6 * kDegree;
   const Vector3 W(0, -w, 0), V(v, 0, 0);
-  const Rot3 nRb0 = Rot3::yaw(M_PI);
+  const Rot3 nRb0 = Rot3::Yaw(M_PI);
   const Pose3 nTb0(nRb0, Point3(1, 2, 3));
   const ConstantTwistScenario scenario(W, V, nTb0);
 
diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp
index 286e3ff5e..195914176 100644
--- a/tests/testManifold.cpp
+++ b/tests/testManifold.cpp
@@ -149,6 +149,7 @@ TEST(Manifold, DefaultChart) {
   EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
 }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 //******************************************************************************
 typedef ProductManifold<Point2,Point2> MyPoint2Pair;
 
@@ -174,6 +175,7 @@ TEST(Manifold, ProductManifold) {
   EXPECT(assert_equal(expected,pair2,1e-9));
   EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9));
 }
+#endif
 
 //******************************************************************************
 int main() {

From ce460eea929d2c4c270f556b5a26213f39b6ec7c Mon Sep 17 00:00:00 2001
From: Jing Dong <jdong@gatech.edu>
Date: Fri, 30 Nov 2018 15:48:16 -0500
Subject: [PATCH 062/281] fix virtual memory range for PCH exceeded

---
 CMakeLists.txt | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 45a57827d..f9196aced 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -121,6 +121,10 @@ if(MSVC)
 		add_definitions(-DBOOST_ALL_NO_LIB)
 	  add_definitions(-DBOOST_ALL_DYN_LINK)
 	endif()
+	# Virtual memory range for PCH exceeded on VS2015
+	if(MSVC_VERSION LESS 1910) # older than VS2017
+	  add_definitions(-Zm295)
+	endif()
 endif()
 
 find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)

From d56033b5a59581138e674c41942687cfbc8009f6 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Tue, 11 Dec 2018 10:56:56 -0800
Subject: [PATCH 063/281] Add CMake check for incompatible MKL + Eigen 3.3.4

---
 CMakeLists.txt | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f9196aced..d471e3fd6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -246,6 +246,13 @@ if(GTSAM_USE_SYSTEM_EIGEN)
 	if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
 	  message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
 	endif()
+
+	# Check for Eigen version which doesn't work with MKL
+	# See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
+	if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
+		message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
+	endif()
+
 else()
 	# Use bundled Eigen include path.
 	# Clear any variables set by FindEigen3

From a2f738951838cb05c358a5dcae7ee79fb714aab5 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Tue, 11 Dec 2018 11:27:31 -0800
Subject: [PATCH 064/281] Upgrade to Eigen 3.3.6

---
 gtsam/3rdparty/Eigen/CMakeLists.txt           |  70 ++--
 gtsam/3rdparty/Eigen/CTestConfig.cmake        |   4 +-
 gtsam/3rdparty/Eigen/CTestCustom.cmake.in     |   1 +
 gtsam/3rdparty/Eigen/Eigen/Cholesky           |   5 +
 gtsam/3rdparty/Eigen/Eigen/Core               |  27 +-
 gtsam/3rdparty/Eigen/Eigen/Eigenvalues        |   4 +
 gtsam/3rdparty/Eigen/Eigen/LU                 |   4 +
 gtsam/3rdparty/Eigen/Eigen/QR                 |   4 +
 gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc    |   2 +-
 gtsam/3rdparty/Eigen/Eigen/SVD                |   4 +
 .../3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h  |  14 +-
 gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h |  26 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h   |   2 -
 .../Eigen/Eigen/src/Core/Assign_MKL.h         |   6 +-
 .../Eigen/Eigen/src/Core/ConditionEstimator.h |   2 +-
 .../Eigen/Eigen/src/Core/CoreEvaluators.h     |  39 +-
 .../3rdparty/Eigen/Eigen/src/Core/Diagonal.h  |   5 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h     |  17 +-
 .../Eigen/Eigen/src/Core/GeneralProduct.h     |  21 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h     |  17 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h |   6 +-
 .../Eigen/Eigen/src/Core/MathFunctions.h      |  64 ++--
 .../Eigen/Eigen/src/Core/MathFunctionsImpl.h  |  23 ++
 gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h  |   2 -
 .../Eigen/Eigen/src/Core/MatrixBase.h         |  35 +-
 .../Eigen/Eigen/src/Core/PlainObjectBase.h    |   4 +
 gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h |  10 +-
 .../Eigen/Eigen/src/Core/ProductEvaluators.h  |  31 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h   |   2 +-
 gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h     |   2 +
 .../Eigen/Eigen/src/Core/SelfAdjointView.h    |   6 +-
 .../Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h  |   4 -
 .../Eigen/Eigen/src/Core/SolveTriangular.h    |   3 +
 .../Eigen/Eigen/src/Core/StableNorm.h         |   2 +-
 .../Eigen/Eigen/src/Core/Transpositions.h     |   2 +-
 .../Eigen/Eigen/src/Core/arch/AVX/Complex.h   |  36 +-
 .../Eigen/src/Core/arch/AVX/PacketMath.h      |  24 +-
 .../src/Core/arch/AVX512/MathFunctions.h      |  33 +-
 .../Eigen/src/Core/arch/AVX512/PacketMath.h   |  12 +-
 .../Eigen/src/Core/arch/AltiVec/Complex.h     |  35 +-
 .../Eigen/src/Core/arch/AltiVec/PacketMath.h  |  44 ++-
 .../Eigen/Eigen/src/Core/arch/CUDA/Half.h     | 253 +++++++------
 .../Eigen/src/Core/arch/CUDA/PacketMathHalf.h |   5 +-
 .../Eigen/src/Core/arch/Default/ConjHelper.h  |  29 ++
 .../Eigen/Eigen/src/Core/arch/NEON/Complex.h  |  12 +-
 .../Eigen/src/Core/arch/NEON/PacketMath.h     |  31 ++
 .../Eigen/Eigen/src/Core/arch/SSE/Complex.h   |  40 +-
 .../Eigen/src/Core/arch/SSE/PacketMath.h      |  24 +-
 .../Eigen/src/Core/arch/SSE/TypeCasting.h     |  28 +-
 .../Eigen/src/Core/arch/ZVector/Complex.h     |   3 +
 .../Eigen/src/Core/functors/BinaryFunctors.h  |  25 +-
 .../Eigen/src/Core/functors/StlFunctors.h     |   4 +
 .../Core/products/GeneralBlockPanelKernel.h   |  15 +-
 .../GeneralMatrixMatrixTriangular_BLAS.h      |   8 +-
 .../Core/products/GeneralMatrixMatrix_BLAS.h  |  19 +-
 .../Core/products/GeneralMatrixVector_BLAS.h  |  19 +-
 .../products/SelfadjointMatrixMatrix_BLAS.h   |  48 ++-
 .../products/SelfadjointMatrixVector_BLAS.h   |   9 +-
 .../Core/products/TriangularMatrixMatrix.h    |  35 +-
 .../products/TriangularMatrixMatrix_BLAS.h    |  39 +-
 .../Core/products/TriangularMatrixVector.h    |  22 +-
 .../products/TriangularMatrixVector_BLAS.h    |  46 ++-
 .../products/TriangularSolverMatrix_BLAS.h    |  40 +-
 .../src/Core/util/DisableStupidWarnings.h     |  14 +-
 .../Eigen/Eigen/src/Core/util/MKL_support.h   |  10 +-
 .../Eigen/Eigen/src/Core/util/Macros.h        |  19 +-
 .../Eigen/Eigen/src/Core/util/Memory.h        |  24 +-
 .../3rdparty/Eigen/Eigen/src/Core/util/Meta.h |  42 +++
 .../src/Core/util/ReenableStupidWarnings.h    |   2 +-
 .../Eigen/Eigen/src/Core/util/StaticAssert.h  | 120 +++---
 .../src/Eigenvalues/GeneralizedEigenSolver.h  |   5 +-
 .../src/Eigenvalues/MatrixBaseEigenvalues.h   |   2 -
 .../Eigen/Eigen/src/Eigenvalues/RealSchur.h   |   4 +-
 .../SelfAdjointEigenSolver_LAPACKE.h          |  23 +-
 .../Eigen/Eigen/src/Geometry/AngleAxis.h      |   2 +-
 .../Eigen/Eigen/src/Geometry/Quaternion.h     |  29 +-
 .../BasicPreconditioners.h                    |   2 +-
 .../ConjugateGradient.h                       |   5 +-
 .../3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h  | 269 +++++++-------
 .../3rdparty/Eigen/Eigen/src/LU/InverseImpl.h |   2 +-
 .../Eigen/src/PaStiXSupport/PaStiXSupport.h   |   8 +-
 gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h   |  33 +-
 .../Eigen/Eigen/src/SVD/JacobiSVD_LAPACKE.h   |   5 +-
 gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h  |   4 +-
 .../Eigen/Eigen/src/SparseCore/AmbiVector.h   |   2 +-
 .../ConservativeSparseSparseProduct.h         |  67 ++--
 .../Eigen/Eigen/src/SparseCore/SparseMatrix.h |   6 +-
 .../src/SparseCore/SparseSelfAdjointView.h    |   8 +-
 .../SparseSparseProductWithPruning.h          |  22 +-
 .../Eigen/Eigen/src/SparseLU/SparseLU.h       |   2 -
 .../Eigen/Eigen/src/SparseQR/SparseQR.h       |  26 +-
 .../Eigen/src/SuperLUSupport/SuperLUSupport.h |   4 +-
 .../Eigen/bench/spbench/CMakeLists.txt        |   2 +-
 gtsam/3rdparty/Eigen/blas/CMakeLists.txt      |  10 +-
 .../Eigen/cmake/EigenConfigureTesting.cmake   |  13 +-
 gtsam/3rdparty/Eigen/cmake/FindBLAS.cmake     |   2 +-
 .../Eigen/cmake/language_support.cmake        |   2 +-
 gtsam/3rdparty/Eigen/doc/CMakeLists.txt       |   4 +-
 .../Eigen/doc/CoeffwiseMathFunctionsTable.dox |   7 +-
 gtsam/3rdparty/Eigen/doc/Doxyfile.in          |   6 +-
 gtsam/3rdparty/Eigen/doc/LeastSquares.dox     |   2 +-
 gtsam/3rdparty/Eigen/doc/Pitfalls.dox         |   6 +
 .../Eigen/doc/PreprocessorDirectives.dox      |   4 +
 .../doc/TopicLinearAlgebraDecompositions.dox  |  14 +-
 .../Eigen/doc/TutorialLinearAlgebra.dox       |  28 +-
 gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox    |   2 +
 .../3rdparty/Eigen/doc/eigen_navtree_hacks.js |  44 ++-
 gtsam/3rdparty/Eigen/doc/eigendoxy.css        |   7 +
 .../Eigen/doc/eigendoxy_footer.html.in        |   4 +-
 .../Eigen/doc/eigendoxy_header.html.in        |  18 +-
 .../Eigen/doc/examples/Cwise_lgamma.cpp       |   2 +-
 .../doc/examples/TutorialLinAlgSVDSolve.cpp   |   2 +-
 .../Tutorial_simple_example_dynamic_size.cpp  |   2 +-
 .../Eigen/doc/examples/matrixfree_cg.cpp      |   1 +
 .../doc/snippets/MatrixBase_cwiseEqual.cpp    |   2 +-
 .../doc/snippets/MatrixBase_cwiseNotEqual.cpp |   2 +-
 .../Eigen/doc/snippets/Matrix_Map_stride.cpp  |   7 +
 .../Eigen/doc/special_examples/CMakeLists.txt |   1 -
 .../Tutorial_sparse_example.cpp               |   6 +-
 gtsam/3rdparty/Eigen/lapack/CMakeLists.txt    |   2 +-
 gtsam/3rdparty/Eigen/test/adjoint.cpp         |   1 -
 gtsam/3rdparty/Eigen/test/array.cpp           |   5 -
 .../3rdparty/Eigen/test/array_for_matrix.cpp  |   4 -
 gtsam/3rdparty/Eigen/test/array_replicate.cpp |   1 -
 gtsam/3rdparty/Eigen/test/array_reverse.cpp   |   1 -
 gtsam/3rdparty/Eigen/test/basicstuff.cpp      |   2 -
 gtsam/3rdparty/Eigen/test/bdcsvd.cpp          |   3 +-
 gtsam/3rdparty/Eigen/test/block.cpp           |  14 +-
 gtsam/3rdparty/Eigen/test/cholesky.cpp        |  36 +-
 .../Eigen/test/conservative_resize.cpp        |   1 -
 gtsam/3rdparty/Eigen/test/corners.cpp         |   1 -
 gtsam/3rdparty/Eigen/test/cuda_basic.cu       |   3 -
 gtsam/3rdparty/Eigen/test/determinant.cpp     |   1 -
 gtsam/3rdparty/Eigen/test/diagonal.cpp        |  10 +-
 .../3rdparty/Eigen/test/diagonalmatrices.cpp  |  39 +-
 gtsam/3rdparty/Eigen/test/dontalign.cpp       |   1 -
 gtsam/3rdparty/Eigen/test/eigen2support.cpp   |   1 -
 .../Eigen/test/eigensolver_complex.cpp        |   1 -
 .../test/eigensolver_generalized_real.cpp     |   8 +-
 .../Eigen/test/eigensolver_generic.cpp        |   1 -
 .../Eigen/test/eigensolver_selfadjoint.cpp    |   1 -
 gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp  |   2 -
 gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp  |   1 -
 .../Eigen/test/geo_parametrizedline.cpp       |   1 -
 gtsam/3rdparty/Eigen/test/geo_quaternion.cpp  |  13 +
 gtsam/3rdparty/Eigen/test/half_float.cpp      |  42 ++-
 gtsam/3rdparty/Eigen/test/householder.cpp     |   1 -
 gtsam/3rdparty/Eigen/test/integer_types.cpp   |   6 +-
 gtsam/3rdparty/Eigen/test/inverse.cpp         |   5 +-
 gtsam/3rdparty/Eigen/test/jacobi.cpp          |   1 -
 gtsam/3rdparty/Eigen/test/jacobisvd.cpp       |  18 +-
 gtsam/3rdparty/Eigen/test/linearstructure.cpp |   1 -
 gtsam/3rdparty/Eigen/test/lu.cpp              |   8 +-
 gtsam/3rdparty/Eigen/test/main.h              |  52 ++-
 gtsam/3rdparty/Eigen/test/mapped_matrix.cpp   |   9 +-
 .../3rdparty/Eigen/test/mapstaticmethods.cpp  |   2 -
 gtsam/3rdparty/Eigen/test/mapstride.cpp       |  59 ++-
 gtsam/3rdparty/Eigen/test/miscmatrices.cpp    |   1 -
 gtsam/3rdparty/Eigen/test/mixingtypes.cpp     |  60 ++-
 gtsam/3rdparty/Eigen/test/nomalloc.cpp        |   1 -
 .../Eigen/test/permutationmatrices.cpp        |  13 +-
 gtsam/3rdparty/Eigen/test/product_extra.cpp   |   1 -
 .../Eigen/test/product_notemporary.cpp        |   1 -
 .../Eigen/test/product_selfadjoint.cpp        |   1 -
 gtsam/3rdparty/Eigen/test/product_symm.cpp    |   1 -
 gtsam/3rdparty/Eigen/test/product_syrk.cpp    |   1 -
 gtsam/3rdparty/Eigen/test/product_trmm.cpp    |  28 +-
 gtsam/3rdparty/Eigen/test/product_trmv.cpp    |   1 -
 gtsam/3rdparty/Eigen/test/qr.cpp              |   2 -
 gtsam/3rdparty/Eigen/test/qr_colpivoting.cpp  |   4 -
 gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp |   2 -
 gtsam/3rdparty/Eigen/test/qtvector.cpp        |   2 -
 gtsam/3rdparty/Eigen/test/real_qz.cpp         |   1 -
 gtsam/3rdparty/Eigen/test/redux.cpp           |   4 +-
 gtsam/3rdparty/Eigen/test/ref.cpp             |  16 +-
 gtsam/3rdparty/Eigen/test/schur_real.cpp      |   2 -
 gtsam/3rdparty/Eigen/test/selfadjoint.cpp     |   5 +-
 gtsam/3rdparty/Eigen/test/sparse_basic.cpp    |   5 +-
 gtsam/3rdparty/Eigen/test/sparse_product.cpp  |  90 +++++
 gtsam/3rdparty/Eigen/test/sparseqr.cpp        |  22 ++
 gtsam/3rdparty/Eigen/test/stable_norm.cpp     |  11 +-
 gtsam/3rdparty/Eigen/test/stddeque.cpp        |   2 -
 gtsam/3rdparty/Eigen/test/stdlist.cpp         |   2 -
 gtsam/3rdparty/Eigen/test/stdvector.cpp       |  10 +
 gtsam/3rdparty/Eigen/test/svd_common.h        |   5 -
 gtsam/3rdparty/Eigen/test/svd_fill.h          |   1 -
 gtsam/3rdparty/Eigen/test/triangular.cpp      |   1 -
 .../Eigen/test/vectorization_logic.cpp        |   6 +
 gtsam/3rdparty/Eigen/test/vectorwiseop.cpp    |   2 -
 gtsam/3rdparty/Eigen/test/visitor.cpp         |   2 -
 .../3rdparty/Eigen/unsupported/CMakeLists.txt |  10 +-
 .../Eigen/unsupported/Eigen/CXX11/Tensor      |   2 +
 .../Eigen/CXX11/src/Tensor/README.md          | 349 +++++++++---------
 .../Eigen/CXX11/src/Tensor/Tensor.h           |   8 +-
 .../Eigen/CXX11/src/Tensor/TensorBase.h       |   6 +-
 .../src/Tensor/TensorContractionThreadPool.h  |   9 -
 .../CXX11/src/Tensor/TensorDeviceThreadPool.h |  11 +-
 .../Eigen/CXX11/src/Tensor/TensorDimensions.h |   4 +-
 .../Eigen/CXX11/src/Tensor/TensorForcedEval.h |  28 +-
 .../Eigen/CXX11/src/Tensor/TensorGenerator.h  |   2 +-
 .../Eigen/CXX11/src/Tensor/TensorMap.h        |  12 +-
 .../Eigen/CXX11/src/Tensor/TensorStorage.h    |   8 +-
 .../src/Tensor/TensorSyclExprConstructor.h    |   2 +-
 .../src/Tensor/TensorSyclExtractAccessor.h    |   2 +-
 .../src/Tensor/TensorSyclExtractFunctors.h    |   2 +-
 .../Eigen/CXX11/src/Tensor/TensorSyclTuple.h  |   5 +-
 .../TensorSymmetry/util/TemplateGroupTheory.h |   7 +-
 .../Eigen/CXX11/src/util/EmulateCXX11Meta.h   |   2 +-
 gtsam/3rdparty/Eigen/unsupported/Eigen/FFT    |   5 +-
 .../Eigen/unsupported/Eigen/MatrixFunctions   |  15 +-
 .../Eigen/unsupported/Eigen/OpenGLSupport     |   2 +-
 .../Eigen/src/AutoDiff/AutoDiffScalar.h       |   3 +-
 .../Eigen/unsupported/Eigen/src/BVH/KdBVH.h   |   1 +
 .../Eigen/src/IterativeSolvers/DGMRES.h       |  61 ++-
 .../Eigen/src/IterativeSolvers/GMRES.h        |   2 +-
 .../src/MatrixFunctions/MatrixExponential.h   |  48 ++-
 .../src/MatrixFunctions/MatrixFunction.h      |   6 +-
 .../src/MatrixFunctions/MatrixLogarithm.h     |   2 +-
 .../Eigen/src/MatrixFunctions/MatrixPower.h   |  12 +-
 .../src/MatrixFunctions/MatrixSquareRoot.h    |   4 -
 .../Eigen/src/Polynomials/PolynomialUtils.h   |   8 +-
 .../Eigen/src/SparseExtra/BlockSparseMatrix.h |   2 +-
 .../src/SparseExtra/DynamicSparseMatrix.h     |  14 +-
 .../Eigen/src/SparseExtra/MarketIO.h          |  19 +-
 .../unsupported/Eigen/src/Splines/Spline.h    |   5 -
 .../Eigen/unsupported/doc/examples/FFT.cpp    |   6 +-
 .../Eigen/unsupported/test/CMakeLists.txt     |  12 +-
 .../test/NonLinearOptimization.cpp            |   2 +-
 .../Eigen/unsupported/test/autodiff.cpp       |  22 +-
 .../unsupported/test/autodiff_scalar.cpp      |   3 +
 .../test/cxx11_tensor_argmax_cuda.cu          |   3 -
 .../test/cxx11_tensor_cast_float16_cuda.cu    |   3 -
 .../test/cxx11_tensor_complex_cuda.cu         |   3 -
 .../cxx11_tensor_complex_cwise_ops_cuda.cu    |   3 -
 .../test/cxx11_tensor_contract_cuda.cu        |   3 -
 .../unsupported/test/cxx11_tensor_cuda.cu     |   3 -
 .../unsupported/test/cxx11_tensor_device.cu   |   3 -
 .../test/cxx11_tensor_of_float16_cuda.cu      |   3 -
 .../test/cxx11_tensor_random_cuda.cu          |   3 -
 .../test/cxx11_tensor_reduction_cuda.cu       |   3 -
 .../test/cxx11_tensor_scan_cuda.cu            |   3 -
 .../unsupported/test/matrix_function.cpp      |   4 -
 .../Eigen/unsupported/test/openglsupport.cpp  |   4 -
 .../unsupported/test/polynomialsolver.cpp     |   2 -
 .../Eigen/unsupported/test/sparse_extra.cpp   |   4 +-
 245 files changed, 2317 insertions(+), 1486 deletions(-)
 create mode 100644 gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/ConjHelper.h
 create mode 100644 gtsam/3rdparty/Eigen/doc/snippets/Matrix_Map_stride.cpp

diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt
index f5840025b..2bfb6d560 100644
--- a/gtsam/3rdparty/Eigen/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/CMakeLists.txt
@@ -41,10 +41,13 @@ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_
 set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
 set(EIGEN_VERSION_NUMBER ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
 
-# if the mercurial program is absent, this will leave the EIGEN_HG_CHANGESET string empty,
-# but won't stop CMake.
-execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
-execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
+# if we are not in a mercurial clone
+if(IS_DIRECTORY ${CMAKE_SOURCE_DIR}/.hg)
+  # if the mercurial program is absent or this will leave the EIGEN_HG_CHANGESET string empty,
+  # but won't stop CMake.
+  execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
+  execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
+endif()
 
 # if this is the default (aka development) branch, extract the mercurial changeset number from the hg tip output...
 if(EIGEN_BRANCH_OUTPUT MATCHES "default")
@@ -64,6 +67,33 @@ include(GNUInstallDirs)
 
 set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
+
+option(EIGEN_TEST_CXX11 "Enable testing with C++11 and C++11 features (e.g. Tensor module)." OFF)
+
+
+macro(ei_add_cxx_compiler_flag FLAG)
+  string(REGEX REPLACE "-" "" SFLAG1 ${FLAG})
+  string(REGEX REPLACE "\\+" "p" SFLAG ${SFLAG1})
+  check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
+  if(COMPILER_SUPPORT_${SFLAG})
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
+  endif()
+endmacro(ei_add_cxx_compiler_flag)
+
+check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11)
+
+if(EIGEN_TEST_CXX11)
+  set(CMAKE_CXX_STANDARD 11)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+  if(EIGEN_COMPILER_SUPPORT_CPP11)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+  endif()
+else()
+  #set(CMAKE_CXX_STANDARD 03)
+  #set(CMAKE_CXX_EXTENSIONS OFF)
+  ei_add_cxx_compiler_flag("-std=c++03")
+endif()
+
 #############################################################################
 # find how to link to the standard libraries                                #
 #############################################################################
@@ -115,15 +145,6 @@ endif()
 
 set(EIGEN_TEST_MAX_SIZE "320" CACHE STRING "Maximal matrix/vector size, default is 320")
 
-macro(ei_add_cxx_compiler_flag FLAG)
-  string(REGEX REPLACE "-" "" SFLAG1 ${FLAG})
-  string(REGEX REPLACE "\\+" "p" SFLAG ${SFLAG1})
-  check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
-  if(COMPILER_SUPPORT_${SFLAG})
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
-  endif()
-endmacro(ei_add_cxx_compiler_flag)
-
 if(NOT MSVC)
   # We assume that other compilers are partly compatible with GNUCC
 
@@ -359,8 +380,6 @@ if(EIGEN_TEST_NO_EXCEPTIONS)
   message(STATUS "Disabling exceptions in tests/examples")
 endif()
 
-option(EIGEN_TEST_CXX11 "Enable testing with C++11 and C++11 features (e.g. Tensor module)." OFF)
-
 set(EIGEN_CUDA_COMPUTE_ARCH 30 CACHE STRING "The CUDA compute architecture level to target when compiling CUDA code")
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
@@ -416,16 +435,15 @@ add_subdirectory(Eigen)
 
 add_subdirectory(doc EXCLUDE_FROM_ALL)
 
-include(EigenConfigureTesting)
+option(BUILD_TESTING "Enable creation of Eigen tests." ON)
+if(BUILD_TESTING)
+  include(EigenConfigureTesting)
 
-# fixme, not sure this line is still needed:
-enable_testing() # must be called from the root CMakeLists, see man page
-
-
-if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
-  add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
-else()
-  add_subdirectory(test EXCLUDE_FROM_ALL)
+  if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
+    add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
+  else()
+    add_subdirectory(test EXCLUDE_FROM_ALL)
+  endif()
 endif()
 
 if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
@@ -461,7 +479,9 @@ endif(NOT WIN32)
 
 configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY)
 
-ei_testing_print_summary()
+if(BUILD_TESTING)
+  ei_testing_print_summary()
+endif()
 
 message(STATUS "")
 message(STATUS "Configured Eigen ${EIGEN_VERSION_NUMBER}")
diff --git a/gtsam/3rdparty/Eigen/CTestConfig.cmake b/gtsam/3rdparty/Eigen/CTestConfig.cmake
index 755b47323..0039bf8ac 100644
--- a/gtsam/3rdparty/Eigen/CTestConfig.cmake
+++ b/gtsam/3rdparty/Eigen/CTestConfig.cmake
@@ -4,10 +4,10 @@
 ## # The following are required to uses Dart and the Cdash dashboard
 ##   ENABLE_TESTING()
 ##   INCLUDE(CTest)
-set(CTEST_PROJECT_NAME "Eigen3.3")
+set(CTEST_PROJECT_NAME "Eigen 3.3")
 set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
 
 set(CTEST_DROP_METHOD "http")
 set(CTEST_DROP_SITE "manao.inria.fr")
-set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.3")
+set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen+3.3")
 set(CTEST_DROP_SITE_CDASH TRUE)
diff --git a/gtsam/3rdparty/Eigen/CTestCustom.cmake.in b/gtsam/3rdparty/Eigen/CTestCustom.cmake.in
index 9fed9d327..89e487f05 100644
--- a/gtsam/3rdparty/Eigen/CTestCustom.cmake.in
+++ b/gtsam/3rdparty/Eigen/CTestCustom.cmake.in
@@ -1,3 +1,4 @@
 
 set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "2000")
 set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS   "2000")
+list(APPEND CTEST_CUSTOM_ERROR_EXCEPTION    @EIGEN_CTEST_ERROR_EXCEPTION@)
diff --git a/gtsam/3rdparty/Eigen/Eigen/Cholesky b/gtsam/3rdparty/Eigen/Eigen/Cholesky
index 369d1f5ec..1332b540d 100644
--- a/gtsam/3rdparty/Eigen/Eigen/Cholesky
+++ b/gtsam/3rdparty/Eigen/Eigen/Cholesky
@@ -9,6 +9,7 @@
 #define EIGEN_CHOLESKY_MODULE_H
 
 #include "Core"
+#include "Jacobi"
 
 #include "src/Core/util/DisableStupidWarnings.h"
 
@@ -31,7 +32,11 @@
 #include "src/Cholesky/LLT.h"
 #include "src/Cholesky/LDLT.h"
 #ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
 #include "src/misc/lapacke.h"
+#endif
 #include "src/Cholesky/LLT_LAPACKE.h"
 #endif
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core
index 0f7fa630d..b923b8c00 100644
--- a/gtsam/3rdparty/Eigen/Eigen/Core
+++ b/gtsam/3rdparty/Eigen/Eigen/Core
@@ -14,6 +14,22 @@
 // first thing Eigen does: stop the compiler from committing suicide
 #include "src/Core/util/DisableStupidWarnings.h"
 
+#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
+  #define EIGEN_CUDACC __CUDACC__
+#endif
+
+#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
+  #define EIGEN_CUDA_ARCH __CUDA_ARCH__
+#endif
+
+#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
+#define EIGEN_CUDACC_VER  ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
+#elif defined(__CUDACC_VER__)
+#define EIGEN_CUDACC_VER __CUDACC_VER__
+#else
+#define EIGEN_CUDACC_VER 0
+#endif
+
 // Handle NVCC/CUDA/SYCL
 #if defined(__CUDACC__) || defined(__SYCL_DEVICE_ONLY__)
   // Do not try asserts on CUDA and SYCL!
@@ -37,9 +53,9 @@
     #endif
 
     #define EIGEN_DEVICE_FUNC __host__ __device__
-    // We need math_functions.hpp to ensure that that EIGEN_USING_STD_MATH macro
+    // We need cuda_runtime.h to ensure that that EIGEN_USING_STD_MATH macro
     // works properly on the device side
-    #include <math_functions.hpp>
+    #include <cuda_runtime.h>
   #else
     #define EIGEN_DEVICE_FUNC
   #endif
@@ -155,6 +171,9 @@
       #ifdef __AVX512DQ__
         #define EIGEN_VECTORIZE_AVX512DQ
       #endif
+      #ifdef __AVX512ER__
+        #define EIGEN_VECTORIZE_AVX512ER
+      #endif
     #endif
 
     // include files
@@ -229,7 +248,7 @@
 #if defined __CUDACC__
   #define EIGEN_VECTORIZE_CUDA
   #include <vector_types.h>
-  #if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
+  #if EIGEN_CUDACC_VER >= 70500
     #define EIGEN_HAS_CUDA_FP16
   #endif
 #endif
@@ -352,6 +371,7 @@ using std::ptrdiff_t;
 #include "src/Core/MathFunctions.h"
 #include "src/Core/GenericPacketMath.h"
 #include "src/Core/MathFunctionsImpl.h"
+#include "src/Core/arch/Default/ConjHelper.h"
 
 #if defined EIGEN_VECTORIZE_AVX512
   #include "src/Core/arch/SSE/PacketMath.h"
@@ -367,6 +387,7 @@ using std::ptrdiff_t;
   #include "src/Core/arch/AVX/MathFunctions.h"
   #include "src/Core/arch/AVX/Complex.h"
   #include "src/Core/arch/AVX/TypeCasting.h"
+  #include "src/Core/arch/SSE/TypeCasting.h"
 #elif defined EIGEN_VECTORIZE_SSE
   #include "src/Core/arch/SSE/PacketMath.h"
   #include "src/Core/arch/SSE/MathFunctions.h"
diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues
index 009e529e1..f3f661b07 100644
--- a/gtsam/3rdparty/Eigen/Eigen/Eigenvalues
+++ b/gtsam/3rdparty/Eigen/Eigen/Eigenvalues
@@ -45,7 +45,11 @@
 #include "src/Eigenvalues/GeneralizedEigenSolver.h"
 #include "src/Eigenvalues/MatrixBaseEigenvalues.h"
 #ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
 #include "src/misc/lapacke.h"
+#endif
 #include "src/Eigenvalues/RealSchur_LAPACKE.h"
 #include "src/Eigenvalues/ComplexSchur_LAPACKE.h"
 #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h"
diff --git a/gtsam/3rdparty/Eigen/Eigen/LU b/gtsam/3rdparty/Eigen/Eigen/LU
index 6f6c55629..6418a86e1 100644
--- a/gtsam/3rdparty/Eigen/Eigen/LU
+++ b/gtsam/3rdparty/Eigen/Eigen/LU
@@ -28,7 +28,11 @@
 #include "src/LU/FullPivLU.h"
 #include "src/LU/PartialPivLU.h"
 #ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
 #include "src/misc/lapacke.h"
+#endif
 #include "src/LU/PartialPivLU_LAPACKE.h"
 #endif
 #include "src/LU/Determinant.h"
diff --git a/gtsam/3rdparty/Eigen/Eigen/QR b/gtsam/3rdparty/Eigen/Eigen/QR
index 80838e3bd..c7e914469 100644
--- a/gtsam/3rdparty/Eigen/Eigen/QR
+++ b/gtsam/3rdparty/Eigen/Eigen/QR
@@ -36,7 +36,11 @@
 #include "src/QR/ColPivHouseholderQR.h"
 #include "src/QR/CompleteOrthogonalDecomposition.h"
 #ifdef EIGEN_USE_LAPACKE
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
 #include "src/misc/lapacke.h"
+#endif
 #include "src/QR/HouseholderQR_LAPACKE.h"
 #include "src/QR/ColPivHouseholderQR_LAPACKE.h"
 #endif
diff --git a/gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc b/gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc
index c6571f129..4f07df02a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc
+++ b/gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc
@@ -27,7 +27,7 @@ void qFree(void *ptr)
 void *qRealloc(void *ptr, std::size_t size)
 {
   void* newPtr = Eigen::internal::aligned_malloc(size);
-  memcpy(newPtr, ptr, size);
+  std::memcpy(newPtr, ptr, size);
   Eigen::internal::aligned_free(ptr);
   return newPtr;
 }
diff --git a/gtsam/3rdparty/Eigen/Eigen/SVD b/gtsam/3rdparty/Eigen/Eigen/SVD
index 86143c23d..5d0e75f7f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/SVD
+++ b/gtsam/3rdparty/Eigen/Eigen/SVD
@@ -37,7 +37,11 @@
 #include "src/SVD/JacobiSVD.h"
 #include "src/SVD/BDCSVD.h"
 #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT)
+#ifdef EIGEN_USE_MKL
+#include "mkl_lapacke.h"
+#else
 #include "src/misc/lapacke.h"
+#endif
 #include "src/SVD/JacobiSVD_LAPACKE.h"
 #endif
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
index fcee7b2e3..15ccf24f1 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
@@ -248,7 +248,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
     /** \brief Reports whether previous computation was successful.
       *
       * \returns \c Success if computation was succesful,
-      *          \c NumericalIssue if the matrix.appears to be negative.
+      *          \c NumericalIssue if the factorization failed because of a zero pivot.
       */
     ComputationInfo info() const
     {
@@ -305,7 +305,8 @@ template<> struct ldlt_inplace<Lower>
     if (size <= 1)
     {
       transpositions.setIdentity();
-      if (numext::real(mat.coeff(0,0)) > static_cast<RealScalar>(0) ) sign = PositiveSemiDef;
+      if(size==0) sign = ZeroSign;
+      else if (numext::real(mat.coeff(0,0)) > static_cast<RealScalar>(0) ) sign = PositiveSemiDef;
       else if (numext::real(mat.coeff(0,0)) < static_cast<RealScalar>(0)) sign = NegativeSemiDef;
       else sign = ZeroSign;
       return true;
@@ -376,6 +377,8 @@ template<> struct ldlt_inplace<Lower>
 
       if((rs>0) && pivot_is_valid)
         A21 /= realAkk;
+      else if(rs>0)
+        ret = ret && (A21.array()==Scalar(0)).all();
 
       if(found_zero_pivot && pivot_is_valid) ret = false; // factorization failed
       else if(!pivot_is_valid) found_zero_pivot = true;
@@ -568,13 +571,14 @@ void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) cons
   // more precisely, use pseudo-inverse of D (see bug 241)
   using std::abs;
   const typename Diagonal<const MatrixType>::RealReturnType vecD(vectorD());
-  // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
-  // as motivated by LAPACK's xGELSS:
+  // In some previous versions, tolerance was set to the max of 1/highest (or rather numeric_limits::min())
+  // and the maximal diagonal entry * epsilon as motivated by LAPACK's xGELSS:
   // RealScalar tolerance = numext::maxi(vecD.array().abs().maxCoeff() * NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
   // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
   // diagonal element is not well justified and leads to numerical issues in some cases.
   // Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
-  RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
+  // Using numeric_limits::min() gives us more robustness to denormals.
+  RealScalar tolerance = (std::numeric_limits<RealScalar>::min)();
 
   for (Index i = 0; i < vecD.size(); ++i)
   {
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h
index 87ca8d423..e1624d21b 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h
@@ -24,7 +24,7 @@ template<typename MatrixType, int UpLo> struct LLT_Traits;
   *
   * \tparam _MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition
   * \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper.
-  *             The other triangular part won't be read.
+  *               The other triangular part won't be read.
   *
   * This class performs a LL^T Cholesky decomposition of a symmetric, positive definite
   * matrix A such that A = LL^* = U^*U, where L is lower triangular.
@@ -41,14 +41,18 @@ template<typename MatrixType, int UpLo> struct LLT_Traits;
   * Example: \include LLT_example.cpp
   * Output: \verbinclude LLT_example.out
   *
+  * \b Performance: for best performance, it is recommended to use a column-major storage format
+  * with the Lower triangular part (the default), or, equivalently, a row-major storage format
+  * with the Upper triangular part. Otherwise, you might get a 20% slowdown for the full factorization
+  * step, and rank-updates can be up to 3 times slower.
+  *
   * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
   *
+  * Note that during the decomposition, only the lower (or upper, as defined by _UpLo) triangular part of A is considered.
+  * Therefore, the strict lower part does not have to store correct values.
+  *
   * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT
   */
- /* HEY THIS DOX IS DISABLED BECAUSE THERE's A BUG EITHER HERE OR IN LDLT ABOUT THAT (OR BOTH)
-  * Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
-  * the strict lower part does not have to store correct values.
-  */
 template<typename _MatrixType, int _UpLo> class LLT
 {
   public:
@@ -146,7 +150,7 @@ template<typename _MatrixType, int _UpLo> class LLT
     }
 
     template<typename Derived>
-    void solveInPlace(MatrixBase<Derived> &bAndX) const;
+    void solveInPlace(const MatrixBase<Derived> &bAndX) const;
 
     template<typename InputType>
     LLT& compute(const EigenBase<InputType>& matrix);
@@ -177,7 +181,7 @@ template<typename _MatrixType, int _UpLo> class LLT
     /** \brief Reports whether previous computation was successful.
       *
       * \returns \c Success if computation was succesful,
-      *          \c NumericalIssue if the matrix.appears to be negative.
+      *          \c NumericalIssue if the matrix.appears not to be positive definite.
       */
     ComputationInfo info() const
     {
@@ -425,7 +429,8 @@ LLT<MatrixType,_UpLo>& LLT<MatrixType,_UpLo>::compute(const EigenBase<InputType>
   eigen_assert(a.rows()==a.cols());
   const Index size = a.rows();
   m_matrix.resize(size, size);
-  m_matrix = a.derived();
+  if (!internal::is_same_dense(m_matrix, a.derived()))
+    m_matrix = a.derived();
 
   // Compute matrix L1 norm = max abs column sum.
   m_l1_norm = RealScalar(0);
@@ -485,11 +490,14 @@ void LLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
   *
   * This version avoids a copy when the right hand side matrix b is not needed anymore.
   *
+  * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
+  * This function will const_cast it, so constness isn't honored here.
+  *
   * \sa LLT::solve(), MatrixBase::llt()
   */
 template<typename MatrixType, int _UpLo>
 template<typename Derived>
-void LLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
+void LLT<MatrixType,_UpLo>::solveInPlace(const MatrixBase<Derived> &bAndX) const
 {
   eigen_assert(m_isInitialized && "LLT is not initialized.");
   eigen_assert(m_matrix.rows()==bAndX.rows());
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h
index e10020d4f..16770fc7b 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h
@@ -153,8 +153,6 @@ class Array
       : Base(std::move(other))
     {
       Base::_check_template_params();
-      if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic)
-        Base::_set_noalias(other);
     }
     EIGEN_DEVICE_FUNC
     Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h
index 6c2ab9264..6866095bf 100755
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h
@@ -84,7 +84,8 @@ class vml_assign_traits
   struct Assignment<DstXprType, CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested>, assign_op<EIGENTYPE,EIGENTYPE>,   \
                    Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> {              \
     typedef CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested> SrcXprType;                                            \
-    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &/*func*/) {                   \
+    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &func) {                       \
+      resize_if_allowed(dst, src, func);                                                                                        \
       eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());                                                       \
       if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal) {                                              \
         VMLOP(dst.size(), (const VMLTYPE*)src.nestedExpression().data(),                                                        \
@@ -144,7 +145,8 @@ EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(ceil,  Ceil,   _)
                    Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> {            \
     typedef CwiseBinaryOp<scalar_##EIGENOP##_op<EIGENTYPE,EIGENTYPE>, SrcXprNested,                                           \
                     const CwiseNullaryOp<internal::scalar_constant_op<EIGENTYPE>,Plain> > SrcXprType;                         \
-    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &/*func*/) {                 \
+    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &func) {                     \
+      resize_if_allowed(dst, src, func);                                                                                      \
       eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());                                                     \
       VMLTYPE exponent = reinterpret_cast<const VMLTYPE&>(src.rhs().functor().m_other);                                       \
       if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal)                                              \
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ConditionEstimator.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ConditionEstimator.h
index aa7efdc76..51a2e5f1b 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ConditionEstimator.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ConditionEstimator.h
@@ -160,7 +160,7 @@ rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, const Deco
 {
   typedef typename Decomposition::RealScalar RealScalar;
   eigen_assert(dec.rows() == dec.cols());
-  if (dec.rows() == 0)              return RealScalar(1);
+  if (dec.rows() == 0)              return NumTraits<RealScalar>::infinity();
   if (matrix_norm == RealScalar(0)) return RealScalar(0);
   if (dec.rows() == 1)              return RealScalar(1);
   const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec);
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CoreEvaluators.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CoreEvaluators.h
index f7c1effca..910889efa 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CoreEvaluators.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CoreEvaluators.h
@@ -977,7 +977,7 @@ struct evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
     OuterStrideAtCompileTime = HasSameStorageOrderAsArgType
                              ? int(outer_stride_at_compile_time<ArgType>::ret)
                              : int(inner_stride_at_compile_time<ArgType>::ret),
-    MaskPacketAccessBit = (InnerStrideAtCompileTime == 1) ? PacketAccessBit : 0,
+    MaskPacketAccessBit = (InnerStrideAtCompileTime == 1 || HasSameStorageOrderAsArgType) ? PacketAccessBit : 0,
     
     FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (evaluator<ArgType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,    
     FlagsRowMajorBit = XprType::Flags&RowMajorBit,
@@ -987,7 +987,9 @@ struct evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
     Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit,
     
     PacketAlignment = unpacket_traits<PacketScalar>::alignment,
-    Alignment0 = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) ? int(PacketAlignment) : 0,
+    Alignment0 = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic)
+                             && (OuterStrideAtCompileTime!=0)
+                             && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) ? int(PacketAlignment) : 0,
     Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<ArgType>::Alignment, Alignment0)
   };
   typedef block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel> block_evaluator_type;
@@ -1018,14 +1020,16 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
   EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& block)
     : m_argImpl(block.nestedExpression()), 
       m_startRow(block.startRow()), 
-      m_startCol(block.startCol()) 
+      m_startCol(block.startCol()),
+      m_linear_offset(InnerPanel?(XprType::IsRowMajor ? block.startRow()*block.cols() : block.startCol()*block.rows()):0)
   { }
  
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
   enum {
-    RowsAtCompileTime = XprType::RowsAtCompileTime
+    RowsAtCompileTime = XprType::RowsAtCompileTime,
+    ForwardLinearAccess = InnerPanel && bool(evaluator<ArgType>::Flags&LinearAccessBit)
   };
  
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
@@ -1037,7 +1041,10 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
   { 
-    return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
+    if (ForwardLinearAccess)
+      return m_argImpl.coeff(m_linear_offset.value() + index); 
+    else
+      return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
@@ -1049,7 +1056,10 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   Scalar& coeffRef(Index index)
   { 
-    return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
+    if (ForwardLinearAccess)
+      return m_argImpl.coeffRef(m_linear_offset.value() + index); 
+    else
+      return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
   }
  
   template<int LoadMode, typename PacketType>
@@ -1063,8 +1073,11 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
   EIGEN_STRONG_INLINE
   PacketType packet(Index index) const 
   { 
-    return packet<LoadMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
-                                       RowsAtCompileTime == 1 ? index : 0);
+    if (ForwardLinearAccess)
+      return m_argImpl.template packet<LoadMode,PacketType>(m_linear_offset.value() + index);
+    else
+      return packet<LoadMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
+                                         RowsAtCompileTime == 1 ? index : 0);
   }
   
   template<int StoreMode, typename PacketType>
@@ -1078,15 +1091,19 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
   EIGEN_STRONG_INLINE
   void writePacket(Index index, const PacketType& x) 
   {
-    return writePacket<StoreMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
-                                             RowsAtCompileTime == 1 ? index : 0,
-                                             x);
+    if (ForwardLinearAccess)
+      return m_argImpl.template writePacket<StoreMode,PacketType>(m_linear_offset.value() + index, x);
+    else
+      return writePacket<StoreMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
+                                              RowsAtCompileTime == 1 ? index : 0,
+                                              x);
   }
  
 protected:
   evaluator<ArgType> m_argImpl;
   const variable_if_dynamic<Index, (ArgType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
   const variable_if_dynamic<Index, (ArgType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
+  const variable_if_dynamic<Index, InnerPanel ? Dynamic : 0> m_linear_offset;
 };
 
 // TODO: This evaluator does not actually use the child evaluator; 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
index 49e711257..afcaf3575 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
@@ -70,7 +70,10 @@ template<typename MatrixType, int _DiagIndex> class Diagonal
     EIGEN_DENSE_PUBLIC_INTERFACE(Diagonal)
 
     EIGEN_DEVICE_FUNC
-    explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index) {}
+    explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index)
+    {
+      eigen_assert( a_index <= m_matrix.cols() && -a_index <= m_matrix.rows() );
+    }
 
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Diagonal)
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h
index 06ef18b8b..1fe7a84a4 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h
@@ -31,7 +31,8 @@ struct dot_nocheck
   typedef scalar_conj_product_op<typename traits<T>::Scalar,typename traits<U>::Scalar> conj_prod;
   typedef typename conj_prod::result_type ResScalar;
   EIGEN_DEVICE_FUNC
-  static inline ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
+  EIGEN_STRONG_INLINE
+  static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
   {
     return a.template binaryExpr<conj_prod>(b).sum();
   }
@@ -43,7 +44,8 @@ struct dot_nocheck<T, U, true>
   typedef scalar_conj_product_op<typename traits<T>::Scalar,typename traits<U>::Scalar> conj_prod;
   typedef typename conj_prod::result_type ResScalar;
   EIGEN_DEVICE_FUNC
-  static inline ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
+  EIGEN_STRONG_INLINE
+  static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
   {
     return a.transpose().template binaryExpr<conj_prod>(b).sum();
   }
@@ -65,6 +67,7 @@ struct dot_nocheck<T, U, true>
 template<typename Derived>
 template<typename OtherDerived>
 EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE
 typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
 MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
 {
@@ -102,7 +105,7 @@ EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scala
   * \sa lpNorm(), dot(), squaredNorm()
   */
 template<typename Derived>
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
+EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
 {
   return numext::sqrt(squaredNorm());
 }
@@ -117,7 +120,7 @@ inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real Matr
   * \sa norm(), normalize()
   */
 template<typename Derived>
-inline const typename MatrixBase<Derived>::PlainObject
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
 MatrixBase<Derived>::normalized() const
 {
   typedef typename internal::nested_eval<Derived,2>::type _Nested;
@@ -139,7 +142,7 @@ MatrixBase<Derived>::normalized() const
   * \sa norm(), normalized()
   */
 template<typename Derived>
-inline void MatrixBase<Derived>::normalize()
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::normalize()
 {
   RealScalar z = squaredNorm();
   // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU
@@ -160,7 +163,7 @@ inline void MatrixBase<Derived>::normalize()
   * \sa stableNorm(), stableNormalize(), normalized()
   */
 template<typename Derived>
-inline const typename MatrixBase<Derived>::PlainObject
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
 MatrixBase<Derived>::stableNormalized() const
 {
   typedef typename internal::nested_eval<Derived,3>::type _Nested;
@@ -185,7 +188,7 @@ MatrixBase<Derived>::stableNormalized() const
   * \sa stableNorm(), stableNormalized(), normalize()
   */
 template<typename Derived>
-inline void MatrixBase<Derived>::stableNormalize()
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::stableNormalize()
 {
   RealScalar w = cwiseAbs().maxCoeff();
   RealScalar z = (derived()/w).squaredNorm();
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
index 0f16cd8e3..6f0cc80e9 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
@@ -24,12 +24,17 @@ template<int Rows, int Cols, int Depth> struct product_type_selector;
 
 template<int Size, int MaxSize> struct product_size_category
 {
-  enum { is_large = MaxSize == Dynamic ||
-                    Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD ||
-                    (Size==Dynamic && MaxSize>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD),
-         value = is_large  ? Large
-               : Size == 1 ? 1
-                           : Small
+  enum {
+    #ifndef EIGEN_CUDA_ARCH
+    is_large = MaxSize == Dynamic ||
+               Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD ||
+               (Size==Dynamic && MaxSize>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD),
+    #else
+    is_large = 0,
+    #endif
+    value = is_large  ? Large
+          : Size == 1 ? 1
+                      : Small
   };
 };
 
@@ -379,8 +384,6 @@ template<> struct gemv_dense_selector<OnTheRight,RowMajor,false>
   *
   * \sa lazyProduct(), operator*=(const MatrixBase&), Cwise::operator*()
   */
-#ifndef __CUDACC__
-
 template<typename Derived>
 template<typename OtherDerived>
 inline const Product<Derived, OtherDerived>
@@ -412,8 +415,6 @@ MatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
   return Product<Derived, OtherDerived>(derived(), other.derived());
 }
 
-#endif // __CUDACC__
-
 /** \returns an expression of the matrix product of \c *this and \a other without implicit evaluation.
   *
   * The returned product will behave like any other expressions: the coefficients of the product will be
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
index 06d196702..548bf9a2d 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
@@ -20,11 +20,17 @@ struct traits<Map<PlainObjectType, MapOptions, StrideType> >
 {
   typedef traits<PlainObjectType> TraitsBase;
   enum {
+    PlainObjectTypeInnerSize = ((traits<PlainObjectType>::Flags&RowMajorBit)==RowMajorBit)
+                             ? PlainObjectType::ColsAtCompileTime
+                             : PlainObjectType::RowsAtCompileTime,
+
     InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0
                              ? int(PlainObjectType::InnerStrideAtCompileTime)
                              : int(StrideType::InnerStrideAtCompileTime),
     OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0
-                             ? int(PlainObjectType::OuterStrideAtCompileTime)
+                             ? (InnerStrideAtCompileTime==Dynamic || PlainObjectTypeInnerSize==Dynamic
+                                ? Dynamic
+                                : int(InnerStrideAtCompileTime) * int(PlainObjectTypeInnerSize))
                              : int(StrideType::OuterStrideAtCompileTime),
     Alignment = int(MapOptions)&int(AlignedMask),
     Flags0 = TraitsBase::Flags & (~NestByRefBit),
@@ -107,10 +113,11 @@ template<typename PlainObjectType, int MapOptions, typename StrideType> class Ma
     EIGEN_DEVICE_FUNC
     inline Index outerStride() const
     {
-      return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer()
-           : IsVectorAtCompileTime ? this->size()
-           : int(Flags)&RowMajorBit ? this->cols()
-           : this->rows();
+      return int(StrideType::OuterStrideAtCompileTime) != 0 ? m_stride.outer()
+           : int(internal::traits<Map>::OuterStrideAtCompileTime) != Dynamic ? Index(internal::traits<Map>::OuterStrideAtCompileTime)
+           : IsVectorAtCompileTime ? (this->size() * innerStride())
+           : (int(Flags)&RowMajorBit) ? (this->cols() * innerStride())
+           : (this->rows() * innerStride());
     }
 
     /** Constructor in the fixed-size case.
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
index 020f939ad..668922ffc 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
@@ -43,6 +43,7 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
     enum {
       RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
       ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
+      InnerStrideAtCompileTime = internal::traits<Derived>::InnerStrideAtCompileTime,
       SizeAtCompileTime = Base::SizeAtCompileTime
     };
 
@@ -187,8 +188,11 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
     void checkSanity(typename internal::enable_if<(internal::traits<T>::Alignment>0),void*>::type = 0) const
     {
 #if EIGEN_MAX_ALIGN_BYTES>0
+      // innerStride() is not set yet when this function is called, so we optimistically assume the lowest plausible value:
+      const Index minInnerStride = InnerStrideAtCompileTime == Dynamic ? 1 : Index(InnerStrideAtCompileTime);
+      EIGEN_ONLY_USED_FOR_DEBUG(minInnerStride);
       eigen_assert((   ((internal::UIntPtr(m_data) % internal::traits<Derived>::Alignment) == 0)
-                    || (cols() * rows() * innerStride() * sizeof(Scalar)) < internal::traits<Derived>::Alignment ) && "data is not aligned");
+                    || (cols() * rows() * minInnerStride * sizeof(Scalar)) < internal::traits<Derived>::Alignment ) && "data is not aligned");
 #endif
     }
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h
index a648aa0fa..b249ce0c8 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h
@@ -348,31 +348,7 @@ struct norm1_retval
 * Implementation of hypot                                                *
 ****************************************************************************/
 
-template<typename Scalar>
-struct hypot_impl
-{
-  typedef typename NumTraits<Scalar>::Real RealScalar;
-  static inline RealScalar run(const Scalar& x, const Scalar& y)
-  {
-    EIGEN_USING_STD_MATH(abs);
-    EIGEN_USING_STD_MATH(sqrt);
-    RealScalar _x = abs(x);
-    RealScalar _y = abs(y);
-    Scalar p, qp;
-    if(_x>_y)
-    {
-      p = _x;
-      qp = _y / p;
-    }
-    else
-    {
-      p = _y;
-      qp = _x / p;
-    }
-    if(p==RealScalar(0)) return RealScalar(0);
-    return p * sqrt(RealScalar(1) + qp*qp);
-  }
-};
+template<typename Scalar> struct hypot_impl;
 
 template<typename Scalar>
 struct hypot_retval
@@ -495,7 +471,7 @@ namespace std_fallback {
     typedef typename NumTraits<Scalar>::Real RealScalar;
     EIGEN_USING_STD_MATH(log);
     Scalar x1p = RealScalar(1) + x;
-    return ( x1p == Scalar(1) ) ? x : x * ( log(x1p) / (x1p - RealScalar(1)) );
+    return numext::equal_strict(x1p, Scalar(1)) ? x : x * ( log(x1p) / (x1p - RealScalar(1)) );
   }
 }
 
@@ -640,21 +616,28 @@ template<typename Scalar>
 struct random_default_impl<Scalar, false, true>
 {
   static inline Scalar run(const Scalar& x, const Scalar& y)
-  { 
-    typedef typename conditional<NumTraits<Scalar>::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX;
-    if(y<x)
+  {
+    if (y <= x)
       return x;
-    // the following difference might overflow on a 32 bits system,
-    // but since y>=x the result converted to an unsigned long is still correct.
-    std::size_t range = ScalarX(y)-ScalarX(x);
-    std::size_t offset = 0;
-    // rejection sampling
-    std::size_t divisor = 1;
-    std::size_t multiplier = 1;
-    if(range<RAND_MAX) divisor = (std::size_t(RAND_MAX)+1)/(range+1);
-    else               multiplier = 1 + range/(std::size_t(RAND_MAX)+1);
+    // ScalarU is the unsigned counterpart of Scalar, possibly Scalar itself.
+    typedef typename make_unsigned<Scalar>::type ScalarU;
+    // ScalarX is the widest of ScalarU and unsigned int.
+    // We'll deal only with ScalarX and unsigned int below thus avoiding signed
+    // types and arithmetic and signed overflows (which are undefined behavior).
+    typedef typename conditional<(ScalarU(-1) > unsigned(-1)), ScalarU, unsigned>::type ScalarX;
+    // The following difference doesn't overflow, provided our integer types are two's
+    // complement and have the same number of padding bits in signed and unsigned variants.
+    // This is the case in most modern implementations of C++.
+    ScalarX range = ScalarX(y) - ScalarX(x);
+    ScalarX offset = 0;
+    ScalarX divisor = 1;
+    ScalarX multiplier = 1;
+    const unsigned rand_max = RAND_MAX;
+    if (range <= rand_max) divisor = (rand_max + 1) / (range + 1);
+    else                   multiplier = 1 + range / (rand_max + 1);
+    // Rejection sampling.
     do {
-      offset = (std::size_t(std::rand()) * multiplier) / divisor;
+      offset = (unsigned(std::rand()) * multiplier) / divisor;
     } while (offset > range);
     return Scalar(ScalarX(x) + offset);
   }
@@ -1030,7 +1013,8 @@ inline int log2(int x)
 
 /** \returns the square root of \a x.
   *
-  * It is essentially equivalent to \code using std::sqrt; return sqrt(x); \endcode,
+  * It is essentially equivalent to
+  * \code using std::sqrt; return sqrt(x); \endcode
   * but slightly faster for float/double and some compilers (e.g., gcc), thanks to
   * specializations when SSE is enabled.
   *
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctionsImpl.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctionsImpl.h
index 3c9ef22fa..9c1ceb0eb 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctionsImpl.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctionsImpl.h
@@ -71,6 +71,29 @@ T generic_fast_tanh_float(const T& a_x)
   return pdiv(p, q);
 }
 
+template<typename RealScalar>
+EIGEN_STRONG_INLINE
+RealScalar positive_real_hypot(const RealScalar& x, const RealScalar& y)
+{
+  EIGEN_USING_STD_MATH(sqrt);
+  RealScalar p, qp;
+  p = numext::maxi(x,y);
+  if(p==RealScalar(0)) return RealScalar(0);
+  qp = numext::mini(y,x) / p;    
+  return p * sqrt(RealScalar(1) + qp*qp);
+}
+
+template<typename Scalar>
+struct hypot_impl
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  static inline RealScalar run(const Scalar& x, const Scalar& y)
+  {
+    EIGEN_USING_STD_MATH(abs);
+    return positive_real_hypot<RealScalar>(abs(x), abs(y));
+  }
+};
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h
index 90c336d8c..7f4a7af93 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h
@@ -274,8 +274,6 @@ class Matrix
       : Base(std::move(other))
     {
       Base::_check_template_params();
-      if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic)
-        Base::_set_noalias(other);
     }
     EIGEN_DEVICE_FUNC
     Matrix& operator=(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
index ce412180a..e6c35907c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
@@ -160,20 +160,11 @@ template<typename Derived> class MatrixBase
     EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     Derived& operator-=(const MatrixBase<OtherDerived>& other);
 
-#ifdef __CUDACC__
     template<typename OtherDerived>
     EIGEN_DEVICE_FUNC
-    const Product<Derived,OtherDerived,LazyProduct>
-    operator*(const MatrixBase<OtherDerived> &other) const
-    { return this->lazyProduct(other); }
-#else
-
-    template<typename OtherDerived>
     const Product<Derived,OtherDerived>
     operator*(const MatrixBase<OtherDerived> &other) const;
 
-#endif
-
     template<typename OtherDerived>
     EIGEN_DEVICE_FUNC
     const Product<Derived,OtherDerived,LazyProduct>
@@ -453,16 +444,24 @@ template<typename Derived> class MatrixBase
 ///////// MatrixFunctions module /////////
 
     typedef typename internal::stem_function<Scalar>::type StemFunction;
-    const MatrixExponentialReturnValue<Derived> exp() const;
+#define EIGEN_MATRIX_FUNCTION(ReturnType, Name, Description) \
+    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
+    const ReturnType<Derived> Name() const;
+#define EIGEN_MATRIX_FUNCTION_1(ReturnType, Name, Description, Argument) \
+    /** \returns an expression of the matrix Description of \c *this. \brief This function requires the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>. To compute the coefficient-wise Description use ArrayBase::##Name . */ \
+    const ReturnType<Derived> Name(Argument) const;
+
+    EIGEN_MATRIX_FUNCTION(MatrixExponentialReturnValue, exp, exponential)
+    /** \brief Helper function for the <a href="unsupported/group__MatrixFunctions__Module.html"> unsupported MatrixFunctions module</a>.*/
     const MatrixFunctionReturnValue<Derived> matrixFunction(StemFunction f) const;
-    const MatrixFunctionReturnValue<Derived> cosh() const;
-    const MatrixFunctionReturnValue<Derived> sinh() const;
-    const MatrixFunctionReturnValue<Derived> cos() const;
-    const MatrixFunctionReturnValue<Derived> sin() const;
-    const MatrixSquareRootReturnValue<Derived> sqrt() const;
-    const MatrixLogarithmReturnValue<Derived> log() const;
-    const MatrixPowerReturnValue<Derived> pow(const RealScalar& p) const;
-    const MatrixComplexPowerReturnValue<Derived> pow(const std::complex<RealScalar>& p) const;
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cosh, hyperbolic cosine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sinh, hyperbolic sine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cos, cosine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sin, sine)
+    EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root)
+    EIGEN_MATRIX_FUNCTION(MatrixLogarithmReturnValue, log, logarithm)
+    EIGEN_MATRIX_FUNCTION_1(MatrixPowerReturnValue,        pow, power to \c p, const RealScalar& p)
+    EIGEN_MATRIX_FUNCTION_1(MatrixComplexPowerReturnValue, pow, power to \c p, const std::complex<RealScalar>& p)
 
   protected:
     EIGEN_DEVICE_FUNC MatrixBase() : Base() {}
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
index 77f4f6066..1dc7e223a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
@@ -577,6 +577,10 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
       * while the AlignedMap() functions return aligned Map objects and thus should be called only with 16-byte-aligned
       * \a data pointers.
       *
+      * Here is an example using strides:
+      * \include Matrix_Map_stride.cpp
+      * Output: \verbinclude Matrix_Map_stride.out
+      *
       * \see class Map
       */
     //@{
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h
index ae0c94b38..676c48027 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h
@@ -97,8 +97,8 @@ class Product : public ProductImpl<_Lhs,_Rhs,Option,
         && "if you wanted a coeff-wise or a dot product use the respective explicit functions");
     }
 
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_lhs.rows(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_rhs.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { return m_lhs.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { return m_rhs.cols(); }
 
     EIGEN_DEVICE_FUNC const LhsNestedCleaned& lhs() const { return m_lhs; }
     EIGEN_DEVICE_FUNC const RhsNestedCleaned& rhs() const { return m_rhs; }
@@ -127,7 +127,7 @@ public:
   using Base::derived;
   typedef typename Base::Scalar Scalar;
   
-  operator const Scalar() const
+  EIGEN_STRONG_INLINE operator const Scalar() const
   {
     return internal::evaluator<ProductXpr>(derived()).coeff(0,0);
   }
@@ -162,7 +162,7 @@ class ProductImpl<Lhs,Rhs,Option,Dense>
     
   public:
   
-    EIGEN_DEVICE_FUNC Scalar coeff(Index row, Index col) const
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index row, Index col) const
     {
       EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
       eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) );
@@ -170,7 +170,7 @@ class ProductImpl<Lhs,Rhs,Option,Dense>
       return internal::evaluator<Derived>(derived()).coeff(row,col);
     }
 
-    EIGEN_DEVICE_FUNC Scalar coeff(Index i) const
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index i) const
     {
       EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
       eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) );
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h
index c42725dbd..9b99bd769 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductEvaluators.h
@@ -32,7 +32,7 @@ struct evaluator<Product<Lhs, Rhs, Options> >
   typedef Product<Lhs, Rhs, Options> XprType;
   typedef product_evaluator<XprType> Base;
   
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr) : Base(xpr) {}
 };
  
 // Catch "scalar * ( A * B )" and transform it to "(A*scalar) * B"
@@ -55,7 +55,7 @@ struct evaluator<CwiseBinaryOp<internal::scalar_product_op<Scalar1,Scalar2>,
                                const Product<Lhs, Rhs, DefaultProduct> > XprType;
   typedef evaluator<Product<EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar1,Lhs,product), Rhs, DefaultProduct> > Base;
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr)
     : Base(xpr.lhs().functor().m_other * xpr.rhs().lhs() * xpr.rhs().rhs())
   {}
 };
@@ -68,7 +68,7 @@ struct evaluator<Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex> >
   typedef Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex> XprType;
   typedef evaluator<Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex> > Base;
   
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr)
     : Base(Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex>(
         Product<Lhs, Rhs, LazyProduct>(xpr.nestedExpression().lhs(), xpr.nestedExpression().rhs()),
         xpr.index() ))
@@ -246,19 +246,19 @@ template<typename Lhs, typename Rhs>
 struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,InnerProduct>
 {
   template<typename Dst>
-  static inline void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     dst.coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum();
   }
   
   template<typename Dst>
-  static inline void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     dst.coeffRef(0,0) += (lhs.transpose().cwiseProduct(rhs)).sum();
   }
   
   template<typename Dst>
-  static void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   { dst.coeffRef(0,0) -= (lhs.transpose().cwiseProduct(rhs)).sum(); }
 };
 
@@ -312,25 +312,25 @@ struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,OuterProduct>
   };
   
   template<typename Dst>
-  static inline void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, set(), is_row_major<Dst>());
   }
   
   template<typename Dst>
-  static inline void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, add(), is_row_major<Dst>());
   }
   
   template<typename Dst>
-  static inline void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, sub(), is_row_major<Dst>());
   }
   
   template<typename Dst>
-  static inline void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+  static EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, adds(alpha), is_row_major<Dst>());
   }
@@ -785,7 +785,11 @@ public:
     _Vectorizable = bool(int(MatrixFlags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagFlags)&PacketAccessBit))),
     _LinearAccessMask = (MatrixType::RowsAtCompileTime==1 || MatrixType::ColsAtCompileTime==1) ? LinearAccessBit : 0,
     Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixFlags)) | (_Vectorizable ? PacketAccessBit : 0),
-    Alignment = evaluator<MatrixType>::Alignment
+    Alignment = evaluator<MatrixType>::Alignment,
+
+    AsScalarProduct =     (DiagonalType::SizeAtCompileTime==1)
+                      ||  (DiagonalType::SizeAtCompileTime==Dynamic && MatrixType::RowsAtCompileTime==1 && ProductOrder==OnTheLeft)
+                      ||  (DiagonalType::SizeAtCompileTime==Dynamic && MatrixType::ColsAtCompileTime==1 && ProductOrder==OnTheRight)
   };
   
   diagonal_product_evaluator_base(const MatrixType &mat, const DiagonalType &diag)
@@ -797,7 +801,10 @@ public:
   
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const
   {
-    return m_diagImpl.coeff(idx) * m_matImpl.coeff(idx);
+    if(AsScalarProduct)
+      return m_diagImpl.coeff(0) * m_matImpl.coeff(idx);
+    else
+      return m_diagImpl.coeff(idx) * m_matImpl.coeff(idx);
   }
   
 protected:
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h
index b6e8f8887..760e9f861 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h
@@ -407,7 +407,7 @@ protected:
   */
 template<typename Derived>
 template<typename Func>
-typename internal::traits<Derived>::Scalar
+EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::redux(const Func& func) const
 {
   eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h
index bdf24f52a..9c6e3c5d9 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h
@@ -95,6 +95,8 @@ protected:
   template<typename Expression>
   EIGEN_DEVICE_FUNC void construct(Expression& expr)
   {
+    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(PlainObjectType,Expression);
+
     if(PlainObjectType::RowsAtCompileTime==1)
     {
       eigen_assert(expr.rows()==1 || expr.cols()==1);
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h
index 504c98f0e..b2e51f37a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h
@@ -71,7 +71,9 @@ template<typename _MatrixType, unsigned int UpLo> class SelfAdjointView
 
     EIGEN_DEVICE_FUNC
     explicit inline SelfAdjointView(MatrixType& matrix) : m_matrix(matrix)
-    {}
+    {
+      EIGEN_STATIC_ASSERT(UpLo==Lower || UpLo==Upper,SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY);
+    }
 
     EIGEN_DEVICE_FUNC
     inline Index rows() const { return m_matrix.rows(); }
@@ -189,7 +191,7 @@ template<typename _MatrixType, unsigned int UpLo> class SelfAdjointView
                                    TriangularView<typename MatrixType::AdjointReturnType,TriMode> >::type(tmp2);
     }
 
-    typedef SelfAdjointView<const MatrixConjugateReturnType,Mode> ConjugateReturnType;
+    typedef SelfAdjointView<const MatrixConjugateReturnType,UpLo> ConjugateReturnType;
     /** \sa MatrixBase::conjugate() const */
     EIGEN_DEVICE_FUNC
     inline const ConjugateReturnType conjugate() const
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h
index 50099df82..7c89c2e23 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h
@@ -17,7 +17,6 @@ namespace Eigen {
 template<typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator*=(const Scalar& other)
 {
-  typedef typename Derived::PlainObject PlainObject;
   internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op<Scalar,Scalar>());
   return derived();
 }
@@ -25,7 +24,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator*=(co
 template<typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator+=(const Scalar& other)
 {
-  typedef typename Derived::PlainObject PlainObject;
   internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op<Scalar,Scalar>());
   return derived();
 }
@@ -33,7 +31,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator+=(co
 template<typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator-=(const Scalar& other)
 {
-  typedef typename Derived::PlainObject PlainObject;
   internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op<Scalar,Scalar>());
   return derived();
 }
@@ -41,7 +38,6 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase<Derived>::operator-=(co
 template<typename Derived>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::operator/=(const Scalar& other)
 {
-  typedef typename Derived::PlainObject PlainObject;
   internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op<Scalar,Scalar>());
   return derived();
 }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h
index 049890b25..4652e2e19 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h
@@ -169,6 +169,9 @@ void TriangularViewImpl<MatrixType,Mode,Dense>::solveInPlace(const MatrixBase<Ot
   OtherDerived& other = _other.const_cast_derived();
   eigen_assert( derived().cols() == derived().rows() && ((Side==OnTheLeft && derived().cols() == other.rows()) || (Side==OnTheRight && derived().cols() == other.cols())) );
   eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+  // If solving for a 0x0 matrix, nothing to do, simply return.
+  if (derived().cols() == 0)
+    return;
 
   enum { copy = (internal::traits<OtherDerived>::Flags & RowMajorBit)  && OtherDerived::IsVectorAtCompileTime && OtherDerived::SizeAtCompileTime!=1};
   typedef typename internal::conditional<copy,
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h
index be04ed44d..88c8d9890 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h
@@ -165,7 +165,7 @@ MatrixBase<Derived>::stableNorm() const
   
   typedef typename internal::nested_eval<Derived,2>::type DerivedCopy;
   typedef typename internal::remove_all<DerivedCopy>::type DerivedCopyClean;
-  DerivedCopy copy(derived());
+  const DerivedCopy copy(derived());
   
   enum {
     CanAlign = (   (int(DerivedCopyClean::Flags)&DirectAccessBit)
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h
index 19c17bb4a..86da5af59 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h
@@ -384,7 +384,7 @@ class Transpose<TranspositionsBase<TranspositionsDerived> >
     const Product<OtherDerived, Transpose, AliasFreeProduct>
     operator*(const MatrixBase<OtherDerived>& matrix, const Transpose& trt)
     {
-      return Product<OtherDerived, Transpose, AliasFreeProduct>(matrix.derived(), trt.derived());
+      return Product<OtherDerived, Transpose, AliasFreeProduct>(matrix.derived(), trt);
     }
 
     /** \returns the \a matrix with the inverse transpositions applied to the rows.
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/Complex.h
index 99439c8aa..7fa61969d 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/Complex.h
@@ -204,23 +204,7 @@ template<> struct conj_helper<Packet4cf, Packet4cf, true,true>
   }
 };
 
-template<> struct conj_helper<Packet8f, Packet4cf, false,false>
-{
-  EIGEN_STRONG_INLINE Packet4cf pmadd(const Packet8f& x, const Packet4cf& y, const Packet4cf& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet4cf pmul(const Packet8f& x, const Packet4cf& y) const
-  { return Packet4cf(Eigen::internal::pmul(x, y.v)); }
-};
-
-template<> struct conj_helper<Packet4cf, Packet8f, false,false>
-{
-  EIGEN_STRONG_INLINE Packet4cf pmadd(const Packet4cf& x, const Packet8f& y, const Packet4cf& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet4cf pmul(const Packet4cf& x, const Packet8f& y) const
-  { return Packet4cf(Eigen::internal::pmul(x.v, y)); }
-};
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet4cf,Packet8f)
 
 template<> EIGEN_STRONG_INLINE Packet4cf pdiv<Packet4cf>(const Packet4cf& a, const Packet4cf& b)
 {
@@ -400,23 +384,7 @@ template<> struct conj_helper<Packet2cd, Packet2cd, true,true>
   }
 };
 
-template<> struct conj_helper<Packet4d, Packet2cd, false,false>
-{
-  EIGEN_STRONG_INLINE Packet2cd pmadd(const Packet4d& x, const Packet2cd& y, const Packet2cd& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet2cd pmul(const Packet4d& x, const Packet2cd& y) const
-  { return Packet2cd(Eigen::internal::pmul(x, y.v)); }
-};
-
-template<> struct conj_helper<Packet2cd, Packet4d, false,false>
-{
-  EIGEN_STRONG_INLINE Packet2cd pmadd(const Packet2cd& x, const Packet4d& y, const Packet2cd& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet2cd pmul(const Packet2cd& x, const Packet4d& y) const
-  { return Packet2cd(Eigen::internal::pmul(x.v, y)); }
-};
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cd,Packet4d)
 
 template<> EIGEN_STRONG_INLINE Packet2cd pdiv<Packet2cd>(const Packet2cd& a, const Packet2cd& b)
 {
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/PacketMath.h
index 195d40fb4..923a124b2 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX/PacketMath.h
@@ -159,11 +159,12 @@ template<> EIGEN_STRONG_INLINE Packet8i pdiv<Packet8i>(const Packet8i& /*a*/, co
 
 #ifdef __FMA__
 template<> EIGEN_STRONG_INLINE Packet8f pmadd(const Packet8f& a, const Packet8f& b, const Packet8f& c) {
-#if ( EIGEN_COMP_GNUC_STRICT || (EIGEN_COMP_CLANG && (EIGEN_COMP_CLANG<308)) )
-  // clang stupidly generates a vfmadd213ps instruction plus some vmovaps on registers,
-  // and gcc stupidly generates a vfmadd132ps instruction,
-  // so let's enforce it to generate a vfmadd231ps instruction since the most common use case is to accumulate
-  // the result of the product.
+#if ( (EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC<80) || (EIGEN_COMP_CLANG) )
+  // Clang stupidly generates a vfmadd213ps instruction plus some vmovaps on registers,
+  //  and even register spilling with clang>=6.0 (bug 1637).
+  // Gcc stupidly generates a vfmadd132ps instruction.
+  // So let's enforce it to generate a vfmadd231ps instruction since the most common use
+  //  case is to accumulate the result of the product.
   Packet8f res = c;
   __asm__("vfmadd231ps %[a], %[b], %[c]" : [c] "+x" (res) : [a] "x" (a), [b] "x" (b));
   return res;
@@ -172,7 +173,7 @@ template<> EIGEN_STRONG_INLINE Packet8f pmadd(const Packet8f& a, const Packet8f&
 #endif
 }
 template<> EIGEN_STRONG_INLINE Packet4d pmadd(const Packet4d& a, const Packet4d& b, const Packet4d& c) {
-#if ( EIGEN_COMP_GNUC_STRICT || (EIGEN_COMP_CLANG && (EIGEN_COMP_CLANG<308)) )
+#if ( (EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC<80) || (EIGEN_COMP_CLANG) )
   // see above
   Packet4d res = c;
   __asm__("vfmadd231pd %[a], %[b], %[c]" : [c] "+x" (res) : [a] "x" (a), [b] "x" (b));
@@ -308,9 +309,9 @@ template<> EIGEN_STRONG_INLINE void pstore1<Packet8i>(int* to, const int& a)
 }
 
 #ifndef EIGEN_VECTORIZE_AVX512
-template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
 #endif
 
 template<> EIGEN_STRONG_INLINE float  pfirst<Packet8f>(const Packet8f& a) {
@@ -333,9 +334,12 @@ template<> EIGEN_STRONG_INLINE Packet4d preverse(const Packet4d& a)
 {
    __m256d tmp = _mm256_shuffle_pd(a,a,5);
   return _mm256_permute2f128_pd(tmp, tmp, 1);
-
+  #if 0
+  // This version is unlikely to be faster as _mm256_shuffle_ps and _mm256_permute_pd
+  // exhibit the same latency/throughput, but it is here for future reference/benchmarking...
   __m256d swap_halves = _mm256_permute2f128_pd(a,a,1);
     return _mm256_permute_pd(swap_halves,5);
+  #endif
 }
 
 // pabs should be ok
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h
index 399be0ee4..9c1717f76 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h
@@ -88,9 +88,9 @@ plog<Packet16f>(const Packet16f& _x) {
   //     x = x + x - 1.0;
   //   } else { x = x - 1.0; }
   __mmask16 mask = _mm512_cmp_ps_mask(x, p16f_cephes_SQRTHF, _CMP_LT_OQ);
-  Packet16f tmp = _mm512_mask_blend_ps(mask, x, _mm512_setzero_ps());
+  Packet16f tmp = _mm512_mask_blend_ps(mask, _mm512_setzero_ps(), x);
   x = psub(x, p16f_1);
-  e = psub(e, _mm512_mask_blend_ps(mask, p16f_1, _mm512_setzero_ps()));
+  e = psub(e, _mm512_mask_blend_ps(mask, _mm512_setzero_ps(), p16f_1));
   x = padd(x, tmp);
 
   Packet16f x2 = pmul(x, x);
@@ -119,8 +119,9 @@ plog<Packet16f>(const Packet16f& _x) {
   x = padd(x, y2);
 
   // Filter out invalid inputs, i.e. negative arg will be NAN, 0 will be -INF.
-  return _mm512_mask_blend_ps(iszero_mask, p16f_minus_inf,
-                              _mm512_mask_blend_ps(invalid_mask, p16f_nan, x));
+  return _mm512_mask_blend_ps(iszero_mask,
+                              _mm512_mask_blend_ps(invalid_mask, x, p16f_nan),
+                              p16f_minus_inf);
 }
 #endif
 
@@ -266,8 +267,7 @@ psqrt<Packet16f>(const Packet16f& _x) {
   // select only the inverse sqrt of positive normal inputs (denormals are
   // flushed to zero and cause infs as well).
   __mmask16 non_zero_mask = _mm512_cmp_ps_mask(_x, p16f_flt_min, _CMP_GE_OQ);
-  Packet16f x = _mm512_mask_blend_ps(non_zero_mask, _mm512_rsqrt14_ps(_x),
-                                     _mm512_setzero_ps());
+  Packet16f x = _mm512_mask_blend_ps(non_zero_mask, _mm512_setzero_ps(), _mm512_rsqrt14_ps(_x));
 
   // Do a single step of Newton's iteration.
   x = pmul(x, pmadd(neg_half, pmul(x, x), p16f_one_point_five));
@@ -289,8 +289,7 @@ psqrt<Packet8d>(const Packet8d& _x) {
   // select only the inverse sqrt of positive normal inputs (denormals are
   // flushed to zero and cause infs as well).
   __mmask8 non_zero_mask = _mm512_cmp_pd_mask(_x, p8d_dbl_min, _CMP_GE_OQ);
-  Packet8d x = _mm512_mask_blend_pd(non_zero_mask, _mm512_rsqrt14_pd(_x),
-                                    _mm512_setzero_pd());
+  Packet8d x = _mm512_mask_blend_pd(non_zero_mask, _mm512_setzero_pd(), _mm512_rsqrt14_pd(_x));
 
   // Do a first step of Newton's iteration.
   x = pmul(x, pmadd(neg_half, pmul(x, x), p8d_one_point_five));
@@ -333,20 +332,18 @@ prsqrt<Packet16f>(const Packet16f& _x) {
   // select only the inverse sqrt of positive normal inputs (denormals are
   // flushed to zero and cause infs as well).
   __mmask16 le_zero_mask = _mm512_cmp_ps_mask(_x, p16f_flt_min, _CMP_LT_OQ);
-  Packet16f x = _mm512_mask_blend_ps(le_zero_mask, _mm512_setzero_ps(),
-                                     _mm512_rsqrt14_ps(_x));
+  Packet16f x = _mm512_mask_blend_ps(le_zero_mask, _mm512_rsqrt14_ps(_x), _mm512_setzero_ps());
 
   // Fill in NaNs and Infs for the negative/zero entries.
   __mmask16 neg_mask = _mm512_cmp_ps_mask(_x, _mm512_setzero_ps(), _CMP_LT_OQ);
   Packet16f infs_and_nans = _mm512_mask_blend_ps(
-      neg_mask, p16f_nan,
-      _mm512_mask_blend_ps(le_zero_mask, p16f_inf, _mm512_setzero_ps()));
+      neg_mask, _mm512_mask_blend_ps(le_zero_mask, _mm512_setzero_ps(), p16f_inf), p16f_nan);
 
   // Do a single step of Newton's iteration.
   x = pmul(x, pmadd(neg_half, pmul(x, x), p16f_one_point_five));
 
   // Insert NaNs and Infs in all the right places.
-  return _mm512_mask_blend_ps(le_zero_mask, infs_and_nans, x);
+  return _mm512_mask_blend_ps(le_zero_mask, x, infs_and_nans);
 }
 
 template <>
@@ -363,14 +360,12 @@ prsqrt<Packet8d>(const Packet8d& _x) {
   // select only the inverse sqrt of positive normal inputs (denormals are
   // flushed to zero and cause infs as well).
   __mmask8 le_zero_mask = _mm512_cmp_pd_mask(_x, p8d_dbl_min, _CMP_LT_OQ);
-  Packet8d x = _mm512_mask_blend_pd(le_zero_mask, _mm512_setzero_pd(),
-                                    _mm512_rsqrt14_pd(_x));
+  Packet8d x = _mm512_mask_blend_pd(le_zero_mask, _mm512_rsqrt14_pd(_x), _mm512_setzero_pd());
 
   // Fill in NaNs and Infs for the negative/zero entries.
   __mmask8 neg_mask = _mm512_cmp_pd_mask(_x, _mm512_setzero_pd(), _CMP_LT_OQ);
   Packet8d infs_and_nans = _mm512_mask_blend_pd(
-      neg_mask, p8d_nan,
-      _mm512_mask_blend_pd(le_zero_mask, p8d_inf, _mm512_setzero_pd()));
+      neg_mask, _mm512_mask_blend_pd(le_zero_mask, _mm512_setzero_pd(), p8d_inf), p8d_nan);
 
   // Do a first step of Newton's iteration.
   x = pmul(x, pmadd(neg_half, pmul(x, x), p8d_one_point_five));
@@ -379,9 +374,9 @@ prsqrt<Packet8d>(const Packet8d& _x) {
   x = pmul(x, pmadd(neg_half, pmul(x, x), p8d_one_point_five));
 
   // Insert NaNs and Infs in all the right places.
-  return _mm512_mask_blend_pd(le_zero_mask, infs_and_nans, x);
+  return _mm512_mask_blend_pd(le_zero_mask, x, infs_and_nans);
 }
-#else
+#elif defined(EIGEN_VECTORIZE_AVX512ER)
 template <>
 EIGEN_STRONG_INLINE Packet16f prsqrt<Packet16f>(const Packet16f& x) {
   return _mm512_rsqrt28_ps(x);
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h
index f6500a16e..5adddc7ae 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AVX512/PacketMath.h
@@ -618,9 +618,9 @@ EIGEN_STRONG_INLINE void pstore1<Packet16i>(int* to, const int& a) {
   pstore(to, pa);
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
 
 template <>
 EIGEN_STRONG_INLINE float pfirst<Packet16f>(const Packet16f& a) {
@@ -648,13 +648,13 @@ template<> EIGEN_STRONG_INLINE Packet8d preverse(const Packet8d& a)
 template<> EIGEN_STRONG_INLINE Packet16f pabs(const Packet16f& a)
 {
   // _mm512_abs_ps intrinsic not found, so hack around it
-  return (__m512)_mm512_and_si512((__m512i)a, _mm512_set1_epi32(0x7fffffff));
+  return _mm512_castsi512_ps(_mm512_and_si512(_mm512_castps_si512(a), _mm512_set1_epi32(0x7fffffff)));
 }
 template <>
 EIGEN_STRONG_INLINE Packet8d pabs(const Packet8d& a) {
   // _mm512_abs_ps intrinsic not found, so hack around it
-  return (__m512d)_mm512_and_si512((__m512i)a,
-                                   _mm512_set1_epi64(0x7fffffffffffffff));
+  return _mm512_castsi512_pd(_mm512_and_si512(_mm512_castpd_si512(a),
+                                   _mm512_set1_epi64(0x7fffffffffffffff)));
 }
 
 #ifdef EIGEN_VECTORIZE_AVX512DQ
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h
index 67db2f8ee..3e665730c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h
@@ -224,23 +224,7 @@ template<> struct conj_helper<Packet2cf, Packet2cf, true,true>
   }
 };
 
-template<> struct conj_helper<Packet4f, Packet2cf, false,false>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet4f& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet4f& x, const Packet2cf& y) const
-  { return Packet2cf(internal::pmul<Packet4f>(x, y.v)); }
-};
-
-template<> struct conj_helper<Packet2cf, Packet4f, false,false>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet4f& y, const Packet2cf& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& x, const Packet4f& y) const
-  { return Packet2cf(internal::pmul<Packet4f>(x.v, y)); }
-};
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
 
 template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
 {
@@ -416,23 +400,8 @@ template<> struct conj_helper<Packet1cd, Packet1cd, true,true>
     return pconj(internal::pmul(a, b));
   }
 };
-template<> struct conj_helper<Packet2d, Packet1cd, false,false>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet2d& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(c, pmul(x,y)); }
 
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet2d& x, const Packet1cd& y) const
-  { return Packet1cd(internal::pmul<Packet2d>(x, y.v)); }
-};
-
-template<> struct conj_helper<Packet1cd, Packet2d, false,false>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet2d& y, const Packet1cd& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& x, const Packet2d& y) const
-  { return Packet1cd(internal::pmul<Packet2d>(x.v, y)); }
-};
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
 
 template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
 {
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h
index b3f1ea199..08a27d153 100755
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h
@@ -103,7 +103,7 @@ static Packet16uc p16uc_PSET32_WODD   = vec_sld((Packet16uc) vec_splat((Packet4u
 static Packet16uc p16uc_PSET32_WEVEN  = vec_sld(p16uc_DUPLICATE32_HI, (Packet16uc) vec_splat((Packet4ui)p16uc_FORWARD, 3), 8);//{ 4,5,6,7, 4,5,6,7, 12,13,14,15, 12,13,14,15 };
 static Packet16uc p16uc_HALF64_0_16 = vec_sld((Packet16uc)p4i_ZERO, vec_splat((Packet16uc) vec_abs(p4i_MINUS16), 3), 8);      //{ 0,0,0,0, 0,0,0,0, 16,16,16,16, 16,16,16,16};
 #else
-static Packet16uc p16uc_FORWARD = p16uc_REVERSE32; 
+static Packet16uc p16uc_FORWARD = p16uc_REVERSE32;
 static Packet16uc p16uc_REVERSE64 = { 8,9,10,11, 12,13,14,15, 0,1,2,3, 4,5,6,7 };
 static Packet16uc p16uc_PSET32_WODD = vec_sld((Packet16uc) vec_splat((Packet4ui)p16uc_FORWARD, 1), (Packet16uc) vec_splat((Packet4ui)p16uc_FORWARD, 3), 8);//{ 0,1,2,3, 0,1,2,3, 8,9,10,11, 8,9,10,11 };
 static Packet16uc p16uc_PSET32_WEVEN = vec_sld((Packet16uc) vec_splat((Packet4ui)p16uc_FORWARD, 0), (Packet16uc) vec_splat((Packet4ui)p16uc_FORWARD, 2), 8);//{ 4,5,6,7, 4,5,6,7, 12,13,14,15, 12,13,14,15 };
@@ -388,10 +388,28 @@ template<> EIGEN_STRONG_INLINE Packet4i pdiv<Packet4i>(const Packet4i& /*a*/, co
 template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) { return vec_madd(a,b,c); }
 template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) { return a*b + c; }
 
-template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) { return vec_min(a, b); }
+template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b)
+{
+  #ifdef __VSX__
+  Packet4f ret;
+  __asm__ ("xvcmpgesp %x0,%x1,%x2\n\txxsel %x0,%x1,%x2,%x0" : "=&wa" (ret) : "wa" (a), "wa" (b));
+  return ret;
+  #else
+  return vec_min(a, b);
+  #endif
+}
 template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b) { return vec_min(a, b); }
 
-template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) { return vec_max(a, b); }
+template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b)
+{
+  #ifdef __VSX__
+  Packet4f ret;
+  __asm__ ("xvcmpgtsp %x0,%x2,%x1\n\txxsel %x0,%x1,%x2,%x0" : "=&wa" (ret) : "wa" (a), "wa" (b));
+  return ret;
+  #else
+  return vec_max(a, b);
+  #endif
+}
 template<> EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b) { return vec_max(a, b); }
 
 template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b) { return vec_and(a, b); }
@@ -764,7 +782,7 @@ typedef __vector __bool long         Packet2bl;
 
 static Packet2l  p2l_ONE  = { 1, 1 };
 static Packet2l  p2l_ZERO = reinterpret_cast<Packet2l>(p4i_ZERO);
-static Packet2d  p2d_ONE  = { 1.0, 1.0 }; 
+static Packet2d  p2d_ONE  = { 1.0, 1.0 };
 static Packet2d  p2d_ZERO = reinterpret_cast<Packet2d>(p4f_ZERO);
 static Packet2d  p2d_MZERO = { -0.0, -0.0 };
 
@@ -910,9 +928,19 @@ template<> EIGEN_STRONG_INLINE Packet2d pdiv<Packet2d>(const Packet2d& a, const
 // for some weird raisons, it has to be overloaded for packet of integers
 template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) { return vec_madd(a, b, c); }
 
-template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) { return vec_min(a, b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b)
+{
+  Packet2d ret;
+  __asm__ ("xvcmpgedp %x0,%x1,%x2\n\txxsel %x0,%x1,%x2,%x0" : "=&wa" (ret) : "wa" (a), "wa" (b));
+  return ret;
+ }
 
-template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) { return vec_max(a, b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b)
+{
+  Packet2d ret;
+  __asm__ ("xvcmpgtdp %x0,%x2,%x1\n\txxsel %x0,%x1,%x2,%x0" : "=&wa" (ret) : "wa" (a), "wa" (b));
+  return ret;
+}
 
 template<> EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b) { return vec_and(a, b); }
 
@@ -969,7 +997,7 @@ template<> EIGEN_STRONG_INLINE Packet2d preduxp<Packet2d>(const Packet2d* vecs)
   Packet2d v[2], sum;
   v[0] = vecs[0] + reinterpret_cast<Packet2d>(vec_sld(reinterpret_cast<Packet4f>(vecs[0]), reinterpret_cast<Packet4f>(vecs[0]), 8));
   v[1] = vecs[1] + reinterpret_cast<Packet2d>(vec_sld(reinterpret_cast<Packet4f>(vecs[1]), reinterpret_cast<Packet4f>(vecs[1]), 8));
- 
+
 #ifdef _BIG_ENDIAN
   sum = reinterpret_cast<Packet2d>(vec_sld(reinterpret_cast<Packet4f>(v[0]), reinterpret_cast<Packet4f>(v[1]), 8));
 #else
@@ -1022,7 +1050,7 @@ ptranspose(PacketBlock<Packet2d,2>& kernel) {
 
 template<> EIGEN_STRONG_INLINE Packet2d pblend(const Selector<2>& ifPacket, const Packet2d& thenPacket, const Packet2d& elsePacket) {
   Packet2l select = { ifPacket.select[0], ifPacket.select[1] };
-  Packet2bl mask = vec_cmpeq(reinterpret_cast<Packet2d>(select), reinterpret_cast<Packet2d>(p2l_ONE));
+  Packet2bl mask = reinterpret_cast<Packet2bl>( vec_cmpeq(reinterpret_cast<Packet2d>(select), reinterpret_cast<Packet2d>(p2l_ONE)) );
   return vec_sel(elsePacket, thenPacket, mask);
 }
 #endif // __VSX__
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h
index 294c517ea..755e6209d 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/Half.h
@@ -29,7 +29,7 @@
 // type Eigen::half (inheriting from CUDA's __half struct) with
 // operator overloads such that it behaves basically as an arithmetic
 // type. It will be quite slow on CPUs (so it is recommended to stay
-// in fp32 for CPUs, except for simple parameter conversions, I/O
+// in float32_bits for CPUs, except for simple parameter conversions, I/O
 // to disk and the likes), but fast on GPUs.
 
 
@@ -50,38 +50,45 @@ struct half;
 namespace half_impl {
 
 #if !defined(EIGEN_HAS_CUDA_FP16)
-
-// Make our own __half definition that is similar to CUDA's.
-struct __half {
-  EIGEN_DEVICE_FUNC __half() {}
-  explicit EIGEN_DEVICE_FUNC __half(unsigned short raw) : x(raw) {}
+// Make our own __half_raw definition that is similar to CUDA's.
+struct __half_raw {
+  EIGEN_DEVICE_FUNC __half_raw() : x(0) {}
+  explicit EIGEN_DEVICE_FUNC __half_raw(unsigned short raw) : x(raw) {}
   unsigned short x;
 };
-
+#elif defined(EIGEN_CUDACC_VER) && EIGEN_CUDACC_VER < 90000
+// In CUDA < 9.0, __half is the equivalent of CUDA 9's __half_raw
+typedef __half __half_raw;
 #endif
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half raw_uint16_to_half(unsigned short x);
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half float_to_half_rtne(float ff);
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half h);
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw raw_uint16_to_half(unsigned short x);
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff);
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h);
 
-struct half_base : public __half {
+struct half_base : public __half_raw {
   EIGEN_DEVICE_FUNC half_base() {}
-  EIGEN_DEVICE_FUNC half_base(const half_base& h) : __half(h) {}
-  EIGEN_DEVICE_FUNC half_base(const __half& h) : __half(h) {}
+  EIGEN_DEVICE_FUNC half_base(const half_base& h) : __half_raw(h) {}
+  EIGEN_DEVICE_FUNC half_base(const __half_raw& h) : __half_raw(h) {}
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDACC_VER) && EIGEN_CUDACC_VER >= 90000
+  EIGEN_DEVICE_FUNC half_base(const __half& h) : __half_raw(*(__half_raw*)&h) {}
+#endif
 };
 
 } // namespace half_impl
 
 // Class definition.
 struct half : public half_impl::half_base {
-  #if !defined(EIGEN_HAS_CUDA_FP16)
-    typedef half_impl::__half __half;
+  #if !defined(EIGEN_HAS_CUDA_FP16) || (defined(EIGEN_CUDACC_VER) && EIGEN_CUDACC_VER < 90000)
+    typedef half_impl::__half_raw __half_raw;
   #endif
 
   EIGEN_DEVICE_FUNC half() {}
 
-  EIGEN_DEVICE_FUNC half(const __half& h) : half_impl::half_base(h) {}
+  EIGEN_DEVICE_FUNC half(const __half_raw& h) : half_impl::half_base(h) {}
   EIGEN_DEVICE_FUNC half(const half& h) : half_impl::half_base(h) {}
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDACC_VER) && EIGEN_CUDACC_VER >= 90000
+  EIGEN_DEVICE_FUNC half(const __half& h) : half_impl::half_base(h) {}
+#endif
 
   explicit EIGEN_DEVICE_FUNC half(bool b)
       : half_impl::half_base(half_impl::raw_uint16_to_half(b ? 0x3c00 : 0)) {}
@@ -138,71 +145,125 @@ struct half : public half_impl::half_base {
   }
 };
 
+} // end namespace Eigen
+
+namespace std {
+template<>
+struct numeric_limits<Eigen::half> {
+  static const bool is_specialized = true;
+  static const bool is_signed = true;
+  static const bool is_integer = false;
+  static const bool is_exact = false;
+  static const bool has_infinity = true;
+  static const bool has_quiet_NaN = true;
+  static const bool has_signaling_NaN = true;
+  static const float_denorm_style has_denorm = denorm_present;
+  static const bool has_denorm_loss = false;
+  static const std::float_round_style round_style = std::round_to_nearest;
+  static const bool is_iec559 = false;
+  static const bool is_bounded = false;
+  static const bool is_modulo = false;
+  static const int digits = 11;
+  static const int digits10 = 3;      // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
+  static const int max_digits10 = 5;  // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
+  static const int radix = 2;
+  static const int min_exponent = -13;
+  static const int min_exponent10 = -4;
+  static const int max_exponent = 16;
+  static const int max_exponent10 = 4;
+  static const bool traps = true;
+  static const bool tinyness_before = false;
+
+  static Eigen::half (min)() { return Eigen::half_impl::raw_uint16_to_half(0x400); }
+  static Eigen::half lowest() { return Eigen::half_impl::raw_uint16_to_half(0xfbff); }
+  static Eigen::half (max)() { return Eigen::half_impl::raw_uint16_to_half(0x7bff); }
+  static Eigen::half epsilon() { return Eigen::half_impl::raw_uint16_to_half(0x0800); }
+  static Eigen::half round_error() { return Eigen::half(0.5); }
+  static Eigen::half infinity() { return Eigen::half_impl::raw_uint16_to_half(0x7c00); }
+  static Eigen::half quiet_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); }
+  static Eigen::half signaling_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); }
+  static Eigen::half denorm_min() { return Eigen::half_impl::raw_uint16_to_half(0x1); }
+};
+
+// If std::numeric_limits<T> is specialized, should also specialize
+// std::numeric_limits<const T>, std::numeric_limits<volatile T>, and
+// std::numeric_limits<const volatile T>
+// https://stackoverflow.com/a/16519653/
+template<>
+struct numeric_limits<const Eigen::half> : numeric_limits<Eigen::half> {};
+template<>
+struct numeric_limits<volatile Eigen::half> : numeric_limits<Eigen::half> {};
+template<>
+struct numeric_limits<const volatile Eigen::half> : numeric_limits<Eigen::half> {};
+} // end namespace std
+
+namespace Eigen {
+
 namespace half_impl {
 
-#if defined(EIGEN_HAS_CUDA_FP16) && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 530
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530
 
 // Intrinsics for native fp16 support. Note that on current hardware,
-// these are no faster than fp32 arithmetic (you need to use the half2
+// these are no faster than float32_bits arithmetic (you need to use the half2
 // versions to get the ALU speed increased), but you do save the
 // conversion steps back and forth.
 
-__device__ half operator + (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half operator + (const half& a, const half& b) {
   return __hadd(a, b);
 }
-__device__ half operator * (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half operator * (const half& a, const half& b) {
   return __hmul(a, b);
 }
-__device__ half operator - (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half operator - (const half& a, const half& b) {
   return __hsub(a, b);
 }
-__device__ half operator / (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half operator / (const half& a, const half& b) {
   float num = __half2float(a);
   float denom = __half2float(b);
   return __float2half(num / denom);
 }
-__device__ half operator - (const half& a) {
+EIGEN_STRONG_INLINE __device__ half operator - (const half& a) {
   return __hneg(a);
 }
-__device__ half& operator += (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half& operator += (half& a, const half& b) {
   a = a + b;
   return a;
 }
-__device__ half& operator *= (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half& operator *= (half& a, const half& b) {
   a = a * b;
   return a;
 }
-__device__ half& operator -= (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half& operator -= (half& a, const half& b) {
   a = a - b;
   return a;
 }
-__device__ half& operator /= (half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ half& operator /= (half& a, const half& b) {
   a = a / b;
   return a;
 }
-__device__ bool operator == (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ bool operator == (const half& a, const half& b) {
   return __heq(a, b);
 }
-__device__ bool operator != (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ bool operator != (const half& a, const half& b) {
   return __hne(a, b);
 }
-__device__ bool operator < (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ bool operator < (const half& a, const half& b) {
   return __hlt(a, b);
 }
-__device__ bool operator <= (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ bool operator <= (const half& a, const half& b) {
   return __hle(a, b);
 }
-__device__ bool operator > (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ bool operator > (const half& a, const half& b) {
   return __hgt(a, b);
 }
-__device__ bool operator >= (const half& a, const half& b) {
+EIGEN_STRONG_INLINE __device__ bool operator >= (const half& a, const half& b) {
   return __hge(a, b);
 }
 
 #else  // Emulate support for half floats
 
 // Definitions for CPUs and older CUDA, mostly working through conversion
-// to/from fp32.
+// to/from float32_bits.
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator + (const half& a, const half& b) {
   return half(float(a) + float(b));
@@ -238,10 +299,10 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator /= (half& a, const half& b)
   return a;
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator == (const half& a, const half& b) {
-  return float(a) == float(b);
+  return numext::equal_strict(float(a),float(b));
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator != (const half& a, const half& b) {
-  return float(a) != float(b);
+  return numext::not_equal_strict(float(a), float(b));
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator < (const half& a, const half& b) {
   return float(a) < float(b);
@@ -269,34 +330,35 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator / (const half& a, Index b) {
 // these in hardware. If we need more performance on older/other CPUs, they are
 // also possible to vectorize directly.
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half raw_uint16_to_half(unsigned short x) {
-  __half h;
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw raw_uint16_to_half(unsigned short x) {
+  __half_raw h;
   h.x = x;
   return h;
 }
 
-union FP32 {
+union float32_bits {
   unsigned int u;
   float f;
 };
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half float_to_half_rtne(float ff) {
-#if defined(EIGEN_HAS_CUDA_FP16) && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 300
-  return __float2half(ff);
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff) {
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300
+  __half tmp_ff = __float2half(ff);
+  return *(__half_raw*)&tmp_ff;
 
 #elif defined(EIGEN_HAS_FP16_C)
-  __half h;
+  __half_raw h;
   h.x = _cvtss_sh(ff, 0);
   return h;
 
 #else
-  FP32 f; f.f = ff;
+  float32_bits f; f.f = ff;
 
-  const FP32 f32infty = { 255 << 23 };
-  const FP32 f16max = { (127 + 16) << 23 };
-  const FP32 denorm_magic = { ((127 - 15) + (23 - 10) + 1) << 23 };
+  const float32_bits f32infty = { 255 << 23 };
+  const float32_bits f16max = { (127 + 16) << 23 };
+  const float32_bits denorm_magic = { ((127 - 15) + (23 - 10) + 1) << 23 };
   unsigned int sign_mask = 0x80000000u;
-  __half o;
+  __half_raw o;
   o.x = static_cast<unsigned short>(0x0u);
 
   unsigned int sign = f.u & sign_mask;
@@ -335,17 +397,17 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half float_to_half_rtne(float ff) {
 #endif
 }
 
-EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half h) {
-#if defined(EIGEN_HAS_CUDA_FP16) && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 300
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h) {
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300
   return __half2float(h);
 
 #elif defined(EIGEN_HAS_FP16_C)
   return _cvtsh_ss(h.x);
 
 #else
-  const FP32 magic = { 113 << 23 };
+  const float32_bits magic = { 113 << 23 };
   const unsigned int shifted_exp = 0x7c00 << 13; // exponent mask after shift
-  FP32 o;
+  float32_bits o;
 
   o.u = (h.x & 0x7fff) << 13;             // exponent/mantissa bits
   unsigned int exp = shifted_exp & o.u;   // just the exponent
@@ -370,7 +432,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isinf)(const half& a) {
   return (a.x & 0x7fff) == 0x7c00;
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isnan)(const half& a) {
-#if defined(EIGEN_HAS_CUDA_FP16) && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 530
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530
   return __hisnan(a);
 #else
   return (a.x & 0x7fff) > 0x7c00;
@@ -386,11 +448,15 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half abs(const half& a) {
   return result;
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half exp(const half& a) {
-  return half(::expf(float(a)));
+#if EIGEN_CUDACC_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 530
+  return half(hexp(a));
+#else
+   return half(::expf(float(a)));
+#endif
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log(const half& a) {
-#if defined(EIGEN_HAS_CUDA_FP16) && defined __CUDACC_VER__ && __CUDACC_VER__ >= 80000 && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 530
-  return Eigen::half(::hlog(a));
+#if defined(EIGEN_HAS_CUDA_FP16) && EIGEN_CUDACC_VER >= 80000 && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530
+  return half(::hlog(a));
 #else
   return half(::logf(float(a)));
 #endif
@@ -402,7 +468,11 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log10(const half& a) {
   return half(::log10f(float(a)));
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half sqrt(const half& a) {
-  return half(::sqrtf(float(a)));
+#if EIGEN_CUDACC_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 530
+  return half(hsqrt(a));
+#else
+    return half(::sqrtf(float(a)));
+#endif
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half pow(const half& a, const half& b) {
   return half(::powf(float(a), float(b)));
@@ -420,14 +490,22 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half tanh(const half& a) {
   return half(::tanhf(float(a)));
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half floor(const half& a) {
+#if EIGEN_CUDACC_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 300
+  return half(hfloor(a));
+#else
   return half(::floorf(float(a)));
+#endif
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half ceil(const half& a) {
+#if EIGEN_CUDACC_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 300
+  return half(hceil(a));
+#else
   return half(::ceilf(float(a)));
+#endif
 }
 
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half (min)(const half& a, const half& b) {
-#if defined(EIGEN_HAS_CUDA_FP16) && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 530
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530
   return __hlt(b, a) ? b : a;
 #else
   const float f1 = static_cast<float>(a);
@@ -436,7 +514,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half (min)(const half& a, const half& b) {
 #endif
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half (max)(const half& a, const half& b) {
-#if defined(EIGEN_HAS_CUDA_FP16) && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 530
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530
   return __hlt(a, b) ? b : a;
 #else
   const float f1 = static_cast<float>(a);
@@ -474,49 +552,6 @@ template<> struct is_arithmetic<half> { enum { value = true }; };
 
 } // end namespace internal
 
-}  // end namespace Eigen
-
-namespace std {
-template<>
-struct numeric_limits<Eigen::half> {
-  static const bool is_specialized = true;
-  static const bool is_signed = true;
-  static const bool is_integer = false;
-  static const bool is_exact = false;
-  static const bool has_infinity = true;
-  static const bool has_quiet_NaN = true;
-  static const bool has_signaling_NaN = true;
-  static const float_denorm_style has_denorm = denorm_present;
-  static const bool has_denorm_loss = false;
-  static const std::float_round_style round_style = std::round_to_nearest;
-  static const bool is_iec559 = false;
-  static const bool is_bounded = false;
-  static const bool is_modulo = false;
-  static const int digits = 11;
-  static const int digits10 = 2;
-  //static const int max_digits10 = ;
-  static const int radix = 2;
-  static const int min_exponent = -13;
-  static const int min_exponent10 = -4;
-  static const int max_exponent = 16;
-  static const int max_exponent10 = 4;
-  static const bool traps = true;
-  static const bool tinyness_before = false;
-
-  static Eigen::half (min)() { return Eigen::half_impl::raw_uint16_to_half(0x400); }
-  static Eigen::half lowest() { return Eigen::half_impl::raw_uint16_to_half(0xfbff); }
-  static Eigen::half (max)() { return Eigen::half_impl::raw_uint16_to_half(0x7bff); }
-  static Eigen::half epsilon() { return Eigen::half_impl::raw_uint16_to_half(0x0800); }
-  static Eigen::half round_error() { return Eigen::half(0.5); }
-  static Eigen::half infinity() { return Eigen::half_impl::raw_uint16_to_half(0x7c00); }
-  static Eigen::half quiet_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); }
-  static Eigen::half signaling_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); }
-  static Eigen::half denorm_min() { return Eigen::half_impl::raw_uint16_to_half(0x1); }
-};
-}
-
-namespace Eigen {
-
 template<> struct NumTraits<Eigen::half>
     : GenericNumTraits<Eigen::half>
 {
@@ -557,7 +592,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half exph(const Eigen::half& a) {
   return Eigen::half(::expf(float(a)));
 }
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half logh(const Eigen::half& a) {
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 80000 && defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 530
+#if EIGEN_CUDACC_VER >= 80000 && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530
   return Eigen::half(::hlog(a));
 #else
   return Eigen::half(::logf(float(a)));
@@ -591,14 +626,18 @@ struct hash<Eigen::half> {
 
 
 // Add the missing shfl_xor intrinsic
-#if defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 300
+#if defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300
 __device__ EIGEN_STRONG_INLINE Eigen::half __shfl_xor(Eigen::half var, int laneMask, int width=warpSize) {
+  #if EIGEN_CUDACC_VER < 90000
   return static_cast<Eigen::half>(__shfl_xor(static_cast<float>(var), laneMask, width));
+  #else
+  return static_cast<Eigen::half>(__shfl_xor_sync(0xFFFFFFFF, static_cast<float>(var), laneMask, width));
+  #endif
 }
 #endif
 
-// ldg() has an overload for __half, but we also need one for Eigen::half.
-#if defined(__CUDA_ARCH__) && __CUDA_ARCH__ >= 350
+// ldg() has an overload for __half_raw, but we also need one for Eigen::half.
+#if defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 350
 EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half __ldg(const Eigen::half* ptr) {
   return Eigen::half_impl::raw_uint16_to_half(
       __ldg(reinterpret_cast<const unsigned short*>(ptr)));
@@ -606,7 +645,7 @@ EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half __ldg(const Eigen::half* ptr)
 #endif
 
 
-#if defined(__CUDA_ARCH__)
+#if defined(EIGEN_CUDA_ARCH)
 namespace Eigen {
 namespace numext {
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h
index ae54225f8..c66d38469 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h
@@ -99,7 +99,8 @@ template<> __device__ EIGEN_STRONG_INLINE Eigen::half pfirst<half2>(const half2&
 
 template<> __device__ EIGEN_STRONG_INLINE half2 pabs<half2>(const half2& a) {
   half2 result;
-  result.x = a.x & 0x7FFF7FFF;
+  unsigned temp = *(reinterpret_cast<const unsigned*>(&(a)));
+  *(reinterpret_cast<unsigned*>(&(result))) = temp & 0x7FFF7FFF;
   return result;
 }
 
@@ -275,7 +276,7 @@ template<> __device__ EIGEN_STRONG_INLINE half2 plog1p<half2>(const half2& a) {
   return __floats2half2_rn(r1, r2);
 }
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 80000 && defined __CUDA_ARCH__ && __CUDA_ARCH__ >= 530
+#if EIGEN_CUDACC_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 530
 
 template<>  __device__ EIGEN_STRONG_INLINE
 half2 plog<half2>(const half2& a) {
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/ConjHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/ConjHelper.h
new file mode 100644
index 000000000..4cfe34e05
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/ConjHelper.h
@@ -0,0 +1,29 @@
+
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARCH_CONJ_HELPER_H
+#define EIGEN_ARCH_CONJ_HELPER_H
+
+#define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL)                                                          \
+  template<> struct conj_helper<PACKET_REAL, PACKET_CPLX, false,false> {                                          \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x, const PACKET_CPLX& y, const PACKET_CPLX& c) const \
+    { return padd(c, pmul(x,y)); }                                                                                \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x, const PACKET_CPLX& y) const                        \
+    { return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x, y.v)); }                                           \
+  };                                                                                                              \
+                                                                                                                  \
+  template<> struct conj_helper<PACKET_CPLX, PACKET_REAL, false,false> {                                          \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x, const PACKET_REAL& y, const PACKET_CPLX& c) const \
+    { return padd(c, pmul(x,y)); }                                                                                \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x, const PACKET_REAL& y) const                        \
+    { return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x.v, y)); }                                           \
+  };
+
+#endif // EIGEN_ARCH_CONJ_HELPER_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
index 57e9b431f..306a309be 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
@@ -67,7 +67,7 @@ template<> struct unpacket_traits<Packet2cf> { typedef std::complex<float> type;
 template<> EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>&  from)
 {
   float32x2_t r64;
-  r64 = vld1_f32((float *)&from);
+  r64 = vld1_f32((const float *)&from);
 
   return Packet2cf(vcombine_f32(r64, r64));
 }
@@ -142,7 +142,7 @@ template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf
   to[stride*1] = std::complex<float>(vgetq_lane_f32(from.v, 2), vgetq_lane_f32(from.v, 3));
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { EIGEN_ARM_PREFETCH((float *)addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { EIGEN_ARM_PREFETCH((const float *)addr); }
 
 template<> EIGEN_STRONG_INLINE std::complex<float>  pfirst<Packet2cf>(const Packet2cf& a)
 {
@@ -265,6 +265,8 @@ template<> struct conj_helper<Packet2cf, Packet2cf, true,true>
   }
 };
 
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
+
 template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
 {
   // TODO optimize it for NEON
@@ -275,7 +277,7 @@ template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, con
   s = vmulq_f32(b.v, b.v);
   rev_s = vrev64q_f32(s);
 
-  return Packet2cf(pdiv(res.v, vaddq_f32(s,rev_s)));
+  return Packet2cf(pdiv<Packet4f>(res.v, vaddq_f32(s,rev_s)));
 }
 
 EIGEN_DEVICE_FUNC inline void
@@ -381,7 +383,7 @@ template<> EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<
 template<> EIGEN_STRONG_INLINE void pstore <std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((double*)to, from.v); }
 template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((double*)to, from.v); }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *   addr) { EIGEN_ARM_PREFETCH((double *)addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *   addr) { EIGEN_ARM_PREFETCH((const double *)addr); }
 
 template<> EIGEN_DEVICE_FUNC inline Packet1cd pgather<std::complex<double>, Packet1cd>(const std::complex<double>* from, Index stride)
 {
@@ -456,6 +458,8 @@ template<> struct conj_helper<Packet1cd, Packet1cd, true,true>
   }
 };
 
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
+
 template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
 {
   // TODO optimize it for NEON
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
index 836fbc0dd..3d5ed0d24 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
@@ -36,12 +36,43 @@ namespace internal {
 #endif
 #endif
 
+#if EIGEN_COMP_MSVC
+
+// In MSVC's arm_neon.h header file, all NEON vector types
+// are aliases to the same underlying type __n128.
+// We thus have to wrap them to make them different C++ types.
+// (See also bug 1428)
+
+template<typename T,int unique_id>
+struct eigen_packet_wrapper
+{
+  operator T&() { return m_val; }
+  operator const T&() const { return m_val; }
+  eigen_packet_wrapper() {}
+  eigen_packet_wrapper(const T &v) : m_val(v) {}
+  eigen_packet_wrapper& operator=(const T &v) {
+    m_val = v;
+    return *this;
+  }
+
+  T m_val;
+};
+typedef eigen_packet_wrapper<float32x2_t,0> Packet2f;
+typedef eigen_packet_wrapper<float32x4_t,1> Packet4f;
+typedef eigen_packet_wrapper<int32x4_t  ,2> Packet4i;
+typedef eigen_packet_wrapper<int32x2_t  ,3> Packet2i;
+typedef eigen_packet_wrapper<uint32x4_t ,4> Packet4ui;
+
+#else
+
 typedef float32x2_t Packet2f;
 typedef float32x4_t Packet4f;
 typedef int32x4_t   Packet4i;
 typedef int32x2_t   Packet2i;
 typedef uint32x4_t  Packet4ui;
 
+#endif // EIGEN_COMP_MSVC
+
 #define _EIGEN_DECLARE_CONST_Packet4f(NAME,X) \
   const Packet4f p4f_##NAME = pset1<Packet4f>(X)
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h
index 5607fe0ab..d075043ce 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h
@@ -128,7 +128,7 @@ template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf
                                      _mm_cvtss_f32(_mm_shuffle_ps(from.v, from.v, 3)));
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
 
 template<> EIGEN_STRONG_INLINE std::complex<float>  pfirst<Packet2cf>(const Packet2cf& a)
 {
@@ -229,23 +229,7 @@ template<> struct conj_helper<Packet2cf, Packet2cf, true,true>
   }
 };
 
-template<> struct conj_helper<Packet4f, Packet2cf, false,false>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet4f& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet4f& x, const Packet2cf& y) const
-  { return Packet2cf(Eigen::internal::pmul<Packet4f>(x, y.v)); }
-};
-
-template<> struct conj_helper<Packet2cf, Packet4f, false,false>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet4f& y, const Packet2cf& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& x, const Packet4f& y) const
-  { return Packet2cf(Eigen::internal::pmul<Packet4f>(x.v, y)); }
-};
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
 
 template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
 {
@@ -340,7 +324,7 @@ template<> EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<
 template<> EIGEN_STRONG_INLINE void pstore <std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((double*)to, Packet2d(from.v)); }
 template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((double*)to, Packet2d(from.v)); }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *   addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
 
 template<> EIGEN_STRONG_INLINE std::complex<double>  pfirst<Packet1cd>(const Packet1cd& a)
 {
@@ -430,23 +414,7 @@ template<> struct conj_helper<Packet1cd, Packet1cd, true,true>
   }
 };
 
-template<> struct conj_helper<Packet2d, Packet1cd, false,false>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet2d& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet2d& x, const Packet1cd& y) const
-  { return Packet1cd(Eigen::internal::pmul<Packet2d>(x, y.v)); }
-};
-
-template<> struct conj_helper<Packet1cd, Packet2d, false,false>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet2d& y, const Packet1cd& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& x, const Packet2d& y) const
-  { return Packet1cd(Eigen::internal::pmul<Packet2d>(x.v, y)); }
-};
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
 
 template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
 {
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h
index 3832de147..60e2517e4 100755
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h
@@ -28,7 +28,7 @@ namespace internal {
 #endif
 #endif
 
-#if (defined EIGEN_VECTORIZE_AVX) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_MINGW) && (__GXX_ABI_VERSION < 1004)
+#if ((defined EIGEN_VECTORIZE_AVX) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_MINGW) && (__GXX_ABI_VERSION < 1004)) || EIGEN_OS_QNX
 // With GCC's default ABI version, a __m128 or __m256 are the same types and therefore we cannot
 // have overloads for both types without linking error.
 // One solution is to increase ABI version using -fabi-version=4 (or greater).
@@ -409,10 +409,16 @@ template<> EIGEN_STRONG_INLINE void pstore1<Packet2d>(double* to, const double&
   pstore(to, Packet2d(vec2d_swizzle1(pa,0,0)));
 }
 
+#if EIGEN_COMP_PGI
+typedef const void * SsePrefetchPtrType;
+#else
+typedef const char * SsePrefetchPtrType;
+#endif
+
 #ifndef EIGEN_VECTORIZE_AVX
-template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
-template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((const char*)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<float>(const float*   addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
+template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*       addr) { _mm_prefetch((SsePrefetchPtrType)(addr), _MM_HINT_T0); }
 #endif
 
 #if EIGEN_COMP_MSVC_STRICT && EIGEN_OS_WIN64
@@ -876,4 +882,14 @@ template<> EIGEN_STRONG_INLINE double pmadd(const double& a, const double& b, co
 
 } // end namespace Eigen
 
+#if EIGEN_COMP_PGI
+// PGI++ does not define the following intrinsics in C++ mode.
+static inline __m128  _mm_castpd_ps   (__m128d x) { return reinterpret_cast<__m128&>(x);  }
+static inline __m128i _mm_castpd_si128(__m128d x) { return reinterpret_cast<__m128i&>(x); }
+static inline __m128d _mm_castps_pd   (__m128  x) { return reinterpret_cast<__m128d&>(x); }
+static inline __m128i _mm_castps_si128(__m128  x) { return reinterpret_cast<__m128i&>(x); }
+static inline __m128  _mm_castsi128_ps(__m128i x) { return reinterpret_cast<__m128&>(x);  }
+static inline __m128d _mm_castsi128_pd(__m128i x) { return reinterpret_cast<__m128d&>(x); }
+#endif
+
 #endif // EIGEN_PACKET_MATH_SSE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/TypeCasting.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/TypeCasting.h
index c84893230..c6ca8c716 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/TypeCasting.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/TypeCasting.h
@@ -14,6 +14,7 @@ namespace Eigen {
 
 namespace internal {
 
+#ifndef EIGEN_VECTORIZE_AVX
 template <>
 struct type_casting_traits<float, int> {
   enum {
@@ -23,11 +24,6 @@ struct type_casting_traits<float, int> {
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet4i pcast<Packet4f, Packet4i>(const Packet4f& a) {
-  return _mm_cvttps_epi32(a);
-}
-
-
 template <>
 struct type_casting_traits<int, float> {
   enum {
@@ -37,11 +33,6 @@ struct type_casting_traits<int, float> {
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet4f pcast<Packet4i, Packet4f>(const Packet4i& a) {
-  return _mm_cvtepi32_ps(a);
-}
-
-
 template <>
 struct type_casting_traits<double, float> {
   enum {
@@ -51,10 +42,6 @@ struct type_casting_traits<double, float> {
   };
 };
 
-template<> EIGEN_STRONG_INLINE Packet4f pcast<Packet2d, Packet4f>(const Packet2d& a, const Packet2d& b) {
-  return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6));
-}
-
 template <>
 struct type_casting_traits<float, double> {
   enum {
@@ -63,6 +50,19 @@ struct type_casting_traits<float, double> {
     TgtCoeffRatio = 2
   };
 };
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4i pcast<Packet4f, Packet4i>(const Packet4f& a) {
+  return _mm_cvttps_epi32(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pcast<Packet4i, Packet4f>(const Packet4i& a) {
+  return _mm_cvtepi32_ps(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pcast<Packet2d, Packet4f>(const Packet2d& a, const Packet2d& b) {
+  return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6));
+}
 
 template<> EIGEN_STRONG_INLINE Packet2d pcast<Packet4f, Packet2d>(const Packet4f& a) {
   // Simply discard the second half of the input
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/ZVector/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/ZVector/Complex.h
index d39d2d105..1bfb73397 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/ZVector/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/ZVector/Complex.h
@@ -336,6 +336,9 @@ template<> struct conj_helper<Packet2cf, Packet2cf, true,true>
   }
 };
 
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
+
 template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
 {
   // TODO optimize it for AltiVec
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/BinaryFunctors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/BinaryFunctors.h
index 96747bac7..3eae6b8ca 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/BinaryFunctors.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/BinaryFunctors.h
@@ -255,7 +255,7 @@ struct scalar_cmp_op<LhsScalar,RhsScalar, cmp_NEQ> : binary_op_base<LhsScalar,Rh
 
 
 /** \internal
-  * \brief Template functor to compute the hypot of two scalars
+  * \brief Template functor to compute the hypot of two \b positive \b and \b real scalars
   *
   * \sa MatrixBase::stableNorm(), class Redux
   */
@@ -263,22 +263,15 @@ template<typename Scalar>
 struct scalar_hypot_op<Scalar,Scalar> : binary_op_base<Scalar,Scalar>
 {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_hypot_op)
-//   typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& _x, const Scalar& _y) const
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar &x, const Scalar &y) const
   {
-    EIGEN_USING_STD_MATH(sqrt)
-    Scalar p, qp;
-    if(_x>_y)
-    {
-      p = _x;
-      qp = _y / p;
-    }
-    else
-    {
-      p = _y;
-      qp = _x / p;
-    }
-    return p * sqrt(Scalar(1) + qp*qp);
+    // This functor is used by hypotNorm only for which it is faster to first apply abs
+    // on all coefficients prior to reduction through hypot.
+    // This way we avoid calling abs on positive and real entries, and this also permits
+    // to seamlessly handle complexes. Otherwise we would have to handle both real and complexes
+    // through the same functor...
+    return internal::positive_real_hypot(x,y);
   }
 };
 template<typename Scalar>
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/StlFunctors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/StlFunctors.h
index 6df3fa501..9c1d75850 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/StlFunctors.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/functors/StlFunctors.h
@@ -83,13 +83,17 @@ struct functor_traits<std::binder1st<T> >
 { enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
 #endif
 
+#if (__cplusplus < 201703L) && (EIGEN_COMP_MSVC < 1910)
+// std::unary_negate is deprecated since c++17 and will be removed in c++20
 template<typename T>
 struct functor_traits<std::unary_negate<T> >
 { enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false }; };
 
+// std::binary_negate is deprecated since c++17 and will be removed in c++20
 template<typename T>
 struct functor_traits<std::binary_negate<T> >
 { enum { Cost = 1 + functor_traits<T>::Cost, PacketAccess = false }; };
+#endif
 
 #ifdef EIGEN_STDEXT_SUPPORT
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
index 45230bce5..6be1b49c6 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
@@ -1197,10 +1197,16 @@ void gebp_kernel<LhsScalar,RhsScalar,Index,DataMapper,mr,nr,ConjugateLhs,Conjuga
             EIGEN_ASM_COMMENT("begin gebp micro kernel 2pX4");
             RhsPacket B_0, B1, B2, B3, T0;
 
-   #define EIGEN_GEBGP_ONESTEP(K) \
+          // NOTE: the begin/end asm comments below work around bug 935!
+          // but they are not enough for gcc>=6 without FMA (bug 1637)
+          #if EIGEN_GNUC_AT_LEAST(6,0)
+            #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND __asm__  ("" : [a0] "+rm" (A0),[a1] "+rm" (A1));
+          #else
+            #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND
+          #endif
+          #define EIGEN_GEBGP_ONESTEP(K) \
             do {                                                                \
               EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX4");        \
-              EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
               traits.loadLhs(&blA[(0+2*K)*LhsProgress], A0);                    \
               traits.loadLhs(&blA[(1+2*K)*LhsProgress], A1);                    \
               traits.broadcastRhs(&blB[(0+4*K)*RhsProgress], B_0, B1, B2, B3);  \
@@ -1212,6 +1218,7 @@ void gebp_kernel<LhsScalar,RhsScalar,Index,DataMapper,mr,nr,ConjugateLhs,Conjuga
               traits.madd(A1, B2,  C6, B2);                                     \
               traits.madd(A0, B3,  C3, T0);                                     \
               traits.madd(A1, B3,  C7, B3);                                     \
+              EIGEN_GEBP_2PX4_SPILLING_WORKAROUND                               \
               EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX4");          \
             } while(false)
             
@@ -1526,10 +1533,10 @@ void gebp_kernel<LhsScalar,RhsScalar,Index,DataMapper,mr,nr,ConjugateLhs,Conjuga
           // The following piece of code wont work for 512 bit registers
           // Moreover, if LhsProgress==8 it assumes that there is a half packet of the same size
           // as nr (which is currently 4) for the return type.
-          typedef typename unpacket_traits<SResPacket>::half SResPacketHalf;
+          const int SResPacketHalfSize = unpacket_traits<typename unpacket_traits<SResPacket>::half>::size;
           if ((SwappedTraits::LhsProgress % 4) == 0 &&
               (SwappedTraits::LhsProgress <= 8) &&
-              (SwappedTraits::LhsProgress!=8 || unpacket_traits<SResPacketHalf>::size==nr))
+              (SwappedTraits::LhsProgress!=8 || SResPacketHalfSize==nr))
           {
             SAccPacket C0, C1, C2, C3;
             straits.initAcc(C0);
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
index b91a50340..f6f9ebeca 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h
@@ -88,7 +88,7 @@ struct general_matrix_matrix_rankupdate<Index,EIGTYPE,AStorageOrder,ConjugateA,C
    BlasIndex lda=convert_index<BlasIndex>(lhsStride), ldc=convert_index<BlasIndex>(resStride), n=convert_index<BlasIndex>(size), k=convert_index<BlasIndex>(depth); \
    char uplo=((IsLower) ? 'L' : 'U'), trans=((AStorageOrder==RowMajor) ? 'T':'N'); \
    EIGTYPE beta(1); \
-   BLASFUNC(&uplo, &trans, &n, &k, &numext::real_ref(alpha), lhs, &lda, &numext::real_ref(beta), res, &ldc); \
+   BLASFUNC(&uplo, &trans, &n, &k, (const BLASTYPE*)&numext::real_ref(alpha), lhs, &lda, (const BLASTYPE*)&numext::real_ref(beta), res, &ldc); \
   } \
 };
 
@@ -125,9 +125,13 @@ struct general_matrix_matrix_rankupdate<Index,EIGTYPE,AStorageOrder,ConjugateA,C
   } \
 };
 
-
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_RANKUPDATE_R(double, double, dsyrk)
+EIGEN_BLAS_RANKUPDATE_R(float,  float,  ssyrk)
+#else
 EIGEN_BLAS_RANKUPDATE_R(double, double, dsyrk_)
 EIGEN_BLAS_RANKUPDATE_R(float,  float,  ssyrk_)
+#endif
 
 // TODO hanlde complex cases
 // EIGEN_BLAS_RANKUPDATE_C(dcomplex, double, double, zherk_)
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h
index 7a3bdbf20..b0f6b0d5b 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h
@@ -46,7 +46,7 @@ namespace internal {
 
 // gemm specialization
 
-#define GEMM_SPECIALIZATION(EIGTYPE, EIGPREFIX, BLASTYPE, BLASPREFIX) \
+#define GEMM_SPECIALIZATION(EIGTYPE, EIGPREFIX, BLASTYPE, BLASFUNC) \
 template< \
   typename Index, \
   int LhsStorageOrder, bool ConjugateLhs, \
@@ -100,13 +100,20 @@ static void run(Index rows, Index cols, Index depth, \
     ldb = convert_index<BlasIndex>(b_tmp.outerStride()); \
   } else b = _rhs; \
 \
-  BLASPREFIX##gemm_(&transa, &transb, &m, &n, &k, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, &numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
+  BLASFUNC(&transa, &transb, &m, &n, &k, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
 }};
 
-GEMM_SPECIALIZATION(double,   d,  double, d)
-GEMM_SPECIALIZATION(float,    f,  float,  s)
-GEMM_SPECIALIZATION(dcomplex, cd, double, z)
-GEMM_SPECIALIZATION(scomplex, cf, float,  c)
+#ifdef EIGEN_USE_MKL
+GEMM_SPECIALIZATION(double,   d,  double, dgemm)
+GEMM_SPECIALIZATION(float,    f,  float,  sgemm)
+GEMM_SPECIALIZATION(dcomplex, cd, MKL_Complex16, zgemm)
+GEMM_SPECIALIZATION(scomplex, cf, MKL_Complex8,  cgemm)
+#else
+GEMM_SPECIALIZATION(double,   d,  double, dgemm_)
+GEMM_SPECIALIZATION(float,    f,  float,  sgemm_)
+GEMM_SPECIALIZATION(dcomplex, cd, double, zgemm_)
+GEMM_SPECIALIZATION(scomplex, cf, float,  cgemm_)
+#endif
 
 } // end namespase internal
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h
index e3a5d5892..6e36c2b3c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h
@@ -85,7 +85,7 @@ EIGEN_BLAS_GEMV_SPECIALIZE(float)
 EIGEN_BLAS_GEMV_SPECIALIZE(dcomplex)
 EIGEN_BLAS_GEMV_SPECIALIZE(scomplex)
 
-#define EIGEN_BLAS_GEMV_SPECIALIZATION(EIGTYPE,BLASTYPE,BLASPREFIX) \
+#define EIGEN_BLAS_GEMV_SPECIALIZATION(EIGTYPE,BLASTYPE,BLASFUNC) \
 template<typename Index, int LhsStorageOrder, bool ConjugateLhs, bool ConjugateRhs> \
 struct general_matrix_vector_product_gemv<Index,EIGTYPE,LhsStorageOrder,ConjugateLhs,EIGTYPE,ConjugateRhs> \
 { \
@@ -113,14 +113,21 @@ static void run( \
     x_ptr=x_tmp.data(); \
     incx=1; \
   } else x_ptr=rhs; \
-  BLASPREFIX##gemv_(&trans, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)lhs, &lda, (const BLASTYPE*)x_ptr, &incx, &numext::real_ref(beta), (BLASTYPE*)res, &incy); \
+  BLASFUNC(&trans, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)lhs, &lda, (const BLASTYPE*)x_ptr, &incx, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)res, &incy); \
 }\
 };
 
-EIGEN_BLAS_GEMV_SPECIALIZATION(double,   double, d)
-EIGEN_BLAS_GEMV_SPECIALIZATION(float,    float,  s)
-EIGEN_BLAS_GEMV_SPECIALIZATION(dcomplex, double, z)
-EIGEN_BLAS_GEMV_SPECIALIZATION(scomplex, float,  c)
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_GEMV_SPECIALIZATION(double,   double, dgemv)
+EIGEN_BLAS_GEMV_SPECIALIZATION(float,    float,  sgemv)
+EIGEN_BLAS_GEMV_SPECIALIZATION(dcomplex, MKL_Complex16, zgemv)
+EIGEN_BLAS_GEMV_SPECIALIZATION(scomplex, MKL_Complex8 , cgemv)
+#else
+EIGEN_BLAS_GEMV_SPECIALIZATION(double,   double, dgemv_)
+EIGEN_BLAS_GEMV_SPECIALIZATION(float,    float,  sgemv_)
+EIGEN_BLAS_GEMV_SPECIALIZATION(dcomplex, double, zgemv_)
+EIGEN_BLAS_GEMV_SPECIALIZATION(scomplex, float,  cgemv_)
+#endif
 
 } // end namespase internal
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h
index a45238d69..9a5318507 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h
@@ -40,7 +40,7 @@ namespace internal {
 
 /* Optimized selfadjoint matrix * matrix (?SYMM/?HEMM) product */
 
-#define EIGEN_BLAS_SYMM_L(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_SYMM_L(EIGTYPE, BLASTYPE, EIGPREFIX, BLASFUNC) \
 template <typename Index, \
           int LhsStorageOrder, bool ConjugateLhs, \
           int RhsStorageOrder, bool ConjugateRhs> \
@@ -81,13 +81,13 @@ struct product_selfadjoint_matrix<EIGTYPE,Index,LhsStorageOrder,true,ConjugateLh
       ldb = convert_index<BlasIndex>(b_tmp.outerStride()); \
     } else b = _rhs; \
 \
-    BLASPREFIX##symm_(&side, &uplo, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, &numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
+    BLASFUNC(&side, &uplo, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
 \
   } \
 };
 
 
-#define EIGEN_BLAS_HEMM_L(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_HEMM_L(EIGTYPE, BLASTYPE, EIGPREFIX, BLASFUNC) \
 template <typename Index, \
           int LhsStorageOrder, bool ConjugateLhs, \
           int RhsStorageOrder, bool ConjugateRhs> \
@@ -144,20 +144,26 @@ struct product_selfadjoint_matrix<EIGTYPE,Index,LhsStorageOrder,true,ConjugateLh
       ldb = convert_index<BlasIndex>(b_tmp.outerStride()); \
     } \
 \
-    BLASPREFIX##hemm_(&side, &uplo, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, &numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
+    BLASFUNC(&side, &uplo, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
 \
   } \
 };
 
-EIGEN_BLAS_SYMM_L(double, double, d, d)
-EIGEN_BLAS_SYMM_L(float, float, f, s)
-EIGEN_BLAS_HEMM_L(dcomplex, double, cd, z)
-EIGEN_BLAS_HEMM_L(scomplex, float, cf, c)
-
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_SYMM_L(double, double, d, dsymm)
+EIGEN_BLAS_SYMM_L(float, float, f, ssymm)
+EIGEN_BLAS_HEMM_L(dcomplex, MKL_Complex16, cd, zhemm)
+EIGEN_BLAS_HEMM_L(scomplex, MKL_Complex8, cf, chemm)
+#else
+EIGEN_BLAS_SYMM_L(double, double, d, dsymm_)
+EIGEN_BLAS_SYMM_L(float, float, f, ssymm_)
+EIGEN_BLAS_HEMM_L(dcomplex, double, cd, zhemm_)
+EIGEN_BLAS_HEMM_L(scomplex, float, cf, chemm_)
+#endif
 
 /* Optimized matrix * selfadjoint matrix (?SYMM/?HEMM) product */
 
-#define EIGEN_BLAS_SYMM_R(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_SYMM_R(EIGTYPE, BLASTYPE, EIGPREFIX, BLASFUNC) \
 template <typename Index, \
           int LhsStorageOrder, bool ConjugateLhs, \
           int RhsStorageOrder, bool ConjugateRhs> \
@@ -197,13 +203,13 @@ struct product_selfadjoint_matrix<EIGTYPE,Index,LhsStorageOrder,false,ConjugateL
       ldb = convert_index<BlasIndex>(b_tmp.outerStride()); \
     } else b = _lhs; \
 \
-    BLASPREFIX##symm_(&side, &uplo, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, &numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
+    BLASFUNC(&side, &uplo, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
 \
   } \
 };
 
 
-#define EIGEN_BLAS_HEMM_R(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_HEMM_R(EIGTYPE, BLASTYPE, EIGPREFIX, BLASFUNC) \
 template <typename Index, \
           int LhsStorageOrder, bool ConjugateLhs, \
           int RhsStorageOrder, bool ConjugateRhs> \
@@ -259,15 +265,21 @@ struct product_selfadjoint_matrix<EIGTYPE,Index,LhsStorageOrder,false,ConjugateL
       ldb = convert_index<BlasIndex>(b_tmp.outerStride()); \
     } \
 \
-    BLASPREFIX##hemm_(&side, &uplo, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, &numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
+    BLASFUNC(&side, &uplo, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)b, &ldb, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)res, &ldc); \
   } \
 };
 
-EIGEN_BLAS_SYMM_R(double, double, d, d)
-EIGEN_BLAS_SYMM_R(float, float, f, s)
-EIGEN_BLAS_HEMM_R(dcomplex, double, cd, z)
-EIGEN_BLAS_HEMM_R(scomplex, float, cf, c)
-
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_SYMM_R(double, double, d, dsymm)
+EIGEN_BLAS_SYMM_R(float, float, f, ssymm)
+EIGEN_BLAS_HEMM_R(dcomplex, MKL_Complex16, cd, zhemm)
+EIGEN_BLAS_HEMM_R(scomplex, MKL_Complex8, cf, chemm)
+#else
+EIGEN_BLAS_SYMM_R(double, double, d, dsymm_)
+EIGEN_BLAS_SYMM_R(float, float, f, ssymm_)
+EIGEN_BLAS_HEMM_R(dcomplex, double, cd, zhemm_)
+EIGEN_BLAS_HEMM_R(scomplex, float, cf, chemm_)
+#endif
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h
index 38f23accf..1238345e3 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h
@@ -95,14 +95,21 @@ const EIGTYPE* _rhs, EIGTYPE* res, EIGTYPE alpha) \
     x_tmp=map_x.conjugate(); \
     x_ptr=x_tmp.data(); \
   } else x_ptr=_rhs; \
-  BLASFUNC(&uplo, &n, &numext::real_ref(alpha), (const BLASTYPE*)lhs, &lda, (const BLASTYPE*)x_ptr, &incx, &numext::real_ref(beta), (BLASTYPE*)res, &incy); \
+  BLASFUNC(&uplo, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)lhs, &lda, (const BLASTYPE*)x_ptr, &incx, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)res, &incy); \
 }\
 };
 
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_SYMV_SPECIALIZATION(double,   double, dsymv)
+EIGEN_BLAS_SYMV_SPECIALIZATION(float,    float,  ssymv)
+EIGEN_BLAS_SYMV_SPECIALIZATION(dcomplex, MKL_Complex16, zhemv)
+EIGEN_BLAS_SYMV_SPECIALIZATION(scomplex, MKL_Complex8,  chemv)
+#else
 EIGEN_BLAS_SYMV_SPECIALIZATION(double,   double, dsymv_)
 EIGEN_BLAS_SYMV_SPECIALIZATION(float,    float,  ssymv_)
 EIGEN_BLAS_SYMV_SPECIALIZATION(dcomplex, double, zhemv_)
 EIGEN_BLAS_SYMV_SPECIALIZATION(scomplex, float,  chemv_)
+#endif
 
 } // end namespace internal
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h
index 6ec5a8a0b..f784507e7 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h
@@ -137,7 +137,13 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix<Scalar,Index,Mode,true,
     ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
     ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
-    Matrix<Scalar,SmallPanelWidth,SmallPanelWidth,LhsStorageOrder> triangularBuffer((internal::constructor_without_unaligned_array_assert()));
+    // To work around an "error: member reference base type 'Matrix<...>
+    // (Eigen::internal::constructor_without_unaligned_array_assert (*)())' is
+    // not a structure or union" compilation error in nvcc (tested V8.0.61),
+    // create a dummy internal::constructor_without_unaligned_array_assert
+    // object to pass to the Matrix constructor.
+    internal::constructor_without_unaligned_array_assert a;
+    Matrix<Scalar,SmallPanelWidth,SmallPanelWidth,LhsStorageOrder> triangularBuffer(a);
     triangularBuffer.setZero();
     if((Mode&ZeroDiag)==ZeroDiag)
       triangularBuffer.diagonal().setZero();
@@ -284,7 +290,8 @@ EIGEN_DONT_INLINE void product_triangular_matrix_matrix<Scalar,Index,Mode,false,
     ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
     ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
-    Matrix<Scalar,SmallPanelWidth,SmallPanelWidth,RhsStorageOrder> triangularBuffer((internal::constructor_without_unaligned_array_assert()));
+    internal::constructor_without_unaligned_array_assert a;
+    Matrix<Scalar,SmallPanelWidth,SmallPanelWidth,RhsStorageOrder> triangularBuffer(a);
     triangularBuffer.setZero();
     if((Mode&ZeroDiag)==ZeroDiag)
       triangularBuffer.diagonal().setZero();
@@ -393,7 +400,9 @@ struct triangular_product_impl<Mode,LhsIsTriangular,Lhs,false,Rhs,false>
 {
   template<typename Dest> static void run(Dest& dst, const Lhs &a_lhs, const Rhs &a_rhs, const typename Dest::Scalar& alpha)
   {
-    typedef typename Dest::Scalar     Scalar;
+    typedef typename Lhs::Scalar  LhsScalar;
+    typedef typename Rhs::Scalar  RhsScalar;
+    typedef typename Dest::Scalar Scalar;
     
     typedef internal::blas_traits<Lhs> LhsBlasTraits;
     typedef typename LhsBlasTraits::DirectLinearAccessType ActualLhsType;
@@ -405,8 +414,9 @@ struct triangular_product_impl<Mode,LhsIsTriangular,Lhs,false,Rhs,false>
     typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(a_lhs);
     typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(a_rhs);
 
-    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(a_lhs)
-                               * RhsBlasTraits::extractScalarFactor(a_rhs);
+    LhsScalar lhs_alpha = LhsBlasTraits::extractScalarFactor(a_lhs);
+    RhsScalar rhs_alpha = RhsBlasTraits::extractScalarFactor(a_rhs);
+    Scalar actualAlpha = alpha * lhs_alpha * rhs_alpha;
 
     typedef internal::gemm_blocking_space<(Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,Scalar,Scalar,
               Lhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime, Lhs::MaxColsAtCompileTime,4> BlockingType;
@@ -431,6 +441,21 @@ struct triangular_product_impl<Mode,LhsIsTriangular,Lhs,false,Rhs,false>
         &dst.coeffRef(0,0), dst.outerStride(),    // result info
         actualAlpha, blocking
       );
+
+    // Apply correction if the diagonal is unit and a scalar factor was nested:
+    if ((Mode&UnitDiag)==UnitDiag)
+    {
+      if (LhsIsTriangular && lhs_alpha!=LhsScalar(1))
+      {
+        Index diagSize = (std::min)(lhs.rows(),lhs.cols());
+        dst.topRows(diagSize) -= ((lhs_alpha-LhsScalar(1))*a_rhs).topRows(diagSize);
+      }
+      else if ((!LhsIsTriangular) && rhs_alpha!=RhsScalar(1))
+      {
+        Index diagSize = (std::min)(rhs.rows(),rhs.cols());
+        dst.leftCols(diagSize) -= (rhs_alpha-RhsScalar(1))*a_lhs.leftCols(diagSize);
+      }
+    }
   }
 };
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h
index aecded6bb..a25197ab0 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h
@@ -75,7 +75,7 @@ EIGEN_BLAS_TRMM_SPECIALIZE(scomplex, true)
 EIGEN_BLAS_TRMM_SPECIALIZE(scomplex, false)
 
 // implements col-major += alpha * op(triangular) * op(general)
-#define EIGEN_BLAS_TRMM_L(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_TRMM_L(EIGTYPE, BLASTYPE, EIGPREFIX, BLASFUNC) \
 template <typename Index, int Mode, \
           int LhsStorageOrder, bool ConjugateLhs, \
           int RhsStorageOrder, bool ConjugateRhs> \
@@ -172,7 +172,7 @@ struct product_triangular_matrix_matrix_trmm<EIGTYPE,Index,Mode,true, \
    } \
    /*std::cout << "TRMM_L: A is square! Go to BLAS TRMM implementation! \n";*/ \
 /* call ?trmm*/ \
-   BLASPREFIX##trmm_(&side, &uplo, &transa, &diag, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)b, &ldb); \
+   BLASFUNC(&side, &uplo, &transa, &diag, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)b, &ldb); \
 \
 /* Add op(a_triangular)*b into res*/ \
    Map<MatrixX##EIGPREFIX, 0, OuterStride<> > res_tmp(res,rows,cols,OuterStride<>(resStride)); \
@@ -180,13 +180,20 @@ struct product_triangular_matrix_matrix_trmm<EIGTYPE,Index,Mode,true, \
   } \
 };
 
-EIGEN_BLAS_TRMM_L(double, double, d, d)
-EIGEN_BLAS_TRMM_L(dcomplex, double, cd, z)
-EIGEN_BLAS_TRMM_L(float, float, f, s)
-EIGEN_BLAS_TRMM_L(scomplex, float, cf, c)
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_TRMM_L(double, double, d, dtrmm)
+EIGEN_BLAS_TRMM_L(dcomplex, MKL_Complex16, cd, ztrmm)
+EIGEN_BLAS_TRMM_L(float, float, f, strmm)
+EIGEN_BLAS_TRMM_L(scomplex, MKL_Complex8, cf, ctrmm)
+#else
+EIGEN_BLAS_TRMM_L(double, double, d, dtrmm_)
+EIGEN_BLAS_TRMM_L(dcomplex, double, cd, ztrmm_)
+EIGEN_BLAS_TRMM_L(float, float, f, strmm_)
+EIGEN_BLAS_TRMM_L(scomplex, float, cf, ctrmm_)
+#endif
 
 // implements col-major += alpha * op(general) * op(triangular)
-#define EIGEN_BLAS_TRMM_R(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_TRMM_R(EIGTYPE, BLASTYPE, EIGPREFIX, BLASFUNC) \
 template <typename Index, int Mode, \
           int LhsStorageOrder, bool ConjugateLhs, \
           int RhsStorageOrder, bool ConjugateRhs> \
@@ -282,7 +289,7 @@ struct product_triangular_matrix_matrix_trmm<EIGTYPE,Index,Mode,false, \
    } \
    /*std::cout << "TRMM_R: A is square! Go to BLAS TRMM implementation! \n";*/ \
 /* call ?trmm*/ \
-   BLASPREFIX##trmm_(&side, &uplo, &transa, &diag, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)b, &ldb); \
+   BLASFUNC(&side, &uplo, &transa, &diag, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)b, &ldb); \
 \
 /* Add op(a_triangular)*b into res*/ \
    Map<MatrixX##EIGPREFIX, 0, OuterStride<> > res_tmp(res,rows,cols,OuterStride<>(resStride)); \
@@ -290,11 +297,17 @@ struct product_triangular_matrix_matrix_trmm<EIGTYPE,Index,Mode,false, \
   } \
 };
 
-EIGEN_BLAS_TRMM_R(double, double, d, d)
-EIGEN_BLAS_TRMM_R(dcomplex, double, cd, z)
-EIGEN_BLAS_TRMM_R(float, float, f, s)
-EIGEN_BLAS_TRMM_R(scomplex, float, cf, c)
-
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_TRMM_R(double, double, d, dtrmm)
+EIGEN_BLAS_TRMM_R(dcomplex, MKL_Complex16, cd, ztrmm)
+EIGEN_BLAS_TRMM_R(float, float, f, strmm)
+EIGEN_BLAS_TRMM_R(scomplex, MKL_Complex8, cf, ctrmm)
+#else
+EIGEN_BLAS_TRMM_R(double, double, d, dtrmm_)
+EIGEN_BLAS_TRMM_R(dcomplex, double, cd, ztrmm_)
+EIGEN_BLAS_TRMM_R(float, float, f, strmm_)
+EIGEN_BLAS_TRMM_R(scomplex, float, cf, ctrmm_)
+#endif
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h
index 4b292e74d..76bfa159c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h
@@ -221,8 +221,9 @@ template<int Mode> struct trmv_selector<Mode,ColMajor>
     typename internal::add_const_on_value_type<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(lhs);
     typename internal::add_const_on_value_type<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(rhs);
 
-    ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs)
-                                  * RhsBlasTraits::extractScalarFactor(rhs);
+    LhsScalar lhs_alpha = LhsBlasTraits::extractScalarFactor(lhs);
+    RhsScalar rhs_alpha = RhsBlasTraits::extractScalarFactor(rhs);
+    ResScalar actualAlpha = alpha * lhs_alpha * rhs_alpha;
 
     enum {
       // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
@@ -274,6 +275,12 @@ template<int Mode> struct trmv_selector<Mode,ColMajor>
       else
         dest = MappedDest(actualDestPtr, dest.size());
     }
+
+    if ( ((Mode&UnitDiag)==UnitDiag) && (lhs_alpha!=LhsScalar(1)) )
+    {
+      Index diagSize = (std::min)(lhs.rows(),lhs.cols());
+      dest.head(diagSize) -= (lhs_alpha-LhsScalar(1))*rhs.head(diagSize);
+    }
   }
 };
 
@@ -295,8 +302,9 @@ template<int Mode> struct trmv_selector<Mode,RowMajor>
     typename add_const<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(lhs);
     typename add_const<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(rhs);
 
-    ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs)
-                                  * RhsBlasTraits::extractScalarFactor(rhs);
+    LhsScalar lhs_alpha = LhsBlasTraits::extractScalarFactor(lhs);
+    RhsScalar rhs_alpha = RhsBlasTraits::extractScalarFactor(rhs);
+    ResScalar actualAlpha = alpha * lhs_alpha * rhs_alpha;
 
     enum {
       DirectlyUseRhs = ActualRhsTypeCleaned::InnerStrideAtCompileTime==1
@@ -326,6 +334,12 @@ template<int Mode> struct trmv_selector<Mode,RowMajor>
             actualRhsPtr,1,
             dest.data(),dest.innerStride(),
             actualAlpha);
+
+    if ( ((Mode&UnitDiag)==UnitDiag) && (lhs_alpha!=LhsScalar(1)) )
+    {
+      Index diagSize = (std::min)(lhs.rows(),lhs.cols());
+      dest.head(diagSize) -= (lhs_alpha-LhsScalar(1))*rhs.head(diagSize);
+    }
   }
 };
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h
index 07bf26ce5..3d47a2b94 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h
@@ -71,7 +71,7 @@ EIGEN_BLAS_TRMV_SPECIALIZE(dcomplex)
 EIGEN_BLAS_TRMV_SPECIALIZE(scomplex)
 
 // implements col-major: res += alpha * op(triangular) * vector
-#define EIGEN_BLAS_TRMV_CM(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_TRMV_CM(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX, BLASPOSTFIX) \
 template<typename Index, int Mode, bool ConjLhs, bool ConjRhs> \
 struct triangular_matrix_vector_product_trmv<Index,Mode,EIGTYPE,ConjLhs,EIGTYPE,ConjRhs,ColMajor> { \
   enum { \
@@ -121,10 +121,10 @@ struct triangular_matrix_vector_product_trmv<Index,Mode,EIGTYPE,ConjLhs,EIGTYPE,
    diag = IsUnitDiag ? 'U' : 'N'; \
 \
 /* call ?TRMV*/ \
-   BLASPREFIX##trmv_(&uplo, &trans, &diag, &n, (const BLASTYPE*)_lhs, &lda, (BLASTYPE*)x, &incx); \
+   BLASPREFIX##trmv##BLASPOSTFIX(&uplo, &trans, &diag, &n, (const BLASTYPE*)_lhs, &lda, (BLASTYPE*)x, &incx); \
 \
 /* Add op(a_tr)rhs into res*/ \
-   BLASPREFIX##axpy_(&n, &numext::real_ref(alpha),(const BLASTYPE*)x, &incx, (BLASTYPE*)_res, &incy); \
+   BLASPREFIX##axpy##BLASPOSTFIX(&n, (const BLASTYPE*)&numext::real_ref(alpha),(const BLASTYPE*)x, &incx, (BLASTYPE*)_res, &incy); \
 /* Non-square case - doesn't fit to BLAS ?TRMV. Fall to default triangular product*/ \
    if (size<(std::max)(rows,cols)) { \
      if (ConjRhs) x_tmp = rhs.conjugate(); else x_tmp = rhs; \
@@ -142,18 +142,25 @@ struct triangular_matrix_vector_product_trmv<Index,Mode,EIGTYPE,ConjLhs,EIGTYPE,
        m = convert_index<BlasIndex>(size); \
        n = convert_index<BlasIndex>(cols-size); \
      } \
-     BLASPREFIX##gemv_(&trans, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)x, &incx, &numext::real_ref(beta), (BLASTYPE*)y, &incy); \
+     BLASPREFIX##gemv##BLASPOSTFIX(&trans, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)x, &incx, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)y, &incy); \
    } \
   } \
 };
 
-EIGEN_BLAS_TRMV_CM(double,   double, d,  d)
-EIGEN_BLAS_TRMV_CM(dcomplex, double, cd, z)
-EIGEN_BLAS_TRMV_CM(float,    float,  f,  s)
-EIGEN_BLAS_TRMV_CM(scomplex, float,  cf, c)
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_TRMV_CM(double,   double, d,  d,)
+EIGEN_BLAS_TRMV_CM(dcomplex, MKL_Complex16, cd, z,)
+EIGEN_BLAS_TRMV_CM(float,    float,  f,  s,)
+EIGEN_BLAS_TRMV_CM(scomplex, MKL_Complex8,  cf, c,)
+#else
+EIGEN_BLAS_TRMV_CM(double,   double, d,  d, _)
+EIGEN_BLAS_TRMV_CM(dcomplex, double, cd, z, _)
+EIGEN_BLAS_TRMV_CM(float,    float,  f,  s, _)
+EIGEN_BLAS_TRMV_CM(scomplex, float,  cf, c, _)
+#endif
 
 // implements row-major: res += alpha * op(triangular) * vector
-#define EIGEN_BLAS_TRMV_RM(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX) \
+#define EIGEN_BLAS_TRMV_RM(EIGTYPE, BLASTYPE, EIGPREFIX, BLASPREFIX, BLASPOSTFIX) \
 template<typename Index, int Mode, bool ConjLhs, bool ConjRhs> \
 struct triangular_matrix_vector_product_trmv<Index,Mode,EIGTYPE,ConjLhs,EIGTYPE,ConjRhs,RowMajor> { \
   enum { \
@@ -203,10 +210,10 @@ struct triangular_matrix_vector_product_trmv<Index,Mode,EIGTYPE,ConjLhs,EIGTYPE,
    diag = IsUnitDiag ? 'U' : 'N'; \
 \
 /* call ?TRMV*/ \
-   BLASPREFIX##trmv_(&uplo, &trans, &diag, &n, (const BLASTYPE*)_lhs, &lda, (BLASTYPE*)x, &incx); \
+   BLASPREFIX##trmv##BLASPOSTFIX(&uplo, &trans, &diag, &n, (const BLASTYPE*)_lhs, &lda, (BLASTYPE*)x, &incx); \
 \
 /* Add op(a_tr)rhs into res*/ \
-   BLASPREFIX##axpy_(&n, &numext::real_ref(alpha),(const BLASTYPE*)x, &incx, (BLASTYPE*)_res, &incy); \
+   BLASPREFIX##axpy##BLASPOSTFIX(&n, (const BLASTYPE*)&numext::real_ref(alpha),(const BLASTYPE*)x, &incx, (BLASTYPE*)_res, &incy); \
 /* Non-square case - doesn't fit to BLAS ?TRMV. Fall to default triangular product*/ \
    if (size<(std::max)(rows,cols)) { \
      if (ConjRhs) x_tmp = rhs.conjugate(); else x_tmp = rhs; \
@@ -224,15 +231,22 @@ struct triangular_matrix_vector_product_trmv<Index,Mode,EIGTYPE,ConjLhs,EIGTYPE,
        m = convert_index<BlasIndex>(size); \
        n = convert_index<BlasIndex>(cols-size); \
      } \
-     BLASPREFIX##gemv_(&trans, &n, &m, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)x, &incx, &numext::real_ref(beta), (BLASTYPE*)y, &incy); \
+     BLASPREFIX##gemv##BLASPOSTFIX(&trans, &n, &m, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (const BLASTYPE*)x, &incx, (const BLASTYPE*)&numext::real_ref(beta), (BLASTYPE*)y, &incy); \
    } \
   } \
 };
 
-EIGEN_BLAS_TRMV_RM(double,   double, d,  d)
-EIGEN_BLAS_TRMV_RM(dcomplex, double, cd, z)
-EIGEN_BLAS_TRMV_RM(float,    float,  f,  s)
-EIGEN_BLAS_TRMV_RM(scomplex, float,  cf, c)
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_TRMV_RM(double,   double, d,  d,)
+EIGEN_BLAS_TRMV_RM(dcomplex, MKL_Complex16, cd, z,)
+EIGEN_BLAS_TRMV_RM(float,    float,  f,  s,)
+EIGEN_BLAS_TRMV_RM(scomplex, MKL_Complex8,  cf, c,)
+#else
+EIGEN_BLAS_TRMV_RM(double,   double, d,  d,_)
+EIGEN_BLAS_TRMV_RM(dcomplex, double, cd, z,_)
+EIGEN_BLAS_TRMV_RM(float,    float,  f,  s,_)
+EIGEN_BLAS_TRMV_RM(scomplex, float,  cf, c,_)
+#endif
 
 } // end namespase internal
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h
index 88c0fb794..f0775116a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h
@@ -38,7 +38,7 @@ namespace Eigen {
 namespace internal {
 
 // implements LeftSide op(triangular)^-1 * general
-#define EIGEN_BLAS_TRSM_L(EIGTYPE, BLASTYPE, BLASPREFIX) \
+#define EIGEN_BLAS_TRSM_L(EIGTYPE, BLASTYPE, BLASFUNC) \
 template <typename Index, int Mode, bool Conjugate, int TriStorageOrder> \
 struct triangular_solve_matrix<EIGTYPE,Index,OnTheLeft,Mode,Conjugate,TriStorageOrder,ColMajor> \
 { \
@@ -80,18 +80,24 @@ struct triangular_solve_matrix<EIGTYPE,Index,OnTheLeft,Mode,Conjugate,TriStorage
    } \
    if (IsUnitDiag) diag='U'; \
 /* call ?trsm*/ \
-   BLASPREFIX##trsm_(&side, &uplo, &transa, &diag, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)_other, &ldb); \
+   BLASFUNC(&side, &uplo, &transa, &diag, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)_other, &ldb); \
  } \
 };
 
-EIGEN_BLAS_TRSM_L(double,   double, d)
-EIGEN_BLAS_TRSM_L(dcomplex, double, z)
-EIGEN_BLAS_TRSM_L(float,    float,  s)
-EIGEN_BLAS_TRSM_L(scomplex, float,  c)
-
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_TRSM_L(double,   double, dtrsm)
+EIGEN_BLAS_TRSM_L(dcomplex, MKL_Complex16, ztrsm)
+EIGEN_BLAS_TRSM_L(float,    float,  strsm)
+EIGEN_BLAS_TRSM_L(scomplex, MKL_Complex8, ctrsm)
+#else
+EIGEN_BLAS_TRSM_L(double,   double, dtrsm_)
+EIGEN_BLAS_TRSM_L(dcomplex, double, ztrsm_)
+EIGEN_BLAS_TRSM_L(float,    float,  strsm_)
+EIGEN_BLAS_TRSM_L(scomplex, float,  ctrsm_)
+#endif
 
 // implements RightSide general * op(triangular)^-1
-#define EIGEN_BLAS_TRSM_R(EIGTYPE, BLASTYPE, BLASPREFIX) \
+#define EIGEN_BLAS_TRSM_R(EIGTYPE, BLASTYPE, BLASFUNC) \
 template <typename Index, int Mode, bool Conjugate, int TriStorageOrder> \
 struct triangular_solve_matrix<EIGTYPE,Index,OnTheRight,Mode,Conjugate,TriStorageOrder,ColMajor> \
 { \
@@ -133,16 +139,22 @@ struct triangular_solve_matrix<EIGTYPE,Index,OnTheRight,Mode,Conjugate,TriStorag
    } \
    if (IsUnitDiag) diag='U'; \
 /* call ?trsm*/ \
-   BLASPREFIX##trsm_(&side, &uplo, &transa, &diag, &m, &n, &numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)_other, &ldb); \
+   BLASFUNC(&side, &uplo, &transa, &diag, &m, &n, (const BLASTYPE*)&numext::real_ref(alpha), (const BLASTYPE*)a, &lda, (BLASTYPE*)_other, &ldb); \
    /*std::cout << "TRMS_L specialization!\n";*/ \
  } \
 };
 
-EIGEN_BLAS_TRSM_R(double,   double, d)
-EIGEN_BLAS_TRSM_R(dcomplex, double, z)
-EIGEN_BLAS_TRSM_R(float,    float,  s)
-EIGEN_BLAS_TRSM_R(scomplex, float,  c)
-
+#ifdef EIGEN_USE_MKL
+EIGEN_BLAS_TRSM_R(double,   double, dtrsm)
+EIGEN_BLAS_TRSM_R(dcomplex, MKL_Complex16, ztrsm)
+EIGEN_BLAS_TRSM_R(float,    float,  strsm)
+EIGEN_BLAS_TRSM_R(scomplex, MKL_Complex8,  ctrsm)
+#else
+EIGEN_BLAS_TRSM_R(double,   double, dtrsm_)
+EIGEN_BLAS_TRSM_R(dcomplex, double, ztrsm_)
+EIGEN_BLAS_TRSM_R(float,    float,  strsm_)
+EIGEN_BLAS_TRSM_R(scomplex, float,  ctrsm_)
+#endif
 
 } // end namespace internal
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h
index 7559e129c..351bd6c60 100755
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h
@@ -43,12 +43,20 @@
   #endif
   #pragma clang diagnostic ignored "-Wconstant-logical-operand"
 
-#elif defined __GNUC__ && __GNUC__>=6
+#elif defined __GNUC__
 
-  #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
+  #if (!defined(EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS)) &&  (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
     #pragma GCC diagnostic push
   #endif
-  #pragma GCC diagnostic ignored "-Wignored-attributes"
+  // g++ warns about local variables shadowing member functions, which is too strict
+  #pragma GCC diagnostic ignored "-Wshadow"
+  #if __GNUC__ == 4 && __GNUC_MINOR__ < 8
+    // Until g++-4.7 there are warnings when comparing unsigned int vs 0, even in templated functions:
+    #pragma GCC diagnostic ignored "-Wtype-limits"
+  #endif
+  #if __GNUC__>=6
+    #pragma GCC diagnostic ignored "-Wignored-attributes"
+  #endif
 
 #endif
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h
index 26b59669e..b7d6ecc76 100755
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h
@@ -49,10 +49,11 @@
   #define EIGEN_USE_LAPACKE
 #endif
 
-#if defined(EIGEN_USE_MKL_VML)
+#if defined(EIGEN_USE_MKL_VML) && !defined(EIGEN_USE_MKL)
   #define EIGEN_USE_MKL
 #endif
 
+
 #if defined EIGEN_USE_MKL
 #   include <mkl.h> 
 /*Check IMKL version for compatibility: < 10.3 is not usable with Eigen*/
@@ -108,6 +109,10 @@
 #endif
 #endif
 
+#if defined(EIGEN_USE_BLAS) && !defined(EIGEN_USE_MKL)
+#include "../../misc/blas.h"
+#endif
+
 namespace Eigen {
 
 typedef std::complex<double> dcomplex;
@@ -121,8 +126,5 @@ typedef int BlasIndex;
 
 } // end namespace Eigen
 
-#if defined(EIGEN_USE_BLAS)
-#include "../../misc/blas.h"
-#endif
 
 #endif // EIGEN_MKL_SUPPORT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
index 38d6ddb9a..eef845c5f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
@@ -13,7 +13,7 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 3
-#define EIGEN_MINOR_VERSION 4
+#define EIGEN_MINOR_VERSION 6
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@@ -399,7 +399,7 @@
 // Does the compiler support variadic templates?
 #ifndef EIGEN_HAS_VARIADIC_TEMPLATES
 #if EIGEN_MAX_CPP_VER>=11 && (__cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900) \
-  && ( !defined(__NVCC__) || !EIGEN_ARCH_ARM_OR_ARM64 || (defined __CUDACC_VER__ && __CUDACC_VER__ >= 80000) )
+  && (!defined(__NVCC__) || !EIGEN_ARCH_ARM_OR_ARM64 || (EIGEN_CUDACC_VER >= 80000) )
     // ^^ Disable the use of variadic templates when compiling with versions of nvcc older than 8.0 on ARM devices:
     //    this prevents nvcc from crashing when compiling Eigen on Tegra X1
 #define EIGEN_HAS_VARIADIC_TEMPLATES 1
@@ -413,7 +413,7 @@
 
 #ifdef __CUDACC__
 // Const expressions are supported provided that c++11 is enabled and we're using either clang or nvcc 7.5 or above
-#if EIGEN_MAX_CPP_VER>=14 && (__cplusplus > 199711L && defined(__CUDACC_VER__) && (EIGEN_COMP_CLANG || __CUDACC_VER__ >= 70500))
+#if EIGEN_MAX_CPP_VER>=14 && (__cplusplus > 199711L && (EIGEN_COMP_CLANG || EIGEN_CUDACC_VER >= 70500))
   #define EIGEN_HAS_CONSTEXPR 1
 #endif
 #elif EIGEN_MAX_CPP_VER>=14 && (__has_feature(cxx_relaxed_constexpr) || (defined(__cplusplus) && __cplusplus >= 201402L) || \
@@ -487,11 +487,13 @@
 // EIGEN_STRONG_INLINE is a stronger version of the inline, using __forceinline on MSVC,
 // but it still doesn't use GCC's always_inline. This is useful in (common) situations where MSVC needs forceinline
 // but GCC is still doing fine with just inline.
+#ifndef EIGEN_STRONG_INLINE
 #if EIGEN_COMP_MSVC || EIGEN_COMP_ICC
 #define EIGEN_STRONG_INLINE __forceinline
 #else
 #define EIGEN_STRONG_INLINE inline
 #endif
+#endif
 
 // EIGEN_ALWAYS_INLINE is the stronget, it has the effect of making the function inline and adding every possible
 // attribute to maximize inlining. This should only be used when really necessary: in particular,
@@ -812,7 +814,8 @@ namespace Eigen {
 // just an empty macro !
 #define EIGEN_EMPTY
 
-#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC < 1900 ||  defined(__CUDACC_VER__)) // for older MSVC versions, as well as 1900 && CUDA 8, using the base operator is sufficient (cf Bugs 1000, 1324)
+#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC < 1900 || EIGEN_CUDACC_VER>0)
+  // for older MSVC versions, as well as 1900 && CUDA 8, using the base operator is sufficient (cf Bugs 1000, 1324)
   #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
     using Base::operator =;
 #elif EIGEN_COMP_CLANG // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)
@@ -986,7 +989,13 @@ namespace Eigen {
 #   define EIGEN_NOEXCEPT
 #   define EIGEN_NOEXCEPT_IF(x)
 #   define EIGEN_NO_THROW throw()
-#   define EIGEN_EXCEPTION_SPEC(X) throw(X)
+#   if EIGEN_COMP_MSVC
+      // MSVC does not support exception specifications (warning C4290),
+      // and they are deprecated in c++11 anyway.
+#     define EIGEN_EXCEPTION_SPEC(X) throw()
+#   else
+#     define EIGEN_EXCEPTION_SPEC(X) throw(X)
+#   endif
 #endif
 
 #endif // EIGEN_MACROS_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h
index c634d7ea0..291383c58 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h
@@ -70,7 +70,7 @@ inline void throw_std_bad_alloc()
     throw std::bad_alloc();
   #else
     std::size_t huge = static_cast<std::size_t>(-1);
-    new int[huge];
+    ::operator new(huge);
   #endif
 }
 
@@ -493,7 +493,7 @@ template<typename T> struct smart_copy_helper<T,true> {
     IntPtr size = IntPtr(end)-IntPtr(start);
     if(size==0) return;
     eigen_internal_assert(start!=0 && end!=0 && target!=0);
-    memcpy(target, start, size);
+    std::memcpy(target, start, size);
   }
 };
 
@@ -696,7 +696,15 @@ template<typename T> void swap(scoped_array<T> &a,scoped_array<T> &b)
 /** \class aligned_allocator
 * \ingroup Core_Module
 *
-* \brief STL compatible allocator to use with with 16 byte aligned types
+* \brief STL compatible allocator to use with types requiring a non standrad alignment.
+*
+* The memory is aligned as for dynamically aligned matrix/array types such as MatrixXd.
+* By default, it will thus provide at least 16 bytes alignment and more in following cases:
+*  - 32 bytes alignment if AVX is enabled.
+*  - 64 bytes alignment if AVX512 is enabled.
+*
+* This can be controled using the \c EIGEN_MAX_ALIGN_BYTES macro as documented
+* \link TopicPreprocessorDirectivesPerformance there \endlink.
 *
 * Example:
 * \code
@@ -739,7 +747,15 @@ public:
   pointer allocate(size_type num, const void* /*hint*/ = 0)
   {
     internal::check_size_for_overflow<T>(num);
-    return static_cast<pointer>( internal::aligned_malloc(num * sizeof(T)) );
+    size_type size = num * sizeof(T);
+#if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(7,0)
+    // workaround gcc bug https://gcc.gnu.org/bugzilla/show_bug.cgi?id=87544
+    // It triggered eigen/Eigen/src/Core/util/Memory.h:189:12: warning: argument 1 value '18446744073709551612' exceeds maximum object size 9223372036854775807
+    if(size>=std::size_t((std::numeric_limits<std::ptrdiff_t>::max)()))
+      return 0;
+    else
+#endif
+      return static_cast<pointer>( internal::aligned_malloc(size) );
   }
 
   void deallocate(pointer p, size_type /*num*/)
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h
index 7f6370755..d31e95411 100755
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h
@@ -109,6 +109,28 @@ template<> struct is_integral<unsigned int>    { enum { value = true }; };
 template<> struct is_integral<signed long>     { enum { value = true }; };
 template<> struct is_integral<unsigned long>   { enum { value = true }; };
 
+#if EIGEN_HAS_CXX11
+using std::make_unsigned;
+#else
+// TODO: Possibly improve this implementation of make_unsigned.
+// It is currently used only by
+// template<typename Scalar> struct random_default_impl<Scalar, false, true>.
+template<typename> struct make_unsigned;
+template<> struct make_unsigned<char>             { typedef unsigned char type; };
+template<> struct make_unsigned<signed char>      { typedef unsigned char type; };
+template<> struct make_unsigned<unsigned char>    { typedef unsigned char type; };
+template<> struct make_unsigned<signed short>     { typedef unsigned short type; };
+template<> struct make_unsigned<unsigned short>   { typedef unsigned short type; };
+template<> struct make_unsigned<signed int>       { typedef unsigned int type; };
+template<> struct make_unsigned<unsigned int>     { typedef unsigned int type; };
+template<> struct make_unsigned<signed long>      { typedef unsigned long type; };
+template<> struct make_unsigned<unsigned long>    { typedef unsigned long type; };
+#if EIGEN_COMP_MSVC
+template<> struct make_unsigned<signed __int64>   { typedef unsigned __int64 type; };
+template<> struct make_unsigned<unsigned __int64> { typedef unsigned __int64 type; };
+#endif
+#endif
+
 template <typename T> struct add_const { typedef const T type; };
 template <typename T> struct add_const<T&> { typedef T& type; };
 
@@ -485,6 +507,26 @@ T div_ceil(const T &a, const T &b)
   return (a+b-1) / b;
 }
 
+// The aim of the following functions is to bypass -Wfloat-equal warnings
+// when we really want a strict equality comparison on floating points.
+template<typename X, typename Y> EIGEN_STRONG_INLINE
+bool equal_strict(const X& x,const Y& y) { return x == y; }
+
+template<> EIGEN_STRONG_INLINE
+bool equal_strict(const float& x,const float& y) { return std::equal_to<float>()(x,y); }
+
+template<> EIGEN_STRONG_INLINE
+bool equal_strict(const double& x,const double& y) { return std::equal_to<double>()(x,y); }
+
+template<typename X, typename Y> EIGEN_STRONG_INLINE
+bool not_equal_strict(const X& x,const Y& y) { return x != y; }
+
+template<> EIGEN_STRONG_INLINE
+bool not_equal_strict(const float& x,const float& y) { return std::not_equal_to<float>()(x,y); }
+
+template<> EIGEN_STRONG_INLINE
+bool not_equal_strict(const double& x,const double& y) { return std::not_equal_to<double>()(x,y); }
+
 } // end namespace numext
 
 } // end namespace Eigen
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h
index 86b60f52f..ecc82b7c8 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h
@@ -8,7 +8,7 @@
     #pragma warning pop
   #elif defined __clang__
     #pragma clang diagnostic pop
-  #elif defined __GNUC__ && __GNUC__>=6
+  #elif defined __GNUC__  &&  (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
     #pragma GCC diagnostic pop
   #endif
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h
index 983361a45..500e47792 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h
@@ -24,6 +24,7 @@
  *
  */
 
+#ifndef EIGEN_STATIC_ASSERT
 #ifndef EIGEN_NO_STATIC_ASSERT
 
   #if EIGEN_MAX_CPP_VER>=11 && (__has_feature(cxx_static_assert) || (defined(__cplusplus) && __cplusplus >= 201103L) || (EIGEN_COMP_MSVC >= 1600))
@@ -44,64 +45,65 @@
     struct static_assertion<true>
     {
       enum {
-        YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX,
-        YOU_MIXED_VECTORS_OF_DIFFERENT_SIZES,
-        YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES,
-        THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE,
-        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE,
-        THIS_METHOD_IS_ONLY_FOR_OBJECTS_OF_A_SPECIFIC_SIZE,
-        OUT_OF_RANGE_ACCESS,
-        YOU_MADE_A_PROGRAMMING_MISTAKE,
-        EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT,
-        EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE,
-        YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR,
-        YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR,
-        UNALIGNED_LOAD_AND_STORE_OPERATIONS_UNIMPLEMENTED_ON_ALTIVEC,
-        THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES,
-        FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED,
-        NUMERIC_TYPE_MUST_BE_REAL,
-        COEFFICIENT_WRITE_ACCESS_TO_SELFADJOINT_NOT_SUPPORTED,
-        WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED,
-        THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE,
-        INVALID_MATRIX_PRODUCT,
-        INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS,
-        INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION,
-        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY,
-        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES,
-        THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES,
-        INVALID_MATRIX_TEMPLATE_PARAMETERS,
-        INVALID_MATRIXBASE_TEMPLATE_PARAMETERS,
-        BOTH_MATRICES_MUST_HAVE_THE_SAME_STORAGE_ORDER,
-        THIS_METHOD_IS_ONLY_FOR_DIAGONAL_MATRIX,
-        THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE,
-        THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_WITH_DIRECT_MEMORY_ACCESS_SUCH_AS_MAP_OR_PLAIN_MATRICES,
-        YOU_ALREADY_SPECIFIED_THIS_STRIDE,
-        INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION,
-        THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD,
-        PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1,
-        THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS,
-        YOU_CANNOT_MIX_ARRAYS_AND_MATRICES,
-        YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION,
-        THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY,
-        YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT,
-        THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS,
-        THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS,
-        THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL,
-        THIS_METHOD_IS_ONLY_FOR_ARRAYS_NOT_MATRICES,
-        YOU_PASSED_A_ROW_VECTOR_BUT_A_COLUMN_VECTOR_WAS_EXPECTED,
-        YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED,
-        THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE,
-        THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH,
-        OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG,
-        IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY,
-        STORAGE_LAYOUT_DOES_NOT_MATCH,
-        EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE,
-        THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS,
-        MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY,
-        THIS_TYPE_IS_NOT_SUPPORTED,
-        STORAGE_KIND_MUST_MATCH,
-        STORAGE_INDEX_MUST_MATCH,
-        CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY
+        YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX=1,
+        YOU_MIXED_VECTORS_OF_DIFFERENT_SIZES=1,
+        YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES=1,
+        THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE=1,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE=1,
+        THIS_METHOD_IS_ONLY_FOR_OBJECTS_OF_A_SPECIFIC_SIZE=1,
+        OUT_OF_RANGE_ACCESS=1,
+        YOU_MADE_A_PROGRAMMING_MISTAKE=1,
+        EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT=1,
+        EIGEN_INTERNAL_COMPILATION_ERROR_OR_YOU_MADE_A_PROGRAMMING_MISTAKE=1,
+        YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR=1,
+        YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR=1,
+        UNALIGNED_LOAD_AND_STORE_OPERATIONS_UNIMPLEMENTED_ON_ALTIVEC=1,
+        THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES=1,
+        FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED=1,
+        NUMERIC_TYPE_MUST_BE_REAL=1,
+        COEFFICIENT_WRITE_ACCESS_TO_SELFADJOINT_NOT_SUPPORTED=1,
+        WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED=1,
+        THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE=1,
+        INVALID_MATRIX_PRODUCT=1,
+        INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS=1,
+        INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION=1,
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY=1,
+        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES=1,
+        THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES=1,
+        INVALID_MATRIX_TEMPLATE_PARAMETERS=1,
+        INVALID_MATRIXBASE_TEMPLATE_PARAMETERS=1,
+        BOTH_MATRICES_MUST_HAVE_THE_SAME_STORAGE_ORDER=1,
+        THIS_METHOD_IS_ONLY_FOR_DIAGONAL_MATRIX=1,
+        THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE=1,
+        THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_WITH_DIRECT_MEMORY_ACCESS_SUCH_AS_MAP_OR_PLAIN_MATRICES=1,
+        YOU_ALREADY_SPECIFIED_THIS_STRIDE=1,
+        INVALID_STORAGE_ORDER_FOR_THIS_VECTOR_EXPRESSION=1,
+        THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD=1,
+        PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1=1,
+        THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS=1,
+        YOU_CANNOT_MIX_ARRAYS_AND_MATRICES=1,
+        YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION=1,
+        THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY=1,
+        YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT=1,
+        THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS=1,
+        THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS=1,
+        THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_OF_BOOL=1,
+        THIS_METHOD_IS_ONLY_FOR_ARRAYS_NOT_MATRICES=1,
+        YOU_PASSED_A_ROW_VECTOR_BUT_A_COLUMN_VECTOR_WAS_EXPECTED=1,
+        YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED=1,
+        THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE=1,
+        THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH=1,
+        OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG=1,
+        IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY=1,
+        STORAGE_LAYOUT_DOES_NOT_MATCH=1,
+        EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE=1,
+        THIS_COEFFICIENT_ACCESSOR_TAKING_ONE_ACCESS_IS_ONLY_FOR_EXPRESSIONS_ALLOWING_LINEAR_ACCESS=1,
+        MATRIX_FREE_CONJUGATE_GRADIENT_IS_COMPATIBLE_WITH_UPPER_UNION_LOWER_MODE_ONLY=1,
+        THIS_TYPE_IS_NOT_SUPPORTED=1,
+        STORAGE_KIND_MUST_MATCH=1,
+        STORAGE_INDEX_MUST_MATCH=1,
+        CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY=1,
+        SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY=1
       };
     };
 
@@ -131,7 +133,7 @@
   #define EIGEN_STATIC_ASSERT(CONDITION,MSG) eigen_assert((CONDITION) && #MSG);
 
 #endif // EIGEN_NO_STATIC_ASSERT
-
+#endif // EIGEN_STATIC_ASSERT
 
 // static assertion failing if the type \a TYPE is not a vector type
 #define EIGEN_STATIC_ASSERT_VECTOR_ONLY(TYPE) \
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
index 36a91dffc..87d789b3f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
@@ -311,7 +311,6 @@ GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A, const MatrixTyp
     // Aliases:
     Map<VectorType> v(reinterpret_cast<Scalar*>(m_tmp.data()), size);
     ComplexVectorType &cv = m_tmp;
-    const MatrixType &mZ = m_realQZ.matrixZ();
     const MatrixType &mS = m_realQZ.matrixS();
     const MatrixType &mT = m_realQZ.matrixT();
 
@@ -351,7 +350,7 @@ GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A, const MatrixTyp
               }
             }
           }
-          m_eivec.col(i).real().noalias() = mZ.transpose() * v;
+          m_eivec.col(i).real().noalias() = m_realQZ.matrixZ().transpose() * v;
           m_eivec.col(i).real().normalize();
           m_eivec.col(i).imag().setConstant(0);
         }
@@ -400,7 +399,7 @@ GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A, const MatrixTyp
                               / (alpha*mT.coeffRef(j,j) - static_cast<Scalar>(beta*mS.coeffRef(j,j)));
             }
           }
-          m_eivec.col(i+1).noalias() = (mZ.transpose() * cv);
+          m_eivec.col(i+1).noalias() = (m_realQZ.matrixZ().transpose() * cv);
           m_eivec.col(i+1).normalize();
           m_eivec.col(i) = m_eivec.col(i+1).conjugate();
         }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
index 4fec8af0a..e4e426071 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
@@ -66,7 +66,6 @@ template<typename Derived>
 inline typename MatrixBase<Derived>::EigenvaluesReturnType
 MatrixBase<Derived>::eigenvalues() const
 {
-  typedef typename internal::traits<Derived>::Scalar Scalar;
   return internal::eigenvalues_selector<Derived, NumTraits<Scalar>::IsComplex>::run(derived());
 }
 
@@ -88,7 +87,6 @@ template<typename MatrixType, unsigned int UpLo>
 inline typename SelfAdjointView<MatrixType, UpLo>::EigenvaluesReturnType
 SelfAdjointView<MatrixType, UpLo>::eigenvalues() const
 {
-  typedef typename SelfAdjointView<MatrixType, UpLo>::PlainObject PlainObject;
   PlainObject thisAsMatrix(*this);
   return SelfAdjointEigenSolver<PlainObject>(thisAsMatrix, false).eigenvalues();
 }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h
index f5c86041d..17ea903f5 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h
@@ -303,7 +303,7 @@ RealSchur<MatrixType>& RealSchur<MatrixType>::computeFromHessenberg(const HessMa
   Scalar exshift(0);   // sum of exceptional shifts
   Scalar norm = computeNormOfT();
 
-  if(norm!=0)
+  if(norm!=Scalar(0))
   {
     while (iu >= 0)
     {
@@ -327,7 +327,7 @@ RealSchur<MatrixType>& RealSchur<MatrixType>::computeFromHessenberg(const HessMa
       else // No convergence yet
       {
         // The firstHouseholderVector vector has to be initialized to something to get rid of a silly GCC warning (-O1 -Wall -DNDEBUG )
-        Vector3s firstHouseholderVector(0,0,0), shiftInfo;
+        Vector3s firstHouseholderVector = Vector3s::Zero(), shiftInfo;
         computeShift(iu, iter, exshift, shiftInfo);
         iter = iter + 1;
         totalIter = totalIter + 1;
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h
index 3891cf883..b0c947dc0 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h
@@ -37,7 +37,7 @@ namespace Eigen {
 
 /** \internal Specialization for the data types supported by LAPACKe */
 
-#define EIGEN_LAPACKE_EIG_SELFADJ(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME, EIGCOLROW, LAPACKE_COLROW ) \
+#define EIGEN_LAPACKE_EIG_SELFADJ_2(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME, EIGCOLROW ) \
 template<> template<typename InputType> inline \
 SelfAdjointEigenSolver<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >& \
 SelfAdjointEigenSolver<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(const EigenBase<InputType>& matrix, int options) \
@@ -47,7 +47,7 @@ SelfAdjointEigenSolver<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(c
           && (options&EigVecMask)!=EigVecMask \
           && "invalid option parameter"); \
   bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors; \
-  lapack_int n = internal::convert_index<lapack_int>(matrix.cols()), lda, matrix_order, info; \
+  lapack_int n = internal::convert_index<lapack_int>(matrix.cols()), lda, info; \
   m_eivalues.resize(n,1); \
   m_subdiag.resize(n-1); \
   m_eivec = matrix; \
@@ -63,27 +63,24 @@ SelfAdjointEigenSolver<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(c
   } \
 \
   lda = internal::convert_index<lapack_int>(m_eivec.outerStride()); \
-  matrix_order=LAPACKE_COLROW; \
   char jobz, uplo='L'/*, range='A'*/; \
   jobz = computeEigenvectors ? 'V' : 'N'; \
 \
-  info = LAPACKE_##LAPACKE_NAME( matrix_order, jobz, uplo, n, (LAPACKE_TYPE*)m_eivec.data(), lda, (LAPACKE_RTYPE*)m_eivalues.data() ); \
+  info = LAPACKE_##LAPACKE_NAME( LAPACK_COL_MAJOR, jobz, uplo, n, (LAPACKE_TYPE*)m_eivec.data(), lda, (LAPACKE_RTYPE*)m_eivalues.data() ); \
   m_info = (info==0) ? Success : NoConvergence; \
   m_isInitialized = true; \
   m_eigenvectorsOk = computeEigenvectors; \
   return *this; \
 }
 
+#define EIGEN_LAPACKE_EIG_SELFADJ(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME )              \
+        EIGEN_LAPACKE_EIG_SELFADJ_2(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME, ColMajor )  \
+        EIGEN_LAPACKE_EIG_SELFADJ_2(EIGTYPE, LAPACKE_TYPE, LAPACKE_RTYPE, LAPACKE_NAME, RowMajor ) 
 
-EIGEN_LAPACKE_EIG_SELFADJ(double,   double,                double, dsyev, ColMajor, LAPACK_COL_MAJOR)
-EIGEN_LAPACKE_EIG_SELFADJ(float,    float,                 float,  ssyev, ColMajor, LAPACK_COL_MAJOR)
-EIGEN_LAPACKE_EIG_SELFADJ(dcomplex, lapack_complex_double, double, zheev, ColMajor, LAPACK_COL_MAJOR)
-EIGEN_LAPACKE_EIG_SELFADJ(scomplex, lapack_complex_float,  float,  cheev, ColMajor, LAPACK_COL_MAJOR)
-
-EIGEN_LAPACKE_EIG_SELFADJ(double,   double,                double, dsyev, RowMajor, LAPACK_ROW_MAJOR)
-EIGEN_LAPACKE_EIG_SELFADJ(float,    float,                 float,  ssyev, RowMajor, LAPACK_ROW_MAJOR)
-EIGEN_LAPACKE_EIG_SELFADJ(dcomplex, lapack_complex_double, double, zheev, RowMajor, LAPACK_ROW_MAJOR)
-EIGEN_LAPACKE_EIG_SELFADJ(scomplex, lapack_complex_float,  float,  cheev, RowMajor, LAPACK_ROW_MAJOR)
+EIGEN_LAPACKE_EIG_SELFADJ(double,   double,                double, dsyev)
+EIGEN_LAPACKE_EIG_SELFADJ(float,    float,                 float,  ssyev)
+EIGEN_LAPACKE_EIG_SELFADJ(dcomplex, lapack_complex_double, double, zheev)
+EIGEN_LAPACKE_EIG_SELFADJ(scomplex, lapack_complex_float,  float,  cheev)
 
 } // end namespace Eigen
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h
index 0af3c1b08..83ee1be46 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h
@@ -178,7 +178,7 @@ EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const Quaterni
   if (n != Scalar(0))
   {
     m_angle = Scalar(2)*atan2(n, abs(q.w()));
-    if(q.w() < 0)
+    if(q.w() < Scalar(0))
       n = -n;
     m_axis  = q.vec() / n;
   }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h
index 3e5a9badb..c3fd8c3e0 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h
@@ -43,6 +43,11 @@ class QuaternionBase : public RotationBase<Derived, 3>
   typedef typename internal::traits<Derived>::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef typename internal::traits<Derived>::Coefficients Coefficients;
+  typedef typename Coefficients::CoeffReturnType CoeffReturnType;
+  typedef typename internal::conditional<bool(internal::traits<Derived>::Flags&LvalueBit),
+                                        Scalar&, CoeffReturnType>::type NonConstCoeffReturnType;
+
+
   enum {
     Flags = Eigen::internal::traits<Derived>::Flags
   };
@@ -58,22 +63,22 @@ class QuaternionBase : public RotationBase<Derived, 3>
 
 
   /** \returns the \c x coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar x() const { return this->derived().coeffs().coeff(0); }
+  EIGEN_DEVICE_FUNC inline CoeffReturnType x() const { return this->derived().coeffs().coeff(0); }
   /** \returns the \c y coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar y() const { return this->derived().coeffs().coeff(1); }
+  EIGEN_DEVICE_FUNC inline CoeffReturnType y() const { return this->derived().coeffs().coeff(1); }
   /** \returns the \c z coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar z() const { return this->derived().coeffs().coeff(2); }
+  EIGEN_DEVICE_FUNC inline CoeffReturnType z() const { return this->derived().coeffs().coeff(2); }
   /** \returns the \c w coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar w() const { return this->derived().coeffs().coeff(3); }
+  EIGEN_DEVICE_FUNC inline CoeffReturnType w() const { return this->derived().coeffs().coeff(3); }
 
-  /** \returns a reference to the \c x coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
-  /** \returns a reference to the \c y coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
-  /** \returns a reference to the \c z coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
-  /** \returns a reference to the \c w coefficient */
-  EIGEN_DEVICE_FUNC inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
+  /** \returns a reference to the \c x coefficient (if Derived is a non-const lvalue) */
+  EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType x() { return this->derived().coeffs().x(); }
+  /** \returns a reference to the \c y coefficient (if Derived is a non-const lvalue) */
+  EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType y() { return this->derived().coeffs().y(); }
+  /** \returns a reference to the \c z coefficient (if Derived is a non-const lvalue) */
+  EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType z() { return this->derived().coeffs().z(); }
+  /** \returns a reference to the \c w coefficient (if Derived is a non-const lvalue) */
+  EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType w() { return this->derived().coeffs().w(); }
 
   /** \returns a read-only vector expression of the imaginary part (x,y,z) */
   EIGEN_DEVICE_FUNC inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
index facdaf890..f66c846ef 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
@@ -168,7 +168,7 @@ class LeastSquareDiagonalPreconditioner : public DiagonalPreconditioner<_Scalar>
       {
         for(Index j=0; j<mat.outerSize(); ++j)
         {
-          RealScalar sum = mat.innerVector(j).squaredNorm();
+          RealScalar sum = mat.col(j).squaredNorm();
           if(sum>RealScalar(0))
             m_invdiag(j) = RealScalar(1)/sum;
           else
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
index 395daa8e4..f7ce47134 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
@@ -50,7 +50,8 @@ void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
     tol_error = 0;
     return;
   }
-  RealScalar threshold = tol*tol*rhsNorm2;
+  const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
+  RealScalar threshold = numext::maxi(tol*tol*rhsNorm2,considerAsZero);
   RealScalar residualNorm2 = residual.squaredNorm();
   if (residualNorm2 < threshold)
   {
@@ -58,7 +59,7 @@ void conjugate_gradient(const MatrixType& mat, const Rhs& rhs, Dest& x,
     tol_error = sqrt(residualNorm2 / rhsNorm2);
     return;
   }
-  
+
   VectorType p(n);
   p = precond.solve(residual);      // initial search direction
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h b/gtsam/3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h
index c30326e1d..1998c6322 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h
@@ -65,11 +65,11 @@ template<typename Scalar> class JacobiRotation
     bool makeJacobi(const MatrixBase<Derived>&, Index p, Index q);
     bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
 
-    void makeGivens(const Scalar& p, const Scalar& q, Scalar* z=0);
+    void makeGivens(const Scalar& p, const Scalar& q, Scalar* r=0);
 
   protected:
-    void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::true_type);
-    void makeGivens(const Scalar& p, const Scalar& q, Scalar* z, internal::false_type);
+    void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
+    void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type);
 
     Scalar m_c, m_s;
 };
@@ -84,7 +84,6 @@ bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, co
 {
   using std::sqrt;
   using std::abs;
-  typedef typename NumTraits<Scalar>::Real RealScalar;
   RealScalar deno = RealScalar(2)*abs(y);
   if(deno < (std::numeric_limits<RealScalar>::min)())
   {
@@ -133,7 +132,7 @@ inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Ind
   * \f$ V = \left ( \begin{array}{c} p \\ q \end{array} \right )\f$ yields:
   * \f$ G^* V = \left ( \begin{array}{c} r \\ 0 \end{array} \right )\f$.
   *
-  * The value of \a z is returned if \a z is not null (the default is null).
+  * The value of \a r is returned if \a r is not null (the default is null).
   * Also note that G is built such that the cosine is always real.
   *
   * Example: \include Jacobi_makeGivens.cpp
@@ -146,9 +145,9 @@ inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Ind
   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
   */
 template<typename Scalar>
-void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* z)
+void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r)
 {
-  makeGivens(p, q, z, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
+  makeGivens(p, q, r, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
 }
 
 
@@ -298,16 +297,144 @@ inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiR
 }
 
 namespace internal {
+
+template<typename Scalar, typename OtherScalar,
+         int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
+struct apply_rotation_in_the_plane_selector
+{
+  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
+  {
+    for(Index i=0; i<size; ++i)
+    {
+      Scalar xi = *x;
+      Scalar yi = *y;
+      *x =  c * xi + numext::conj(s) * yi;
+      *y = -s * xi + numext::conj(c) * yi;
+      x += incrx;
+      y += incry;
+    }
+  }
+};
+
+template<typename Scalar, typename OtherScalar,
+         int SizeAtCompileTime, int MinAlignment>
+struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */>
+{
+  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
+  {
+    enum {
+      PacketSize = packet_traits<Scalar>::size,
+      OtherPacketSize = packet_traits<OtherScalar>::size
+    };
+    typedef typename packet_traits<Scalar>::type Packet;
+    typedef typename packet_traits<OtherScalar>::type OtherPacket;
+
+    /*** dynamic-size vectorized paths ***/
+    if(SizeAtCompileTime == Dynamic && ((incrx==1 && incry==1) || PacketSize == 1))
+    {
+      // both vectors are sequentially stored in memory => vectorization
+      enum { Peeling = 2 };
+
+      Index alignedStart = internal::first_default_aligned(y, size);
+      Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
+
+      const OtherPacket pc = pset1<OtherPacket>(c);
+      const OtherPacket ps = pset1<OtherPacket>(s);
+      conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj;
+      conj_helper<OtherPacket,Packet,false,false> pm;
+
+      for(Index i=0; i<alignedStart; ++i)
+      {
+        Scalar xi = x[i];
+        Scalar yi = y[i];
+        x[i] =  c * xi + numext::conj(s) * yi;
+        y[i] = -s * xi + numext::conj(c) * yi;
+      }
+
+      Scalar* EIGEN_RESTRICT px = x + alignedStart;
+      Scalar* EIGEN_RESTRICT py = y + alignedStart;
+
+      if(internal::first_default_aligned(x, size)==alignedStart)
+      {
+        for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
+        {
+          Packet xi = pload<Packet>(px);
+          Packet yi = pload<Packet>(py);
+          pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
+          pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
+          px += PacketSize;
+          py += PacketSize;
+        }
+      }
+      else
+      {
+        Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
+        for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
+        {
+          Packet xi   = ploadu<Packet>(px);
+          Packet xi1  = ploadu<Packet>(px+PacketSize);
+          Packet yi   = pload <Packet>(py);
+          Packet yi1  = pload <Packet>(py+PacketSize);
+          pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
+          pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
+          pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
+          pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
+          px += Peeling*PacketSize;
+          py += Peeling*PacketSize;
+        }
+        if(alignedEnd!=peelingEnd)
+        {
+          Packet xi = ploadu<Packet>(x+peelingEnd);
+          Packet yi = pload <Packet>(y+peelingEnd);
+          pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
+          pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
+        }
+      }
+
+      for(Index i=alignedEnd; i<size; ++i)
+      {
+        Scalar xi = x[i];
+        Scalar yi = y[i];
+        x[i] =  c * xi + numext::conj(s) * yi;
+        y[i] = -s * xi + numext::conj(c) * yi;
+      }
+    }
+
+    /*** fixed-size vectorized path ***/
+    else if(SizeAtCompileTime != Dynamic && MinAlignment>0) // FIXME should be compared to the required alignment
+    {
+      const OtherPacket pc = pset1<OtherPacket>(c);
+      const OtherPacket ps = pset1<OtherPacket>(s);
+      conj_helper<OtherPacket,Packet,NumTraits<OtherPacket>::IsComplex,false> pcj;
+      conj_helper<OtherPacket,Packet,false,false> pm;
+      Scalar* EIGEN_RESTRICT px = x;
+      Scalar* EIGEN_RESTRICT py = y;
+      for(Index i=0; i<size; i+=PacketSize)
+      {
+        Packet xi = pload<Packet>(px);
+        Packet yi = pload<Packet>(py);
+        pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
+        pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
+        px += PacketSize;
+        py += PacketSize;
+      }
+    }
+
+    /*** non-vectorized path ***/
+    else
+    {
+      apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,false>::run(x,incrx,y,incry,size,c,s);
+    }
+  }
+};
+
 template<typename VectorX, typename VectorY, typename OtherScalar>
 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
 {
   typedef typename VectorX::Scalar Scalar;
-  enum {
-    PacketSize = packet_traits<Scalar>::size,
-    OtherPacketSize = packet_traits<OtherScalar>::size
-  };
-  typedef typename packet_traits<Scalar>::type Packet;
-  typedef typename packet_traits<OtherScalar>::type OtherPacket;
+  const bool Vectorizable =    (VectorX::Flags & VectorY::Flags & PacketAccessBit)
+                            && (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
+
   eigen_assert(xpr_x.size() == xpr_y.size());
   Index size = xpr_x.size();
   Index incrx = xpr_x.derived().innerStride();
@@ -321,117 +448,11 @@ void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x
   if (c==OtherScalar(1) && s==OtherScalar(0))
     return;
 
-  /*** dynamic-size vectorized paths ***/
-
-  if(VectorX::SizeAtCompileTime == Dynamic &&
-    (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
-    (PacketSize == OtherPacketSize) &&
-    ((incrx==1 && incry==1) || PacketSize == 1))
-  {
-    // both vectors are sequentially stored in memory => vectorization
-    enum { Peeling = 2 };
-
-    Index alignedStart = internal::first_default_aligned(y, size);
-    Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
-
-    const OtherPacket pc = pset1<OtherPacket>(c);
-    const OtherPacket ps = pset1<OtherPacket>(s);
-    conj_helper<OtherPacket,Packet,NumTraits<OtherScalar>::IsComplex,false> pcj;
-    conj_helper<OtherPacket,Packet,false,false> pm;
-
-    for(Index i=0; i<alignedStart; ++i)
-    {
-      Scalar xi = x[i];
-      Scalar yi = y[i];
-      x[i] =  c * xi + numext::conj(s) * yi;
-      y[i] = -s * xi + numext::conj(c) * yi;
-    }
-
-    Scalar* EIGEN_RESTRICT px = x + alignedStart;
-    Scalar* EIGEN_RESTRICT py = y + alignedStart;
-
-    if(internal::first_default_aligned(x, size)==alignedStart)
-    {
-      for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
-      {
-        Packet xi = pload<Packet>(px);
-        Packet yi = pload<Packet>(py);
-        pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-        pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
-        px += PacketSize;
-        py += PacketSize;
-      }
-    }
-    else
-    {
-      Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
-      for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
-      {
-        Packet xi   = ploadu<Packet>(px);
-        Packet xi1  = ploadu<Packet>(px+PacketSize);
-        Packet yi   = pload <Packet>(py);
-        Packet yi1  = pload <Packet>(py+PacketSize);
-        pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-        pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
-        pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
-        pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
-        px += Peeling*PacketSize;
-        py += Peeling*PacketSize;
-      }
-      if(alignedEnd!=peelingEnd)
-      {
-        Packet xi = ploadu<Packet>(x+peelingEnd);
-        Packet yi = pload <Packet>(y+peelingEnd);
-        pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-        pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
-      }
-    }
-
-    for(Index i=alignedEnd; i<size; ++i)
-    {
-      Scalar xi = x[i];
-      Scalar yi = y[i];
-      x[i] =  c * xi + numext::conj(s) * yi;
-      y[i] = -s * xi + numext::conj(c) * yi;
-    }
-  }
-
-  /*** fixed-size vectorized path ***/
-  else if(VectorX::SizeAtCompileTime != Dynamic &&
-          (VectorX::Flags & VectorY::Flags & PacketAccessBit) &&
-          (PacketSize == OtherPacketSize) &&
-          (EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment)>0)) // FIXME should be compared to the required alignment
-  {
-    const OtherPacket pc = pset1<OtherPacket>(c);
-    const OtherPacket ps = pset1<OtherPacket>(s);
-    conj_helper<OtherPacket,Packet,NumTraits<OtherPacket>::IsComplex,false> pcj;
-    conj_helper<OtherPacket,Packet,false,false> pm;
-    Scalar* EIGEN_RESTRICT px = x;
-    Scalar* EIGEN_RESTRICT py = y;
-    for(Index i=0; i<size; i+=PacketSize)
-    {
-      Packet xi = pload<Packet>(px);
-      Packet yi = pload<Packet>(py);
-      pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
-      pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
-      px += PacketSize;
-      py += PacketSize;
-    }
-  }
-
-  /*** non-vectorized path ***/
-  else
-  {
-    for(Index i=0; i<size; ++i)
-    {
-      Scalar xi = *x;
-      Scalar yi = *y;
-      *x =  c * xi + numext::conj(s) * yi;
-      *y = -s * xi + numext::conj(c) * yi;
-      x += incrx;
-      y += incry;
-    }
-  }
+  apply_rotation_in_the_plane_selector<
+    Scalar,OtherScalar,
+    VectorX::SizeAtCompileTime,
+    EIGEN_PLAIN_ENUM_MIN(evaluator<VectorX>::Alignment, evaluator<VectorY>::Alignment),
+    Vectorizable>::run(x,incrx,y,incry,size,c,s);
 }
 
 } // end namespace internal
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/InverseImpl.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/InverseImpl.h
index 018f99b58..f49f23360 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/LU/InverseImpl.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/InverseImpl.h
@@ -404,7 +404,7 @@ inline void MatrixBase<Derived>::computeInverseWithCheck(
     const RealScalar& absDeterminantThreshold
   ) const
 {
-  RealScalar determinant;
+  Scalar determinant;
   // i'd love to put some static assertions there, but SFINAE means that they have no effect...
   eigen_assert(rows() == cols());
   computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h
index d2ebfd7bb..160d8a523 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h
@@ -64,28 +64,28 @@ namespace internal
     typedef typename _MatrixType::StorageIndex StorageIndex;
   };
   
-  void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, float *vals, int *perm, int * invp, float *x, int nbrhs, int *iparm, double *dparm)
+  inline void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, float *vals, int *perm, int * invp, float *x, int nbrhs, int *iparm, double *dparm)
   {
     if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; }
     if (nbrhs == 0) {x = NULL; nbrhs=1;}
     s_pastix(pastix_data, pastix_comm, n, ptr, idx, vals, perm, invp, x, nbrhs, iparm, dparm); 
   }
   
-  void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, double *vals, int *perm, int * invp, double *x, int nbrhs, int *iparm, double *dparm)
+  inline void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, double *vals, int *perm, int * invp, double *x, int nbrhs, int *iparm, double *dparm)
   {
     if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; }
     if (nbrhs == 0) {x = NULL; nbrhs=1;}
     d_pastix(pastix_data, pastix_comm, n, ptr, idx, vals, perm, invp, x, nbrhs, iparm, dparm); 
   }
   
-  void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, std::complex<float> *vals, int *perm, int * invp, std::complex<float> *x, int nbrhs, int *iparm, double *dparm)
+  inline void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, std::complex<float> *vals, int *perm, int * invp, std::complex<float> *x, int nbrhs, int *iparm, double *dparm)
   {
     if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; }
     if (nbrhs == 0) {x = NULL; nbrhs=1;}
     c_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast<PASTIX_COMPLEX*>(vals), perm, invp, reinterpret_cast<PASTIX_COMPLEX*>(x), nbrhs, iparm, dparm); 
   }
   
-  void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, std::complex<double> *vals, int *perm, int * invp, std::complex<double> *x, int nbrhs, int *iparm, double *dparm)
+  inline void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, std::complex<double> *vals, int *perm, int * invp, std::complex<double> *x, int nbrhs, int *iparm, double *dparm)
   {
     if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; }
     if (nbrhs == 0) {x = NULL; nbrhs=1;}
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h
index d7a4271cb..1134d66e7 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/BDCSVD.h
@@ -11,7 +11,7 @@
 // Copyright (C) 2013 Jean Ceccato <jean.ceccato@ensimag.fr>
 // Copyright (C) 2013 Pierre Zoppitelli <pierre.zoppitelli@ensimag.fr>
 // Copyright (C) 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
-// Copyright (C) 2014-2016 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2014-2017 Gael Guennebaud <gael.guennebaud@inria.fr>
 //
 // Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -696,7 +696,9 @@ typename BDCSVD<MatrixType>::RealScalar BDCSVD<MatrixType>::secularEq(RealScalar
   for(Index i=0; i<m; ++i)
   {
     Index j = perm(i);
-    res += numext::abs2(col0(j)) / ((diagShifted(j) - mu) * (diag(j) + shift + mu));
+    // The following expression could be rewritten to involve only a single division,
+    // but this would make the expression more sensitive to overflow.
+    res += (col0(j) / (diagShifted(j) - mu)) * (col0(j) / (diag(j) + shift + mu));
   }
   return res;
 
@@ -708,9 +710,12 @@ void BDCSVD<MatrixType>::computeSingVals(const ArrayRef& col0, const ArrayRef& d
 {
   using std::abs;
   using std::swap;
+  using std::sqrt;
 
   Index n = col0.size();
   Index actual_n = n;
+  // Note that here actual_n is computed based on col0(i)==0 instead of diag(i)==0 as above
+  // because 1) we have diag(i)==0 => col0(i)==0 and 2) if col0(i)==0, then diag(i) is already a singular value.
   while(actual_n>1 && col0(actual_n-1)==Literal(0)) --actual_n;
 
   for (Index k = 0; k < n; ++k)
@@ -732,7 +737,9 @@ void BDCSVD<MatrixType>::computeSingVals(const ArrayRef& col0, const ArrayRef& d
       right = (diag(actual_n-1) + col0.matrix().norm());
     else
     {
-      // Skip deflated singular values
+      // Skip deflated singular values,
+      // recall that at this stage we assume that z[j]!=0 and all entries for which z[j]==0 have been put aside.
+      // This should be equivalent to using perm[]
       Index l = k+1;
       while(col0(l)==Literal(0)) { ++l; eigen_internal_assert(l<actual_n); }
       right = diag(l);
@@ -818,15 +825,23 @@ void BDCSVD<MatrixType>::computeSingVals(const ArrayRef& col0, const ArrayRef& d
       RealScalar leftShifted, rightShifted;
       if (shift == left)
       {
-        leftShifted = (std::numeric_limits<RealScalar>::min)();
+        // to avoid overflow, we must have mu > max(real_min, |z(k)|/sqrt(real_max)),
+        // the factor 2 is to be more conservative
+        leftShifted = numext::maxi<RealScalar>( (std::numeric_limits<RealScalar>::min)(), Literal(2) * abs(col0(k)) / sqrt((std::numeric_limits<RealScalar>::max)()) );
+
+        // check that we did it right:
+        eigen_internal_assert( (numext::isfinite)( (col0(k)/leftShifted)*(col0(k)/(diag(k)+shift+leftShifted)) ) );
         // I don't understand why the case k==0 would be special there:
-        // if (k == 0) rightShifted = right - left; else 
-        rightShifted = (k==actual_n-1) ? right : ((right - left) * RealScalar(0.6)); // theoretically we can take 0.5, but let's be safe
+        // if (k == 0) rightShifted = right - left; else
+        rightShifted = (k==actual_n-1) ? right : ((right - left) * RealScalar(0.51)); // theoretically we can take 0.5, but let's be safe
       }
       else
       {
-        leftShifted = -(right - left) * RealScalar(0.6);
-        rightShifted = -(std::numeric_limits<RealScalar>::min)();
+        leftShifted = -(right - left) * RealScalar(0.51);
+        if(k+1<n)
+          rightShifted = -numext::maxi<RealScalar>( (std::numeric_limits<RealScalar>::min)(), abs(col0(k+1)) / sqrt((std::numeric_limits<RealScalar>::max)()) );
+        else
+          rightShifted = -(std::numeric_limits<RealScalar>::min)();
       }
       
       RealScalar fLeft = secularEq(leftShifted, col0, diag, perm, diagShifted, shift);
@@ -980,7 +995,7 @@ void BDCSVD<MatrixType>::deflation43(Index firstCol, Index shift, Index i, Index
   Index start = firstCol + shift;
   RealScalar c = m_computed(start, start);
   RealScalar s = m_computed(start+i, start);
-  RealScalar r = sqrt(numext::abs2(c) + numext::abs2(s));
+  RealScalar r = numext::hypot(c,s);
   if (r == Literal(0))
   {
     m_computed(start+i, start+i) = Literal(0);
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_LAPACKE.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_LAPACKE.h
index 50272154f..ff0516f61 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_LAPACKE.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_LAPACKE.h
@@ -61,9 +61,10 @@ JacobiSVD<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>, ColPiv
     u    = (LAPACKE_TYPE*)m_matrixU.data(); \
   } else { ldu=1; u=&dummy; }\
   MatrixType localV; \
-  ldvt = (m_computeFullV) ? internal::convert_index<lapack_int>(m_cols) : (m_computeThinV) ? internal::convert_index<lapack_int>(m_diagSize) : 1; \
+  lapack_int vt_rows = (m_computeFullV) ? internal::convert_index<lapack_int>(m_cols) : (m_computeThinV) ? internal::convert_index<lapack_int>(m_diagSize) : 1; \
   if (computeV()) { \
-    localV.resize(ldvt, m_cols); \
+    localV.resize(vt_rows, m_cols); \
+    ldvt  = internal::convert_index<lapack_int>(localV.outerStride()); \
     vt   = (LAPACKE_TYPE*)localV.data(); \
   } else { ldvt=1; vt=&dummy; }\
   Matrix<LAPACKE_RTYPE, Dynamic, Dynamic> superb; superb.resize(m_diagSize, 1); \
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h
index cc90a3b75..3d1ef373e 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/SVDBase.h
@@ -180,8 +180,10 @@ public:
   RealScalar threshold() const
   {
     eigen_assert(m_isInitialized || m_usePrescribedThreshold);
+    // this temporary is needed to workaround a MSVC issue
+    Index diagSize = (std::max<Index>)(1,m_diagSize);
     return m_usePrescribedThreshold ? m_prescribedThreshold
-                                    : (std::max<Index>)(1,m_diagSize)*NumTraits<Scalar>::epsilon();
+                                    : diagSize*NumTraits<Scalar>::epsilon();
   }
 
   /** \returns true if \a U (full or thin) is asked for in this SVD decomposition */
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h
index 8a5cc91f2..e0295f2af 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h
@@ -94,7 +94,7 @@ class AmbiVector
       Index allocSize = m_allocatedElements * sizeof(ListEl);
       allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar);
       Scalar* newBuffer = new Scalar[allocSize];
-      memcpy(newBuffer,  m_buffer,  copyElements * sizeof(ListEl));
+      std::memcpy(newBuffer,  m_buffer,  copyElements * sizeof(ListEl));
       delete[] m_buffer;
       m_buffer = newBuffer;
     }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
index 492eb0a29..9db119b67 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
@@ -17,7 +17,9 @@ namespace internal {
 template<typename Lhs, typename Rhs, typename ResultType>
 static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res, bool sortedInsertion = false)
 {
-  typedef typename remove_all<Lhs>::type::Scalar Scalar;
+  typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
+  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+  typedef typename remove_all<ResultType>::type::Scalar ResScalar;
 
   // make sure to call innerSize/outerSize since we fake the storage order.
   Index rows = lhs.innerSize();
@@ -25,7 +27,7 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r
   eigen_assert(lhs.outerSize() == rhs.innerSize());
   
   ei_declare_aligned_stack_constructed_variable(bool,   mask,     rows, 0);
-  ei_declare_aligned_stack_constructed_variable(Scalar, values,   rows, 0);
+  ei_declare_aligned_stack_constructed_variable(ResScalar, values,   rows, 0);
   ei_declare_aligned_stack_constructed_variable(Index,  indices,  rows, 0);
   
   std::memset(mask,0,sizeof(bool)*rows);
@@ -51,12 +53,12 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r
     Index nnz = 0;
     for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
     {
-      Scalar y = rhsIt.value();
+      RhsScalar y = rhsIt.value();
       Index k = rhsIt.index();
       for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
       {
         Index i = lhsIt.index();
-        Scalar x = lhsIt.value();
+        LhsScalar x = lhsIt.value();
         if(!mask[i])
         {
           mask[i] = true;
@@ -166,11 +168,12 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,C
 {
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
   {
-     typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
-     RowMajorMatrix rhsRow = rhs;
-     RowMajorMatrix resRow(lhs.rows(), rhs.cols());
-     internal::conservative_sparse_sparse_product_impl<RowMajorMatrix,Lhs,RowMajorMatrix>(rhsRow, lhs, resRow);
-     res = resRow;
+    typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRhs;
+    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+    RowMajorRhs rhsRow = rhs;
+    RowMajorRes resRow(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<RowMajorRhs,Lhs,RowMajorRes>(rhsRow, lhs, resRow);
+    res = resRow;
   }
 };
 
@@ -179,10 +182,11 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,R
 {
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorMatrix;
-    RowMajorMatrix lhsRow = lhs;
-    RowMajorMatrix resRow(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<Rhs,RowMajorMatrix,RowMajorMatrix>(rhs, lhsRow, resRow);
+    typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorLhs;
+    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::StorageIndex> RowMajorRes;
+    RowMajorLhs lhsRow = lhs;
+    RowMajorRes resRow(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Rhs,RowMajorLhs,RowMajorRes>(rhs, lhsRow, resRow);
     res = resRow;
   }
 };
@@ -219,10 +223,11 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,C
 {
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
-    ColMajorMatrix lhsCol = lhs;
-    ColMajorMatrix resCol(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<ColMajorMatrix,Rhs,ColMajorMatrix>(lhsCol, rhs, resCol);
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+    ColMajorLhs lhsCol = lhs;
+    ColMajorRes resCol(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<ColMajorLhs,Rhs,ColMajorRes>(lhsCol, rhs, resCol);
     res = resCol;
   }
 };
@@ -232,10 +237,11 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,R
 {
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
-    ColMajorMatrix rhsCol = rhs;
-    ColMajorMatrix resCol(lhs.rows(), rhs.cols());
-    internal::conservative_sparse_sparse_product_impl<Lhs,ColMajorMatrix,ColMajorMatrix>(lhs, rhsCol, resCol);
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
+    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRes;
+    ColMajorRhs rhsCol = rhs;
+    ColMajorRes resCol(lhs.rows(), rhs.cols());
+    internal::conservative_sparse_sparse_product_impl<Lhs,ColMajorRhs,ColMajorRes>(lhs, rhsCol, resCol);
     res = resCol;
   }
 };
@@ -263,7 +269,8 @@ namespace internal {
 template<typename Lhs, typename Rhs, typename ResultType>
 static void sparse_sparse_to_dense_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res)
 {
-  typedef typename remove_all<Lhs>::type::Scalar Scalar;
+  typedef typename remove_all<Lhs>::type::Scalar LhsScalar;
+  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
   Index cols = rhs.outerSize();
   eigen_assert(lhs.outerSize() == rhs.innerSize());
 
@@ -274,12 +281,12 @@ static void sparse_sparse_to_dense_product_impl(const Lhs& lhs, const Rhs& rhs,
   {
     for (typename evaluator<Rhs>::InnerIterator rhsIt(rhsEval, j); rhsIt; ++rhsIt)
     {
-      Scalar y = rhsIt.value();
+      RhsScalar y = rhsIt.value();
       Index k = rhsIt.index();
       for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, k); lhsIt; ++lhsIt)
       {
         Index i = lhsIt.index();
-        Scalar x = lhsIt.value();
+        LhsScalar x = lhsIt.value();
         res.coeffRef(i,j) += x * y;
       }
     }
@@ -310,9 +317,9 @@ struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMa
 {
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
-    ColMajorMatrix lhsCol(lhs);
-    internal::sparse_sparse_to_dense_product_impl<ColMajorMatrix,Rhs,ResultType>(lhsCol, rhs, res);
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorLhs;
+    ColMajorLhs lhsCol(lhs);
+    internal::sparse_sparse_to_dense_product_impl<ColMajorLhs,Rhs,ResultType>(lhsCol, rhs, res);
   }
 };
 
@@ -321,9 +328,9 @@ struct sparse_sparse_to_dense_product_selector<Lhs,Rhs,ResultType,ColMajor,RowMa
 {
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorMatrix;
-    ColMajorMatrix rhsCol(rhs);
-    internal::sparse_sparse_to_dense_product_impl<Lhs,ColMajorMatrix,ResultType>(lhs, rhsCol, res);
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename ResultType::StorageIndex> ColMajorRhs;
+    ColMajorRhs rhsCol(rhs);
+    internal::sparse_sparse_to_dense_product_impl<Lhs,ColMajorRhs,ResultType>(lhs, rhsCol, res);
   }
 };
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h
index 323c2323b..0a2490bcc 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h
@@ -893,7 +893,7 @@ public:
 
       Index p = m_outerIndex[outer] + m_innerNonZeros[outer]++;
       m_data.index(p) = convert_index(inner);
-      return (m_data.value(p) = 0);
+      return (m_data.value(p) = Scalar(0));
     }
 
 private:
@@ -1274,7 +1274,7 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar&
   m_innerNonZeros[outer]++;
 
   m_data.index(p) = inner;
-  return (m_data.value(p) = 0);
+  return (m_data.value(p) = Scalar(0));
 }
 
 template<typename _Scalar, int _Options, typename _StorageIndex>
@@ -1381,7 +1381,7 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_StorageIndex>::Scalar&
   }
 
   m_data.index(p) = inner;
-  return (m_data.value(p) = 0);
+  return (m_data.value(p) = Scalar(0));
 }
 
 namespace internal {
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h
index 5ab64f1a8..65611b3d4 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h
@@ -311,7 +311,7 @@ inline void sparse_selfadjoint_time_dense_product(const SparseLhsType& lhs, cons
         while (i && i.index()<j) ++i;
         if(i && i.index()==j)
         {
-          res(j,k) += alpha * i.value() * rhs(j,k);
+          res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
           ++i;
         }
       }
@@ -324,14 +324,14 @@ inline void sparse_selfadjoint_time_dense_product(const SparseLhsType& lhs, cons
       {
         LhsScalar lhs_ij = i.value();
         if(!LhsIsRowMajor) lhs_ij = numext::conj(lhs_ij);
-        res_j += lhs_ij * rhs(i.index(),k);
+        res_j += lhs_ij * rhs.coeff(i.index(),k);
         res(i.index(),k) += numext::conj(lhs_ij) * rhs_j;
       }
-      res(j,k) += alpha * res_j;
+      res.coeffRef(j,k) += alpha * res_j;
 
       // handle diagonal coeff
       if (ProcessFirstHalf && i && (i.index()==j))
-        res(j,k) += alpha * i.value() * rhs(j,k);
+        res.coeffRef(j,k) += alpha * i.value() * rhs.coeff(j,k);
     }
   }
 }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
index 21c419002..88820a48f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
@@ -21,7 +21,8 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r
 {
   // return sparse_sparse_product_with_pruning_impl2(lhs,rhs,res);
 
-  typedef typename remove_all<Lhs>::type::Scalar Scalar;
+  typedef typename remove_all<Rhs>::type::Scalar RhsScalar;
+  typedef typename remove_all<ResultType>::type::Scalar ResScalar;
   typedef typename remove_all<Lhs>::type::StorageIndex StorageIndex;
 
   // make sure to call innerSize/outerSize since we fake the storage order.
@@ -31,7 +32,7 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r
   eigen_assert(lhs.outerSize() == rhs.innerSize());
 
   // allocate a temporary buffer
-  AmbiVector<Scalar,StorageIndex> tempVector(rows);
+  AmbiVector<ResScalar,StorageIndex> tempVector(rows);
 
   // mimics a resizeByInnerOuter:
   if(ResultType::IsRowMajor)
@@ -63,14 +64,14 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r
     {
       // FIXME should be written like this: tmp += rhsIt.value() * lhs.col(rhsIt.index())
       tempVector.restart();
-      Scalar x = rhsIt.value();
+      RhsScalar x = rhsIt.value();
       for (typename evaluator<Lhs>::InnerIterator lhsIt(lhsEval, rhsIt.index()); lhsIt; ++lhsIt)
       {
         tempVector.coeffRef(lhsIt.index()) += lhsIt.value() * x;
       }
     }
     res.startVec(j);
-    for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector,tolerance); it; ++it)
+    for (typename AmbiVector<ResScalar,StorageIndex>::Iterator it(tempVector,tolerance); it; ++it)
       res.insertBackByOuterInner(j,it.index()) = it.value();
   }
   res.finalize();
@@ -85,7 +86,6 @@ struct sparse_sparse_product_with_pruning_selector;
 template<typename Lhs, typename Rhs, typename ResultType>
 struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
 {
-  typedef typename traits<typename remove_all<Lhs>::type>::Scalar Scalar;
   typedef typename ResultType::RealScalar RealScalar;
 
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
@@ -129,8 +129,8 @@ struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,R
   typedef typename ResultType::RealScalar RealScalar;
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
     ColMajorMatrixLhs colLhs(lhs);
     ColMajorMatrixRhs colRhs(rhs);
     internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,ColMajorMatrixRhs,ResultType>(colLhs, colRhs, res, tolerance);
@@ -149,7 +149,7 @@ struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,R
   typedef typename ResultType::RealScalar RealScalar;
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixLhs;
+    typedef SparseMatrix<typename Lhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixLhs;
     RowMajorMatrixLhs rowLhs(lhs);
     sparse_sparse_product_with_pruning_selector<RowMajorMatrixLhs,Rhs,ResultType,RowMajor,RowMajor>(rowLhs,rhs,res,tolerance);
   }
@@ -161,7 +161,7 @@ struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,C
   typedef typename ResultType::RealScalar RealScalar;
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixRhs;
+    typedef SparseMatrix<typename Rhs::Scalar,RowMajor,typename Lhs::StorageIndex> RowMajorMatrixRhs;
     RowMajorMatrixRhs rowRhs(rhs);
     sparse_sparse_product_with_pruning_selector<Lhs,RowMajorMatrixRhs,ResultType,RowMajor,RowMajor,RowMajor>(lhs,rowRhs,res,tolerance);
   }
@@ -173,7 +173,7 @@ struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,ColMajor,R
   typedef typename ResultType::RealScalar RealScalar;
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
+    typedef SparseMatrix<typename Rhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixRhs;
     ColMajorMatrixRhs colRhs(rhs);
     internal::sparse_sparse_product_with_pruning_impl<Lhs,ColMajorMatrixRhs,ResultType>(lhs, colRhs, res, tolerance);
   }
@@ -185,7 +185,7 @@ struct sparse_sparse_product_with_pruning_selector<Lhs,Rhs,ResultType,RowMajor,C
   typedef typename ResultType::RealScalar RealScalar;
   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res, const RealScalar& tolerance)
   {
-    typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
+    typedef SparseMatrix<typename Lhs::Scalar,ColMajor,typename Lhs::StorageIndex> ColMajorMatrixLhs;
     ColMajorMatrixLhs colLhs(lhs);
     internal::sparse_sparse_product_with_pruning_impl<ColMajorMatrixLhs,Rhs,ResultType>(colLhs, rhs, res, tolerance);
   }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h
index f883ab383..7104831c0 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h
@@ -499,8 +499,6 @@ void SparseLU<MatrixType, OrderingType>::factorize(const MatrixType& matrix)
   eigen_assert(m_analysisIsOk && "analyzePattern() should be called first"); 
   eigen_assert((matrix.rows() == matrix.cols()) && "Only for squared matrices");
   
-  typedef typename IndexVector::Scalar StorageIndex; 
-  
   m_isInitialized = true;
   
   
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h
index 2d4498b03..7409fcae9 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h
@@ -52,7 +52,7 @@ namespace internal {
   * rank-revealing permutations. Use colsPermutation() to get it.
   * 
   * Q is the orthogonal matrix represented as products of Householder reflectors. 
-  * Use matrixQ() to get an expression and matrixQ().transpose() to get the transpose.
+  * Use matrixQ() to get an expression and matrixQ().adjoint() to get the adjoint.
   * You can then apply it to a vector.
   * 
   * R is the sparse triangular or trapezoidal matrix. The later occurs when A is rank-deficient.
@@ -65,6 +65,7 @@ namespace internal {
   * \implsparsesolverconcept
   *
   * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
+  * \warning For complex matrices matrixQ().transpose() will actually return the adjoint matrix.
   * 
   */
 template<typename _MatrixType, typename _OrderingType>
@@ -196,9 +197,9 @@ class SparseQR : public SparseSolverBase<SparseQR<_MatrixType,_OrderingType> >
 
       Index rank = this->rank();
       
-      // Compute Q^T * b;
+      // Compute Q^* * b;
       typename Dest::PlainObject y, b;
-      y = this->matrixQ().transpose() * B; 
+      y = this->matrixQ().adjoint() * B;
       b = y;
       
       // Solve with the triangular matrix R
@@ -604,7 +605,7 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
   // Get the references 
   SparseQR_QProduct(const SparseQRType& qr, const Derived& other, bool transpose) : 
   m_qr(qr),m_other(other),m_transpose(transpose) {}
-  inline Index rows() const { return m_transpose ? m_qr.rows() : m_qr.cols(); }
+  inline Index rows() const { return m_qr.matrixQ().rows(); }
   inline Index cols() const { return m_other.cols(); }
   
   // Assign to a vector
@@ -632,7 +633,10 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
     }
     else
     {
-      eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
+      eigen_assert(m_qr.matrixQ().cols() == m_other.rows() && "Non conforming object sizes");
+
+      res.conservativeResize(rows(), cols());
+
       // Compute res = Q * other column by column
       for(Index j = 0; j < res.cols(); j++)
       {
@@ -641,7 +645,7 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
           Scalar tau = Scalar(0);
           tau = m_qr.m_Q.col(k).dot(res.col(j));
           if(tau==Scalar(0)) continue;
-          tau = tau * m_qr.m_hcoeffs(k);
+          tau = tau * numext::conj(m_qr.m_hcoeffs(k));
           res.col(j) -= tau * m_qr.m_Q.col(k);
         }
       }
@@ -650,7 +654,7 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
   
   const SparseQRType& m_qr;
   const Derived& m_other;
-  bool m_transpose;
+  bool m_transpose; // TODO this actually means adjoint
 };
 
 template<typename SparseQRType>
@@ -668,13 +672,14 @@ struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<Sp
   {
     return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(),false);
   }
+  // To use for operations with the adjoint of Q
   SparseQRMatrixQTransposeReturnType<SparseQRType> adjoint() const
   {
     return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
   }
   inline Index rows() const { return m_qr.rows(); }
-  inline Index cols() const { return (std::min)(m_qr.rows(),m_qr.cols()); }
-  // To use for operations with the transpose of Q
+  inline Index cols() const { return m_qr.rows(); }
+  // To use for operations with the transpose of Q FIXME this is the same as adjoint at the moment
   SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const
   {
     return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
@@ -682,6 +687,7 @@ struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<Sp
   const SparseQRType& m_qr;
 };
 
+// TODO this actually represents the adjoint of Q
 template<typename SparseQRType>
 struct SparseQRMatrixQTransposeReturnType
 {
@@ -712,7 +718,7 @@ struct Assignment<DstXprType, SparseQRMatrixQReturnType<SparseQRType>, internal:
   typedef typename DstXprType::StorageIndex StorageIndex;
   static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &/*func*/)
   {
-    typename DstXprType::PlainObject idMat(src.m_qr.rows(), src.m_qr.rows());
+    typename DstXprType::PlainObject idMat(src.rows(), src.cols());
     idMat.setIdentity();
     // Sort the sparse householder reflectors if needed
     const_cast<SparseQRType *>(&src.m_qr)->_sort_matrix_Q();
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h
index 50a69f306..7261c7d0f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h
@@ -297,8 +297,8 @@ SluMatrix asSluMatrix(MatrixType& mat)
 template<typename Scalar, int Flags, typename Index>
 MappedSparseMatrix<Scalar,Flags,Index> map_superlu(SluMatrix& sluMat)
 {
-  eigen_assert((Flags&RowMajor)==RowMajor && sluMat.Stype == SLU_NR
-         || (Flags&ColMajor)==ColMajor && sluMat.Stype == SLU_NC);
+  eigen_assert(((Flags&RowMajor)==RowMajor && sluMat.Stype == SLU_NR)
+         || ((Flags&ColMajor)==ColMajor && sluMat.Stype == SLU_NC));
 
   Index outerSize = (Flags&RowMajor)==RowMajor ? sluMat.ncol : sluMat.nrow;
 
diff --git a/gtsam/3rdparty/Eigen/bench/spbench/CMakeLists.txt b/gtsam/3rdparty/Eigen/bench/spbench/CMakeLists.txt
index 932735698..029ba6d6b 100644
--- a/gtsam/3rdparty/Eigen/bench/spbench/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/bench/spbench/CMakeLists.txt
@@ -60,7 +60,7 @@ if(PASTIX_FOUND AND PASTIX_pastix_nompi.h_INCLUDE_DIRS AND BLAS_FOUND)
   endif(SCOTCH_FOUND)
   set(SPARSE_LIBS ${SPARSE_LIBS} ${PASTIX_LIBRARIES_DEP} ${ORDERING_LIBRARIES})
   set(PASTIX_ALL_LIBS ${PASTIX_LIBRARIES_DEP})
-endif(PASTIX_FOUND AND BLAS_FOUND)
+endif()
 
 if(METIS_FOUND)
   include_directories(${METIS_INCLUDE_DIRS})
diff --git a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt
index d0efb4188..9887d5804 100644
--- a/gtsam/3rdparty/Eigen/blas/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/blas/CMakeLists.txt
@@ -45,10 +45,12 @@ install(TARGETS eigen_blas eigen_blas_static
 
 if(EIGEN_Fortran_COMPILER_WORKS)
 
-if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
-  add_subdirectory(testing) # can't do EXCLUDE_FROM_ALL here, breaks CTest
-else()
-  add_subdirectory(testing EXCLUDE_FROM_ALL)
+if(BUILD_TESTING)
+  if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
+    add_subdirectory(testing) # can't do EXCLUDE_FROM_ALL here, breaks CTest
+  else()
+    add_subdirectory(testing EXCLUDE_FROM_ALL)
+  endif()
 endif()
 
 endif()
diff --git a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake
index afc24b5e9..3a824397f 100644
--- a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake
@@ -11,13 +11,15 @@ add_custom_target(buildtests)
 add_custom_target(check COMMAND "ctest")
 add_dependencies(check buildtests)
 
-# check whether /bin/bash exists
-find_file(EIGEN_BIN_BASH_EXISTS "/bin/bash" PATHS "/" NO_DEFAULT_PATH)
+# check whether /bin/bash exists (disabled as not used anymore)
+# find_file(EIGEN_BIN_BASH_EXISTS "/bin/bash" PATHS "/" NO_DEFAULT_PATH)
 
 # This call activates testing and generates the DartConfiguration.tcl
 include(CTest)
 
 set(EIGEN_TEST_BUILD_FLAGS "" CACHE STRING "Options passed to the build command of unit tests")
+set(EIGEN_DASHBOARD_BUILD_TARGET "buildtests" CACHE STRING "Target to be built in dashboard mode, default is buildtests")
+set(EIGEN_CTEST_ERROR_EXCEPTION "" CACHE STRING "Regular expression for build error messages to be filtered out")
 
 # Overwrite default DartConfiguration.tcl such that ctest can build our unit tests.
 # Recall that our unit tests are not in the "all" target, so we have to explicitely ask ctest to build our custom 'buildtests' target.
@@ -28,7 +30,7 @@ string(REGEX MATCH "MakeCommand:.*-- (.*)\nDefaultCTestConfigurationType" EIGEN_
 if(NOT CMAKE_MATCH_1)
 string(REGEX MATCH "MakeCommand:.*[^c]make (.*)\nDefaultCTestConfigurationType" EIGEN_DUMMY ${EIGEN_DART_CONFIG_FILE})
 endif()
-string(REGEX REPLACE "MakeCommand:.*DefaultCTestConfigurationType" "MakeCommand: ${CMAKE_COMMAND} --build . --target buildtests --config \"\${CTEST_CONFIGURATION_TYPE}\" -- ${CMAKE_MATCH_1} ${EIGEN_TEST_BUILD_FLAGS}\nDefaultCTestConfigurationType"
+string(REGEX REPLACE "MakeCommand:.*DefaultCTestConfigurationType" "MakeCommand: ${CMAKE_COMMAND} --build . --target ${EIGEN_DASHBOARD_BUILD_TARGET} --config \"\${CTEST_CONFIGURATION_TYPE}\" -- ${CMAKE_MATCH_1} ${EIGEN_TEST_BUILD_FLAGS}\nDefaultCTestConfigurationType"
        EIGEN_DART_CONFIG_FILE2 ${EIGEN_DART_CONFIG_FILE})
 file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl" ${EIGEN_DART_CONFIG_FILE2})
 
@@ -54,8 +56,3 @@ elseif(MSVC)
 endif(CMAKE_COMPILER_IS_GNUCXX)
 
 
-check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CXX11)
-
-if(EIGEN_TEST_CXX11 AND EIGEN_COMPILER_SUPPORT_CXX11)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-endif()
diff --git a/gtsam/3rdparty/Eigen/cmake/FindBLAS.cmake b/gtsam/3rdparty/Eigen/cmake/FindBLAS.cmake
index 9f74b07fe..e3395bc10 100644
--- a/gtsam/3rdparty/Eigen/cmake/FindBLAS.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/FindBLAS.cmake
@@ -152,7 +152,7 @@ set(_blas_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES})
 
 # Check the language being used
 get_property( _LANGUAGES_ GLOBAL PROPERTY ENABLED_LANGUAGES )
-if( _LANGUAGES_ MATCHES Fortran )
+if( _LANGUAGES_ MATCHES Fortran AND CMAKE_Fortran_COMPILER)
   set( _CHECK_FORTRAN TRUE )
 elseif( (_LANGUAGES_ MATCHES C) OR (_LANGUAGES_ MATCHES CXX) )
   set( _CHECK_FORTRAN FALSE )
diff --git a/gtsam/3rdparty/Eigen/cmake/language_support.cmake b/gtsam/3rdparty/Eigen/cmake/language_support.cmake
index 2f14f30b8..ddba50945 100644
--- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake
@@ -26,7 +26,7 @@ function(workaround_9220 language language_works)
     cmake_minimum_required(VERSION 2.8.0)
     set (CMAKE_Fortran_FLAGS \"${CMAKE_Fortran_FLAGS}\")
     set (CMAKE_EXE_LINKER_FLAGS \"${CMAKE_EXE_LINKER_FLAGS}\")
-    enable_language(${language} OPTIONAL)
+    enable_language(${language})
   ")
   file(REMOVE_RECURSE ${CMAKE_BINARY_DIR}/language_tests/${language})
   file(MAKE_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language})
diff --git a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt
index db413bc65..8ff755988 100644
--- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt
@@ -34,8 +34,8 @@ set(EIGEN_DOXY_PROJECT_NAME             "Eigen-unsupported")
 set(EIGEN_DOXY_OUTPUT_DIRECTORY_SUFFIX  "/unsupported")
 set(EIGEN_DOXY_INPUT                    "\"${Eigen_SOURCE_DIR}/unsupported/Eigen\" \"${Eigen_SOURCE_DIR}/unsupported/doc\"")
 set(EIGEN_DOXY_HTML_COLORSTYLE_HUE      "0")
-# set(EIGEN_DOXY_TAGFILES                 "\"${Eigen_BINARY_DIR}/doc/eigen.doxytags =../\"")
-set(EIGEN_DOXY_TAGFILES                 "")
+set(EIGEN_DOXY_TAGFILES                 "\"${Eigen_BINARY_DIR}/doc/Eigen.doxytags=..\"")
+#set(EIGEN_DOXY_TAGFILES                 "")
 
 configure_file(
   ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in
diff --git a/gtsam/3rdparty/Eigen/doc/CoeffwiseMathFunctionsTable.dox b/gtsam/3rdparty/Eigen/doc/CoeffwiseMathFunctionsTable.dox
index 3ae9420dc..12a565b16 100644
--- a/gtsam/3rdparty/Eigen/doc/CoeffwiseMathFunctionsTable.dox
+++ b/gtsam/3rdparty/Eigen/doc/CoeffwiseMathFunctionsTable.dox
@@ -63,7 +63,7 @@ This also means that, unless specified, if the function \c std::foo is available
   \anchor cwisetable_conj
   a.\link ArrayBase::conjugate conjugate\endlink(); \n
   \link Eigen::conj conj\endlink(a); \n
-  m.\link MatrixBase::conjugate conjugate();
+  m.\link MatrixBase::conjugate conjugate\endlink();
   </td>
   <td><a href="https://en.wikipedia.org/wiki/Complex_conjugate">complex conjugate</a> (\f$ \bar{a_i} \f$),\n
   no-op for real </td>
@@ -133,8 +133,9 @@ This also means that, unless specified, if the function \c std::foo is available
   <td class="code">
   \anchor cwisetable_pow
   a.\link ArrayBase::pow pow\endlink(b); \n
-  \link Eigen::pow pow\endlink(a,b);
+  \link ArrayBase::pow(const Eigen::ArrayBase< Derived > &x, const Eigen::ArrayBase< ExponentDerived > &exponents) pow\endlink(a,b);
   </td>
+  <!-- For some reason Doxygen thinks that pow is in ArrayBase namespace -->
   <td>raises a number to the given power (\f$ a_i ^ {b_i} \f$) \n \c a and \c b can be either an array or scalar.</td>
   <td class="code">
   using <a href="http://en.cppreference.com/w/cpp/numeric/math/pow">std::pow</a>; \n
@@ -271,7 +272,7 @@ This also means that, unless specified, if the function \c std::foo is available
 <tr>
   <td class="code">
   \anchor cwisetable_atan
-  a.\link ArrayBase::atan tan\endlink(); \n
+  a.\link ArrayBase::atan atan\endlink(); \n
   \link Eigen::atan atan\endlink(a);
   </td>
   <td>computes arc tangent (\f$ \tan^{-1} a_i \f$)</td>
diff --git a/gtsam/3rdparty/Eigen/doc/Doxyfile.in b/gtsam/3rdparty/Eigen/doc/Doxyfile.in
index 48bb0a8ec..37948a612 100644
--- a/gtsam/3rdparty/Eigen/doc/Doxyfile.in
+++ b/gtsam/3rdparty/Eigen/doc/Doxyfile.in
@@ -1596,6 +1596,7 @@ PREDEFINED             = EIGEN_EMPTY_STRUCT \
                          "EIGEN_CAT2(a,b)= a ## b"\
                          "EIGEN_CAT(a,b)=EIGEN_CAT2(a,b)"\
                          "EIGEN_CWISE_BINARY_RETURN_TYPE(LHS,RHS,OPNAME)=CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(internal::scalar_,OPNAME),_op)<LHS::Scalar, RHS::Scalar>, const LHS, const RHS>"\
+                         "EIGEN_ALIGN_TO_BOUNDARY(x)="\
                          DOXCOMMA=,
 
 
@@ -1618,6 +1619,9 @@ EXPAND_AS_DEFINED      = EIGEN_MAKE_TYPEDEFS \
                          EIGEN_EULER_ANGLES_TYPEDEFS \
                          EIGEN_EULER_ANGLES_SINGLE_TYPEDEF \
                          EIGEN_EULER_SYSTEM_TYPEDEF \
+                         EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY \
+                         EIGEN_MATRIX_FUNCTION \
+                         EIGEN_MATRIX_FUNCTION_1 \
                          EIGEN_DOC_UNARY_ADDONS \
                          EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL \
                          EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
@@ -1665,7 +1669,7 @@ ALLEXTERNALS           = NO
 # in the modules index. If set to NO, only the current project's groups will
 # be listed.
 
-EXTERNAL_GROUPS        = YES
+EXTERNAL_GROUPS        = NO
 
 # The PERL_PATH should be the absolute path and name of the perl script
 # interpreter (i.e. the result of `which perl').
diff --git a/gtsam/3rdparty/Eigen/doc/LeastSquares.dox b/gtsam/3rdparty/Eigen/doc/LeastSquares.dox
index e2191a22f..24dfe4b4f 100644
--- a/gtsam/3rdparty/Eigen/doc/LeastSquares.dox
+++ b/gtsam/3rdparty/Eigen/doc/LeastSquares.dox
@@ -16,7 +16,7 @@ equations is the fastest but least accurate, and the QR decomposition is in betw
 
 \section LeastSquaresSVD Using the SVD decomposition
 
-The \link JacobiSVD::solve() solve() \endlink method in the JacobiSVD class can be directly used to
+The \link BDCSVD::solve() solve() \endlink method in the BDCSVD class can be directly used to
 solve linear squares systems. It is not enough to compute only the singular values (the default for
 this class); you also need the singular vectors but the thin SVD decomposition suffices for
 computing least squares solutions:
diff --git a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox
index cf42effef..3f395053d 100644
--- a/gtsam/3rdparty/Eigen/doc/Pitfalls.dox
+++ b/gtsam/3rdparty/Eigen/doc/Pitfalls.dox
@@ -2,10 +2,16 @@ namespace Eigen {
 
 /** \page TopicPitfalls Common pitfalls
 
+
 \section TopicPitfalls_template_keyword Compilation error with template methods
 
 See this \link TopicTemplateKeyword page \endlink.
 
+\section TopicPitfalls_aliasing Aliasing
+
+Don't miss this \link TopicAliasing page \endlink on aliasing,
+especially if you got wrong results in statements where the destination appears on the right hand side of the expression.
+
 \section TopicPitfalls_auto_keyword C++11 and the auto keyword
 
 In short: do not use the auto keywords with Eigen's expressions, unless you are 100% sure about what you are doing. In particular, do not use the auto keyword as a replacement for a Matrix<> type. Here is an example:
diff --git a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox
index f01b39aec..40cceb944 100644
--- a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox
+++ b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox
@@ -120,6 +120,10 @@ run time. However, these assertions do cost time and can thus be turned off.
  - \b \c EIGEN_STACK_ALLOCATION_LIMIT - defines the maximum bytes for a buffer to be allocated on the stack. For internal
    temporary buffers, dynamic memory allocation is employed as a fall back. For fixed-size matrices or arrays, exceeding
    this threshold raises a compile time assertion. Use 0 to set no limit. Default is 128 KB.
+ - \b \c EIGEN_STRONG_INLINE - This macro is used to qualify critical functions and methods that we expect the compiler to inline.
+   By default it is defined to \c __forceinline for MSVC and ICC, and to \c inline for other compilers. A tipical usage is to
+   define it to \c inline for MSVC users wanting faster compilation times, at the risk of performance degradations in some rare
+   cases for which MSVC inliner fails to do a good job.
 
 
  - \c EIGEN_DONT_ALIGN - Deprecated, it is a synonym for \c EIGEN_MAX_ALIGN_BYTES=0. It disables alignment completely. %Eigen will not try to align its objects and does not expect that any objects passed to it are aligned. This will turn off vectorization if \b EIGEN_UNALIGNED_VECTORIZE=1. Not defined by default.
diff --git a/gtsam/3rdparty/Eigen/doc/TopicLinearAlgebraDecompositions.dox b/gtsam/3rdparty/Eigen/doc/TopicLinearAlgebraDecompositions.dox
index 491470627..d9db67755 100644
--- a/gtsam/3rdparty/Eigen/doc/TopicLinearAlgebraDecompositions.dox
+++ b/gtsam/3rdparty/Eigen/doc/TopicLinearAlgebraDecompositions.dox
@@ -4,7 +4,7 @@ namespace Eigen {
 
 This page presents a catalogue of the dense matrix decompositions offered by Eigen.
 For an introduction on linear solvers and decompositions, check this \link TutorialLinearAlgebra page \endlink.
-To get an overview of the true relative speed of the different decomposition, check this \link DenseDecompositionBenchmark benchmark \endlink.
+To get an overview of the true relative speed of the different decompositions, check this \link DenseDecompositionBenchmark benchmark \endlink.
 
 \section TopicLinAlgBigTable Catalogue of decompositions offered by Eigen
 
@@ -113,6 +113,18 @@ To get an overview of the true relative speed of the different decomposition, ch
 
     <tr><th class="inter" colspan="9">\n Singular values and eigenvalues decompositions</th></tr>
 
+    <tr>
+        <td>BDCSVD (divide \& conquer)</td>
+        <td>-</td>
+        <td>One of the fastest SVD algorithms</td>
+        <td>Excellent</td>
+        <td>Yes</td>
+        <td>Singular values/vectors, least squares</td>
+        <td>Yes (and does least squares)</td>
+        <td>Excellent</td>
+        <td>Blocked bidiagonalization</td>
+    </tr>
+
     <tr>
         <td>JacobiSVD (two-sided)</td>
         <td>-</td>
diff --git a/gtsam/3rdparty/Eigen/doc/TutorialLinearAlgebra.dox b/gtsam/3rdparty/Eigen/doc/TutorialLinearAlgebra.dox
index cb92ceeae..a72724143 100644
--- a/gtsam/3rdparty/Eigen/doc/TutorialLinearAlgebra.dox
+++ b/gtsam/3rdparty/Eigen/doc/TutorialLinearAlgebra.dox
@@ -73,7 +73,7 @@ depending on your matrix and the trade-off you want to make:
         <td>ColPivHouseholderQR</td>
         <td>colPivHouseholderQr()</td>
         <td>None</td>
-        <td>++</td>
+        <td>+</td>
         <td>-</td>
         <td>+++</td>
     </tr>
@@ -85,6 +85,14 @@ depending on your matrix and the trade-off you want to make:
         <td>- -</td>
         <td>+++</td>
     </tr>
+    <tr class="alt">
+        <td>CompleteOrthogonalDecomposition</td>
+        <td>completeOrthogonalDecomposition()</td>
+        <td>None</td>
+        <td>+</td>
+        <td>-</td>
+        <td>+++</td>
+    </tr>
     <tr class="alt">
         <td>LLT</td>
         <td>llt()</td>
@@ -101,15 +109,24 @@ depending on your matrix and the trade-off you want to make:
         <td>+</td>
         <td>++</td>
     </tr>
+    <tr class="alt">
+        <td>BDCSVD</td>
+        <td>bdcSvd()</td>
+        <td>None</td>
+        <td>-</td>
+        <td>-</td>
+        <td>+++</td>
+    </tr>
     <tr class="alt">
         <td>JacobiSVD</td>
         <td>jacobiSvd()</td>
         <td>None</td>
-        <td>- -</td>
+        <td>-</td>
         <td>- - -</td>
         <td>+++</td>
     </tr>
 </table>
+To get an overview of the true relative speed of the different decompositions, check this \link DenseDecompositionBenchmark benchmark \endlink.
 
 All of these decompositions offer a solve() method that works as in the above example.
 
@@ -183,8 +200,11 @@ Here is an example:
 
 \section TutorialLinAlgLeastsquares Least squares solving
 
-The most accurate method to do least squares solving is with a SVD decomposition. Eigen provides one
-as the JacobiSVD class, and its solve() is doing least-squares solving.
+The most accurate method to do least squares solving is with a SVD decomposition.
+Eigen provides two implementations.
+The recommended one is the BDCSVD class, which scale well for large problems
+and automatically fall-back to the JacobiSVD class for smaller problems.
+For both classes, their solve() method is doing least-squares solving.
 
 Here is an example:
 <table class="example">
diff --git a/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox b/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox
index a1a3a18f2..6de14afad 100644
--- a/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox
+++ b/gtsam/3rdparty/Eigen/doc/UsingIntelMKL.dox
@@ -63,6 +63,8 @@ In addition you can choose which parts will be substituted by defining one or mu
 <tr><td>\c EIGEN_USE_MKL_ALL </td><td>Defines \c EIGEN_USE_BLAS, \c EIGEN_USE_LAPACKE, and \c EIGEN_USE_MKL_VML </td></tr>
 </table>
 
+The options can be combined with \b MKL_DIRECT_CALL to enable MKL direct call feature. This may help to increase performance of some MKL BLAS (?GEMM, ?GEMV, ?TRSM, ?AXPY and ?DOT) and LAPACK (LU, Cholesky and QR) routines for very small matrices. To make it work properly, the macro \c EIGEN_USE_MKL must also be defined in the case none of the other \c EIGEN_USE_MKL_* macros has been defined.
+
 Note that the BLAS and LAPACKE backends can be enabled for any F77 compatible BLAS and LAPACK libraries. See this \link TopicUsingBlasLapack page \endlink for the details.
 
 Finally, the PARDISO sparse solver shipped with Intel MKL can be used through the \ref PardisoLU, \ref PardisoLLT and \ref PardisoLDLT classes of the \ref PardisoSupport_Module.
diff --git a/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js b/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js
index bd7e02b38..a6f8c3428 100644
--- a/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js
+++ b/gtsam/3rdparty/Eigen/doc/eigen_navtree_hacks.js
@@ -64,14 +64,20 @@ function getNode(o, po)
 // Overloaded to adjust the size of the navtree wrt the toc
 function resizeHeight() 
 {
-  var toc = $("#nav-toc");
-  var tocHeight = toc.height();  // <- we added this line
-  var headerHeight = header.height();
-  var footerHeight = footer.height();
+  var header  = $("#top");
+  var sidenav = $("#side-nav");
+  var content = $("#doc-content");
+  var navtree = $("#nav-tree");
+  var footer  = $("#nav-path");
+  var toc     = $("#nav-toc");
+
+  var headerHeight = header.outerHeight();
+  var footerHeight = footer.outerHeight();
+  var tocHeight    = toc.height();
   var windowHeight = $(window).height() - headerHeight - footerHeight;
   content.css({height:windowHeight + "px"});
-  navtree.css({height:(windowHeight-tocHeight) + "px"}); // <- we modified this line
-  sidenav.css({height:(windowHeight) + "px",top: headerHeight+"px"});
+  navtree.css({height:(windowHeight-tocHeight) + "px"});
+  sidenav.css({height:windowHeight + "px"});
 }
 
 // Overloaded to save the root node into global_navtree_object
@@ -155,19 +161,18 @@ function createIndent(o,domNode,node,level)
   var level=-2; // <- we replaced level=-1 by level=-2
   var n = node;
   while (n.parentNode) { level++; n=n.parentNode; }
-  var imgNode = document.createElement("img");
-  imgNode.style.paddingLeft=(16*(level)).toString()+'px';
-  imgNode.width  = 16;
-  imgNode.height = 22;
-  imgNode.border = 0;
   if (checkChildrenData(node)) { // <- we modified this line to use checkChildrenData(node) instead of node.childrenData
+    var imgNode = document.createElement("span");
+    imgNode.className = 'arrow';
+    imgNode.style.paddingLeft=(16*level).toString()+'px';
+    imgNode.innerHTML=arrowRight;
     node.plus_img = imgNode;
     node.expandToggle = document.createElement("a");
     node.expandToggle.href = "javascript:void(0)";
     node.expandToggle.onclick = function() {
       if (node.expanded) {
         $(node.getChildrenUL()).slideUp("fast");
-        node.plus_img.src = node.relpath+"ftv2pnode.png";
+        node.plus_img.innerHTML=arrowRight;
         node.expanded = false;
       } else {
         expandNode(o, node, false, false);
@@ -175,11 +180,13 @@ function createIndent(o,domNode,node,level)
     }
     node.expandToggle.appendChild(imgNode);
     domNode.appendChild(node.expandToggle);
-    imgNode.src = node.relpath+"ftv2pnode.png";
   } else {
-    imgNode.src = node.relpath+"ftv2node.png";
-    domNode.appendChild(imgNode);
-  } 
+    var span = document.createElement("span");
+    span.className = 'arrow';
+    span.style.width   = 16*(level+1)+'px';
+    span.innerHTML = '&#160;';
+    domNode.appendChild(span);
+  }
 }
 
 // Overloaded to automatically expand the selected node
@@ -233,8 +240,7 @@ $(document).ready(function() {
       setTimeout(arguments.callee, 10);
     }
   })();
+
+  $(window).load(resizeHeight);
 });
 
-$(window).load(function() {
-  resizeHeight();
-});
diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy.css b/gtsam/3rdparty/Eigen/doc/eigendoxy.css
index 6274e6c70..225f5d32e 100644
--- a/gtsam/3rdparty/Eigen/doc/eigendoxy.css
+++ b/gtsam/3rdparty/Eigen/doc/eigendoxy.css
@@ -165,6 +165,8 @@ div.toc {
   bottom:0;
   border-radius:0px;
   border-style: solid none none none;
+  max-height:50%;
+  overflow-y: scroll;
 }
 
 div.toc h3 {
@@ -214,3 +216,8 @@ h3.version {
 td.width20em p.endtd {
   width:  20em;
 }
+
+/* needed for huge screens */
+.ui-resizable-e {
+  background-repeat: repeat-y;
+}
\ No newline at end of file
diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in
index 878244a19..9ac0596cb 100644
--- a/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in
+++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_footer.html.in
@@ -5,14 +5,14 @@
     $navpath
     <li class="footer">$generatedby
     <a href="http://www.doxygen.org/index.html">
-    <img class="footer" src="$relpath$doxygen.png" alt="doxygen"/></a> $doxygenversion </li>
+    <img class="footer" src="$relpath^doxygen.png" alt="doxygen"/></a> $doxygenversion </li>
   </ul>
 </div>
 <!--END GENERATE_TREEVIEW-->
 <!--BEGIN !GENERATE_TREEVIEW-->
 <hr class="footer"/><address class="footer"><small>
 $generatedby &#160;<a href="http://www.doxygen.org/index.html">
-<img class="footer" src="$relpath$doxygen.png" alt="doxygen"/>
+<img class="footer" src="$relpath^doxygen.png" alt="doxygen"/>
 </a> $doxygenversion
 </small></address>
 <!--END !GENERATE_TREEVIEW-->
diff --git a/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in b/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in
index 0f3859f40..bb149f8f0 100644
--- a/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in
+++ b/gtsam/3rdparty/Eigen/doc/eigendoxy_header.html.in
@@ -4,25 +4,23 @@
 <meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
 <meta http-equiv="X-UA-Compatible" content="IE=9"/>
 <meta name="generator" content="Doxygen $doxygenversion"/>
+<meta name="viewport" content="width=device-width, initial-scale=1"/>
 <!--BEGIN PROJECT_NAME--><title>$projectname: $title</title><!--END PROJECT_NAME-->
 <!--BEGIN !PROJECT_NAME--><title>$title</title><!--END !PROJECT_NAME-->
-<link href="$relpath$tabs.css" rel="stylesheet" type="text/css"/>
-<script type="text/javascript" src="$relpath$jquery.js"></script>
-<script type="text/javascript" src="$relpath$dynsections.js"></script>
+<link href="$relpath^tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="$relpath^jquery.js"></script>
+<script type="text/javascript" src="$relpath^dynsections.js"></script>
 $treeview
 $search
 $mathjax
-<link href="$relpath$$stylesheet"   rel="stylesheet" type="text/css" />
+<link href="$relpath^$stylesheet" rel="stylesheet" type="text/css" />
 <link href="$relpath$eigendoxy.css" rel="stylesheet" type="text/css">
 <!-- $extrastylesheet -->
 <script type="text/javascript" src="$relpath$eigen_navtree_hacks.js"></script>
-<!-- <script type="text/javascript"> -->
-<!-- </script> -->
 
 </head>
 <body>
 <div id="top"><!-- do not remove this div, it is closed by doxygen! -->
-<!-- <a name="top"></a> -->
 
 <!--BEGIN TITLEAREA-->
 <div id="titlearea">
@@ -30,10 +28,10 @@ $mathjax
  <tbody>
  <tr style="height: 56px;">
   <!--BEGIN PROJECT_LOGO-->
-  <td id="projectlogo"><img alt="Logo" src="$relpath$$projectlogo"/></td>
+  <td id="projectlogo"><img alt="Logo" src="$relpath^$projectlogo"/></td>
   <!--END PROJECT_LOGO-->
   <!--BEGIN PROJECT_NAME-->
-  <td style="padding-left: 0.5em;">
+  <td id="projectalign" style="padding-left: 0.5em;">
    <div id="projectname"><a href="http://eigen.tuxfamily.org">$projectname</a>
    <!--BEGIN PROJECT_NUMBER-->&#160;<span id="projectnumber">$projectnumber</span><!--END PROJECT_NUMBER-->
    </div>
@@ -42,7 +40,7 @@ $mathjax
   <!--END PROJECT_NAME-->
   <!--BEGIN !PROJECT_NAME-->
    <!--BEGIN PROJECT_BRIEF-->
-    <td style="padding-left: 0.5em;">
+    <td id="projectalign" style="padding-left: 0.5em;">
     <div id="projectbrief">$projectbrief</div>
     </td>
    <!--END PROJECT_BRIEF-->
diff --git a/gtsam/3rdparty/Eigen/doc/examples/Cwise_lgamma.cpp b/gtsam/3rdparty/Eigen/doc/examples/Cwise_lgamma.cpp
index f1c4f503e..6bfaccbce 100644
--- a/gtsam/3rdparty/Eigen/doc/examples/Cwise_lgamma.cpp
+++ b/gtsam/3rdparty/Eigen/doc/examples/Cwise_lgamma.cpp
@@ -6,4 +6,4 @@ int main()
 {
   Array4d v(0.5,10,0,-1);
   std::cout << v.lgamma() << std::endl;
-}
\ No newline at end of file
+}
diff --git a/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSVDSolve.cpp b/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSVDSolve.cpp
index 9fbc031de..f109f04e5 100644
--- a/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSVDSolve.cpp
+++ b/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSVDSolve.cpp
@@ -11,5 +11,5 @@ int main()
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "The least-squares solution is:\n"
-        << A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
+        << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
 }
diff --git a/gtsam/3rdparty/Eigen/doc/examples/Tutorial_simple_example_dynamic_size.cpp b/gtsam/3rdparty/Eigen/doc/examples/Tutorial_simple_example_dynamic_size.cpp
index 0f0280e0e..defcb1ee4 100644
--- a/gtsam/3rdparty/Eigen/doc/examples/Tutorial_simple_example_dynamic_size.cpp
+++ b/gtsam/3rdparty/Eigen/doc/examples/Tutorial_simple_example_dynamic_size.cpp
@@ -10,7 +10,7 @@ int main()
     MatrixXi m(size,size+1);         // a (size)x(size+1)-matrix of int's
     for (int j=0; j<m.cols(); ++j)   // loop over columns
       for (int i=0; i<m.rows(); ++i) // loop over rows
-        m(i,j) = i+j*m.rows();       // to access matrix coefficients,
+        m(i,j) = i+j*size;           // to access matrix coefficients,
                                      // use operator()(int,int)
     std::cout << m << "\n\n";
   }
diff --git a/gtsam/3rdparty/Eigen/doc/examples/matrixfree_cg.cpp b/gtsam/3rdparty/Eigen/doc/examples/matrixfree_cg.cpp
index 6a205aea3..74699381c 100644
--- a/gtsam/3rdparty/Eigen/doc/examples/matrixfree_cg.cpp
+++ b/gtsam/3rdparty/Eigen/doc/examples/matrixfree_cg.cpp
@@ -67,6 +67,7 @@ namespace internal {
       // This method should implement "dst += alpha * lhs * rhs" inplace,
       // however, for iterative solvers, alpha is always equal to 1, so let's not bother about it.
       assert(alpha==Scalar(1) && "scaling is not implemented");
+      EIGEN_ONLY_USED_FOR_DEBUG(alpha);
 
       // Here we could simply call dst.noalias() += lhs.my_matrix() * rhs,
       // but let's do something fancier (and less efficient):
diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseEqual.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseEqual.cpp
index eb3656f4c..469af642c 100644
--- a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseEqual.cpp
+++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseEqual.cpp
@@ -3,5 +3,5 @@ m << 1, 0,
      1, 1;
 cout << "Comparing m with identity matrix:" << endl;
 cout << m.cwiseEqual(MatrixXi::Identity(2,2)) << endl;
-int count = m.cwiseEqual(MatrixXi::Identity(2,2)).count();
+Index count = m.cwiseEqual(MatrixXi::Identity(2,2)).count();
 cout << "Number of coefficients that are equal: " << count << endl;
diff --git a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseNotEqual.cpp b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseNotEqual.cpp
index 6a2e4fb6c..7f0a105d6 100644
--- a/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseNotEqual.cpp
+++ b/gtsam/3rdparty/Eigen/doc/snippets/MatrixBase_cwiseNotEqual.cpp
@@ -3,5 +3,5 @@ m << 1, 0,
      1, 1;
 cout << "Comparing m with identity matrix:" << endl;
 cout << m.cwiseNotEqual(MatrixXi::Identity(2,2)) << endl;
-int count = m.cwiseNotEqual(MatrixXi::Identity(2,2)).count();
+Index count = m.cwiseNotEqual(MatrixXi::Identity(2,2)).count();
 cout << "Number of coefficients that are not equal: " << count << endl;
diff --git a/gtsam/3rdparty/Eigen/doc/snippets/Matrix_Map_stride.cpp b/gtsam/3rdparty/Eigen/doc/snippets/Matrix_Map_stride.cpp
new file mode 100644
index 000000000..ae42a127a
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/doc/snippets/Matrix_Map_stride.cpp
@@ -0,0 +1,7 @@
+Matrix4i A;
+A << 1,  2,  3,  4,
+     5,  6,  7,  8,
+     9, 10, 11, 12,
+    13, 14, 15, 16;
+
+std::cout << Matrix2i::Map(&A(1,1),Stride<8,2>()) << std::endl;
diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt
index 101fbc5f9..66ba4deee 100644
--- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt
@@ -19,7 +19,6 @@ if(QT4_FOUND)
   add_dependencies(all_examples Tutorial_sparse_example)
 endif(QT4_FOUND)
 
-check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11)
 if(EIGEN_COMPILER_SUPPORT_CPP11)
   add_executable(random_cpp11 random_cpp11.cpp)
   target_link_libraries(random_cpp11 ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO})
diff --git a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example.cpp b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example.cpp
index 830e196ea..c5767a8d3 100644
--- a/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example.cpp
+++ b/gtsam/3rdparty/Eigen/doc/special_examples/Tutorial_sparse_example.cpp
@@ -1,5 +1,6 @@
 #include <Eigen/Sparse>
 #include <vector>
+#include <iostream>
 
 typedef Eigen::SparseMatrix<double> SpMat; // declares a column-major sparse matrix type of double
 typedef Eigen::Triplet<double> T;
@@ -9,7 +10,10 @@ void saveAsBitmap(const Eigen::VectorXd& x, int n, const char* filename);
 
 int main(int argc, char** argv)
 {
-  assert(argc==2);
+  if(argc!=2) {
+    std::cerr << "Error: expected one and only one argument.\n";
+    return -1;
+  }
   
   int n = 300;  // size of the image
   int m = n*n;  // number of unknows (=number of pixels)
diff --git a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt
index 9883d4c72..6df1fa958 100644
--- a/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/lapack/CMakeLists.txt
@@ -49,7 +49,7 @@ if(EIGEN_ENABLE_LAPACK_TESTS)
                   INACTIVITY_TIMEOUT 15
                   TIMEOUT 240
                   STATUS download_status
-                  EXPECTED_MD5 5758ce55afcf79da98de8b9de1615ad5
+                  EXPECTED_MD5 ab5742640617e3221a873aba44bbdc93
                   SHOW_PROGRESS)
                   
     message(STATUS ${download_status})
diff --git a/gtsam/3rdparty/Eigen/test/adjoint.cpp b/gtsam/3rdparty/Eigen/test/adjoint.cpp
index bdea51c10..37032d220 100644
--- a/gtsam/3rdparty/Eigen/test/adjoint.cpp
+++ b/gtsam/3rdparty/Eigen/test/adjoint.cpp
@@ -70,7 +70,6 @@ template<typename MatrixType> void adjoint(const MatrixType& m)
      Transpose.h Conjugate.h Dot.h
   */
   using std::abs;
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
diff --git a/gtsam/3rdparty/Eigen/test/array.cpp b/gtsam/3rdparty/Eigen/test/array.cpp
index 15c3266a9..7afd3ed3f 100644
--- a/gtsam/3rdparty/Eigen/test/array.cpp
+++ b/gtsam/3rdparty/Eigen/test/array.cpp
@@ -11,7 +11,6 @@
 
 template<typename ArrayType> void array(const ArrayType& m)
 {
-  typedef typename ArrayType::Index Index;
   typedef typename ArrayType::Scalar Scalar;
   typedef typename ArrayType::RealScalar RealScalar;
   typedef Array<Scalar, ArrayType::RowsAtCompileTime, 1> ColVectorType;
@@ -130,7 +129,6 @@ template<typename ArrayType> void array(const ArrayType& m)
 template<typename ArrayType> void comparisons(const ArrayType& m)
 {
   using std::abs;
-  typedef typename ArrayType::Index Index;
   typedef typename ArrayType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
 
@@ -208,7 +206,6 @@ template<typename ArrayType> void array_real(const ArrayType& m)
 {
   using std::abs;
   using std::sqrt;
-  typedef typename ArrayType::Index Index;
   typedef typename ArrayType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
 
@@ -319,7 +316,6 @@ template<typename ArrayType> void array_real(const ArrayType& m)
 
 template<typename ArrayType> void array_complex(const ArrayType& m)
 {
-  typedef typename ArrayType::Index Index;
   typedef typename ArrayType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
 
@@ -424,7 +420,6 @@ template<typename ArrayType> void array_complex(const ArrayType& m)
 
 template<typename ArrayType> void min_max(const ArrayType& m)
 {
-  typedef typename ArrayType::Index Index;
   typedef typename ArrayType::Scalar Scalar;
 
   Index rows = m.rows();
diff --git a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp
index b8721391f..a05bba191 100644
--- a/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp
+++ b/gtsam/3rdparty/Eigen/test/array_for_matrix.cpp
@@ -11,7 +11,6 @@
 
 template<typename MatrixType> void array_for_matrix(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVectorType;
   typedef Matrix<Scalar, 1, MatrixType::ColsAtCompileTime> RowVectorType; 
@@ -83,7 +82,6 @@ template<typename MatrixType> void array_for_matrix(const MatrixType& m)
 template<typename MatrixType> void comparisons(const MatrixType& m)
 {
   using std::abs;
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
 
@@ -172,7 +170,6 @@ template<typename VectorType> void lpNorm(const VectorType& v)
 
 template<typename MatrixType> void cwise_min_max(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = m.rows();
@@ -211,7 +208,6 @@ template<typename MatrixType> void cwise_min_max(const MatrixType& m)
 
 template<typename MatrixTraits> void resize(const MatrixTraits& t)
 {
-  typedef typename MatrixTraits::Index Index;
   typedef typename MatrixTraits::Scalar Scalar;
   typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
   typedef Array<Scalar,Dynamic,Dynamic> Array2DType;
diff --git a/gtsam/3rdparty/Eigen/test/array_replicate.cpp b/gtsam/3rdparty/Eigen/test/array_replicate.cpp
index 779c8fc2f..0dad5bace 100644
--- a/gtsam/3rdparty/Eigen/test/array_replicate.cpp
+++ b/gtsam/3rdparty/Eigen/test/array_replicate.cpp
@@ -14,7 +14,6 @@ template<typename MatrixType> void replicate(const MatrixType& m)
   /* this test covers the following files:
      Replicate.cpp
   */
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
   typedef Matrix<Scalar, Dynamic, Dynamic> MatrixX;
diff --git a/gtsam/3rdparty/Eigen/test/array_reverse.cpp b/gtsam/3rdparty/Eigen/test/array_reverse.cpp
index c9d9f90c3..9d5b9a66d 100644
--- a/gtsam/3rdparty/Eigen/test/array_reverse.cpp
+++ b/gtsam/3rdparty/Eigen/test/array_reverse.cpp
@@ -15,7 +15,6 @@ using namespace std;
 
 template<typename MatrixType> void reverse(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
 
diff --git a/gtsam/3rdparty/Eigen/test/basicstuff.cpp b/gtsam/3rdparty/Eigen/test/basicstuff.cpp
index 99d91f9da..2e532f7a5 100644
--- a/gtsam/3rdparty/Eigen/test/basicstuff.cpp
+++ b/gtsam/3rdparty/Eigen/test/basicstuff.cpp
@@ -13,7 +13,6 @@
 
 template<typename MatrixType> void basicStuff(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;
@@ -144,7 +143,6 @@ template<typename MatrixType> void basicStuff(const MatrixType& m)
 
 template<typename MatrixType> void basicStuffComplex(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef Matrix<RealScalar, MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime> RealMatrixType;
diff --git a/gtsam/3rdparty/Eigen/test/bdcsvd.cpp b/gtsam/3rdparty/Eigen/test/bdcsvd.cpp
index f9f687aac..6c7b09696 100644
--- a/gtsam/3rdparty/Eigen/test/bdcsvd.cpp
+++ b/gtsam/3rdparty/Eigen/test/bdcsvd.cpp
@@ -104,7 +104,8 @@ void test_bdcsvd()
   CALL_SUBTEST_7( BDCSVD<MatrixXf>(10,10) );
 
   // Check that preallocation avoids subsequent mallocs
-  CALL_SUBTEST_9( svd_preallocate<void>() );
+  // Disbaled because not supported by BDCSVD
+  // CALL_SUBTEST_9( svd_preallocate<void>() );
 
   CALL_SUBTEST_2( svd_underoverflow<void>() );
 }
diff --git a/gtsam/3rdparty/Eigen/test/block.cpp b/gtsam/3rdparty/Eigen/test/block.cpp
index 39565af83..ca9c21fe3 100644
--- a/gtsam/3rdparty/Eigen/test/block.cpp
+++ b/gtsam/3rdparty/Eigen/test/block.cpp
@@ -32,12 +32,11 @@ block_real_only(const MatrixType &, Index, Index, Index, Index, const Scalar&) {
 
 template<typename MatrixType> void block(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
   typedef Matrix<Scalar, 1, MatrixType::ColsAtCompileTime> RowVectorType;
-  typedef Matrix<Scalar, Dynamic, Dynamic> DynamicMatrixType;
+  typedef Matrix<Scalar, Dynamic, Dynamic, MatrixType::IsRowMajor?RowMajor:ColMajor> DynamicMatrixType;
   typedef Matrix<Scalar, Dynamic, 1> DynamicVectorType;
   
   Index rows = m.rows();
@@ -131,7 +130,7 @@ template<typename MatrixType> void block(const MatrixType& m)
   VERIFY(numext::real(ones.col(c1).dot(ones.col(c2))) == RealScalar(rows));
   VERIFY(numext::real(ones.row(r1).dot(ones.row(r2))) == RealScalar(cols));
   
-  // chekc that linear acccessors works on blocks
+  // check that linear acccessors works on blocks
   m1 = m1_copy;
   if((MatrixType::Flags&RowMajorBit)==0)
     VERIFY_IS_EQUAL(m1.leftCols(c1).coeff(r1+c1*rows), m1(r1,c1));
@@ -155,6 +154,13 @@ template<typename MatrixType> void block(const MatrixType& m)
   VERIFY_IS_APPROX( ((m1+m2).block(r1,c1,r2-r1+1,c2-c1+1).transpose().col(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)).transpose() );
   VERIFY_IS_APPROX( ((m1+m2).transpose().block(c1,r1,c2-c1+1,r2-r1+1).col(0)) , ((m1+m2).row(r1).segment(c1,c2-c1+1)).transpose() );
 
+  VERIFY_IS_APPROX( (m1*1).topRows(r1),  m1.topRows(r1) );
+  VERIFY_IS_APPROX( (m1*1).leftCols(c1), m1.leftCols(c1) );
+  VERIFY_IS_APPROX( (m1*1).transpose().topRows(c1), m1.transpose().topRows(c1) );
+  VERIFY_IS_APPROX( (m1*1).transpose().leftCols(r1), m1.transpose().leftCols(r1) );
+  VERIFY_IS_APPROX( (m1*1).transpose().middleRows(c1,c2-c1+1), m1.transpose().middleRows(c1,c2-c1+1) );
+  VERIFY_IS_APPROX( (m1*1).transpose().middleCols(r1,r2-r1+1), m1.transpose().middleCols(r1,r2-r1+1) );
+
   // evaluation into plain matrices from expressions with direct access (stress MapBase)
   DynamicMatrixType dm;
   DynamicVectorType dv;
@@ -200,7 +206,6 @@ template<typename MatrixType> void block(const MatrixType& m)
 template<typename MatrixType>
 void compare_using_data_and_stride(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
   Index size = m.size();
@@ -234,7 +239,6 @@ void compare_using_data_and_stride(const MatrixType& m)
 template<typename MatrixType>
 void data_and_stride(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
 
diff --git a/gtsam/3rdparty/Eigen/test/cholesky.cpp b/gtsam/3rdparty/Eigen/test/cholesky.cpp
index 8ad5ac639..5cf842d6d 100644
--- a/gtsam/3rdparty/Eigen/test/cholesky.cpp
+++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp
@@ -19,6 +19,7 @@
 
 template<typename MatrixType, int UpLo>
 typename MatrixType::RealScalar matrix_l1_norm(const MatrixType& m) {
+  if(m.cols()==0) return typename MatrixType::RealScalar(0);
   MatrixType symm = m.template selfadjointView<UpLo>();
   return symm.cwiseAbs().colwise().sum().maxCoeff();
 }
@@ -57,7 +58,6 @@ template<typename MatrixType,template <typename,int> class CholType> void test_c
 
 template<typename MatrixType> void cholesky(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   /* this test covers the following files:
      LLT.h LDLT.h
   */
@@ -97,7 +97,7 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
     RealScalar rcond_est = chollo.rcond();
     // Verify that the estimated condition number is within a factor of 10 of the
     // truth.
-    VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10);
+    VERIFY(rcond_est >= rcond / 10 && rcond_est <= rcond * 10);
 
     // test the upper mode
     LLT<SquareMatrixType,Upper> cholup(symmUp);
@@ -113,12 +113,12 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
     rcond = (RealScalar(1) / matrix_l1_norm<MatrixType, Upper>(symmUp)) /
                              matrix_l1_norm<MatrixType, Upper>(symmUp_inverse);
     rcond_est = cholup.rcond();
-    VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10);
+    VERIFY(rcond_est >= rcond / 10 && rcond_est <= rcond * 10);
 
 
     MatrixType neg = -symmLo;
     chollo.compute(neg);
-    VERIFY(chollo.info()==NumericalIssue);
+    VERIFY(neg.size()==0 || chollo.info()==NumericalIssue);
 
     VERIFY_IS_APPROX(MatrixType(chollo.matrixL().transpose().conjugate()), MatrixType(chollo.matrixU()));
     VERIFY_IS_APPROX(MatrixType(chollo.matrixU().transpose().conjugate()), MatrixType(chollo.matrixL()));
@@ -167,7 +167,7 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
     RealScalar rcond_est = ldltlo.rcond();
     // Verify that the estimated condition number is within a factor of 10 of the
     // truth.
-    VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10);
+    VERIFY(rcond_est >= rcond / 10 && rcond_est <= rcond * 10);
 
 
     LDLT<SquareMatrixType,Upper> ldltup(symmUp);
@@ -184,7 +184,7 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
     rcond = (RealScalar(1) / matrix_l1_norm<MatrixType, Upper>(symmUp)) /
                              matrix_l1_norm<MatrixType, Upper>(symmUp_inverse);
     rcond_est = ldltup.rcond();
-    VERIFY(rcond_est > rcond / 10 && rcond_est < rcond * 10);
+    VERIFY(rcond_est >= rcond / 10 && rcond_est <= rcond * 10);
 
     VERIFY_IS_APPROX(MatrixType(ldltlo.matrixL().transpose().conjugate()), MatrixType(ldltlo.matrixU()));
     VERIFY_IS_APPROX(MatrixType(ldltlo.matrixU().transpose().conjugate()), MatrixType(ldltlo.matrixL()));
@@ -289,8 +289,6 @@ template<typename MatrixType> void cholesky_cplx(const MatrixType& m)
 
   // test mixing real/scalar types
 
-  typedef typename MatrixType::Index Index;
-
   Index rows = m.rows();
   Index cols = m.cols();
 
@@ -373,6 +371,7 @@ template<typename MatrixType> void cholesky_definiteness(const MatrixType& m)
     VERIFY(ldlt.info()==Success);
     VERIFY(!ldlt.isNegative());
     VERIFY(!ldlt.isPositive());
+    VERIFY_IS_APPROX(mat,ldlt.reconstructedMatrix());
   }
   {
     mat << 1, 2, 2, 1;
@@ -380,6 +379,7 @@ template<typename MatrixType> void cholesky_definiteness(const MatrixType& m)
     VERIFY(ldlt.info()==Success);
     VERIFY(!ldlt.isNegative());
     VERIFY(!ldlt.isPositive());
+    VERIFY_IS_APPROX(mat,ldlt.reconstructedMatrix());
   }
   {
     mat << 0, 0, 0, 0;
@@ -387,6 +387,7 @@ template<typename MatrixType> void cholesky_definiteness(const MatrixType& m)
     VERIFY(ldlt.info()==Success);
     VERIFY(ldlt.isNegative());
     VERIFY(ldlt.isPositive());
+    VERIFY_IS_APPROX(mat,ldlt.reconstructedMatrix());
   }
   {
     mat << 0, 0, 0, 1;
@@ -394,6 +395,7 @@ template<typename MatrixType> void cholesky_definiteness(const MatrixType& m)
     VERIFY(ldlt.info()==Success);
     VERIFY(!ldlt.isNegative());
     VERIFY(ldlt.isPositive());
+    VERIFY_IS_APPROX(mat,ldlt.reconstructedMatrix());
   }
   {
     mat << -1, 0, 0, 0;
@@ -401,6 +403,7 @@ template<typename MatrixType> void cholesky_definiteness(const MatrixType& m)
     VERIFY(ldlt.info()==Success);
     VERIFY(ldlt.isNegative());
     VERIFY(!ldlt.isPositive());
+    VERIFY_IS_APPROX(mat,ldlt.reconstructedMatrix());
   }
 }
 
@@ -452,6 +455,18 @@ void cholesky_faillure_cases()
     VERIFY(ldlt.info()==NumericalIssue);
     VERIFY_IS_NOT_APPROX(mat,ldlt.reconstructedMatrix());
   }
+
+  // bug 1479
+  {
+    mat.resize(4,4);
+    mat <<  1, 2, 0, 1,
+            2, 4, 0, 2,
+            0, 0, 0, 1,
+            1, 2, 1, 1;
+    ldlt.compute(mat);
+    VERIFY(ldlt.info()==NumericalIssue);
+    VERIFY_IS_NOT_APPROX(mat,ldlt.reconstructedMatrix());
+  }
 }
 
 template<typename MatrixType> void cholesky_verify_assert()
@@ -493,6 +508,11 @@ void test_cholesky()
     CALL_SUBTEST_6( cholesky_cplx(MatrixXcd(s,s)) );
     TEST_SET_BUT_UNUSED_VARIABLE(s)
   }
+  // empty matrix, regression test for Bug 785:
+  CALL_SUBTEST_2( cholesky(MatrixXd(0,0)) );
+
+  // This does not work yet:
+  // CALL_SUBTEST_2( cholesky(Matrix<double,0,0>()) );
 
   CALL_SUBTEST_4( cholesky_verify_assert<Matrix3f>() );
   CALL_SUBTEST_7( cholesky_verify_assert<Matrix3d>() );
diff --git a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp
index 498421b4c..21a1db4ac 100644
--- a/gtsam/3rdparty/Eigen/test/conservative_resize.cpp
+++ b/gtsam/3rdparty/Eigen/test/conservative_resize.cpp
@@ -17,7 +17,6 @@ template <typename Scalar, int Storage>
 void run_matrix_tests()
 {
   typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Storage> MatrixType;
-  typedef typename MatrixType::Index Index;
 
   MatrixType m, n;
 
diff --git a/gtsam/3rdparty/Eigen/test/corners.cpp b/gtsam/3rdparty/Eigen/test/corners.cpp
index 3c64c32a1..32edadb25 100644
--- a/gtsam/3rdparty/Eigen/test/corners.cpp
+++ b/gtsam/3rdparty/Eigen/test/corners.cpp
@@ -15,7 +15,6 @@
 
 template<typename MatrixType> void corners(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
 
diff --git a/gtsam/3rdparty/Eigen/test/cuda_basic.cu b/gtsam/3rdparty/Eigen/test/cuda_basic.cu
index cb2e4167a..ce66c2c78 100644
--- a/gtsam/3rdparty/Eigen/test/cuda_basic.cu
+++ b/gtsam/3rdparty/Eigen/test/cuda_basic.cu
@@ -20,9 +20,6 @@
 
 #include <math_constants.h>
 #include <cuda.h>
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include "cuda_common.h"
 
diff --git a/gtsam/3rdparty/Eigen/test/determinant.cpp b/gtsam/3rdparty/Eigen/test/determinant.cpp
index 758f3afbb..b8c9babb3 100644
--- a/gtsam/3rdparty/Eigen/test/determinant.cpp
+++ b/gtsam/3rdparty/Eigen/test/determinant.cpp
@@ -16,7 +16,6 @@ template<typename MatrixType> void determinant(const MatrixType& m)
   /* this test covers the following files:
      Determinant.h
   */
-  typedef typename MatrixType::Index Index;
   Index size = m.rows();
 
   MatrixType m1(size, size), m2(size, size);
diff --git a/gtsam/3rdparty/Eigen/test/diagonal.cpp b/gtsam/3rdparty/Eigen/test/diagonal.cpp
index c1546e97d..8ed9b4682 100644
--- a/gtsam/3rdparty/Eigen/test/diagonal.cpp
+++ b/gtsam/3rdparty/Eigen/test/diagonal.cpp
@@ -11,7 +11,6 @@
 
 template<typename MatrixType> void diagonal(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = m.rows();
@@ -66,6 +65,9 @@ template<typename MatrixType> void diagonal(const MatrixType& m)
     m2.diagonal(N2).coeffRef(0) = Scalar(2)*s1;
     VERIFY_IS_APPROX(m2.diagonal(N2).coeff(0), Scalar(2)*s1);
   }
+
+  VERIFY( m1.diagonal( cols).size()==0 );
+  VERIFY( m1.diagonal(-rows).size()==0 );
 }
 
 template<typename MatrixType> void diagonal_assert(const MatrixType& m) {
@@ -81,6 +83,9 @@ template<typename MatrixType> void diagonal_assert(const MatrixType& m) {
     VERIFY_RAISES_ASSERT( m1.array() *= m1.diagonal().array() );
     VERIFY_RAISES_ASSERT( m1.array() /= m1.diagonal().array() );
   }
+
+  VERIFY_RAISES_ASSERT( m1.diagonal(cols+1) );
+  VERIFY_RAISES_ASSERT( m1.diagonal(-(rows+1)) );
 }
 
 void test_diagonal()
@@ -95,7 +100,6 @@ void test_diagonal()
     CALL_SUBTEST_2( diagonal(MatrixXcd(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
     CALL_SUBTEST_1( diagonal(MatrixXf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
     CALL_SUBTEST_1( diagonal(Matrix<float,Dynamic,4>(3, 4)) );
+    CALL_SUBTEST_1( diagonal_assert(MatrixXf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
   }
-
-  CALL_SUBTEST_1( diagonal_assert(MatrixXf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
 }
diff --git a/gtsam/3rdparty/Eigen/test/diagonalmatrices.cpp b/gtsam/3rdparty/Eigen/test/diagonalmatrices.cpp
index cd6dc8cf0..c55733df8 100644
--- a/gtsam/3rdparty/Eigen/test/diagonalmatrices.cpp
+++ b/gtsam/3rdparty/Eigen/test/diagonalmatrices.cpp
@@ -11,7 +11,6 @@
 using namespace std;
 template<typename MatrixType> void diagonalmatrices(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   enum { Rows = MatrixType::RowsAtCompileTime, Cols = MatrixType::ColsAtCompileTime };
   typedef Matrix<Scalar, Rows, 1> VectorType;
@@ -30,6 +29,7 @@ template<typename MatrixType> void diagonalmatrices(const MatrixType& m)
              v2 = VectorType::Random(rows);
   RowVectorType rv1 = RowVectorType::Random(cols),
              rv2 = RowVectorType::Random(cols);
+
   LeftDiagonalMatrix ldm1(v1), ldm2(v2);
   RightDiagonalMatrix rdm1(rv1), rdm2(rv2);
   
@@ -99,6 +99,38 @@ template<typename MatrixType> void diagonalmatrices(const MatrixType& m)
   VERIFY_IS_APPROX( (sq_m1 += (s1*v1).asDiagonal()), sq_m2 += (s1*v1).asDiagonal().toDenseMatrix() );
   VERIFY_IS_APPROX( (sq_m1 -= (s1*v1).asDiagonal()), sq_m2 -= (s1*v1).asDiagonal().toDenseMatrix() );
   VERIFY_IS_APPROX( (sq_m1 = (s1*v1).asDiagonal()), (s1*v1).asDiagonal().toDenseMatrix() );
+
+  sq_m1.setRandom();
+  sq_m2 = v1.asDiagonal();
+  sq_m2 = sq_m1 * sq_m2;
+  VERIFY_IS_APPROX( (sq_m1*v1.asDiagonal()).col(i), sq_m2.col(i) );
+  VERIFY_IS_APPROX( (sq_m1*v1.asDiagonal()).row(i), sq_m2.row(i) );
+}
+
+template<typename MatrixType> void as_scalar_product(const MatrixType& m)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
+  typedef Matrix<Scalar, Dynamic, Dynamic> DynMatrixType;
+  typedef Matrix<Scalar, Dynamic, 1> DynVectorType;
+  typedef Matrix<Scalar, 1, Dynamic> DynRowVectorType;
+
+  Index rows = m.rows();
+  Index depth = internal::random<Index>(1,EIGEN_TEST_MAX_SIZE);
+
+  VectorType v1 = VectorType::Random(rows);  
+  DynVectorType     dv1  = DynVectorType::Random(depth);
+  DynRowVectorType  drv1 = DynRowVectorType::Random(depth);
+  DynMatrixType     dm1  = dv1;
+  DynMatrixType     drm1 = drv1;
+  
+  Scalar s = v1(0);
+
+  VERIFY_IS_APPROX( v1.asDiagonal() * drv1, s*drv1 );
+  VERIFY_IS_APPROX( dv1 * v1.asDiagonal(), dv1*s );
+
+  VERIFY_IS_APPROX( v1.asDiagonal() * drm1, s*drm1 );
+  VERIFY_IS_APPROX( dm1 * v1.asDiagonal(), dm1*s );
 }
 
 template<int>
@@ -116,14 +148,19 @@ void test_diagonalmatrices()
 {
   for(int i = 0; i < g_repeat; i++) {
     CALL_SUBTEST_1( diagonalmatrices(Matrix<float, 1, 1>()) );
+    CALL_SUBTEST_1( as_scalar_product(Matrix<float, 1, 1>()) );
+
     CALL_SUBTEST_2( diagonalmatrices(Matrix3f()) );
     CALL_SUBTEST_3( diagonalmatrices(Matrix<double,3,3,RowMajor>()) );
     CALL_SUBTEST_4( diagonalmatrices(Matrix4d()) );
     CALL_SUBTEST_5( diagonalmatrices(Matrix<float,4,4,RowMajor>()) );
     CALL_SUBTEST_6( diagonalmatrices(MatrixXcf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
+    CALL_SUBTEST_6( as_scalar_product(MatrixXcf(1,1)) );
     CALL_SUBTEST_7( diagonalmatrices(MatrixXi(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
     CALL_SUBTEST_8( diagonalmatrices(Matrix<double,Dynamic,Dynamic,RowMajor>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
     CALL_SUBTEST_9( diagonalmatrices(MatrixXf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
+    CALL_SUBTEST_9( diagonalmatrices(MatrixXf(1,1)) );
+    CALL_SUBTEST_9( as_scalar_product(MatrixXf(1,1)) );
   }
   CALL_SUBTEST_10( bug987<0>() );
 }
diff --git a/gtsam/3rdparty/Eigen/test/dontalign.cpp b/gtsam/3rdparty/Eigen/test/dontalign.cpp
index 4643cfed6..ac00112ed 100644
--- a/gtsam/3rdparty/Eigen/test/dontalign.cpp
+++ b/gtsam/3rdparty/Eigen/test/dontalign.cpp
@@ -19,7 +19,6 @@
 template<typename MatrixType>
 void dontalign(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;
diff --git a/gtsam/3rdparty/Eigen/test/eigen2support.cpp b/gtsam/3rdparty/Eigen/test/eigen2support.cpp
index ad1d98091..ac6931a0e 100644
--- a/gtsam/3rdparty/Eigen/test/eigen2support.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2support.cpp
@@ -13,7 +13,6 @@
 
 template<typename MatrixType> void eigen2support(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = m.rows();
diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_complex.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_complex.cpp
index 293b1b265..726945259 100644
--- a/gtsam/3rdparty/Eigen/test/eigensolver_complex.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigensolver_complex.cpp
@@ -71,7 +71,6 @@ void verify_is_approx_upto_permutation(const VectorType& vec1, const VectorType&
 
 template<typename MatrixType> void eigensolver(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   /* this test covers the following files:
      ComplexEigenSolver.h, and indirectly ComplexSchur.h
   */
diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_generalized_real.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_generalized_real.cpp
index 9c0838ba4..9dd44c89d 100644
--- a/gtsam/3rdparty/Eigen/test/eigensolver_generalized_real.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigensolver_generalized_real.cpp
@@ -15,7 +15,6 @@
 
 template<typename MatrixType> void generalized_eigensolver_real(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   /* this test covers the following files:
      GeneralizedEigenSolver.h
   */
@@ -77,6 +76,13 @@ template<typename MatrixType> void generalized_eigensolver_real(const MatrixType
     GeneralizedEigenSolver<MatrixType> eig2(a.adjoint() * a,b.adjoint() * b);
     eig2.compute(a.adjoint() * a,b.adjoint() * b);
   }
+
+  // check without eigenvectors
+  {
+    GeneralizedEigenSolver<MatrixType> eig1(spdA, spdB, true);
+    GeneralizedEigenSolver<MatrixType> eig2(spdA, spdB, false);
+    VERIFY_IS_APPROX(eig1.eigenvalues(), eig2.eigenvalues());
+  }
 }
 
 void test_eigensolver_generalized_real()
diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp
index d0e644d4b..07bf65e03 100644
--- a/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigensolver_generic.cpp
@@ -14,7 +14,6 @@
 
 template<typename MatrixType> void eigensolver(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   /* this test covers the following files:
      EigenSolver.h
   */
diff --git a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp
index 39ad4130e..0e39b5364 100644
--- a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp
@@ -68,7 +68,6 @@ template<typename MatrixType> void selfadjointeigensolver_essential_check(const
 
 template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   /* this test covers the following files:
      EigenSolver.h, SelfAdjointEigenSolver.h (and indirectly: Tridiagonalization.h)
   */
diff --git a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp
index d2339a651..b64ea3bdc 100644
--- a/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp
+++ b/gtsam/3rdparty/Eigen/test/geo_alignedbox.cpp
@@ -24,7 +24,6 @@ template<typename BoxType> void alignedbox(const BoxType& _box)
   /* this test covers the following files:
      AlignedBox.h
   */
-  typedef typename BoxType::Index Index;  
   typedef typename BoxType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;
@@ -86,7 +85,6 @@ template<typename BoxType>
 void alignedboxCastTests(const BoxType& _box)
 {
   // casting  
-  typedef typename BoxType::Index Index;
   typedef typename BoxType::Scalar Scalar;
   typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;
 
diff --git a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp
index 27892850d..b3a48c585 100644
--- a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp
+++ b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp
@@ -19,7 +19,6 @@ template<typename HyperplaneType> void hyperplane(const HyperplaneType& _plane)
      Hyperplane.h
   */
   using std::abs;
-  typedef typename HyperplaneType::Index Index;
   const Index dim = _plane.dim();
   enum { Options = HyperplaneType::Options };
   typedef typename HyperplaneType::Scalar Scalar;
diff --git a/gtsam/3rdparty/Eigen/test/geo_parametrizedline.cpp b/gtsam/3rdparty/Eigen/test/geo_parametrizedline.cpp
index 9bf5f3c1d..6a8794726 100644
--- a/gtsam/3rdparty/Eigen/test/geo_parametrizedline.cpp
+++ b/gtsam/3rdparty/Eigen/test/geo_parametrizedline.cpp
@@ -19,7 +19,6 @@ template<typename LineType> void parametrizedline(const LineType& _line)
      ParametrizedLine.h
   */
   using std::abs;
-  typedef typename LineType::Index Index;
   const Index dim = _line.dim();
   typedef typename LineType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
diff --git a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp
index 96889e722..8ee8fdb27 100644
--- a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp
+++ b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp
@@ -231,6 +231,19 @@ template<typename Scalar> void mapQuaternion(void){
   VERIFY_IS_APPROX(mq3*mq2, q3*q2);
   VERIFY_IS_APPROX(mcq1*mq2, q1*q2);
   VERIFY_IS_APPROX(mcq3*mq2, q3*q2);
+
+  // Bug 1461, compilation issue with Map<const Quat>::w(), and other reference/constness checks:
+  VERIFY_IS_APPROX(mcq3.coeffs().x() + mcq3.coeffs().y() + mcq3.coeffs().z() + mcq3.coeffs().w(), mcq3.coeffs().sum());
+  VERIFY_IS_APPROX(mcq3.x() + mcq3.y() + mcq3.z() + mcq3.w(), mcq3.coeffs().sum());
+  mq3.w() = 1;
+  const Quaternionx& cq3(q3);
+  VERIFY( &cq3.x() == &q3.x() );
+  const MQuaternionUA& cmq3(mq3);
+  VERIFY( &cmq3.x() == &mq3.x() );
+  // FIXME the following should be ok. The problem is that currently the LValueBit flag
+  // is used to determine wether we can return a coeff by reference or not, which is not enough for Map<const ...>.
+  //const MCQuaternionUA& cmcq3(mcq3);
+  //VERIFY( &cmcq3.x() == &mcq3.x() );
 }
 
 template<typename Scalar> void quaternionAlignment(void){
diff --git a/gtsam/3rdparty/Eigen/test/half_float.cpp b/gtsam/3rdparty/Eigen/test/half_float.cpp
index 3d2410aef..b37b81903 100644
--- a/gtsam/3rdparty/Eigen/test/half_float.cpp
+++ b/gtsam/3rdparty/Eigen/test/half_float.cpp
@@ -11,6 +11,10 @@
 
 #include <Eigen/src/Core/arch/CUDA/Half.h>
 
+#ifdef EIGEN_HAS_CUDA_FP16
+#error "EIGEN_HAS_CUDA_FP16 should not be defined in this CPU unit test"
+#endif
+
 // Make sure it's possible to forward declare Eigen::half
 namespace Eigen {
 struct half;
@@ -20,7 +24,7 @@ using Eigen::half;
 
 void test_conversion()
 {
-  using Eigen::half_impl::__half;
+  using Eigen::half_impl::__half_raw;
 
   // Conversion from float.
   VERIFY_IS_EQUAL(half(1.0f).x, 0x3c00);
@@ -37,9 +41,9 @@ void test_conversion()
   VERIFY_IS_EQUAL(half(1.19209e-07f).x, 0x0002);
 
   // Verify round-to-nearest-even behavior.
-  float val1 = float(half(__half(0x3c00)));
-  float val2 = float(half(__half(0x3c01)));
-  float val3 = float(half(__half(0x3c02)));
+  float val1 = float(half(__half_raw(0x3c00)));
+  float val2 = float(half(__half_raw(0x3c01)));
+  float val3 = float(half(__half_raw(0x3c02)));
   VERIFY_IS_EQUAL(half(0.5f * (val1 + val2)).x, 0x3c00);
   VERIFY_IS_EQUAL(half(0.5f * (val2 + val3)).x, 0x3c02);
 
@@ -55,21 +59,21 @@ void test_conversion()
   VERIFY_IS_EQUAL(half(true).x, 0x3c00);
 
   // Conversion to float.
-  VERIFY_IS_EQUAL(float(half(__half(0x0000))), 0.0f);
-  VERIFY_IS_EQUAL(float(half(__half(0x3c00))), 1.0f);
+  VERIFY_IS_EQUAL(float(half(__half_raw(0x0000))), 0.0f);
+  VERIFY_IS_EQUAL(float(half(__half_raw(0x3c00))), 1.0f);
 
   // Denormals.
-  VERIFY_IS_APPROX(float(half(__half(0x8001))), -5.96046e-08f);
-  VERIFY_IS_APPROX(float(half(__half(0x0001))), 5.96046e-08f);
-  VERIFY_IS_APPROX(float(half(__half(0x0002))), 1.19209e-07f);
+  VERIFY_IS_APPROX(float(half(__half_raw(0x8001))), -5.96046e-08f);
+  VERIFY_IS_APPROX(float(half(__half_raw(0x0001))), 5.96046e-08f);
+  VERIFY_IS_APPROX(float(half(__half_raw(0x0002))), 1.19209e-07f);
 
   // NaNs and infinities.
   VERIFY(!(numext::isinf)(float(half(65504.0f))));  // Largest finite number.
   VERIFY(!(numext::isnan)(float(half(0.0f))));
-  VERIFY((numext::isinf)(float(half(__half(0xfc00)))));
-  VERIFY((numext::isnan)(float(half(__half(0xfc01)))));
-  VERIFY((numext::isinf)(float(half(__half(0x7c00)))));
-  VERIFY((numext::isnan)(float(half(__half(0x7c01)))));
+  VERIFY((numext::isinf)(float(half(__half_raw(0xfc00)))));
+  VERIFY((numext::isnan)(float(half(__half_raw(0xfc01)))));
+  VERIFY((numext::isinf)(float(half(__half_raw(0x7c00)))));
+  VERIFY((numext::isnan)(float(half(__half_raw(0x7c01)))));
 
 #if !EIGEN_COMP_MSVC
   // Visual Studio errors out on divisions by 0
@@ -79,12 +83,12 @@ void test_conversion()
 #endif
 
   // Exactly same checks as above, just directly on the half representation.
-  VERIFY(!(numext::isinf)(half(__half(0x7bff))));
-  VERIFY(!(numext::isnan)(half(__half(0x0000))));
-  VERIFY((numext::isinf)(half(__half(0xfc00))));
-  VERIFY((numext::isnan)(half(__half(0xfc01))));
-  VERIFY((numext::isinf)(half(__half(0x7c00))));
-  VERIFY((numext::isnan)(half(__half(0x7c01))));
+  VERIFY(!(numext::isinf)(half(__half_raw(0x7bff))));
+  VERIFY(!(numext::isnan)(half(__half_raw(0x0000))));
+  VERIFY((numext::isinf)(half(__half_raw(0xfc00))));
+  VERIFY((numext::isnan)(half(__half_raw(0xfc01))));
+  VERIFY((numext::isinf)(half(__half_raw(0x7c00))));
+  VERIFY((numext::isnan)(half(__half_raw(0x7c01))));
 
 #if !EIGEN_COMP_MSVC
   // Visual Studio errors out on divisions by 0
diff --git a/gtsam/3rdparty/Eigen/test/householder.cpp b/gtsam/3rdparty/Eigen/test/householder.cpp
index c5f6b5e4f..e70b7ea25 100644
--- a/gtsam/3rdparty/Eigen/test/householder.cpp
+++ b/gtsam/3rdparty/Eigen/test/householder.cpp
@@ -12,7 +12,6 @@
 
 template<typename MatrixType> void householder(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   static bool even = true;
   even = !even;
   /* this test covers the following files:
diff --git a/gtsam/3rdparty/Eigen/test/integer_types.cpp b/gtsam/3rdparty/Eigen/test/integer_types.cpp
index a21f73a81..36295598f 100644
--- a/gtsam/3rdparty/Eigen/test/integer_types.cpp
+++ b/gtsam/3rdparty/Eigen/test/integer_types.cpp
@@ -18,7 +18,6 @@
 
 template<typename MatrixType> void signed_integer_type_tests(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   enum { is_signed = (Scalar(-1) > Scalar(0)) ? 0 : 1 };
@@ -49,7 +48,6 @@ template<typename MatrixType> void signed_integer_type_tests(const MatrixType& m
 
 template<typename MatrixType> void integer_type_tests(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   VERIFY(NumTraits<Scalar>::IsInteger);
@@ -162,8 +160,8 @@ void test_integer_types()
   VERIFY_IS_EQUAL(internal::scalar_div_cost<int>::value, 8);
   VERIFY_IS_EQUAL(internal::scalar_div_cost<unsigned int>::value, 8);
   if(sizeof(long)>sizeof(int)) {
-    VERIFY(internal::scalar_div_cost<long>::value > internal::scalar_div_cost<int>::value);
-    VERIFY(internal::scalar_div_cost<unsigned long>::value > internal::scalar_div_cost<int>::value);
+    VERIFY(int(internal::scalar_div_cost<long>::value) > int(internal::scalar_div_cost<int>::value));
+    VERIFY(int(internal::scalar_div_cost<unsigned long>::value) > int(internal::scalar_div_cost<int>::value));
   }
 #endif
 }
diff --git a/gtsam/3rdparty/Eigen/test/inverse.cpp b/gtsam/3rdparty/Eigen/test/inverse.cpp
index 5c6777a18..be607cc8b 100644
--- a/gtsam/3rdparty/Eigen/test/inverse.cpp
+++ b/gtsam/3rdparty/Eigen/test/inverse.cpp
@@ -14,7 +14,6 @@
 template<typename MatrixType> void inverse(const MatrixType& m)
 {
   using std::abs;
-  typedef typename MatrixType::Index Index;
   /* this test covers the following files:
      Inverse.h
   */
@@ -47,7 +46,7 @@ template<typename MatrixType> void inverse(const MatrixType& m)
   //computeInverseAndDetWithCheck tests
   //First: an invertible matrix
   bool invertible;
-  RealScalar det;
+  Scalar det;
 
   m2.setZero();
   m1.computeInverseAndDetWithCheck(m2, det, invertible);
@@ -113,5 +112,7 @@ void test_inverse()
     
     CALL_SUBTEST_7( inverse(Matrix4d()) );
     CALL_SUBTEST_7( inverse(Matrix<double,4,4,DontAlign>()) );
+
+    CALL_SUBTEST_8( inverse(Matrix4cd()) );
   }
 }
diff --git a/gtsam/3rdparty/Eigen/test/jacobi.cpp b/gtsam/3rdparty/Eigen/test/jacobi.cpp
index 7ccd4124b..319e4767a 100644
--- a/gtsam/3rdparty/Eigen/test/jacobi.cpp
+++ b/gtsam/3rdparty/Eigen/test/jacobi.cpp
@@ -14,7 +14,6 @@
 template<typename MatrixType, typename JacobiScalar>
 void jacobi(const MatrixType& m = MatrixType())
 {
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
 
diff --git a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp
index 7f5f71562..64b866358 100644
--- a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp
+++ b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp
@@ -36,7 +36,6 @@ void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
 template<typename MatrixType> void jacobisvd_verify_assert(const MatrixType& m)
 {
   svd_verify_assert<JacobiSVD<MatrixType> >(m);
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
 
@@ -70,6 +69,21 @@ void jacobisvd_method()
   VERIFY_IS_APPROX(m.jacobiSvd(ComputeFullU|ComputeFullV).solve(m), m);
 }
 
+namespace Foo {
+// older compiler require a default constructor for Bar
+// cf: https://stackoverflow.com/questions/7411515/
+class Bar {public: Bar() {}};
+bool operator<(const Bar&, const Bar&) { return true; }
+}
+// regression test for a very strange MSVC issue for which simply
+// including SVDBase.h messes up with std::max and custom scalar type
+void msvc_workaround()
+{
+  const Foo::Bar a;
+  const Foo::Bar b;
+  std::max EIGEN_NOT_A_MACRO (a,b);
+}
+
 void test_jacobisvd()
 {
   CALL_SUBTEST_3(( jacobisvd_verify_assert(Matrix3f()) ));
@@ -123,4 +137,6 @@ void test_jacobisvd()
   CALL_SUBTEST_9( svd_preallocate<void>() );
 
   CALL_SUBTEST_2( svd_underoverflow<void>() );
+
+  msvc_workaround();
 }
diff --git a/gtsam/3rdparty/Eigen/test/linearstructure.cpp b/gtsam/3rdparty/Eigen/test/linearstructure.cpp
index 17474af10..b6559b2a0 100644
--- a/gtsam/3rdparty/Eigen/test/linearstructure.cpp
+++ b/gtsam/3rdparty/Eigen/test/linearstructure.cpp
@@ -19,7 +19,6 @@ template<typename MatrixType> void linearStructure(const MatrixType& m)
   /* this test covers the following files:
      CwiseUnaryOp.h, CwiseBinaryOp.h, SelfCwiseBinaryOp.h 
   */
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
 
diff --git a/gtsam/3rdparty/Eigen/test/lu.cpp b/gtsam/3rdparty/Eigen/test/lu.cpp
index 9787f4d86..176a2f090 100644
--- a/gtsam/3rdparty/Eigen/test/lu.cpp
+++ b/gtsam/3rdparty/Eigen/test/lu.cpp
@@ -18,7 +18,6 @@ typename MatrixType::RealScalar matrix_l1_norm(const MatrixType& m) {
 
 template<typename MatrixType> void lu_non_invertible()
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::RealScalar RealScalar;
   /* this test covers the following files:
      LU.h
@@ -58,6 +57,10 @@ template<typename MatrixType> void lu_non_invertible()
   // The image of the zero matrix should consist of a single (zero) column vector
   VERIFY((MatrixType::Zero(rows,cols).fullPivLu().image(MatrixType::Zero(rows,cols)).cols() == 1));
 
+  // The kernel of the zero matrix is the entire space, and thus is an invertible matrix of dimensions cols.
+  KernelMatrixType kernel = MatrixType::Zero(rows,cols).fullPivLu().kernel();
+  VERIFY((kernel.fullPivLu().isInvertible()));
+
   MatrixType m1(rows, cols), m3(rows, cols2);
   CMatrixType m2(cols, cols2);
   createRandomPIMatrixOfRank(rank, rows, cols, m1);
@@ -87,7 +90,7 @@ template<typename MatrixType> void lu_non_invertible()
   VERIFY(!lu.isInjective());
   VERIFY(!lu.isInvertible());
   VERIFY(!lu.isSurjective());
-  VERIFY((m1 * m1kernel).isMuchSmallerThan(m1));
+  VERIFY_IS_MUCH_SMALLER_THAN((m1 * m1kernel), m1);
   VERIFY(m1image.fullPivLu().rank() == rank);
   VERIFY_IS_APPROX(m1 * m1.adjoint() * m1image, m1image);
 
@@ -181,7 +184,6 @@ template<typename MatrixType> void lu_partial_piv()
   /* this test covers the following files:
      PartialPivLU.h
   */
-  typedef typename MatrixType::Index Index;
   typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
   Index size = internal::random<Index>(1,4);
 
diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h
index bd5325196..8c868ee79 100644
--- a/gtsam/3rdparty/Eigen/test/main.h
+++ b/gtsam/3rdparty/Eigen/test/main.h
@@ -50,6 +50,19 @@
 #endif
 #endif
 
+// Same for cuda_fp16.h
+#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
+#define EIGEN_TEST_CUDACC_VER  ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
+#elif defined(__CUDACC_VER__)
+#define EIGEN_TEST_CUDACC_VER __CUDACC_VER__
+#else
+#define EIGEN_TEST_CUDACC_VER 0
+#endif
+
+#if EIGEN_TEST_CUDACC_VER >= 70500
+#include <cuda_fp16.h>
+#endif
+
 // To test that all calls from Eigen code to std::min() and std::max() are
 // protected by parenthesis against macro expansion, the min()/max() macros
 // are defined here and any not-parenthesized min/max call will cause a
@@ -162,6 +175,12 @@ namespace Eigen
       eigen_assert_exception(void) {}
       ~eigen_assert_exception() { Eigen::no_more_assert = false; }
     };
+
+    struct eigen_static_assert_exception
+    {
+      eigen_static_assert_exception(void) {}
+      ~eigen_static_assert_exception() { Eigen::no_more_assert = false; }
+    };
   }
   // If EIGEN_DEBUG_ASSERTS is defined and if no assertion is triggered while
   // one should have been, then the list of excecuted assertions is printed out.
@@ -225,6 +244,7 @@ namespace Eigen
         else                                  \
           EIGEN_THROW_X(Eigen::eigen_assert_exception()); \
       }
+
     #ifdef EIGEN_EXCEPTIONS
       #define VERIFY_RAISES_ASSERT(a) {                           \
         Eigen::no_more_assert = false;                            \
@@ -236,13 +256,39 @@ namespace Eigen
         catch (Eigen::eigen_assert_exception&) { VERIFY(true); }  \
         Eigen::report_on_cerr_on_assert_failure = true;           \
       }
-    #endif //EIGEN_EXCEPTIONS
+    #endif // EIGEN_EXCEPTIONS
   #endif // EIGEN_DEBUG_ASSERTS
 
+  #if defined(TEST_CHECK_STATIC_ASSERTIONS) && defined(EIGEN_EXCEPTIONS)
+    #define EIGEN_STATIC_ASSERT(a,MSG) \
+      if( (!Eigen::internal::copy_bool(a)) && (!no_more_assert) )\
+      {                                       \
+        Eigen::no_more_assert = true;         \
+        if(report_on_cerr_on_assert_failure)  \
+          eigen_plain_assert((a) && #MSG);      \
+        else                                  \
+          EIGEN_THROW_X(Eigen::eigen_static_assert_exception()); \
+      }
+    #define VERIFY_RAISES_STATIC_ASSERT(a) {                    \
+      Eigen::no_more_assert = false;                            \
+      Eigen::report_on_cerr_on_assert_failure = false;          \
+      try {                                                     \
+        a;                                                      \
+        VERIFY(Eigen::should_raise_an_assert && # a);           \
+      }                                                         \
+      catch (Eigen::eigen_static_assert_exception&) { VERIFY(true); }  \
+      Eigen::report_on_cerr_on_assert_failure = true;           \
+    }
+  #endif // TEST_CHECK_STATIC_ASSERTIONS
+
 #ifndef VERIFY_RAISES_ASSERT
   #define VERIFY_RAISES_ASSERT(a) \
     std::cout << "Can't VERIFY_RAISES_ASSERT( " #a " ) with exceptions disabled\n";
 #endif
+#ifndef VERIFY_RAISES_STATIC_ASSERT
+  #define VERIFY_RAISES_STATIC_ASSERT(a) \
+    std::cout << "Can't VERIFY_RAISES_STATIC_ASSERT( " #a " ) with exceptions disabled\n";
+#endif
     
   #if !defined(__CUDACC__)
   #define EIGEN_USE_CUSTOM_ASSERT
@@ -251,10 +297,10 @@ namespace Eigen
 #else // EIGEN_NO_ASSERTION_CHECKING
 
   #define VERIFY_RAISES_ASSERT(a) {}
+  #define VERIFY_RAISES_STATIC_ASSERT(a) {}
 
 #endif // EIGEN_NO_ASSERTION_CHECKING
 
-
 #define EIGEN_INTERNAL_DEBUGGING
 #include <Eigen/QR> // required for createRandomPIMatrixOfRank
 
@@ -313,7 +359,7 @@ template<> inline long double test_precision<std::complex<long double> >() { ret
 inline bool test_isApprox(const short& a, const short& b)
 { return internal::isApprox(a, b, test_precision<short>()); }
 inline bool test_isApprox(const unsigned short& a, const unsigned short& b)
-{ return internal::isApprox(a, b, test_precision<unsigned long>()); }
+{ return internal::isApprox(a, b, test_precision<unsigned short>()); }
 inline bool test_isApprox(const unsigned int& a, const unsigned int& b)
 { return internal::isApprox(a, b, test_precision<unsigned int>()); }
 inline bool test_isApprox(const long& a, const long& b)
diff --git a/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp b/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp
index 6a84c5897..bc8a694ab 100644
--- a/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp
+++ b/gtsam/3rdparty/Eigen/test/mapped_matrix.cpp
@@ -17,7 +17,6 @@
 
 template<typename VectorType> void map_class_vector(const VectorType& m)
 {
-  typedef typename VectorType::Index Index;
   typedef typename VectorType::Scalar Scalar;
 
   Index size = m.size();
@@ -51,7 +50,6 @@ template<typename VectorType> void map_class_vector(const VectorType& m)
 
 template<typename MatrixType> void map_class_matrix(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = m.rows(), cols = m.cols(), size = rows*cols;
@@ -64,8 +62,9 @@ template<typename MatrixType> void map_class_matrix(const MatrixType& m)
   for(int i = 0; i < size; i++) array2[i] = Scalar(1);
   // array3unaligned -> unaligned pointer to heap
   Scalar* array3 = new Scalar[size+1];
-  for(int i = 0; i < size+1; i++) array3[i] = Scalar(1);
-  Scalar* array3unaligned = internal::UIntPtr(array3)%EIGEN_MAX_ALIGN_BYTES == 0 ? array3+1 : array3;
+  Index sizep1 = size + 1; // <- without this temporary MSVC 2103 generates bad code
+  for(Index i = 0; i < sizep1; i++) array3[i] = Scalar(1);
+  Scalar* array3unaligned = (internal::UIntPtr(array3)%EIGEN_MAX_ALIGN_BYTES) == 0 ? array3+1 : array3;
   Scalar array4[256];
   if(size<=256)
     for(int i = 0; i < size; i++) array4[i] = Scalar(1);
@@ -121,7 +120,6 @@ template<typename MatrixType> void map_class_matrix(const MatrixType& m)
 
 template<typename VectorType> void map_static_methods(const VectorType& m)
 {
-  typedef typename VectorType::Index Index;
   typedef typename VectorType::Scalar Scalar;
 
   Index size = m.size();
@@ -163,7 +161,6 @@ template<typename Scalar>
 void map_not_aligned_on_scalar()
 {
   typedef Matrix<Scalar,Dynamic,Dynamic> MatrixType;
-  typedef typename MatrixType::Index Index;
   Index size = 11;
   Scalar* array1 = internal::aligned_new<Scalar>((size+1)*(size+1)+1);
   Scalar* array2 = reinterpret_cast<Scalar*>(sizeof(Scalar)/2+std::size_t(array1));
diff --git a/gtsam/3rdparty/Eigen/test/mapstaticmethods.cpp b/gtsam/3rdparty/Eigen/test/mapstaticmethods.cpp
index 06272d106..8156ca939 100644
--- a/gtsam/3rdparty/Eigen/test/mapstaticmethods.cpp
+++ b/gtsam/3rdparty/Eigen/test/mapstaticmethods.cpp
@@ -69,7 +69,6 @@ struct mapstaticmethods_impl<PlainObjectType, true, false>
 {
   static void run(const PlainObjectType& m)
   {
-    typedef typename PlainObjectType::Index Index;
     Index rows = m.rows(), cols = m.cols();
 
     int i = internal::random<int>(2,5), j = internal::random<int>(2,5);
@@ -116,7 +115,6 @@ struct mapstaticmethods_impl<PlainObjectType, true, true>
 {
   static void run(const PlainObjectType& v)
   {
-    typedef typename PlainObjectType::Index Index;
     Index size = v.size();
 
     int i = internal::random<int>(2,5);
diff --git a/gtsam/3rdparty/Eigen/test/mapstride.cpp b/gtsam/3rdparty/Eigen/test/mapstride.cpp
index 4858f8fea..d785148cf 100644
--- a/gtsam/3rdparty/Eigen/test/mapstride.cpp
+++ b/gtsam/3rdparty/Eigen/test/mapstride.cpp
@@ -11,7 +11,6 @@
 
 template<int Alignment,typename VectorType> void map_class_vector(const VectorType& m)
 {
-  typedef typename VectorType::Index Index;
   typedef typename VectorType::Scalar Scalar;
 
   Index size = m.size();
@@ -50,7 +49,6 @@ template<int Alignment,typename VectorType> void map_class_vector(const VectorTy
 
 template<int Alignment,typename MatrixType> void map_class_matrix(const MatrixType& _m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = _m.rows(), cols = _m.cols();
@@ -58,7 +56,7 @@ template<int Alignment,typename MatrixType> void map_class_matrix(const MatrixTy
   MatrixType m = MatrixType::Random(rows,cols);
   Scalar s1 = internal::random<Scalar>();
 
-  Index arraysize = 2*(rows+4)*(cols+4);
+  Index arraysize = 4*(rows+4)*(cols+4);
 
   Scalar* a_array1 = internal::aligned_new<Scalar>(arraysize+1);
   Scalar* array1 = a_array1;
@@ -143,9 +141,62 @@ template<int Alignment,typename MatrixType> void map_class_matrix(const MatrixTy
     VERIFY_IS_APPROX(map,s1*m);
   }
 
+  // test inner stride and no outer stride
+  for(int k=0; k<2; ++k)
+  {
+    if(k==1 && (m.innerSize()*2)*m.outerSize() > maxsize2)
+      break;
+    Scalar* array = (k==0 ? array1 : array2);
+
+    Map<MatrixType, Alignment, InnerStride<Dynamic> > map(array, rows, cols, InnerStride<Dynamic>(2));
+    map = m;
+    VERIFY(map.outerStride() == map.innerSize()*2);
+    for(int i = 0; i < m.outerSize(); ++i)
+      for(int j = 0; j < m.innerSize(); ++j)
+      {
+        VERIFY(array[map.innerSize()*i*2+j*2] == m.coeffByOuterInner(i,j));
+        VERIFY(map.coeffByOuterInner(i,j) == m.coeffByOuterInner(i,j));
+      }
+    VERIFY_IS_APPROX(s1*map,s1*m);
+    map *= s1;
+    VERIFY_IS_APPROX(map,s1*m);
+  }
+
   internal::aligned_delete(a_array1, arraysize+1);
 }
 
+// Additional tests for inner-stride but no outer-stride
+template<int>
+void bug1453()
+{
+  const int data[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31};
+  typedef Matrix<int,Dynamic,Dynamic,RowMajor> RowMatrixXi;
+  typedef Matrix<int,2,3,ColMajor> ColMatrix23i;
+  typedef Matrix<int,3,2,ColMajor> ColMatrix32i;
+  typedef Matrix<int,2,3,RowMajor> RowMatrix23i;
+  typedef Matrix<int,3,2,RowMajor> RowMatrix32i;
+
+  VERIFY_IS_APPROX(MatrixXi::Map(data, 2, 3, InnerStride<2>()), MatrixXi::Map(data, 2, 3, Stride<4,2>()));
+  VERIFY_IS_APPROX(MatrixXi::Map(data, 2, 3, InnerStride<>(2)), MatrixXi::Map(data, 2, 3, Stride<4,2>()));
+  VERIFY_IS_APPROX(MatrixXi::Map(data, 3, 2, InnerStride<2>()), MatrixXi::Map(data, 3, 2, Stride<6,2>()));
+  VERIFY_IS_APPROX(MatrixXi::Map(data, 3, 2, InnerStride<>(2)), MatrixXi::Map(data, 3, 2, Stride<6,2>()));
+
+  VERIFY_IS_APPROX(RowMatrixXi::Map(data, 2, 3, InnerStride<2>()), RowMatrixXi::Map(data, 2, 3, Stride<6,2>()));
+  VERIFY_IS_APPROX(RowMatrixXi::Map(data, 2, 3, InnerStride<>(2)), RowMatrixXi::Map(data, 2, 3, Stride<6,2>()));
+  VERIFY_IS_APPROX(RowMatrixXi::Map(data, 3, 2, InnerStride<2>()), RowMatrixXi::Map(data, 3, 2, Stride<4,2>()));
+  VERIFY_IS_APPROX(RowMatrixXi::Map(data, 3, 2, InnerStride<>(2)), RowMatrixXi::Map(data, 3, 2, Stride<4,2>()));
+
+  VERIFY_IS_APPROX(ColMatrix23i::Map(data, InnerStride<2>()), MatrixXi::Map(data, 2, 3, Stride<4,2>()));
+  VERIFY_IS_APPROX(ColMatrix23i::Map(data, InnerStride<>(2)), MatrixXi::Map(data, 2, 3, Stride<4,2>()));
+  VERIFY_IS_APPROX(ColMatrix32i::Map(data, InnerStride<2>()), MatrixXi::Map(data, 3, 2, Stride<6,2>()));
+  VERIFY_IS_APPROX(ColMatrix32i::Map(data, InnerStride<>(2)), MatrixXi::Map(data, 3, 2, Stride<6,2>()));
+
+  VERIFY_IS_APPROX(RowMatrix23i::Map(data, InnerStride<2>()), RowMatrixXi::Map(data, 2, 3, Stride<6,2>()));
+  VERIFY_IS_APPROX(RowMatrix23i::Map(data, InnerStride<>(2)), RowMatrixXi::Map(data, 2, 3, Stride<6,2>()));
+  VERIFY_IS_APPROX(RowMatrix32i::Map(data, InnerStride<2>()), RowMatrixXi::Map(data, 3, 2, Stride<4,2>()));
+  VERIFY_IS_APPROX(RowMatrix32i::Map(data, InnerStride<>(2)), RowMatrixXi::Map(data, 3, 2, Stride<4,2>()));
+}
+
 void test_mapstride()
 {
   for(int i = 0; i < g_repeat; i++) {
@@ -175,6 +226,8 @@ void test_mapstride()
     CALL_SUBTEST_5( map_class_matrix<Unaligned>(MatrixXi(internal::random<int>(1,maxn),internal::random<int>(1,maxn))) );
     CALL_SUBTEST_6( map_class_matrix<Aligned>(MatrixXcd(internal::random<int>(1,maxn),internal::random<int>(1,maxn))) );
     CALL_SUBTEST_6( map_class_matrix<Unaligned>(MatrixXcd(internal::random<int>(1,maxn),internal::random<int>(1,maxn))) );
+
+    CALL_SUBTEST_5( bug1453<0>() );
     
     TEST_SET_BUT_UNUSED_VARIABLE(maxn);
   }
diff --git a/gtsam/3rdparty/Eigen/test/miscmatrices.cpp b/gtsam/3rdparty/Eigen/test/miscmatrices.cpp
index ef20dc749..f17291c40 100644
--- a/gtsam/3rdparty/Eigen/test/miscmatrices.cpp
+++ b/gtsam/3rdparty/Eigen/test/miscmatrices.cpp
@@ -14,7 +14,6 @@ template<typename MatrixType> void miscMatrices(const MatrixType& m)
   /* this test covers the following files:
      DiagonalMatrix.h Ones.h
   */
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
   Index rows = m.rows();
diff --git a/gtsam/3rdparty/Eigen/test/mixingtypes.cpp b/gtsam/3rdparty/Eigen/test/mixingtypes.cpp
index ad9c2c652..45d79aa0c 100644
--- a/gtsam/3rdparty/Eigen/test/mixingtypes.cpp
+++ b/gtsam/3rdparty/Eigen/test/mixingtypes.cpp
@@ -8,13 +8,27 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-// work around "uninitialized" warnings and give that option some testing
-#define EIGEN_INITIALIZE_MATRICES_BY_ZERO
+#if defined(EIGEN_TEST_PART_7)
 
 #ifndef EIGEN_NO_STATIC_ASSERT
 #define EIGEN_NO_STATIC_ASSERT // turn static asserts into runtime asserts in order to check them
 #endif
 
+// ignore double-promotion diagnostic for clang and gcc, if we check for static assertion anyway:
+// TODO do the same for MSVC?
+#if defined(__clang__)
+#  if (__clang_major__ * 100 + __clang_minor__) >= 308
+#    pragma clang diagnostic ignored "-Wdouble-promotion"
+#  endif
+#elif defined(__GNUC__)
+  // TODO is there a minimal GCC version for this? At least g++-4.7 seems to be fine with this.
+#  pragma GCC diagnostic ignored "-Wdouble-promotion"
+#endif
+
+#endif
+
+
+
 #if defined(EIGEN_TEST_PART_1) || defined(EIGEN_TEST_PART_2) || defined(EIGEN_TEST_PART_3)
 
 #ifndef EIGEN_DONT_VECTORIZE
@@ -35,6 +49,28 @@ using namespace std;
   VERIFY_IS_APPROX(XPR,REF); \
   VERIFY( g_called && #XPR" not properly optimized");
 
+template<int SizeAtCompileType>
+void raise_assertion(Index size = SizeAtCompileType)
+{
+  // VERIFY_RAISES_ASSERT(mf+md); // does not even compile
+  Matrix<float, SizeAtCompileType, 1> vf; vf.setRandom(size);
+  Matrix<double, SizeAtCompileType, 1> vd; vd.setRandom(size);
+  VERIFY_RAISES_ASSERT(vf=vd);
+  VERIFY_RAISES_ASSERT(vf+=vd);
+  VERIFY_RAISES_ASSERT(vf-=vd);
+  VERIFY_RAISES_ASSERT(vd=vf);
+  VERIFY_RAISES_ASSERT(vd+=vf);
+  VERIFY_RAISES_ASSERT(vd-=vf);
+
+  //   vd.asDiagonal() * mf;    // does not even compile
+  //   vcd.asDiagonal() * mf;   // does not even compile
+
+#if 0 // we get other compilation errors here than just static asserts
+  VERIFY_RAISES_ASSERT(vd.dot(vf));
+#endif
+}
+
+
 template<int SizeAtCompileType> void mixingtypes(int size = SizeAtCompileType)
 {
   typedef std::complex<float>   CF;
@@ -69,17 +105,10 @@ template<int SizeAtCompileType> void mixingtypes(int size = SizeAtCompileType)
   double epsd = std::sqrt(std::numeric_limits<double>::min EIGEN_EMPTY ());
 
   while(std::abs(sf )<epsf) sf  = internal::random<float>();
-  while(std::abs(sd )<epsd) sf  = internal::random<double>();
+  while(std::abs(sd )<epsd) sd  = internal::random<double>();
   while(std::abs(scf)<epsf) scf = internal::random<CF>();
   while(std::abs(scd)<epsd) scd = internal::random<CD>();
 
-//   VERIFY_RAISES_ASSERT(mf+md); // does not even compile
-
-#ifdef EIGEN_DONT_VECTORIZE
-  VERIFY_RAISES_ASSERT(vf=vd);
-  VERIFY_RAISES_ASSERT(vf+=vd);
-#endif
-  
   // check scalar products
   VERIFY_MIX_SCALAR(vcf * sf , vcf * complex<float>(sf));
   VERIFY_MIX_SCALAR(sd * vcd , complex<double>(sd) * vcd);
@@ -119,9 +148,6 @@ template<int SizeAtCompileType> void mixingtypes(int size = SizeAtCompileType)
 
   // check dot product
   vf.dot(vf);
-#if 0 // we get other compilation errors here than just static asserts
-  VERIFY_RAISES_ASSERT(vd.dot(vf));
-#endif
   VERIFY_IS_APPROX(vcf.dot(vf), vcf.dot(vf.template cast<complex<float> >()));
 
   // check diagonal product
@@ -130,9 +156,6 @@ template<int SizeAtCompileType> void mixingtypes(int size = SizeAtCompileType)
   VERIFY_IS_APPROX(mcf * vf.asDiagonal(), mcf * vf.template cast<complex<float> >().asDiagonal());
   VERIFY_IS_APPROX(md * vcd.asDiagonal(), md.template cast<complex<double> >() * vcd.asDiagonal());
 
-//   vd.asDiagonal() * mf;    // does not even compile
-//   vcd.asDiagonal() * mf;   // does not even compile
-
   // check inner product
   VERIFY_IS_APPROX((vf.transpose() * vcf).value(), (vf.template cast<complex<float> >().transpose() * vcf).value());
 
@@ -296,5 +319,10 @@ void test_mixingtypes()
     CALL_SUBTEST_4(mixingtypes<3>());
     CALL_SUBTEST_5(mixingtypes<4>());
     CALL_SUBTEST_6(mixingtypes<Dynamic>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE)));
+    CALL_SUBTEST_7(raise_assertion<Dynamic>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE)));
   }
+  CALL_SUBTEST_7(raise_assertion<0>());
+  CALL_SUBTEST_7(raise_assertion<3>());
+  CALL_SUBTEST_7(raise_assertion<4>());
+  CALL_SUBTEST_7(raise_assertion<Dynamic>(0));
 }
diff --git a/gtsam/3rdparty/Eigen/test/nomalloc.cpp b/gtsam/3rdparty/Eigen/test/nomalloc.cpp
index 50756c2fb..b7ea4d362 100644
--- a/gtsam/3rdparty/Eigen/test/nomalloc.cpp
+++ b/gtsam/3rdparty/Eigen/test/nomalloc.cpp
@@ -24,7 +24,6 @@ template<typename MatrixType> void nomalloc(const MatrixType& m)
 {
   /* this test check no dynamic memory allocation are issued with fixed-size matrices
   */
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = m.rows();
diff --git a/gtsam/3rdparty/Eigen/test/permutationmatrices.cpp b/gtsam/3rdparty/Eigen/test/permutationmatrices.cpp
index db1266579..e885f0e04 100644
--- a/gtsam/3rdparty/Eigen/test/permutationmatrices.cpp
+++ b/gtsam/3rdparty/Eigen/test/permutationmatrices.cpp
@@ -14,14 +14,15 @@
 using namespace std;
 template<typename MatrixType> void permutationmatrices(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   enum { Rows = MatrixType::RowsAtCompileTime, Cols = MatrixType::ColsAtCompileTime,
          Options = MatrixType::Options };
   typedef PermutationMatrix<Rows> LeftPermutationType;
+  typedef Transpositions<Rows> LeftTranspositionsType;
   typedef Matrix<int, Rows, 1> LeftPermutationVectorType;
   typedef Map<LeftPermutationType> MapLeftPerm;
   typedef PermutationMatrix<Cols> RightPermutationType;
+  typedef Transpositions<Cols> RightTranspositionsType;
   typedef Matrix<int, Cols, 1> RightPermutationVectorType;
   typedef Map<RightPermutationType> MapRightPerm;
 
@@ -35,6 +36,8 @@ template<typename MatrixType> void permutationmatrices(const MatrixType& m)
   RightPermutationVectorType rv;
   randomPermutationVector(rv, cols);
   RightPermutationType rp(rv);
+  LeftTranspositionsType lt(lv);
+  RightTranspositionsType rt(rv);
   MatrixType m_permuted = MatrixType::Random(rows,cols);
   
   VERIFY_EVALUATION_COUNT(m_permuted = lp * m_original * rp, 1); // 1 temp for sub expression "lp * m_original"
@@ -115,6 +118,14 @@ template<typename MatrixType> void permutationmatrices(const MatrixType& m)
     Matrix<Scalar, Cols, Cols> B = rp.transpose();
     VERIFY_IS_APPROX(A, B.transpose());
   }
+
+  m_permuted = m_original;
+  lp = lt;
+  rp = rt;
+  VERIFY_EVALUATION_COUNT(m_permuted = lt * m_permuted * rt, 1);
+  VERIFY_IS_APPROX(m_permuted, lp*m_original*rp.transpose());
+  
+  VERIFY_IS_APPROX(lt.inverse()*m_permuted*rt.inverse(), m_original);
 }
 
 template<typename T>
diff --git a/gtsam/3rdparty/Eigen/test/product_extra.cpp b/gtsam/3rdparty/Eigen/test/product_extra.cpp
index e2b855bff..de2709d8b 100644
--- a/gtsam/3rdparty/Eigen/test/product_extra.cpp
+++ b/gtsam/3rdparty/Eigen/test/product_extra.cpp
@@ -11,7 +11,6 @@
 
 template<typename MatrixType> void product_extra(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, 1, Dynamic> RowVectorType;
   typedef Matrix<Scalar, Dynamic, 1> ColVectorType;
diff --git a/gtsam/3rdparty/Eigen/test/product_notemporary.cpp b/gtsam/3rdparty/Eigen/test/product_notemporary.cpp
index 30592b79e..28865d398 100644
--- a/gtsam/3rdparty/Eigen/test/product_notemporary.cpp
+++ b/gtsam/3rdparty/Eigen/test/product_notemporary.cpp
@@ -15,7 +15,6 @@ template<typename MatrixType> void product_notemporary(const MatrixType& m)
 {
   /* This test checks the number of temporaries created
    * during the evaluation of a complex expression */
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   typedef Matrix<Scalar, 1, Dynamic> RowVectorType;
diff --git a/gtsam/3rdparty/Eigen/test/product_selfadjoint.cpp b/gtsam/3rdparty/Eigen/test/product_selfadjoint.cpp
index 3d768aa7e..88d68391b 100644
--- a/gtsam/3rdparty/Eigen/test/product_selfadjoint.cpp
+++ b/gtsam/3rdparty/Eigen/test/product_selfadjoint.cpp
@@ -11,7 +11,6 @@
 
 template<typename MatrixType> void product_selfadjoint(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
   typedef Matrix<Scalar, 1, MatrixType::RowsAtCompileTime> RowVectorType;
diff --git a/gtsam/3rdparty/Eigen/test/product_symm.cpp b/gtsam/3rdparty/Eigen/test/product_symm.cpp
index 8c44383f9..7d1042a4f 100644
--- a/gtsam/3rdparty/Eigen/test/product_symm.cpp
+++ b/gtsam/3rdparty/Eigen/test/product_symm.cpp
@@ -16,7 +16,6 @@ template<typename Scalar, int Size, int OtherSize> void symm(int size = Size, in
   typedef Matrix<Scalar, OtherSize, Size> Rhs2;
   enum { order = OtherSize==1 ? 0 : RowMajor };
   typedef Matrix<Scalar, Size, OtherSize,order> Rhs3;
-  typedef typename MatrixType::Index Index;
 
   Index rows = size;
   Index cols = size;
diff --git a/gtsam/3rdparty/Eigen/test/product_syrk.cpp b/gtsam/3rdparty/Eigen/test/product_syrk.cpp
index e10f0f2f2..3ebbe14ca 100644
--- a/gtsam/3rdparty/Eigen/test/product_syrk.cpp
+++ b/gtsam/3rdparty/Eigen/test/product_syrk.cpp
@@ -11,7 +11,6 @@
 
 template<typename MatrixType> void syrk(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime, RowMajor> RMatrixType;
   typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, Dynamic> Rhs1;
diff --git a/gtsam/3rdparty/Eigen/test/product_trmm.cpp b/gtsam/3rdparty/Eigen/test/product_trmm.cpp
index 12e554410..e08d9f39f 100644
--- a/gtsam/3rdparty/Eigen/test/product_trmm.cpp
+++ b/gtsam/3rdparty/Eigen/test/product_trmm.cpp
@@ -29,7 +29,7 @@ void trmm(int rows=get_random_size<Scalar>(),
   typedef Matrix<Scalar,Dynamic,OtherCols,OtherCols==1?ColMajor:ResOrder> ResXS;
   typedef Matrix<Scalar,OtherCols,Dynamic,OtherCols==1?RowMajor:ResOrder> ResSX;
 
-  TriMatrix  mat(rows,cols), tri(rows,cols), triTr(cols,rows);
+  TriMatrix  mat(rows,cols), tri(rows,cols), triTr(cols,rows), s1tri(rows,cols), s1triTr(cols,rows);
   
   OnTheRight  ge_right(cols,otherCols);
   OnTheLeft   ge_left(otherCols,rows);
@@ -42,6 +42,8 @@ void trmm(int rows=get_random_size<Scalar>(),
   mat.setRandom();
   tri = mat.template triangularView<Mode>();
   triTr = mat.transpose().template triangularView<Mode>();
+  s1tri = (s1*mat).template triangularView<Mode>();
+  s1triTr = (s1*mat).transpose().template triangularView<Mode>();
   ge_right.setRandom();
   ge_left.setRandom();
 
@@ -51,19 +53,29 @@ void trmm(int rows=get_random_size<Scalar>(),
   VERIFY_IS_APPROX( ge_xs.noalias() = mat.template triangularView<Mode>() * ge_right, tri * ge_right);
   VERIFY_IS_APPROX( ge_sx.noalias() = ge_left * mat.template triangularView<Mode>(), ge_left * tri);
 
-  VERIFY_IS_APPROX( ge_xs.noalias() = (s1*mat.adjoint()).template triangularView<Mode>() * (s2*ge_left.transpose()), s1*triTr.conjugate() * (s2*ge_left.transpose()));
-  VERIFY_IS_APPROX( ge_sx.noalias() = ge_right.transpose() * mat.adjoint().template triangularView<Mode>(), ge_right.transpose() * triTr.conjugate());
+  if((Mode&UnitDiag)==0)
+    VERIFY_IS_APPROX( ge_xs.noalias() = (s1*mat.adjoint()).template triangularView<Mode>() * (s2*ge_left.transpose()), s1*triTr.conjugate() * (s2*ge_left.transpose()));
   
-  VERIFY_IS_APPROX( ge_xs.noalias() = (s1*mat.adjoint()).template triangularView<Mode>() * (s2*ge_left.adjoint()), s1*triTr.conjugate() * (s2*ge_left.adjoint()));
-  VERIFY_IS_APPROX( ge_sx.noalias() = ge_right.adjoint() * mat.adjoint().template triangularView<Mode>(), ge_right.adjoint() * triTr.conjugate());
+  VERIFY_IS_APPROX( ge_xs.noalias() = (s1*mat.transpose()).template triangularView<Mode>() * (s2*ge_left.transpose()), s1triTr * (s2*ge_left.transpose()));
+  VERIFY_IS_APPROX( ge_sx.noalias() = (s2*ge_left) * (s1*mat).template triangularView<Mode>(), (s2*ge_left)*s1tri);
 
+  VERIFY_IS_APPROX( ge_sx.noalias() = ge_right.transpose() * mat.adjoint().template triangularView<Mode>(), ge_right.transpose() * triTr.conjugate());
+  VERIFY_IS_APPROX( ge_sx.noalias() = ge_right.adjoint() * mat.adjoint().template triangularView<Mode>(), ge_right.adjoint() * triTr.conjugate());
+  
   ge_xs_save = ge_xs;
-  VERIFY_IS_APPROX( (ge_xs_save + s1*triTr.conjugate() * (s2*ge_left.adjoint())).eval(), ge_xs.noalias() += (s1*mat.adjoint()).template triangularView<Mode>() * (s2*ge_left.adjoint()) );
+  if((Mode&UnitDiag)==0)
+    VERIFY_IS_APPROX( (ge_xs_save + s1*triTr.conjugate() * (s2*ge_left.adjoint())).eval(), ge_xs.noalias() += (s1*mat.adjoint()).template triangularView<Mode>() * (s2*ge_left.adjoint()) );
+  ge_xs_save = ge_xs;
+  VERIFY_IS_APPROX( (ge_xs_save + s1triTr * (s2*ge_left.adjoint())).eval(), ge_xs.noalias() += (s1*mat.transpose()).template triangularView<Mode>() * (s2*ge_left.adjoint()) );
   ge_sx.setRandom();
   ge_sx_save = ge_sx;
-  VERIFY_IS_APPROX( ge_sx_save - (ge_right.adjoint() * (-s1 * triTr).conjugate()).eval(), ge_sx.noalias() -= (ge_right.adjoint() * (-s1 * mat).adjoint().template triangularView<Mode>()).eval());
+  if((Mode&UnitDiag)==0)
+    VERIFY_IS_APPROX( ge_sx_save - (ge_right.adjoint() * (-s1 * triTr).conjugate()).eval(), ge_sx.noalias() -= (ge_right.adjoint() * (-s1 * mat).adjoint().template triangularView<Mode>()).eval());
   
-  VERIFY_IS_APPROX( ge_xs = (s1*mat).adjoint().template triangularView<Mode>() * ge_left.adjoint(), numext::conj(s1) * triTr.conjugate() * ge_left.adjoint());
+  if((Mode&UnitDiag)==0)
+    VERIFY_IS_APPROX( ge_xs = (s1*mat).adjoint().template triangularView<Mode>() * ge_left.adjoint(), numext::conj(s1) * triTr.conjugate() * ge_left.adjoint());
+  VERIFY_IS_APPROX( ge_xs = (s1*mat).transpose().template triangularView<Mode>() * ge_left.adjoint(), s1triTr * ge_left.adjoint());
+
   
   // TODO check with sub-matrix expressions ?
 }
diff --git a/gtsam/3rdparty/Eigen/test/product_trmv.cpp b/gtsam/3rdparty/Eigen/test/product_trmv.cpp
index 57a202afc..65d66e57b 100644
--- a/gtsam/3rdparty/Eigen/test/product_trmv.cpp
+++ b/gtsam/3rdparty/Eigen/test/product_trmv.cpp
@@ -11,7 +11,6 @@
 
 template<typename MatrixType> void trmv(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
diff --git a/gtsam/3rdparty/Eigen/test/qr.cpp b/gtsam/3rdparty/Eigen/test/qr.cpp
index dfcc1e8f9..56884605b 100644
--- a/gtsam/3rdparty/Eigen/test/qr.cpp
+++ b/gtsam/3rdparty/Eigen/test/qr.cpp
@@ -12,8 +12,6 @@
 
 template<typename MatrixType> void qr(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
-
   Index rows = m.rows();
   Index cols = m.cols();
 
diff --git a/gtsam/3rdparty/Eigen/test/qr_colpivoting.cpp b/gtsam/3rdparty/Eigen/test/qr_colpivoting.cpp
index 26ed27f5c..96c0badb7 100644
--- a/gtsam/3rdparty/Eigen/test/qr_colpivoting.cpp
+++ b/gtsam/3rdparty/Eigen/test/qr_colpivoting.cpp
@@ -14,8 +14,6 @@
 
 template <typename MatrixType>
 void cod() {
-  typedef typename MatrixType::Index Index;
-
   Index rows = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
   Index cols = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
   Index cols2 = internal::random<Index>(2, EIGEN_TEST_MAX_SIZE);
@@ -94,7 +92,6 @@ void cod_fixedsize() {
 template<typename MatrixType> void qr()
 {
   using std::sqrt;
-  typedef typename MatrixType::Index Index;
 
   Index rows = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE), cols = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE), cols2 = internal::random<Index>(2,EIGEN_TEST_MAX_SIZE);
   Index rank = internal::random<Index>(1, (std::min)(rows, cols)-1);
@@ -211,7 +208,6 @@ template<typename MatrixType> void qr_kahan_matrix()
 {
   using std::sqrt;
   using std::abs;
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
 
diff --git a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp
index 70e89c198..4d8ef686e 100644
--- a/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp
+++ b/gtsam/3rdparty/Eigen/test/qr_fullpivoting.cpp
@@ -13,8 +13,6 @@
 
 template<typename MatrixType> void qr()
 {
-  typedef typename MatrixType::Index Index;
-
   Index max_size = EIGEN_TEST_MAX_SIZE;
   Index min_size = numext::maxi(1,EIGEN_TEST_MAX_SIZE/10);
   Index rows  = internal::random<Index>(min_size,max_size),
diff --git a/gtsam/3rdparty/Eigen/test/qtvector.cpp b/gtsam/3rdparty/Eigen/test/qtvector.cpp
index 2be885e48..22df0d515 100644
--- a/gtsam/3rdparty/Eigen/test/qtvector.cpp
+++ b/gtsam/3rdparty/Eigen/test/qtvector.cpp
@@ -18,8 +18,6 @@
 template<typename MatrixType>
 void check_qtvector_matrix(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
-
   Index rows = m.rows();
   Index cols = m.cols();
   MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols);
diff --git a/gtsam/3rdparty/Eigen/test/real_qz.cpp b/gtsam/3rdparty/Eigen/test/real_qz.cpp
index 99ac31235..3c1492e4b 100644
--- a/gtsam/3rdparty/Eigen/test/real_qz.cpp
+++ b/gtsam/3rdparty/Eigen/test/real_qz.cpp
@@ -18,7 +18,6 @@ template<typename MatrixType> void real_qz(const MatrixType& m)
      RealQZ.h
   */
   using std::abs;
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   
   Index dim = m.cols();
diff --git a/gtsam/3rdparty/Eigen/test/redux.cpp b/gtsam/3rdparty/Eigen/test/redux.cpp
index 989e1057b..213f080aa 100644
--- a/gtsam/3rdparty/Eigen/test/redux.cpp
+++ b/gtsam/3rdparty/Eigen/test/redux.cpp
@@ -9,12 +9,13 @@
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 #define TEST_ENABLE_TEMPORARY_TRACKING
+#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8
+// ^^ see bug 1449
 
 #include "main.h"
 
 template<typename MatrixType> void matrixRedux(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
 
@@ -79,7 +80,6 @@ template<typename MatrixType> void matrixRedux(const MatrixType& m)
 template<typename VectorType> void vectorRedux(const VectorType& w)
 {
   using std::abs;
-  typedef typename VectorType::Index Index;
   typedef typename VectorType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   Index size = w.size();
diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp
index 769db0414..704495aff 100644
--- a/gtsam/3rdparty/Eigen/test/ref.cpp
+++ b/gtsam/3rdparty/Eigen/test/ref.cpp
@@ -13,7 +13,7 @@
 #endif
 
 #define TEST_ENABLE_TEMPORARY_TRACKING
-
+#define TEST_CHECK_STATIC_ASSERTIONS
 #include "main.h"
 
 // test Ref.h
@@ -32,7 +32,6 @@
 
 template<typename MatrixType> void ref_matrix(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   typedef Matrix<Scalar,Dynamic,Dynamic,MatrixType::Options> DynMatrixType;
@@ -80,7 +79,6 @@ template<typename MatrixType> void ref_matrix(const MatrixType& m)
 
 template<typename VectorType> void ref_vector(const VectorType& m)
 {
-  typedef typename VectorType::Index Index;
   typedef typename VectorType::Scalar Scalar;
   typedef typename VectorType::RealScalar RealScalar;
   typedef Matrix<Scalar,Dynamic,1,VectorType::Options> DynMatrixType;
@@ -255,6 +253,17 @@ void test_ref_overloads()
   test_ref_ambiguous(A, B);
 }
 
+void test_ref_fixed_size_assert()
+{
+  Vector4f v4;
+  VectorXf vx(10);
+  VERIFY_RAISES_STATIC_ASSERT( Ref<Vector3f> y = v4; (void)y; );
+  VERIFY_RAISES_STATIC_ASSERT( Ref<Vector3f> y = vx.head<4>(); (void)y; );
+  VERIFY_RAISES_STATIC_ASSERT( Ref<const Vector3f> y = v4; (void)y; );
+  VERIFY_RAISES_STATIC_ASSERT( Ref<const Vector3f> y = vx.head<4>(); (void)y; );
+  VERIFY_RAISES_STATIC_ASSERT( Ref<const Vector3f> y = 2*v4; (void)y; );
+}
+
 void test_ref()
 {
   for(int i = 0; i < g_repeat; i++) {
@@ -277,4 +286,5 @@ void test_ref()
   }
   
   CALL_SUBTEST_7( test_ref_overloads() );
+  CALL_SUBTEST_7( test_ref_fixed_size_assert() );
 }
diff --git a/gtsam/3rdparty/Eigen/test/schur_real.cpp b/gtsam/3rdparty/Eigen/test/schur_real.cpp
index 4aede87df..e5229e6e8 100644
--- a/gtsam/3rdparty/Eigen/test/schur_real.cpp
+++ b/gtsam/3rdparty/Eigen/test/schur_real.cpp
@@ -13,8 +13,6 @@
 
 template<typename MatrixType> void verifyIsQuasiTriangular(const MatrixType& T)
 {
-  typedef typename MatrixType::Index Index;
-
   const Index size = T.cols();
   typedef typename MatrixType::Scalar Scalar;
 
diff --git a/gtsam/3rdparty/Eigen/test/selfadjoint.cpp b/gtsam/3rdparty/Eigen/test/selfadjoint.cpp
index 92401e506..bb11cc351 100644
--- a/gtsam/3rdparty/Eigen/test/selfadjoint.cpp
+++ b/gtsam/3rdparty/Eigen/test/selfadjoint.cpp
@@ -7,6 +7,7 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
+#define TEST_CHECK_STATIC_ASSERTIONS
 #include "main.h"
 
 // This file tests the basic selfadjointView API,
@@ -14,7 +15,6 @@
 
 template<typename MatrixType> void selfadjoint(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
 
   Index rows = m.rows();
@@ -45,6 +45,9 @@ template<typename MatrixType> void selfadjoint(const MatrixType& m)
   m4 = m2;
   m4 -= m1.template selfadjointView<Lower>();
   VERIFY_IS_APPROX(m4, m2-m3);
+
+  VERIFY_RAISES_STATIC_ASSERT(m2.template selfadjointView<StrictlyUpper>());
+  VERIFY_RAISES_STATIC_ASSERT(m2.template selfadjointView<UnitLower>());
 }
 
 void bug_159()
diff --git a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp
index 384985028..d0ef722b6 100644
--- a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp
+++ b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp
@@ -228,8 +228,8 @@ template<typename SparseMatrixType> void sparse_basic(const SparseMatrixType& re
       VERIFY_RAISES_ASSERT( m1 -= m1.innerVector(0) );
       VERIFY_RAISES_ASSERT( refM1 -= m1.innerVector(0) );
       VERIFY_RAISES_ASSERT( refM1 += m1.innerVector(0) );
-      m1 = m4; refM1 = refM4;
     }
+    m1 = m4; refM1 = refM4;
 
     // test aliasing
     VERIFY_IS_APPROX((m1 = -m1), (refM1 = -refM1));
@@ -630,7 +630,8 @@ void big_sparse_triplet(Index rows, Index cols, double density) {
   {
     Index r = internal::random<Index>(0,rows-1);
     Index c = internal::random<Index>(0,cols-1);
-    Scalar v = internal::random<Scalar>();
+    // use positive values to prevent numerical cancellation errors in sum
+    Scalar v = numext::abs(internal::random<Scalar>());
     triplets.push_back(TripletType(r,c,v));
     sum += v;
   }
diff --git a/gtsam/3rdparty/Eigen/test/sparse_product.cpp b/gtsam/3rdparty/Eigen/test/sparse_product.cpp
index 197586741..7f77bb742 100644
--- a/gtsam/3rdparty/Eigen/test/sparse_product.cpp
+++ b/gtsam/3rdparty/Eigen/test/sparse_product.cpp
@@ -7,6 +7,12 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
+#if defined(_MSC_VER) && (_MSC_VER==1800)
+// This unit test takes forever to compile in Release mode with MSVC 2013,
+// multiple hours. So let's switch off optimization for this one.
+#pragma optimize("",off)
+#endif
+
 static long int nb_temporaries;
 
 inline void on_temporary_creation() {
@@ -371,6 +377,88 @@ void bug_942()
   VERIFY_IS_APPROX( ( d.asDiagonal()*cmA ).eval().coeff(0,0), res );
 }
 
+template<typename Real>
+void test_mixing_types()
+{
+  typedef std::complex<Real> Cplx;
+  typedef SparseMatrix<Real> SpMatReal;
+  typedef SparseMatrix<Cplx> SpMatCplx;
+  typedef SparseMatrix<Cplx,RowMajor> SpRowMatCplx;
+  typedef Matrix<Real,Dynamic,Dynamic> DenseMatReal;
+  typedef Matrix<Cplx,Dynamic,Dynamic> DenseMatCplx;
+
+  Index n = internal::random<Index>(1,100);
+  double density = (std::max)(8./(n*n), 0.2);
+
+  SpMatReal sR1(n,n);
+  SpMatCplx sC1(n,n), sC2(n,n), sC3(n,n);
+  SpRowMatCplx sCR(n,n);
+  DenseMatReal dR1(n,n);
+  DenseMatCplx dC1(n,n), dC2(n,n), dC3(n,n);
+
+  initSparse<Real>(density, dR1, sR1);
+  initSparse<Cplx>(density, dC1, sC1);
+  initSparse<Cplx>(density, dC2, sC2);
+
+  VERIFY_IS_APPROX( sC2 = (sR1 * sC1),                         dC3 = dR1.template cast<Cplx>() * dC1 );
+  VERIFY_IS_APPROX( sC2 = (sC1 * sR1),                         dC3 = dC1 * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sC2 = (sR1.transpose() * sC1),             dC3 = dR1.template cast<Cplx>().transpose() * dC1 );
+  VERIFY_IS_APPROX( sC2 = (sC1.transpose() * sR1),             dC3 = dC1.transpose() * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sC2 = (sR1 * sC1.transpose()),             dC3 = dR1.template cast<Cplx>() * dC1.transpose() );
+  VERIFY_IS_APPROX( sC2 = (sC1 * sR1.transpose()),             dC3 = dC1 * dR1.template cast<Cplx>().transpose() );
+  VERIFY_IS_APPROX( sC2 = (sR1.transpose() * sC1.transpose()), dC3 = dR1.template cast<Cplx>().transpose() * dC1.transpose() );
+  VERIFY_IS_APPROX( sC2 = (sC1.transpose() * sR1.transpose()), dC3 = dC1.transpose() * dR1.template cast<Cplx>().transpose() );
+
+  VERIFY_IS_APPROX( sCR = (sR1 * sC1),                         dC3 = dR1.template cast<Cplx>() * dC1 );
+  VERIFY_IS_APPROX( sCR = (sC1 * sR1),                         dC3 = dC1 * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sCR = (sR1.transpose() * sC1),             dC3 = dR1.template cast<Cplx>().transpose() * dC1 );
+  VERIFY_IS_APPROX( sCR = (sC1.transpose() * sR1),             dC3 = dC1.transpose() * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sCR = (sR1 * sC1.transpose()),             dC3 = dR1.template cast<Cplx>() * dC1.transpose() );
+  VERIFY_IS_APPROX( sCR = (sC1 * sR1.transpose()),             dC3 = dC1 * dR1.template cast<Cplx>().transpose() );
+  VERIFY_IS_APPROX( sCR = (sR1.transpose() * sC1.transpose()), dC3 = dR1.template cast<Cplx>().transpose() * dC1.transpose() );
+  VERIFY_IS_APPROX( sCR = (sC1.transpose() * sR1.transpose()), dC3 = dC1.transpose() * dR1.template cast<Cplx>().transpose() );
+
+
+  VERIFY_IS_APPROX( sC2 = (sR1 * sC1).pruned(),                         dC3 = dR1.template cast<Cplx>() * dC1 );
+  VERIFY_IS_APPROX( sC2 = (sC1 * sR1).pruned(),                         dC3 = dC1 * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sC2 = (sR1.transpose() * sC1).pruned(),             dC3 = dR1.template cast<Cplx>().transpose() * dC1 );
+  VERIFY_IS_APPROX( sC2 = (sC1.transpose() * sR1).pruned(),             dC3 = dC1.transpose() * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sC2 = (sR1 * sC1.transpose()).pruned(),             dC3 = dR1.template cast<Cplx>() * dC1.transpose() );
+  VERIFY_IS_APPROX( sC2 = (sC1 * sR1.transpose()).pruned(),             dC3 = dC1 * dR1.template cast<Cplx>().transpose() );
+  VERIFY_IS_APPROX( sC2 = (sR1.transpose() * sC1.transpose()).pruned(), dC3 = dR1.template cast<Cplx>().transpose() * dC1.transpose() );
+  VERIFY_IS_APPROX( sC2 = (sC1.transpose() * sR1.transpose()).pruned(), dC3 = dC1.transpose() * dR1.template cast<Cplx>().transpose() );
+
+  VERIFY_IS_APPROX( sCR = (sR1 * sC1).pruned(),                         dC3 = dR1.template cast<Cplx>() * dC1 );
+  VERIFY_IS_APPROX( sCR = (sC1 * sR1).pruned(),                         dC3 = dC1 * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sCR = (sR1.transpose() * sC1).pruned(),             dC3 = dR1.template cast<Cplx>().transpose() * dC1 );
+  VERIFY_IS_APPROX( sCR = (sC1.transpose() * sR1).pruned(),             dC3 = dC1.transpose() * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( sCR = (sR1 * sC1.transpose()).pruned(),             dC3 = dR1.template cast<Cplx>() * dC1.transpose() );
+  VERIFY_IS_APPROX( sCR = (sC1 * sR1.transpose()).pruned(),             dC3 = dC1 * dR1.template cast<Cplx>().transpose() );
+  VERIFY_IS_APPROX( sCR = (sR1.transpose() * sC1.transpose()).pruned(), dC3 = dR1.template cast<Cplx>().transpose() * dC1.transpose() );
+  VERIFY_IS_APPROX( sCR = (sC1.transpose() * sR1.transpose()).pruned(), dC3 = dC1.transpose() * dR1.template cast<Cplx>().transpose() );
+
+
+  VERIFY_IS_APPROX( dC2 = (sR1 * sC1),                         dC3 = dR1.template cast<Cplx>() * dC1 );
+  VERIFY_IS_APPROX( dC2 = (sC1 * sR1),                         dC3 = dC1 * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( dC2 = (sR1.transpose() * sC1),             dC3 = dR1.template cast<Cplx>().transpose() * dC1 );
+  VERIFY_IS_APPROX( dC2 = (sC1.transpose() * sR1),             dC3 = dC1.transpose() * dR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( dC2 = (sR1 * sC1.transpose()),             dC3 = dR1.template cast<Cplx>() * dC1.transpose() );
+  VERIFY_IS_APPROX( dC2 = (sC1 * sR1.transpose()),             dC3 = dC1 * dR1.template cast<Cplx>().transpose() );
+  VERIFY_IS_APPROX( dC2 = (sR1.transpose() * sC1.transpose()), dC3 = dR1.template cast<Cplx>().transpose() * dC1.transpose() );
+  VERIFY_IS_APPROX( dC2 = (sC1.transpose() * sR1.transpose()), dC3 = dC1.transpose() * dR1.template cast<Cplx>().transpose() );
+
+
+  VERIFY_IS_APPROX( dC2 = dR1 * sC1, dC3 = dR1.template cast<Cplx>() * sC1 );
+  VERIFY_IS_APPROX( dC2 = sR1 * dC1, dC3 = sR1.template cast<Cplx>() * dC1 );
+  VERIFY_IS_APPROX( dC2 = dC1 * sR1, dC3 = dC1 * sR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( dC2 = sC1 * dR1, dC3 = sC1 * dR1.template cast<Cplx>() );
+
+  VERIFY_IS_APPROX( dC2 = dR1.row(0) * sC1, dC3 = dR1.template cast<Cplx>().row(0) * sC1 );
+  VERIFY_IS_APPROX( dC2 = sR1 * dC1.col(0), dC3 = sR1.template cast<Cplx>() * dC1.col(0) );
+  VERIFY_IS_APPROX( dC2 = dC1.row(0) * sR1, dC3 = dC1.row(0) * sR1.template cast<Cplx>() );
+  VERIFY_IS_APPROX( dC2 = sC1 * dR1.col(0), dC3 = sC1 * dR1.template cast<Cplx>().col(0) );
+}
+
 void test_sparse_product()
 {
   for(int i = 0; i < g_repeat; i++) {
@@ -381,5 +469,7 @@ void test_sparse_product()
     CALL_SUBTEST_2( (sparse_product<SparseMatrix<std::complex<double>, RowMajor > >()) );
     CALL_SUBTEST_3( (sparse_product<SparseMatrix<float,ColMajor,long int> >()) );
     CALL_SUBTEST_4( (sparse_product_regression_test<SparseMatrix<double,RowMajor>, Matrix<double, Dynamic, Dynamic, RowMajor> >()) );
+
+    CALL_SUBTEST_5( (test_mixing_types<float>()) );
   }
 }
diff --git a/gtsam/3rdparty/Eigen/test/sparseqr.cpp b/gtsam/3rdparty/Eigen/test/sparseqr.cpp
index e8605fd21..f0e721fce 100644
--- a/gtsam/3rdparty/Eigen/test/sparseqr.cpp
+++ b/gtsam/3rdparty/Eigen/test/sparseqr.cpp
@@ -54,6 +54,28 @@ template<typename Scalar> void test_sparseqr_scalar()
   
   b = dA * DenseVector::Random(A.cols());
   solver.compute(A);
+
+  // Q should be MxM
+  VERIFY_IS_EQUAL(solver.matrixQ().rows(), A.rows());
+  VERIFY_IS_EQUAL(solver.matrixQ().cols(), A.rows());
+
+  // R should be MxN
+  VERIFY_IS_EQUAL(solver.matrixR().rows(), A.rows());
+  VERIFY_IS_EQUAL(solver.matrixR().cols(), A.cols());
+
+  // Q and R can be multiplied
+  DenseMat recoveredA = solver.matrixQ()
+                      * DenseMat(solver.matrixR().template triangularView<Upper>())
+                      * solver.colsPermutation().transpose();
+  VERIFY_IS_EQUAL(recoveredA.rows(), A.rows());
+  VERIFY_IS_EQUAL(recoveredA.cols(), A.cols());
+
+  // and in the full rank case the original matrix is recovered
+  if (solver.rank() == A.cols())
+  {
+      VERIFY_IS_APPROX(A, recoveredA);
+  }
+
   if(internal::random<float>(0,1)>0.5f)
     solver.factorize(A);  // this checks that calling analyzePattern is not needed if the pattern do not change.
   if (solver.info() != Success)
diff --git a/gtsam/3rdparty/Eigen/test/stable_norm.cpp b/gtsam/3rdparty/Eigen/test/stable_norm.cpp
index c3eb5ff31..ac8b12911 100644
--- a/gtsam/3rdparty/Eigen/test/stable_norm.cpp
+++ b/gtsam/3rdparty/Eigen/test/stable_norm.cpp
@@ -21,7 +21,6 @@ template<typename MatrixType> void stable_norm(const MatrixType& m)
   */
   using std::sqrt;
   using std::abs;
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   
@@ -65,6 +64,8 @@ template<typename MatrixType> void stable_norm(const MatrixType& m)
     factor = internal::random<Scalar>();
   Scalar small = factor * ((std::numeric_limits<RealScalar>::min)() * RealScalar(1e4));
 
+  Scalar one(1);
+
   MatrixType  vzero = MatrixType::Zero(rows, cols),
               vrand = MatrixType::Random(rows, cols),
               vbig(rows, cols),
@@ -78,6 +79,14 @@ template<typename MatrixType> void stable_norm(const MatrixType& m)
   VERIFY_IS_APPROX(vrand.blueNorm(),        vrand.norm());
   VERIFY_IS_APPROX(vrand.hypotNorm(),       vrand.norm());
 
+  // test with expressions as input
+  VERIFY_IS_APPROX((one*vrand).stableNorm(),      vrand.norm());
+  VERIFY_IS_APPROX((one*vrand).blueNorm(),        vrand.norm());
+  VERIFY_IS_APPROX((one*vrand).hypotNorm(),       vrand.norm());
+  VERIFY_IS_APPROX((one*vrand+one*vrand-one*vrand).stableNorm(),      vrand.norm());
+  VERIFY_IS_APPROX((one*vrand+one*vrand-one*vrand).blueNorm(),        vrand.norm());
+  VERIFY_IS_APPROX((one*vrand+one*vrand-one*vrand).hypotNorm(),       vrand.norm());
+
   RealScalar size = static_cast<RealScalar>(m.size());
 
   // test numext::isfinite
diff --git a/gtsam/3rdparty/Eigen/test/stddeque.cpp b/gtsam/3rdparty/Eigen/test/stddeque.cpp
index bb4b476f3..b511c4e61 100644
--- a/gtsam/3rdparty/Eigen/test/stddeque.cpp
+++ b/gtsam/3rdparty/Eigen/test/stddeque.cpp
@@ -15,8 +15,6 @@
 template<typename MatrixType>
 void check_stddeque_matrix(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
-  
   Index rows = m.rows();
   Index cols = m.cols();
   MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols);
diff --git a/gtsam/3rdparty/Eigen/test/stdlist.cpp b/gtsam/3rdparty/Eigen/test/stdlist.cpp
index 17cce779a..23cbe9039 100644
--- a/gtsam/3rdparty/Eigen/test/stdlist.cpp
+++ b/gtsam/3rdparty/Eigen/test/stdlist.cpp
@@ -15,8 +15,6 @@
 template<typename MatrixType>
 void check_stdlist_matrix(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
-  
   Index rows = m.rows();
   Index cols = m.cols();
   MatrixType x = MatrixType::Random(rows,cols), y = MatrixType::Random(rows,cols);
diff --git a/gtsam/3rdparty/Eigen/test/stdvector.cpp b/gtsam/3rdparty/Eigen/test/stdvector.cpp
index 50cb3341d..fa928ea4f 100644
--- a/gtsam/3rdparty/Eigen/test/stdvector.cpp
+++ b/gtsam/3rdparty/Eigen/test/stdvector.cpp
@@ -117,6 +117,16 @@ void check_stdvector_quaternion(const QuaternionType&)
   }
 }
 
+// the code below triggered an invalid warning with gcc >= 7
+// eigen/Eigen/src/Core/util/Memory.h:189:12: warning: argument 1 value '18446744073709551612' exceeds maximum object size 9223372036854775807
+// This has been reported to gcc there: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=87544
+void std_vector_gcc_warning()
+{
+  typedef Eigen::Vector3f T;
+  std::vector<T, Eigen::aligned_allocator<T> > v;
+  v.push_back(T());
+}
+
 void test_stdvector()
 {
   // some non vectorizable fixed sizes
diff --git a/gtsam/3rdparty/Eigen/test/svd_common.h b/gtsam/3rdparty/Eigen/test/svd_common.h
index 605d5dfef..cba066593 100644
--- a/gtsam/3rdparty/Eigen/test/svd_common.h
+++ b/gtsam/3rdparty/Eigen/test/svd_common.h
@@ -23,7 +23,6 @@
 template<typename SvdType, typename MatrixType>
 void svd_check_full(const MatrixType& m, const SvdType& svd)
 {
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
 
@@ -101,7 +100,6 @@ void svd_least_square(const MatrixType& m, unsigned int computationOptions)
 {
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
 
@@ -168,7 +166,6 @@ template<typename MatrixType>
 void svd_min_norm(const MatrixType& m, unsigned int computationOptions)
 {
   typedef typename MatrixType::Scalar Scalar;
-  typedef typename MatrixType::Index Index;
   Index cols = m.cols();
 
   enum {
@@ -261,7 +258,6 @@ void svd_test_all_computation_options(const MatrixType& m, bool full_only)
     CALL_SUBTEST(( svd_min_norm(m, ComputeThinU | ComputeThinV) ));
 
     // test reconstruction
-    typedef typename MatrixType::Index Index;
     Index diagSize = (std::min)(m.rows(), m.cols());
     SvdType svd(m, ComputeThinU | ComputeThinV);
     VERIFY_IS_APPROX(m, svd.matrixU().leftCols(diagSize) * svd.singularValues().asDiagonal() * svd.matrixV().leftCols(diagSize).adjoint());
@@ -437,7 +433,6 @@ template<typename SvdType,typename MatrixType>
 void svd_verify_assert(const MatrixType& m)
 {
   typedef typename MatrixType::Scalar Scalar;
-  typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
 
diff --git a/gtsam/3rdparty/Eigen/test/svd_fill.h b/gtsam/3rdparty/Eigen/test/svd_fill.h
index 3877c0c7e..d68647e99 100644
--- a/gtsam/3rdparty/Eigen/test/svd_fill.h
+++ b/gtsam/3rdparty/Eigen/test/svd_fill.h
@@ -23,7 +23,6 @@ void svd_fill_random(MatrixType &m, int Option = 0)
   using std::pow;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
-  typedef typename MatrixType::Index Index;
   Index diagSize = (std::min)(m.rows(), m.cols());
   RealScalar s = std::numeric_limits<RealScalar>::max_exponent10/4;
   s = internal::random<RealScalar>(1,s);
diff --git a/gtsam/3rdparty/Eigen/test/triangular.cpp b/gtsam/3rdparty/Eigen/test/triangular.cpp
index b96856486..328eef4de 100644
--- a/gtsam/3rdparty/Eigen/test/triangular.cpp
+++ b/gtsam/3rdparty/Eigen/test/triangular.cpp
@@ -134,7 +134,6 @@ template<typename MatrixType> void triangular_square(const MatrixType& m)
 
 template<typename MatrixType> void triangular_rect(const MatrixType& m)
 {
-  typedef const typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   enum { Rows =  MatrixType::RowsAtCompileTime, Cols =  MatrixType::ColsAtCompileTime };
diff --git a/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp b/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp
index 83c1439ad..37e7495f5 100644
--- a/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp
+++ b/gtsam/3rdparty/Eigen/test/vectorization_logic.cpp
@@ -207,6 +207,12 @@ struct vectorization_logic
     VERIFY(test_redux(Vector1(),
       LinearVectorizedTraversal,CompleteUnrolling));
 
+    VERIFY(test_redux(Vector1().array()*Vector1().array(),
+      LinearVectorizedTraversal,CompleteUnrolling));
+
+    VERIFY(test_redux((Vector1().array()*Vector1().array()).col(0),
+      LinearVectorizedTraversal,CompleteUnrolling));
+
     VERIFY(test_redux(Matrix<Scalar,PacketSize,3>(),
       LinearVectorizedTraversal,CompleteUnrolling));
 
diff --git a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp
index f3ab561ee..a099d17c8 100644
--- a/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp
+++ b/gtsam/3rdparty/Eigen/test/vectorwiseop.cpp
@@ -15,7 +15,6 @@
 
 template<typename ArrayType> void vectorwiseop_array(const ArrayType& m)
 {
-  typedef typename ArrayType::Index Index;
   typedef typename ArrayType::Scalar Scalar;
   typedef Array<Scalar, ArrayType::RowsAtCompileTime, 1> ColVectorType;
   typedef Array<Scalar, 1, ArrayType::ColsAtCompileTime> RowVectorType;
@@ -129,7 +128,6 @@ template<typename ArrayType> void vectorwiseop_array(const ArrayType& m)
 
 template<typename MatrixType> void vectorwiseop_matrix(const MatrixType& m)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVectorType;
diff --git a/gtsam/3rdparty/Eigen/test/visitor.cpp b/gtsam/3rdparty/Eigen/test/visitor.cpp
index 844170ec6..7f4efab97 100644
--- a/gtsam/3rdparty/Eigen/test/visitor.cpp
+++ b/gtsam/3rdparty/Eigen/test/visitor.cpp
@@ -12,7 +12,6 @@
 template<typename MatrixType> void matrixVisitor(const MatrixType& p)
 {
   typedef typename MatrixType::Scalar Scalar;
-  typedef typename MatrixType::Index Index;
 
   Index rows = p.rows();
   Index cols = p.cols();
@@ -65,7 +64,6 @@ template<typename MatrixType> void matrixVisitor(const MatrixType& p)
 template<typename VectorType> void vectorVisitor(const VectorType& w)
 {
   typedef typename VectorType::Scalar Scalar;
-  typedef typename VectorType::Index Index;
 
   Index size = w.size();
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/CMakeLists.txt
index 4fef40a86..9a5666105 100644
--- a/gtsam/3rdparty/Eigen/unsupported/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/unsupported/CMakeLists.txt
@@ -1,7 +1,9 @@
 add_subdirectory(Eigen)
 add_subdirectory(doc EXCLUDE_FROM_ALL)
-if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
-  add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
-else()
-  add_subdirectory(test EXCLUDE_FROM_ALL)
+if(BUILD_TESTING)
+  if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
+    add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
+  else()
+    add_subdirectory(test EXCLUDE_FROM_ALL)
+  endif()
 endif()
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/Tensor b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/Tensor
index 7ecb4c74d..bb6523d15 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/Tensor
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/Tensor
@@ -39,6 +39,8 @@
   * \code
   * #include <Eigen/CXX11/Tensor>
   * \endcode
+  *
+  * Much of the documentation can be found \ref eigen_tensors "here".
   */
 
 #include <cmath>
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/README.md b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/README.md
index 98e83811b..da70fa216 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/README.md
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/README.md
@@ -1,4 +1,4 @@
-# Eigen Tensors
+# Eigen Tensors {#eigen_tensors}
 
 Tensors are multidimensional arrays of elements. Elements are typically scalars,
 but more complex types such as strings are also supported.
@@ -8,7 +8,7 @@ but more complex types such as strings are also supported.
 ## Tensor Classes
 
 You can manipulate a tensor with one of the following classes.  They all are in
-the namespace ```::Eigen.```
+the namespace `::Eigen.`
 
 
 ### Class Tensor<data_type, rank>
@@ -21,10 +21,10 @@ matrix.
 Tensors of this class are resizable.  For example, if you assign a tensor of a
 different size to a Tensor, that tensor is resized to match its new value.
 
-#### Constructor Tensor<data_type, rank>(size0, size1, ...)
+#### Constructor `Tensor<data_type, rank>(size0, size1, ...)`
 
-Constructor for a Tensor.  The constructor must be passed ```rank``` integers
-indicating the sizes of the instance along each of the the ```rank```
+Constructor for a Tensor.  The constructor must be passed `rank` integers
+indicating the sizes of the instance along each of the the `rank`
 dimensions.
 
     // Create a tensor of rank 3 of sizes 2, 3, 4.  This tensor owns
@@ -34,18 +34,18 @@ dimensions.
     // Resize t_3d by assigning a tensor of different sizes, but same rank.
     t_3d = Tensor<float, 3>(3, 4, 3);
 
-#### Constructor Tensor<data_type, rank>(size_array)
+#### Constructor `Tensor<data_type, rank>(size_array)`
 
 Constructor where the sizes for the constructor are specified as an array of
 values instead of an explicitly list of parameters.  The array type to use is
-```Eigen::array<Eigen::Index>```.  The array can be constructed automatically
+`Eigen::array<Eigen::Index>`.  The array can be constructed automatically
 from an initializer list.
 
     // Create a tensor of strings of rank 2 with sizes 5, 7.
     Tensor<string, 2> t_2d({5, 7});
 
 
-### Class TensorFixedSize<data_type, Sizes<size0, size1, ...>>
+### Class `TensorFixedSize<data_type, Sizes<size0, size1, ...>>`
 
 Class to use for tensors of fixed size, where the size is known at compile
 time.  Fixed sized tensors can provide very fast computations because all their
@@ -57,7 +57,7 @@ tensor data is held onto the stack and does not cause heap allocation and free.
     // Create a 4 x 3 tensor of floats.
     TensorFixedSize<float, Sizes<4, 3>> t_4x3;
 
-### Class TensorMap<Tensor<data_type, rank>>
+### Class `TensorMap<Tensor<data_type, rank>>`
 
 This is the class to use to create a tensor on top of memory allocated and
 owned by another part of your code.  It allows to view any piece of allocated
@@ -67,7 +67,7 @@ data are stored.
 A TensorMap is not resizable because it does not own the memory where its data
 are stored.
 
-#### Constructor TensorMap<Tensor<data_type, rank>>(data, size0, size1, ...)
+#### Constructor `TensorMap<Tensor<data_type, rank>>(data, size0, size1, ...)`
 
 Constructor for a Tensor.  The constructor must be passed a pointer to the
 storage for the data, and "rank" size attributes.  The storage has to be
@@ -83,20 +83,20 @@ large enough to hold all the data.
 
     // You can also map fixed-size tensors.  Here we get a 1d view of
     // the 2d fixed-size tensor.
-    Tensor<float, Sizes<4, 5>> t_4x3;
-    TensorMap<Tensor<float, 1>> t_12(t_4x3, 12);
+    TensorFixedSize<float, Sizes<4, 5>> t_4x3;
+    TensorMap<Tensor<float, 1>> t_12(t_4x3.data(), 12);
 
 
-#### Class TensorRef
+#### Class `TensorRef`
 
 See Assigning to a TensorRef below.
 
 ## Accessing Tensor Elements
 
-#### <data_type> tensor(index0, index1...)
+#### `<data_type> tensor(index0, index1...)`
 
-Return the element at position ```(index0, index1...)``` in tensor
-```tensor```.  You must pass as many parameters as the rank of ```tensor```.
+Return the element at position `(index0, index1...)` in tensor
+`tensor`.  You must pass as many parameters as the rank of `tensor`.
 The expression can be used as an l-value to set the value of the element at the
 specified position.  The value returned is of the datatype of the tensor.
 
@@ -121,8 +121,8 @@ specified position.  The value returned is of the datatype of the tensor.
 
 ## TensorLayout
 
-The tensor library supports 2 layouts: ```ColMajor``` (the default) and
-```RowMajor```.  Only the default column major layout is currently fully
+The tensor library supports 2 layouts: `ColMajor` (the default) and
+`RowMajor`.  Only the default column major layout is currently fully
 supported, and it is therefore not recommended to attempt to use the row major
 layout at the moment.
 
@@ -136,7 +136,7 @@ All the arguments to an expression must use the same layout. Attempting to mix
 different layouts will result in a compilation error.
 
 It is possible to change the layout of a tensor or an expression using the
-```swap_layout()``` method.  Note that this will also reverse the order of the
+`swap_layout()` method.  Note that this will also reverse the order of the
 dimensions.
 
     Tensor<float, 2, ColMajor> col_major(2, 4);
@@ -173,35 +173,35 @@ the following code computes the elementwise addition of two tensors:
     Tensor<float, 3> t3 = t1 + t2;
 
 While the code above looks easy enough, it is important to understand that the
-expression ```t1 + t2``` is not actually adding the values of the tensors.  The
+expression `t1 + t2` is not actually adding the values of the tensors.  The
 expression instead constructs a "tensor operator" object of the class
 TensorCwiseBinaryOp<scalar_sum>, which has references to the tensors
-```t1``` and ```t2```.  This is a small C++ object that knows how to add
-```t1``` and ```t2```.  It is only when the value of the expression is assigned
-to the tensor ```t3``` that the addition is actually performed.  Technically,
-this happens through the overloading of ```operator=()``` in the Tensor class.
+`t1` and `t2`.  This is a small C++ object that knows how to add
+`t1` and `t2`.  It is only when the value of the expression is assigned
+to the tensor `t3` that the addition is actually performed.  Technically,
+this happens through the overloading of `operator=()` in the Tensor class.
 
 This mechanism for computing tensor expressions allows for lazy evaluation and
 optimizations which are what make the tensor library very fast.
 
-Of course, the tensor operators do nest, and the expression ```t1 + t2 *
-0.3f``` is actually represented with the (approximate) tree of operators:
+Of course, the tensor operators do nest, and the expression `t1 + t2 * 0.3f`
+is actually represented with the (approximate) tree of operators:
 
     TensorCwiseBinaryOp<scalar_sum>(t1, TensorCwiseUnaryOp<scalar_mul>(t2, 0.3f))
 
 
 ### Tensor Operations and C++ "auto"
 
-Because Tensor operations create tensor operators, the C++ ```auto``` keyword
+Because Tensor operations create tensor operators, the C++ `auto` keyword
 does not have its intuitive meaning.  Consider these 2 lines of code:
 
     Tensor<float, 3> t3 = t1 + t2;
     auto t4 = t1 + t2;
 
-In the first line we allocate the tensor ```t3``` and it will contain the
-result of the addition of ```t1``` and ```t2```.  In the second line, ```t4```
+In the first line we allocate the tensor `t3` and it will contain the
+result of the addition of `t1` and `t2`.  In the second line, `t4`
 is actually the tree of tensor operators that will compute the addition of
-```t1``` and ```t2```.  In fact, ```t4``` is *not* a tensor and you cannot get
+`t1` and `t2`.  In fact, `t4` is *not* a tensor and you cannot get
 the values of its elements:
 
     Tensor<float, 3> t3 = t1 + t2;
@@ -210,8 +210,8 @@ the values of its elements:
     auto t4 = t1 + t2;
     cout << t4(0, 0, 0);  // Compilation error!
 
-When you use ```auto``` you do not get a Tensor as a result but instead a
-non-evaluated expression.  So only use ```auto``` to delay evaluation.
+When you use `auto` you do not get a Tensor as a result but instead a
+non-evaluated expression.  So only use `auto` to delay evaluation.
 
 Unfortunately, there is no single underlying concrete type for holding
 non-evaluated expressions, hence you have to use auto in the case when you do
@@ -257,9 +257,9 @@ There are several ways to control when expressions are evaluated:
 #### Assigning to a Tensor, TensorFixedSize, or TensorMap.
 
 The most common way to evaluate an expression is to assign it to a Tensor.  In
-the example below, the ```auto``` declarations make the intermediate values
+the example below, the `auto` declarations make the intermediate values
 "Operations", not Tensors, and do not cause the expressions to be evaluated.
-The assignment to the Tensor ```result``` causes the evaluation of all the
+The assignment to the Tensor `result` causes the evaluation of all the
 operations.
 
     auto t3 = t1 + t2;             // t3 is an Operation.
@@ -272,17 +272,17 @@ Operation to a TensorFixedSize instead of a Tensor, which is a bit more
 efficient.
 
     // We know that the result is a 4x4x2 tensor!
-    TensorFixedSize<float, 4, 4, 2> result = t5;
+    TensorFixedSize<float, Sizes<4, 4, 2>> result = t5;
 
 Simiarly, assigning an expression to a TensorMap causes its evaluation.  Like
 tensors of type TensorFixedSize, TensorMaps cannot be resized so they have to
 have the rank and sizes of the expression that are assigned to them.
 
-#### Calling eval().
+#### Calling `eval()`.
 
 When you compute large composite expressions, you sometimes want to tell Eigen
 that an intermediate value in the expression tree is worth evaluating ahead of
-time.  This is done by inserting a call to the ```eval()``` method of the
+time.  This is done by inserting a call to the `eval()` method of the
 expression Operation.
 
     // The previous example could have been written:
@@ -291,15 +291,15 @@ expression Operation.
     // If you want to compute (t1 + t2) once ahead of time you can write:
     Tensor<float, 3> result = ((t1 + t2).eval() * 0.2f).exp();
 
-Semantically, calling ```eval()``` is equivalent to materializing the value of
+Semantically, calling `eval()` is equivalent to materializing the value of
 the expression in a temporary Tensor of the right size.  The code above in
 effect does:
 
     // .eval() knows the size!
-    TensorFixedSize<float, 4, 4, 2> tmp = t1 + t2;
+    TensorFixedSize<float, Sizes<4, 4, 2>> tmp = t1 + t2;
     Tensor<float, 3> result = (tmp * 0.2f).exp();
 
-Note that the return value of ```eval()``` is itself an Operation, so the
+Note that the return value of `eval()` is itself an Operation, so the
 following code does not do what you may think:
 
     // Here t3 is an evaluation Operation.  t3 has not been evaluated yet.
@@ -312,24 +312,24 @@ following code does not do what you may think:
     // an intermediate tensor to represent t3.x
     Tensor<float, 3> result = t4;
 
-While in the examples above calling ```eval()``` does not make a difference in
+While in the examples above calling `eval()` does not make a difference in
 performance, in other cases it can make a huge difference.  In the expression
-below the ```broadcast()``` expression causes the ```X.maximum()``` expression
+below the `broadcast()` expression causes the `X.maximum()` expression
 to be evaluated many times:
 
     Tensor<...> X ...;
     Tensor<...> Y = ((X - X.maximum(depth_dim).reshape(dims2d).broadcast(bcast))
                      * beta).exp();
 
-Inserting a call to ```eval()``` between the ```maximum()``` and
-```reshape()``` calls guarantees that maximum() is only computed once and
+Inserting a call to `eval()` between the `maximum()` and
+`reshape()` calls guarantees that maximum() is only computed once and
 greatly speeds-up execution:
 
     Tensor<...> Y =
       ((X - X.maximum(depth_dim).eval().reshape(dims2d).broadcast(bcast))
         * beta).exp();
 
-In the other example below, the tensor ```Y``` is both used in the expression
+In the other example below, the tensor `Y` is both used in the expression
 and its assignment.  This is an aliasing problem and if the evaluation is not
 done in the right order Y will be updated incrementally during the evaluation
 resulting in bogus results:
@@ -337,8 +337,8 @@ resulting in bogus results:
      Tensor<...> Y ...;
      Y = Y / (Y.sum(depth_dim).reshape(dims2d).broadcast(bcast));
 
-Inserting a call to ```eval()``` between the ```sum()``` and ```reshape()```
-expressions ensures that the sum is computed before any updates to ```Y``` are
+Inserting a call to `eval()` between the `sum()` and `reshape()`
+expressions ensures that the sum is computed before any updates to `Y` are
 done.
 
      Y = Y / (Y.sum(depth_dim).eval().reshape(dims2d).broadcast(bcast));
@@ -347,21 +347,21 @@ Note that an eval around the full right hand side expression is not needed
 because the generated has to compute the i-th value of the right hand side
 before assigning it to the left hand side.
 
-However, if you were assigning the expression value to a shuffle of ```Y```
-then you would need to force an eval for correctness by adding an ```eval()```
+However, if you were assigning the expression value to a shuffle of `Y`
+then you would need to force an eval for correctness by adding an `eval()`
 call for the right hand side:
 
      Y.shuffle(...) =
         (Y / (Y.sum(depth_dim).eval().reshape(dims2d).broadcast(bcast))).eval();
 
 
-#### Assigning to a TensorRef.
+#### Assigning to a `TensorRef`.
 
 If you need to access only a few elements from the value of an expression you
 can avoid materializing the value in a full tensor by using a TensorRef.
 
 A TensorRef is a small wrapper class for any Eigen Operation.  It provides
-overloads for the ```()``` operator that let you access individual values in
+overloads for the `()` operator that let you access individual values in
 the expression.  TensorRef is convenient, because the Operation themselves do
 not provide a way to access individual elements.
 
@@ -390,7 +390,7 @@ such as contractions and convolutions.  The implementations are optimized for
 different environments: single threaded on CPU, multi threaded on CPU, or on a
 GPU using cuda.  Additional implementations may be added later.
 
-You can choose which implementation to use with the ```device()``` call.  If
+You can choose which implementation to use with the `device()` call.  If
 you do not choose an implementation explicitly the default implementation that
 uses a single thread on the CPU is used.
 
@@ -406,7 +406,7 @@ single-threaded CPU implementation:
     Tensor<float, 2> b(30, 40);
     Tensor<float, 2> c = a + b;
 
-To choose a different implementation you have to insert a ```device()``` call
+To choose a different implementation you have to insert a `device()` call
 before the assignment of the result.  For technical C++ reasons this requires
 that the Tensor for the result be declared on its own.  This means that you
 have to know the size of the result.
@@ -414,16 +414,16 @@ have to know the size of the result.
     Eigen::Tensor<float, 2> c(30, 40);
     c.device(...) = a + b;
 
-The call to ```device()``` must be the last call on the left of the operator=.
+The call to `device()` must be the last call on the left of the operator=.
 
-You must pass to the ```device()``` call an Eigen device object.  There are
+You must pass to the `device()` call an Eigen device object.  There are
 presently three devices you can use: DefaultDevice, ThreadPoolDevice and
 GpuDevice.
 
 
 #### Evaluating With the DefaultDevice
 
-This is exactly the same as not inserting a ```device()``` call.
+This is exactly the same as not inserting a `device()` call.
 
     DefaultDevice my_device;
     c.device(my_device) = a + b;
@@ -452,24 +452,24 @@ memory for tensors with cuda.
 In the documentation of the tensor methods and Operation we mention datatypes
 that are tensor-type specific:
 
-#### <Tensor-Type>::Dimensions
+#### `<Tensor-Type>::``Dimensions`
 
-Acts like an array of ints.  Has an ```int size``` attribute, and can be
+Acts like an array of ints.  Has an `int size` attribute, and can be
 indexed like an array to access individual values.  Used to represent the
-dimensions of a tensor.  See ```dimensions()```.
+dimensions of a tensor.  See `dimensions()`.
 
-#### <Tensor-Type>::Index
+#### `<Tensor-Type>::``Index`
 
-Acts like an ```int```.  Used for indexing tensors along their dimensions.  See
-```operator()```, ```dimension()```, and ```size()```.
+Acts like an `int`.  Used for indexing tensors along their dimensions.  See
+`operator()`, `dimension()`, and `size()`.
 
-#### <Tensor-Type>::Scalar
+#### `<Tensor-Type>::``Scalar`
 
 Represents the datatype of individual tensor elements.  For example, for a
-```Tensor<float>```, ```Scalar``` is the type ```float```.  See
-```setConstant()```.
+`Tensor<float>`, `Scalar` is the type `float`.  See
+`setConstant()`.
 
-#### <Operation>
+#### `<Operation>`
 
 We use this pseudo type to indicate that a tensor Operation is returned by a
 method.  We indicate in the text the type and dimensions of the tensor that the
@@ -489,7 +489,7 @@ Tensor, TensorFixedSize, and TensorMap.
 
 ## Metadata
 
-### int NumDimensions
+### `int NumDimensions`
 
 Constant value indicating the number of dimensions of a Tensor.  This is also
 known as the tensor "rank".
@@ -498,10 +498,10 @@ known as the tensor "rank".
       cout << "Dims " << a.NumDimensions;
       => Dims 2
 
-### Dimensions dimensions()
+### `Dimensions dimensions()`
 
 Returns an array-like object representing the dimensions of the tensor.
-The actual type of the dimensions() result is <Tensor-Type>::Dimensions.
+The actual type of the `dimensions()` result is `<Tensor-Type>::``Dimensions`.
 
     Eigen::Tensor<float, 2> a(3, 4);
     const Eigen::Tensor<float, 2>::Dimensions& d = a.dimensions();
@@ -509,17 +509,17 @@ The actual type of the dimensions() result is <Tensor-Type>::Dimensions.
          << ", dim 1: " << d[1];
     => Dim size: 2, dim 0: 3, dim 1: 4
 
-If you use a C++11 compiler, you can use ```auto``` to simplify the code:
+If you use a C++11 compiler, you can use `auto` to simplify the code:
 
     const auto& d = a.dimensions();
     cout << "Dim size: " << d.size << ", dim 0: " << d[0]
          << ", dim 1: " << d[1];
     => Dim size: 2, dim 0: 3, dim 1: 4
 
-### Index dimension(Index n)
+### `Index dimension(Index n)`
 
 Returns the n-th dimension of the tensor.  The actual type of the
-```dimension()``` result is ```<Tensor-Type>::Index```, but you can
+`dimension()` result is `<Tensor-Type>::``Index`, but you can
 always use it like an int.
 
       Eigen::Tensor<float, 2> a(3, 4);
@@ -527,11 +527,11 @@ always use it like an int.
       cout << "Dim 1: " << dim1;
       => Dim 1: 4
 
-### Index size()
+### `Index size()`
 
 Returns the total number of elements in the tensor.  This is the product of all
-the tensor dimensions.  The actual type of the ```size()``` result is
-```<Tensor-Type>::Index```, but you can always use it like an int.
+the tensor dimensions.  The actual type of the `size()` result is
+`<Tensor-Type>::``Index`, but you can always use it like an int.
 
     Eigen::Tensor<float, 2> a(3, 4);
     cout << "Size: " << a.size();
@@ -540,11 +540,11 @@ the tensor dimensions.  The actual type of the ```size()``` result is
 
 ### Getting Dimensions From An Operation
 
-A few operations provide ```dimensions()``` directly,
-e.g. ```TensorReslicingOp```.  Most operations defer calculating dimensions
+A few operations provide `dimensions()` directly,
+e.g. `TensorReslicingOp`.  Most operations defer calculating dimensions
 until the operation is being evaluated.  If you need access to the dimensions
 of a deferred operation, you can wrap it in a TensorRef (see Assigning to a
-TensorRef above), which provides ```dimensions()``` and ```dimension()``` as
+TensorRef above), which provides `dimensions()` and `dimension()` as
 above.
 
 TensorRef can also wrap the plain Tensor types, so this is a useful idiom in
@@ -567,11 +567,11 @@ to the rank of the tensor. The content of the tensor is not initialized.
 
 ### TensorFixedSize
 
-Creates a tensor of the specified size. The number of arguments in the Size<>
+Creates a tensor of the specified size. The number of arguments in the Sizes<>
 template parameter determines the rank of the tensor. The content of the tensor
 is not initialized.
 
-    Eigen::TensorFixedSize<float, Size<3, 4>> a;
+    Eigen::TensorFixedSize<float, Sizes<3, 4>> a;
     cout << "Rank: " << a.rank() << endl;
     => Rank: 2
     cout << "NumRows: " << a.dimension(0) << " NumCols: " << a.dimension(1) << endl;
@@ -581,14 +581,14 @@ is not initialized.
 
 Creates a tensor mapping an existing array of data. The data must not be freed
 until the TensorMap is discarded, and the size of the data must be large enough
-to accomodate of the coefficients of the tensor.
+to accommodate the coefficients of the tensor.
 
     float data[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
-    Eigen::TensorMap<float, 2> a(data, 3, 4);
+    Eigen::TensorMap<Tensor<float, 2>> a(data, 3, 4);
     cout << "NumRows: " << a.dimension(0) << " NumCols: " << a.dimension(1) << endl;
     => NumRows: 3 NumCols: 4
     cout << "a(1, 2): " << a(1, 2) << endl;
-    => a(1, 2): 9
+    => a(1, 2): 7
 
 
 ## Contents Initialization
@@ -602,9 +602,9 @@ You can use one of the methods below to initialize the tensor memory.  These
 have an immediate effect on the tensor and return the tensor itself as a
 result.  These are not tensor Operations which delay evaluation.
 
-### <Tensor-Type> setConstant(const Scalar& val)
+### `<Tensor-Type> setConstant(const Scalar& val)`
 
-Sets all elements of the tensor to the constant value ```val```.  ```Scalar```
+Sets all elements of the tensor to the constant value `val`.  `Scalar`
 is the type of data stored in the tensor.  You can pass any value that is
 convertible to that type.
 
@@ -618,8 +618,8 @@ Returns the tensor itself in case you want to chain another call.
     12.3 12.3 12.3 12.3
     12.3 12.3 12.3 12.3
 
-Note that ```setConstant()``` can be used on any tensor where the element type
-has a copy constructor and an ```operator=()```:
+Note that `setConstant()` can be used on any tensor where the element type
+has a copy constructor and an `operator=()`:
 
     Eigen::Tensor<string, 2> a(2, 3);
     a.setConstant("yolo");
@@ -630,9 +630,9 @@ has a copy constructor and an ```operator=()```:
     yolo yolo yolo
 
 
-### <Tensor-Type> setZero()
+### `<Tensor-Type> setZero()`
 
-Fills the tensor with zeros.  Equivalent to ```setConstant(Scalar(0))```.
+Fills the tensor with zeros.  Equivalent to `setConstant(Scalar(0))`.
 Returns the tensor itself in case you want to chain another call.
 
     a.setZero();
@@ -644,7 +644,7 @@ Returns the tensor itself in case you want to chain another call.
     0 0 0 0
 
 
-### <Tensor-Type> setValues({..initializer_list})
+### `<Tensor-Type> setValues({..initializer_list})`
 
 Fills the tensor with explicit values specified in a std::initializer_list.
 The type of the initializer list depends on the type and rank of the tensor.
@@ -653,10 +653,10 @@ If the tensor has rank N, the initializer list must be nested N times.  The
 most deeply nested lists must contains P scalars of the Tensor type where P is
 the size of the last dimension of the Tensor.
 
-For example, for a ```TensorFixedSize<float, 2, 3>``` the initializer list must
+For example, for a `TensorFixedSize<float, 2, 3>` the initializer list must
 contains 2 lists of 3 floats each.
 
-```setValues()``` returns the tensor itself in case you want to chain another
+`setValues()` returns the tensor itself in case you want to chain another
 call.
 
     Eigen::Tensor<float, 2> a(2, 3);
@@ -680,7 +680,7 @@ code only sets the values of the first row of the tensor.
     10   20   30
     1000 1000 1000
 
-### <Tensor-Type> setRandom()
+### `<Tensor-Type> setRandom()`
 
 Fills the tensor with random values.  Returns the tensor itself in case you
 want to chain another call.
@@ -693,16 +693,16 @@ want to chain another call.
      -0.211234   0.823295   0.536459 -0.0452059
       0.566198  -0.604897  -0.444451   0.257742
 
-You can customize ```setRandom()``` by providing your own random number
+You can customize `setRandom()` by providing your own random number
 generator as a template argument:
 
     a.setRandom<MyRandomGenerator>();
 
-Here, ```MyRandomGenerator``` must be a struct with the following member
-functions, where Scalar and Index are the same as ```<Tensor-Type>::Scalar```
-and ```<Tensor-Type>::Index```.
+Here, `MyRandomGenerator` must be a struct with the following member
+functions, where Scalar and Index are the same as `<Tensor-Type>::``Scalar`
+and `<Tensor-Type>::``Index`.
 
-See ```struct UniformRandomGenerator``` in TensorFunctors.h for an example.
+See `struct UniformRandomGenerator` in TensorFunctors.h for an example.
 
     // Custom number generator for use with setRandom().
     struct MyRandomGenerator {
@@ -747,7 +747,7 @@ values of a tensor expression, the expression must either be evaluated or
 wrapped in a TensorRef.
 
 
-### Scalar* data() and const Scalar* data() const
+### `Scalar* data()` and `const Scalar* data() const`
 
 Returns a pointer to the storage for the tensor.  The pointer is const if the
 tensor was const.  This allows direct access to the data.  The layout of the
@@ -767,7 +767,7 @@ Scalar is the type of data stored in the tensor.
 
 ## Tensor Operations
 
-All the methods documented below return non evaluated tensor ```Operations```.
+All the methods documented below return non evaluated tensor `Operations`.
 These can be chained: you can apply another Tensor Operation to the value
 returned by the method.
 
@@ -775,10 +775,10 @@ The chain of Operation is evaluated lazily, typically when it is assigned to a
 tensor.  See "Controlling when Expression are Evaluated" for more details about
 their evaluation.
 
-### <Operation> constant(const Scalar& val)
+### `<Operation> constant(const Scalar& val)`
 
 Returns a tensor of the same type and dimensions as the original tensor but
-where all elements have the value ```val```.
+where all elements have the value `val`.
 
 This is useful, for example, when you want to add or subtract a constant from a
 tensor, or multiply every element of a tensor by a scalar.
@@ -803,14 +803,14 @@ tensor, or multiply every element of a tensor by a scalar.
     0.6 0.6 0.6
     0.6 0.6 0.6
 
-### <Operation> random()
+### `<Operation> random()`
 
 Returns a tensor of the same type and dimensions as the current tensor
 but where all elements have random values.
 
 This is for example useful to add random values to an existing tensor.
 The generation of random values can be customized in the same manner
-as for ```setRandom()```.
+as for `setRandom()`.
 
     Eigen::Tensor<float, 2> a(2, 3);
     a.setConstant(1.0f);
@@ -833,7 +833,7 @@ All these operations take a single input tensor as argument and return a tensor
 of the same type and dimensions as the tensor to which they are applied.  The
 requested operations are applied to each element independently.
 
-### <Operation> operator-()
+### `<Operation> operator-()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the opposite values of the original tensor.
@@ -852,42 +852,42 @@ containing the opposite values of the original tensor.
     -1 -1 -1
     -1 -1 -1
 
-### <Operation> sqrt()
+### `<Operation> sqrt()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the square roots of the original tensor.
 
-### <Operation> rsqrt()
+### `<Operation> rsqrt()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the inverse square roots of the original tensor.
 
-### <Operation> square()
+### `<Operation> square()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the squares of the original tensor values.
 
-### <Operation> inverse()
+### `<Operation> inverse()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the inverse of the original tensor values.
 
-### <Operation> exp()
+### `<Operation> exp()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the exponential of the original tensor.
 
-### <Operation> log()
+### `<Operation> log()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the natural logarithms of the original tensor.
 
-### <Operation> abs()
+### `<Operation> abs()`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the absolute values of the original tensor.
 
-### <Operation> pow(Scalar exponent)
+### `<Operation> pow(Scalar exponent)`
 
 Returns a tensor of the same type and dimensions as the original tensor
 containing the coefficients of the original tensor to the power of the
@@ -914,17 +914,17 @@ cubic roots of an int Tensor:
     0 1 2
     3 4 5
 
-### <Operation>  operator * (Scalar scale)
+### `<Operation>  operator * (Scalar scale)`
 
 Multiplies all the coefficients of the input tensor by the provided scale.
 
-### <Operation>  cwiseMax(Scalar threshold)
+### `<Operation>  cwiseMax(Scalar threshold)`
 TODO
 
-### <Operation>  cwiseMin(Scalar threshold)
+### `<Operation>  cwiseMin(Scalar threshold)`
 TODO
 
-### <Operation>  unaryExpr(const CustomUnaryOp& func)
+### `<Operation>  unaryExpr(const CustomUnaryOp& func)`
 TODO
 
 
@@ -936,39 +936,39 @@ dimensions as the tensors to which they are applied, and unless otherwise
 specified it is also of the same type. The requested operations are applied to
 each pair of elements independently.
 
-### <Operation> operator+(const OtherDerived& other)
+### `<Operation> operator+(const OtherDerived& other)`
 
 Returns a tensor of the same type and dimensions as the input tensors
 containing the coefficient wise sums of the inputs.
 
-### <Operation> operator-(const OtherDerived& other)
+### `<Operation> operator-(const OtherDerived& other)`
 
 Returns a tensor of the same type and dimensions as the input tensors
 containing the coefficient wise differences of the inputs.
 
-### <Operation> operator*(const OtherDerived& other)
+### `<Operation> operator*(const OtherDerived& other)`
 
 Returns a tensor of the same type and dimensions as the input tensors
 containing the coefficient wise products of the inputs.
 
-### <Operation> operator/(const OtherDerived& other)
+### `<Operation> operator/(const OtherDerived& other)`
 
 Returns a tensor of the same type and dimensions as the input tensors
 containing the coefficient wise quotients of the inputs.
 
 This operator is not supported for integer types.
 
-### <Operation> cwiseMax(const OtherDerived& other)
+### `<Operation> cwiseMax(const OtherDerived& other)`
 
 Returns a tensor of the same type and dimensions as the input tensors
 containing the coefficient wise maximums of the inputs.
 
-### <Operation> cwiseMin(const OtherDerived& other)
+### `<Operation> cwiseMin(const OtherDerived& other)`
 
 Returns a tensor of the same type and dimensions as the input tensors
 containing the coefficient wise mimimums of the inputs.
 
-### <Operation> Logical operators
+### `<Operation> Logical operators`
 
 The following logical operators are supported as well:
 
@@ -1013,16 +1013,23 @@ multidimensional case.
     Eigen::Tensor<int, 2> a(2, 3);
     a.setValues({{1, 2, 3}, {6, 5, 4}});
     Eigen::Tensor<int, 2> b(3, 2);
-    a.setValues({{1, 2}, {4, 5}, {5, 6}});
+    b.setValues({{1, 2}, {4, 5}, {5, 6}});
 
     // Compute the traditional matrix product
-    array<IndexPair<int>, 1> product_dims = { IndexPair(1, 0) };
+    Eigen::array<Eigen::IndexPair<int>, 1> product_dims = { Eigen::IndexPair<int>(1, 0) };
     Eigen::Tensor<int, 2> AB = a.contract(b, product_dims);
 
     // Compute the product of the transpose of the matrices
-    array<IndexPair<int>, 1> transpose_product_dims = { IndexPair(0, 1) };
+    Eigen::array<Eigen::IndexPair<int>, 1> transposed_product_dims = { Eigen::IndexPair<int>(0, 1) };
     Eigen::Tensor<int, 2> AtBt = a.contract(b, transposed_product_dims);
 
+    // Contraction to scalar value using a double contraction.
+    // First coordinate of both tensors are contracted as well as both second coordinates, i.e., this computes the sum of the squares of the elements.
+    Eigen::array<Eigen::IndexPair<int>, 2> double_contraction_product_dims = { Eigen::IndexPair<int>(0, 0), Eigen::IndexPair<int>(1, 1) };
+    Eigen::Tensor<int, 0> AdoubleContractedA = a.contract(a, double_contraction_product_dims);
+
+    // Extracting the scalar value of the tensor contraction for further usage
+    int value = AdoubleContractedA(0);
 
 ## Reduction Operations
 
@@ -1032,13 +1039,13 @@ original tensor.  The values in the returned tensor are computed by applying a
 the dimensions along which the slices are made.
 
 The Eigen Tensor library provides a set of predefined reduction operators such
-as ```maximum()``` and ```sum()``` and lets you define additional operators by
+as `maximum()` and `sum()` and lets you define additional operators by
 implementing a few methods from a reductor template.
 
 ### Reduction Dimensions
 
 All reduction operations take a single parameter of type
-```<TensorType>::Dimensions``` which can always be specified as an array of
+`<TensorType>::``Dimensions` which can always be specified as an array of
 ints.  These are called the "reduction dimensions."  The values are the indices
 of the dimensions of the input tensor over which the reduction is done.  The
 parameter can have at most as many element as the rank of the input tensor;
@@ -1119,52 +1126,52 @@ scalar, represented as a zero-dimension tensor.
     276
 
 
-### <Operation> sum(const Dimensions& new_dims)
-### <Operation> sum()
+### `<Operation> sum(const Dimensions& new_dims)`
+### `<Operation> sum()`
 
 Reduce a tensor using the sum() operator.  The resulting values
 are the sum of the reduced values.
 
-### <Operation> mean(const Dimensions& new_dims)
-### <Operation> mean()
+### `<Operation> mean(const Dimensions& new_dims)`
+### `<Operation> mean()`
 
 Reduce a tensor using the mean() operator.  The resulting values
 are the mean of the reduced values.
 
-### <Operation> maximum(const Dimensions& new_dims)
-### <Operation> maximum()
+### `<Operation> maximum(const Dimensions& new_dims)`
+### `<Operation> maximum()`
 
 Reduce a tensor using the maximum() operator.  The resulting values are the
 largest of the reduced values.
 
-### <Operation> minimum(const Dimensions& new_dims)
-### <Operation> minimum()
+### `<Operation> minimum(const Dimensions& new_dims)`
+### `<Operation> minimum()`
 
 Reduce a tensor using the minimum() operator.  The resulting values
 are the smallest of the reduced values.
 
-### <Operation> prod(const Dimensions& new_dims)
-### <Operation> prod()
+### `<Operation> prod(const Dimensions& new_dims)`
+### `<Operation> prod()`
 
 Reduce a tensor using the prod() operator.  The resulting values
 are the product of the reduced values.
 
-### <Operation> all(const Dimensions& new_dims)
-### <Operation> all()
+### `<Operation> all(const Dimensions& new_dims)`
+### `<Operation> all()`
 Reduce a tensor using the all() operator.  Casts tensor to bool and then checks
 whether all elements are true.  Runs through all elements rather than
 short-circuiting, so may be significantly inefficient.
 
-### <Operation> any(const Dimensions& new_dims)
-### <Operation> any()
+### `<Operation> any(const Dimensions& new_dims)`
+### `<Operation> any()`
 Reduce a tensor using the any() operator.  Casts tensor to bool and then checks
 whether any element is true.  Runs through all elements rather than
 short-circuiting, so may be significantly inefficient.
 
 
-### <Operation> reduce(const Dimensions& new_dims, const Reducer& reducer)
+### `<Operation> reduce(const Dimensions& new_dims, const Reducer& reducer)`
 
-Reduce a tensor using a user-defined reduction operator.  See ```SumReducer```
+Reduce a tensor using a user-defined reduction operator.  See `SumReducer`
 in TensorFunctors.h for information on how to implement a reduction operator.
 
 
@@ -1191,24 +1198,24 @@ dd a comment to this line
     =>
     a
     1 2 3
-    6 5 4
+    4 5 6
 
     b
     1  3  6
     4  9 15
 
-### <Operation> cumsum(const Index& axis)
+### `<Operation> cumsum(const Index& axis)`
 
 Perform a scan by summing consecutive entries.
 
-### <Operation> cumprod(const Index& axis)
+### `<Operation> cumprod(const Index& axis)`
 
 Perform a scan by multiplying consecutive entries.
 
 
 ## Convolutions
 
-### <Operation> convolve(const Kernel& kernel, const Dimensions& dims)
+### `<Operation> convolve(const Kernel& kernel, const Dimensions& dims)`
 
 Returns a tensor that is the output of the convolution of the input tensor with the kernel,
 along the specified dimensions of the input tensor. The dimension size for dimensions of the output tensor
@@ -1251,7 +1258,7 @@ These operations return a Tensor with different dimensions than the original
 Tensor.  They can be used to access slices of tensors, see them with different
 dimensions, or pad tensors with additional data.
 
-### <Operation> reshape(const Dimensions& new_dims)
+### `<Operation> reshape(const Dimensions& new_dims)`
 
 Returns a view of the input tensor that has been reshaped to the specified
 new dimensions.  The argument new_dims is an array of Index values.  The
@@ -1273,7 +1280,7 @@ the number of elements in the input tensor.
 This operation does not move any data in the input tensor, so the resulting
 contents of a reshaped Tensor depend on the data layout of the original Tensor.
 
-For example this is what happens when you ```reshape()``` a 2D ColMajor tensor
+For example this is what happens when you `reshape()` a 2D ColMajor tensor
 to one dimension:
 
     Eigen::Tensor<float, 2, Eigen::ColMajor> a(2, 3);
@@ -1314,7 +1321,7 @@ The previous example can be rewritten as follow:
     Eigen::Tensor<float, 2, Eigen::ColMajor> a(2, 3);
     a.setValues({{0.0f, 100.0f, 200.0f}, {300.0f, 400.0f, 500.0f}});
     Eigen::array<Eigen::DenseIndex, 2> two_dim({2, 3});
-    Eigen::Tensor<float, 1, Eigen::ColMajor> b;
+    Eigen::Tensor<float, 1, Eigen::ColMajor> b(6);
     b.reshape(two_dim) = a;
     cout << "b" << endl << b << endl;
     =>
@@ -1330,7 +1337,7 @@ Note that "b" itself was not reshaped but that instead the assignment is done to
 the reshape view of b.
 
 
-### <Operation> shuffle(const Shuffle& shuffle)
+### `<Operation> shuffle(const Shuffle& shuffle)`
 
 Returns a copy of the input tensor whose dimensions have been
 reordered according to the specified permutation. The argument shuffle
@@ -1371,14 +1378,14 @@ Let's rewrite the previous example to take advantage of this feature:
     output.shuffle({2, 0, 1}) = input;
 
 
-### <Operation> stride(const Strides& strides)
+### `<Operation> stride(const Strides& strides)`
 
 Returns a view of the input tensor that strides (skips stride-1
 elements) along each of the dimensions.  The argument strides is an
 array of Index values.  The dimensions of the resulting tensor are
 ceil(input_dimensions[i] / strides[i]).
 
-For example this is what happens when you ```stride()``` a 2D tensor:
+For example this is what happens when you `stride()` a 2D tensor:
 
     Eigen::Tensor<int, 2> a(4, 3);
     a.setValues({{0, 100, 200}, {300, 400, 500}, {600, 700, 800}, {900, 1000, 1100}});
@@ -1397,7 +1404,7 @@ It is possible to assign a tensor to a stride:
     output.stride({2, 3, 4}) = input;
 
 
-### <Operation> slice(const StartIndices& offsets, const Sizes& extents)
+### `<Operation> slice(const StartIndices& offsets, const Sizes& extents)`
 
 Returns a sub-tensor of the given tensor. For each dimension i, the slice is
 made of the coefficients stored between offset[i] and offset[i] + extents[i] in
@@ -1423,7 +1430,7 @@ the input tensor.
      600   700
 
 
-### <Operation> chip(const Index offset, const Index dim)
+### `<Operation> chip(const Index offset, const Index dim)`
 
 A chip is a special kind of slice. It is the subtensor at the given offset in
 the dimension dim. The returned tensor has one fewer dimension than the input
@@ -1474,7 +1481,7 @@ lvalue. For example:
          0     0     0
 
 
-### <Operation> reverse(const ReverseDimensions& reverse)
+### `<Operation> reverse(const ReverseDimensions& reverse)`
 
 Returns a view of the input tensor that reverses the order of the coefficients
 along a subset of the dimensions.  The argument reverse is an array of boolean
@@ -1482,7 +1489,7 @@ values that indicates whether or not the order of the coefficients should be
 reversed along each of the dimensions.  This operation preserves the dimensions
 of the input tensor.
 
-For example this is what happens when you ```reverse()``` the first dimension
+For example this is what happens when you `reverse()` the first dimension
 of a 2D tensor:
 
     Eigen::Tensor<int, 2> a(4, 3);
@@ -1504,7 +1511,7 @@ of a 2D tensor:
        0   100   200
 
 
-### <Operation> broadcast(const Broadcast& broadcast)
+### `<Operation> broadcast(const Broadcast& broadcast)`
 
 Returns a view of the input tensor in which the input is replicated one to many
 times.
@@ -1528,11 +1535,11 @@ made in each of the dimensions.
        0   100   200    0   100   200
      300   400   500  300   400   500
 
-### <Operation> concatenate(const OtherDerived& other, Axis axis)
+### `<Operation> concatenate(const OtherDerived& other, Axis axis)`
 
 TODO
 
-### <Operation>  pad(const PaddingDimensions& padding)
+### `<Operation>  pad(const PaddingDimensions& padding)`
 
 Returns a view of the input tensor in which the input is padded with zeros.
 
@@ -1557,7 +1564,7 @@ Returns a view of the input tensor in which the input is padded with zeros.
        0     0     0    0
 
 
-### <Operation>  extract_patches(const PatchDims& patch_dims)
+### `<Operation>  extract_patches(const PatchDims& patch_dims)`
 
 Returns a tensor of coefficient patches extracted from the input tensor, where
 each patch is of dimension specified by 'patch_dims'. The returned tensor has
@@ -1644,9 +1651,7 @@ patch index: 5
 6 7
 10 11
 
-### <Operation>  extract_image_patches(const Index patch_rows, const Index patch_cols,
-                          const Index row_stride, const Index col_stride,
-                          const PaddingType padding_type)
+### `<Operation>  extract_image_patches(const Index patch_rows, const Index patch_cols, const Index row_stride, const Index col_stride, const PaddingType padding_type)`
 
 Returns a tensor of coefficient image patches extracted from the input tensor,
 which is expected to have dimensions ordered as follows (depending on the data
@@ -1701,7 +1706,7 @@ sizes:
 
 ## Special Operations
 
-### <Operation> cast<T>()
+### `<Operation> cast<T>()`
 
 Returns a tensor of type T with the same dimensions as the original tensor.
 The returned tensor contains the values of the original tensor converted to
@@ -1730,18 +1735,16 @@ but you can easily cast the tensors to floats to do the division:
     1 2 2
 
 
-### <Operation>     eval()
+### `<Operation>     eval()`
 
 TODO
 
 
 ## Representation of scalar values
 
-Scalar values are often represented by tensors of size 1 and rank 1. It would be
-more logical and user friendly to use tensors of rank 0 instead. For example
-Tensor<T, N>::maximum() currently returns a Tensor<T, 1>. Similarly, the inner
-product of 2 1d tensors (through contractions) returns a 1d tensor. In the
-future these operations might be updated to return 0d tensors instead.
+Scalar values are often represented by tensors of size 1 and rank 0.For example
+Tensor<T, N>::maximum() currently returns a Tensor<T, 0>. Similarly, the inner
+product of 2 1d tensors (through contractions) returns a 0d tensor.
 
 ## Limitations
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h
index 1940a9692..00295a255 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h
@@ -23,12 +23,12 @@ namespace Eigen {
   * The %Tensor class encompasses only dynamic-size objects so far.
   *
   * The first two template parameters are required:
-  * \tparam Scalar_ \anchor tensor_tparam_scalar Numeric type, e.g. float, double, int or std::complex<float>.
+  * \tparam Scalar_  Numeric type, e.g. float, double, int or `std::complex<float>`.
   *                 User defined scalar types are supported as well (see \ref user_defined_scalars "here").
   * \tparam NumIndices_ Number of indices (i.e. rank of the tensor)
   *
   * The remaining template parameters are optional -- in most cases you don't have to worry about them.
-  * \tparam Options_ \anchor tensor_tparam_options A combination of either \b #RowMajor or \b #ColMajor, and of either
+  * \tparam Options_  A combination of either \b #RowMajor or \b #ColMajor, and of either
   *                 \b #AutoAlign or \b #DontAlign.
   *                 The former controls \ref TopicStorageOrders "storage order", and defaults to column-major. The latter controls alignment, which is required
   *                 for vectorization. It defaults to aligning tensors. Note that tensors currently do not support any operations that profit from vectorization.
@@ -42,13 +42,13 @@ namespace Eigen {
   * \endcode
   *
   * This class can be extended with the help of the plugin mechanism described on the page
-  * \ref TopicCustomizingEigen by defining the preprocessor symbol \c EIGEN_TENSOR_PLUGIN.
+  * \ref TopicCustomizing_Plugins by defining the preprocessor symbol \c EIGEN_TENSOR_PLUGIN.
   *
   * <i><b>Some notes:</b></i>
   *
   * <dl>
   * <dt><b>Relation to other parts of Eigen:</b></dt>
-  * <dd>The midterm developement goal for this class is to have a similar hierarchy as Eigen uses for matrices, so that
+  * <dd>The midterm development goal for this class is to have a similar hierarchy as Eigen uses for matrices, so that
   * taking blocks or using tensors in expressions is easily possible, including an interface with the vector/matrix code
   * by providing .asMatrix() and .asVector() (or similar) methods for rank 2 and 1 tensors. However, currently, the %Tensor
   * class does not provide any of these features and is only available as a stand-alone class that just allows for
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h
index 7a45a5cf4..f573608d9 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h
@@ -22,7 +22,9 @@ namespace Eigen {
   * This class is the common parent of the Tensor and TensorMap class, thus
   * making it possible to use either class interchangably in expressions.
   */
-
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+// FIXME Doxygen does not like the inheritance with different template parameters
+// Since there is no doxygen documentation inside, we disable it for now
 template<typename Derived>
 class TensorBase<Derived, ReadOnlyAccessors>
 {
@@ -1004,7 +1006,7 @@ class TensorBase : public TensorBase<Derived, ReadOnlyAccessors> {
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE const Derived& derived() const { return *static_cast<const Derived*>(this); }
 };
-
+#endif // EIGEN_PARSED_BY_DOXYGEN
 } // end namespace Eigen
 
 #endif // EIGEN_CXX11_TENSOR_TENSOR_BASE_H
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h
index ee16cde9b..c70dea053 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h
@@ -116,15 +116,6 @@ struct TensorEvaluator<const TensorContractionOp<Indices, LeftArgType, RightArgT
   template <bool lhs_inner_dim_contiguous, bool rhs_inner_dim_contiguous,
             bool rhs_inner_dim_reordered, int Alignment>
   void evalProduct(Scalar* buffer) const {
-    typedef
-        typename internal::remove_const<typename EvalLeftArgType::Scalar>::type
-            LhsScalar;
-    typedef
-        typename internal::remove_const<typename EvalRightArgType::Scalar>::type
-            RhsScalar;
-    typedef typename internal::gebp_traits<LhsScalar, RhsScalar> Traits;
-    typedef TensorEvaluator<EvalLeftArgType, Device> LeftEvaluator;
-    typedef TensorEvaluator<EvalRightArgType, Device> RightEvaluator;
     typedef internal::TensorContractionInputMapper<
         LhsScalar, Index, internal::Lhs, LeftEvaluator, left_nocontract_t,
         contract_t, internal::packet_traits<LhsScalar>::size,
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h
index 069680a11..17f04665a 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h
@@ -196,9 +196,11 @@ struct ThreadPoolDevice {
     // of blocks to be evenly dividable across threads.
 
     double block_size_f = 1.0 / CostModel::taskSize(1, cost);
-    Index block_size = numext::mini(n, numext::maxi<Index>(1, block_size_f));
-    const Index max_block_size =
-        numext::mini(n, numext::maxi<Index>(1, 2 * block_size_f));
+    const Index max_oversharding_factor = 4;
+    Index block_size = numext::mini(
+        n, numext::maxi<Index>(divup<Index>(n, max_oversharding_factor * numThreads()),
+                               block_size_f));
+    const Index max_block_size = numext::mini(n, 2 * block_size);
     if (block_align) {
       Index new_block_size = block_align(block_size);
       eigen_assert(new_block_size >= block_size);
@@ -212,7 +214,8 @@ struct ThreadPoolDevice {
         (divup<int>(block_count, numThreads()) * numThreads());
     // Now try to increase block size up to max_block_size as long as it
     // doesn't decrease parallel efficiency.
-    for (Index prev_block_count = block_count; prev_block_count > 1;) {
+    for (Index prev_block_count = block_count;
+         max_efficiency < 1.0 && prev_block_count > 1;) {
       // This is the next block size that divides size into a smaller number
       // of blocks than the current block_size.
       Index coarser_block_size = divup(n, prev_block_count - 1);
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h
index b24cdebf1..451940de3 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h
@@ -192,7 +192,7 @@ template <std::size_t V1=0, std::size_t V2=0, std::size_t V3=0, std::size_t V4=0
   }
 #endif
 
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE DenseIndex operator[] (const int index) const {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index operator[] (const Index index) const {
     switch (index) {
       case 0:
         return internal::get<0, Base>::value;
@@ -206,7 +206,7 @@ template <std::size_t V1=0, std::size_t V2=0, std::size_t V3=0, std::size_t V4=0
         return internal::get<4, Base>::value;
       default:
         eigen_assert(false && "index overflow");
-        return static_cast<DenseIndex>(-1);
+        return static_cast<Index>(-1);
     }
   }
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h
index bbd5eb374..8bece4e65 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h
@@ -12,19 +12,6 @@
 
 namespace Eigen {
 
-/** \class TensorForcedEval
-  * \ingroup CXX11_Tensor_Module
-  *
-  * \brief Tensor reshaping class.
-  *
-  *
-  */
-/// template <class> class MakePointer_ is added to convert the host pointer to the device pointer.
-/// It is added due to the fact that for our device compiler T* is not allowed.
-/// If we wanted to use the same Evaluator functions we have to convert that type to our pointer T.
-/// This is done through our MakePointer_ class. By default the Type in the MakePointer_<T> is T* .
-/// Therefore, by adding the default value, we managed to convert the type and it does not break any
-/// existing code as its default value is T*.
 namespace internal {
 template<typename XprType, template <class> class MakePointer_>
 struct traits<TensorForcedEvalOp<XprType, MakePointer_> >
@@ -65,6 +52,21 @@ struct nested<TensorForcedEvalOp<XprType, MakePointer_>, 1, typename eval<Tensor
 
 
 
+// FIXME use proper doxygen documentation (e.g. \tparam MakePointer_)
+
+/** \class TensorForcedEvalOp
+  * \ingroup CXX11_Tensor_Module
+  *
+  * \brief Tensor reshaping class.
+  *
+  *
+  */
+/// `template <class> class MakePointer_` is added to convert the host pointer to the device pointer.
+/// It is added due to the fact that for our device compiler `T*` is not allowed.
+/// If we wanted to use the same Evaluator functions we have to convert that type to our pointer `T`.
+/// This is done through our `MakePointer_` class. By default the Type in the `MakePointer_<T>` is `T*` .
+/// Therefore, by adding the default value, we managed to convert the type and it does not break any
+/// existing code as its default value is `T*`.
 template<typename XprType, template <class> class MakePointer_>
 class TensorForcedEvalOp : public TensorBase<TensorForcedEvalOp<XprType, MakePointer_>, ReadOnlyAccessors>
 {
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h
index eb1d4934e..e27753b19 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h
@@ -12,7 +12,7 @@
 
 namespace Eigen {
 
-/** \class TensorGenerator
+/** \class TensorGeneratorOp
   * \ingroup CXX11_Tensor_Module
   *
   * \brief Tensor generator class.
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h
index a8e55757e..e4fc86a40 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h
@@ -12,18 +12,20 @@
 
 namespace Eigen {
 
+// FIXME use proper doxygen documentation (e.g. \tparam MakePointer_)
+
 /** \class TensorMap
   * \ingroup CXX11_Tensor_Module
   *
   * \brief A tensor expression mapping an existing array of data.
   *
   */
-/// template <class> class MakePointer_ is added to convert the host pointer to the device pointer.
-/// It is added due to the fact that for our device compiler T* is not allowed.
-/// If we wanted to use the same Evaluator functions we have to convert that type to our pointer T.
-/// This is done through our MakePointer_ class. By default the Type in the MakePointer_<T> is T* .
+/// `template <class> class MakePointer_` is added to convert the host pointer to the device pointer.
+/// It is added due to the fact that for our device compiler `T*` is not allowed.
+/// If we wanted to use the same Evaluator functions we have to convert that type to our pointer `T`.
+/// This is done through our `MakePointer_` class. By default the Type in the `MakePointer_<T>` is `T*` .
 /// Therefore, by adding the default value, we managed to convert the type and it does not break any
-/// existing code as its default value is T*.
+/// existing code as its default value is `T*`.
 template<typename PlainObjectType, int Options_, template <class> class MakePointer_> class TensorMap : public TensorBase<TensorMap<PlainObjectType, Options_, MakePointer_> >
 {
   public:
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorStorage.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorStorage.h
index 2854a4a17..e6a666f78 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorStorage.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorStorage.h
@@ -31,12 +31,12 @@ namespace Eigen {
   *
   * \sa Tensor
   */
-template<typename T, typename Dimensions, int Options_> class TensorStorage;
+template<typename T, typename Dimensions, int Options> class TensorStorage;
 
 
 // Pure fixed-size storage
-template<typename T, int Options_, typename FixedDimensions>
-class TensorStorage<T, FixedDimensions, Options_>
+template<typename T, typename FixedDimensions, int Options_>
+class TensorStorage
 {
  private:
   static const std::size_t Size = FixedDimensions::total_size;
@@ -66,7 +66,7 @@ class TensorStorage<T, FixedDimensions, Options_>
 
 
 // pure dynamic
-template<typename T, int Options_, typename IndexType, int NumIndices_>
+template<typename T, typename IndexType, int NumIndices_, int Options_>
 class TensorStorage<T, DSizes<IndexType, NumIndices_>, Options_>
 {
   public:
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExprConstructor.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExprConstructor.h
index 7ed3a3a56..983f63180 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExprConstructor.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExprConstructor.h
@@ -33,7 +33,7 @@ struct EvalToLHSConstructor {
   EvalToLHSConstructor(const utility::tuple::Tuple<Params...> &t): expr((&(*(utility::tuple::get<N>(t).get_pointer())))) {}
 };
 
-/// \struct ExprConstructor is used to reconstruct the expression on the device and
+/// struct ExprConstructor is used to reconstruct the expression on the device and
 /// recreate the expression with MakeGlobalPointer containing the device address
 /// space for the TensorMap pointers used in eval function.
 /// It receives the original expression type, the functor of the node, the tuple
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractAccessor.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractAccessor.h
index b1da6858e..cc18fcdf9 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractAccessor.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractAccessor.h
@@ -35,7 +35,7 @@
 namespace Eigen {
 namespace TensorSycl {
 namespace internal {
-/// \struct ExtractAccessor: Extract Accessor Class is used to extract the
+/// struct ExtractAccessor: Extract Accessor Class is used to extract the
 /// accessor from a buffer.
 /// Depending on the type of the leaf node we can get a read accessor or a
 /// read_write accessor
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractFunctors.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractFunctors.h
index 427125343..9edd38ea4 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractFunctors.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractFunctors.h
@@ -25,7 +25,7 @@
 namespace Eigen {
 namespace TensorSycl {
 namespace internal {
-/// \struct FunctorExtractor:  This struct is used to extract the functors
+/// struct FunctorExtractor:  This struct is used to extract the functors
 /// constructed on
 /// the host-side, to pack them and reuse them in reconstruction of the
 /// expression on the device.
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclTuple.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclTuple.h
index 063b027e8..83915f31a 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclTuple.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclTuple.h
@@ -34,7 +34,7 @@ struct StaticIf<true, T> {
 
 /// \struct Tuple
 /// \brief is a fixed-size collection of heterogeneous values
-/// \ztparam Ts...	-	the types of the elements that the tuple stores.
+/// \tparam Ts...	-	the types of the elements that the tuple stores.
 /// Empty list is supported.
 template <class... Ts>
 struct Tuple {};
@@ -147,6 +147,8 @@ struct IndexList {};
 template <size_t MIN, size_t N, size_t... Is>
 struct RangeBuilder;
 
+// FIXME Doxygen has problems with recursive inheritance
+#ifndef EIGEN_PARSED_BY_DOXYGEN
 /// \brief base Step: Specialisation of the \ref RangeBuilder when the
 /// MIN==MAX. In this case the Is... is [0 to sizeof...(tuple elements))
 /// \tparam MIN is the starting index of the tuple
@@ -164,6 +166,7 @@ struct RangeBuilder<MIN, MIN, Is...> {
 /// \tparam Is... are the list of generated index so far
 template <size_t MIN, size_t N, size_t... Is>
 struct RangeBuilder : public RangeBuilder<MIN, N - 1, N - 1, Is...> {};
+#endif // EIGEN_PARSED_BY_DOXYGEN
 
 /// \brief IndexRange that returns a [MIN, MAX) index range
 /// \tparam MIN is the starting index in the tuple
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/util/TemplateGroupTheory.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/util/TemplateGroupTheory.h
index 0fe0b7c46..5e97d07a9 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/util/TemplateGroupTheory.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/util/TemplateGroupTheory.h
@@ -17,7 +17,7 @@ namespace internal {
 namespace group_theory {
 
 /** \internal
-  * \file CXX11/Tensor/util/TemplateGroupTheory.h
+  * \file CXX11/src/TensorSymmetry/util/TemplateGroupTheory.h
   * This file contains C++ templates that implement group theory algorithms.
   *
   * The algorithms allow for a compile-time analysis of finite groups.
@@ -167,7 +167,9 @@ template<
   typename elements,
   bool dont_add_current_element   // = false
 >
-struct dimino_first_step_elements_helper :
+struct dimino_first_step_elements_helper
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+  : // recursive inheritance is too difficult for Doxygen
   public dimino_first_step_elements_helper<
     Multiply,
     Equality,
@@ -187,6 +189,7 @@ template<
   typename elements
 >
 struct dimino_first_step_elements_helper<Multiply, Equality, id, g, current_element, elements, true>
+#endif // EIGEN_PARSED_BY_DOXYGEN
 {
   typedef elements type;
   constexpr static int global_flags = Equality<current_element, id>::global_flags;
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/util/EmulateCXX11Meta.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/util/EmulateCXX11Meta.h
index f3aa1b144..8a536faf6 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/util/EmulateCXX11Meta.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/CXX11/src/util/EmulateCXX11Meta.h
@@ -188,7 +188,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE t array_prod(const array<t, n>& a) {
 }
 template<typename t>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE t array_prod(const array<t, 0>& /*a*/) {
-  return 0;
+  return 1;
 }
 
 template<typename t>
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/FFT b/gtsam/3rdparty/Eigen/unsupported/Eigen/FFT
index 2c45b3999..d8cf3e642 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/FFT
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/FFT
@@ -289,6 +289,7 @@ class FFT
     void inv( MatrixBase<OutputDerived> & dst, const MatrixBase<ComplexDerived> & src, Index nfft=-1)
     {
       typedef typename ComplexDerived::Scalar src_type;
+      typedef typename ComplexDerived::RealScalar real_type;
       typedef typename OutputDerived::Scalar dst_type;
       const bool realfft= (NumTraits<dst_type>::IsComplex == 0);
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(OutputDerived)
@@ -329,9 +330,9 @@ class FFT
             tmp.head(nhead) = src.head(nhead);
             tmp.tail(ntail) = src.tail(ntail);
             if (resize_input<0) { //shrinking -- create the Nyquist bin as the average of the two bins that fold into it
-              tmp(nhead) = ( src(nfft/2) + src( src.size() - nfft/2 ) )*src_type(.5);
+              tmp(nhead) = ( src(nfft/2) + src( src.size() - nfft/2 ) )*real_type(.5);
             }else{ // expanding -- split the old Nyquist bin into two halves
-              tmp(nhead) = src(nhead) * src_type(.5);
+              tmp(nhead) = src(nhead) * real_type(.5);
               tmp(tmp.size()-nhead) = tmp(nhead);
             }
           }
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/MatrixFunctions b/gtsam/3rdparty/Eigen/unsupported/Eigen/MatrixFunctions
index 0320606c1..60dc0a69b 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/MatrixFunctions
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/MatrixFunctions
@@ -161,8 +161,8 @@ the z-axis.
 \include MatrixExponential.cpp
 Output: \verbinclude MatrixExponential.out
 
-\note \p M has to be a matrix of \c float, \c double, \c long double
-\c complex<float>, \c complex<double>, or \c complex<long double> .
+\note \p M has to be a matrix of \c float, \c double, `long double`
+\c complex<float>, \c complex<double>, or `complex<long double>` .
 
 
 \subsection matrixbase_log MatrixBase::log()
@@ -219,9 +219,8 @@ documentation of \ref matrixbase_exp "exp()".
 \include MatrixLogarithm.cpp
 Output: \verbinclude MatrixLogarithm.out
 
-\note \p M has to be a matrix of \c float, \c double, <tt>long
-double</tt>, \c complex<float>, \c complex<double>, or \c complex<long
-double> .
+\note \p M has to be a matrix of \c float, \c double, `long
+double`, \c complex<float>, \c complex<double>, or `complex<long double>`.
 
 \sa MatrixBase::exp(), MatrixBase::matrixFunction(), 
     class MatrixLogarithmAtomic, MatrixBase::sqrt().
@@ -326,9 +325,9 @@ Example:
 \include MatrixPower_optimal.cpp
 Output: \verbinclude MatrixPower_optimal.out
 
-\note \p M has to be a matrix of \c float, \c double, <tt>long
-double</tt>, \c complex<float>, \c complex<double>, or \c complex<long
-double> .
+\note \p M has to be a matrix of \c float, \c double, `long
+double`, \c complex<float>, \c complex<double>, or
+\c complex<long double> .
 
 \sa MatrixBase::exp(), MatrixBase::log(), class MatrixPower.
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport
index 87f50947d..085325ce1 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport
@@ -184,7 +184,7 @@ inline void glRotate(const Rotation2D<float>& rot)
 }
 inline void glRotate(const Rotation2D<double>& rot)
 {
-  glRotated(rot.angle()*180.0/EIGEN_PI, 0.0, 0.0, 1.0);
+  glRotated(rot.angle()*180.0/double(EIGEN_PI), 0.0, 0.0, 1.0);
 }
 
 template<typename Derived> void glRotate(const RotationBase<Derived,3>& rot)
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
index 279fe5cd3..2f50e9968 100755
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
@@ -534,7 +534,8 @@ struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, Bi
   EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
   FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
     using namespace Eigen; \
-    EIGEN_UNUSED typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
+    typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
+    EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
     CODE; \
   }
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/KdBVH.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/KdBVH.h
index 1b8d75865..5e39af26c 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/KdBVH.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/KdBVH.h
@@ -35,6 +35,7 @@ struct get_boxes_helper {
   {
     outBoxes.insert(outBoxes.end(), boxBegin, boxEnd);
     eigen_assert(outBoxes.size() == objects.size());
+    EIGEN_ONLY_USED_FOR_DEBUG(objects);
   }
 };
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h
index bae04fc30..4079e2367 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h
@@ -39,7 +39,6 @@ template <typename VectorType, typename IndexType>
 void sortWithPermutation (VectorType& vec, IndexType& perm, typename IndexType::Scalar& ncut)
 {
   eigen_assert(vec.size() == perm.size());
-  typedef typename IndexType::Scalar Index; 
   bool flag; 
   for (Index k  = 0; k < ncut; k++)
   {
@@ -112,7 +111,6 @@ class DGMRES : public IterativeSolverBase<DGMRES<_MatrixType,_Preconditioner> >
     using Base::_solve_impl;
     typedef _MatrixType MatrixType;
     typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::Index Index;
     typedef typename MatrixType::StorageIndex StorageIndex;
     typedef typename MatrixType::RealScalar RealScalar;
     typedef _Preconditioner Preconditioner;
@@ -146,7 +144,7 @@ class DGMRES : public IterativeSolverBase<DGMRES<_MatrixType,_Preconditioner> >
   void _solve_with_guess_impl(const Rhs& b, Dest& x) const
   {    
     bool failed = false;
-    for(int j=0; j<b.cols(); ++j)
+    for(Index j=0; j<b.cols(); ++j)
     {
       m_iterations = Base::maxIterations();
       m_error = Base::m_tolerance;
@@ -170,17 +168,17 @@ class DGMRES : public IterativeSolverBase<DGMRES<_MatrixType,_Preconditioner> >
   /** 
    * Get the restart value
     */
-  int restart() { return m_restart; }
+  Index restart() { return m_restart; }
   
   /** 
    * Set the restart value (default is 30)  
    */
-  void set_restart(const int restart) { m_restart=restart; }
+  void set_restart(const Index restart) { m_restart=restart; }
   
   /** 
    * Set the number of eigenvalues to deflate at each restart 
    */
-  void setEigenv(const int neig) 
+  void setEigenv(const Index neig) 
   {
     m_neig = neig;
     if (neig+1 > m_maxNeig) m_maxNeig = neig+1; // To allow for complex conjugates
@@ -189,12 +187,12 @@ class DGMRES : public IterativeSolverBase<DGMRES<_MatrixType,_Preconditioner> >
   /** 
    * Get the size of the deflation subspace size
    */ 
-  int deflSize() {return m_r; }
+  Index deflSize() {return m_r; }
   
   /**
    * Set the maximum size of the deflation subspace
    */
-  void setMaxEigenv(const int maxNeig) { m_maxNeig = maxNeig; }
+  void setMaxEigenv(const Index maxNeig) { m_maxNeig = maxNeig; }
   
   protected:
     // DGMRES algorithm 
@@ -202,12 +200,12 @@ class DGMRES : public IterativeSolverBase<DGMRES<_MatrixType,_Preconditioner> >
     void dgmres(const MatrixType& mat,const Rhs& rhs, Dest& x, const Preconditioner& precond) const;
     // Perform one cycle of GMRES
     template<typename Dest>
-    int dgmresCycle(const MatrixType& mat, const Preconditioner& precond, Dest& x, DenseVector& r0, RealScalar& beta, const RealScalar& normRhs, int& nbIts) const; 
+    Index dgmresCycle(const MatrixType& mat, const Preconditioner& precond, Dest& x, DenseVector& r0, RealScalar& beta, const RealScalar& normRhs, Index& nbIts) const; 
     // Compute data to use for deflation 
-    int dgmresComputeDeflationData(const MatrixType& mat, const Preconditioner& precond, const Index& it, StorageIndex& neig) const;
+    Index dgmresComputeDeflationData(const MatrixType& mat, const Preconditioner& precond, const Index& it, StorageIndex& neig) const;
     // Apply deflation to a vector
     template<typename RhsType, typename DestType>
-    int dgmresApplyDeflation(const RhsType& In, DestType& Out) const; 
+    Index dgmresApplyDeflation(const RhsType& In, DestType& Out) const; 
     ComplexVector schurValues(const ComplexSchur<DenseMatrix>& schurofH) const;
     ComplexVector schurValues(const RealSchur<DenseMatrix>& schurofH) const;
     // Init data for deflation
@@ -221,8 +219,8 @@ class DGMRES : public IterativeSolverBase<DGMRES<_MatrixType,_Preconditioner> >
     mutable DenseMatrix m_T; /* T=U^T*M^{-1}*A*U */
     mutable PartialPivLU<DenseMatrix> m_luT; // LU factorization of m_T
     mutable StorageIndex m_neig; //Number of eigenvalues to extract at each restart
-    mutable int m_r; // Current number of deflated eigenvalues, size of m_U
-    mutable int m_maxNeig; // Maximum number of eigenvalues to deflate
+    mutable Index m_r; // Current number of deflated eigenvalues, size of m_U
+    mutable Index m_maxNeig; // Maximum number of eigenvalues to deflate
     mutable RealScalar m_lambdaN; //Modulus of the largest eigenvalue of A
     mutable bool m_isDeflAllocated;
     mutable bool m_isDeflInitialized;
@@ -244,9 +242,9 @@ void DGMRES<_MatrixType, _Preconditioner>::dgmres(const MatrixType& mat,const Rh
               const Preconditioner& precond) const
 {
   //Initialization
-  int n = mat.rows(); 
+  Index n = mat.rows(); 
   DenseVector r0(n); 
-  int nbIts = 0; 
+  Index nbIts = 0; 
   m_H.resize(m_restart+1, m_restart);
   m_Hes.resize(m_restart, m_restart);
   m_V.resize(n,m_restart+1);
@@ -284,7 +282,7 @@ void DGMRES<_MatrixType, _Preconditioner>::dgmres(const MatrixType& mat,const Rh
  */
 template< typename _MatrixType, typename _Preconditioner>
 template<typename Dest>
-int DGMRES<_MatrixType, _Preconditioner>::dgmresCycle(const MatrixType& mat, const Preconditioner& precond, Dest& x, DenseVector& r0, RealScalar& beta, const RealScalar& normRhs, int& nbIts) const
+Index DGMRES<_MatrixType, _Preconditioner>::dgmresCycle(const MatrixType& mat, const Preconditioner& precond, Dest& x, DenseVector& r0, RealScalar& beta, const RealScalar& normRhs, Index& nbIts) const
 {
   //Initialization 
   DenseVector g(m_restart+1); // Right hand side of the least square problem
@@ -293,8 +291,8 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresCycle(const MatrixType& mat, con
   m_V.col(0) = r0/beta; 
   m_info = NoConvergence; 
   std::vector<JacobiRotation<Scalar> >gr(m_restart); // Givens rotations
-  int it = 0; // Number of inner iterations 
-  int n = mat.rows();
+  Index it = 0; // Number of inner iterations 
+  Index n = mat.rows();
   DenseVector tv1(n), tv2(n);  //Temporary vectors
   while (m_info == NoConvergence && it < m_restart && nbIts < m_iterations)
   {    
@@ -312,7 +310,7 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresCycle(const MatrixType& mat, con
    
     // Orthogonalize it with the previous basis in the basis using modified Gram-Schmidt
     Scalar coef; 
-    for (int i = 0; i <= it; ++i)
+    for (Index i = 0; i <= it; ++i)
     { 
       coef = tv1.dot(m_V.col(i));
       tv1 = tv1 - coef * m_V.col(i); 
@@ -328,7 +326,7 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresCycle(const MatrixType& mat, con
     // FIXME Check for happy breakdown 
     
     // Update Hessenberg matrix with Givens rotations
-    for (int i = 1; i <= it; ++i) 
+    for (Index i = 1; i <= it; ++i) 
     {
       m_H.col(it).applyOnTheLeft(i-1,i,gr[i-1].adjoint());
     }
@@ -394,7 +392,6 @@ inline typename DGMRES<_MatrixType, _Preconditioner>::ComplexVector DGMRES<_Matr
 template< typename _MatrixType, typename _Preconditioner>
 inline typename DGMRES<_MatrixType, _Preconditioner>::ComplexVector DGMRES<_MatrixType, _Preconditioner>::schurValues(const RealSchur<DenseMatrix>& schurofH) const
 {
-  typedef typename MatrixType::Index Index;
   const DenseMatrix& T = schurofH.matrixT();
   Index it = T.rows();
   ComplexVector eig(it);
@@ -418,7 +415,7 @@ inline typename DGMRES<_MatrixType, _Preconditioner>::ComplexVector DGMRES<_Matr
 }
 
 template< typename _MatrixType, typename _Preconditioner>
-int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const MatrixType& mat, const Preconditioner& precond, const Index& it, StorageIndex& neig) const
+Index DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const MatrixType& mat, const Preconditioner& precond, const Index& it, StorageIndex& neig) const
 {
   // First, find the Schur form of the Hessenberg matrix H
   typename internal::conditional<NumTraits<Scalar>::IsComplex, ComplexSchur<DenseMatrix>, RealSchur<DenseMatrix> >::type schurofH; 
@@ -433,8 +430,8 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const Matri
   
   // Reorder the absolute values of Schur values
   DenseRealVector modulEig(it); 
-  for (int j=0; j<it; ++j) modulEig(j) = std::abs(eig(j)); 
-  perm.setLinSpaced(it,0,it-1);
+  for (Index j=0; j<it; ++j) modulEig(j) = std::abs(eig(j)); 
+  perm.setLinSpaced(it,0,internal::convert_index<StorageIndex>(it-1));
   internal::sortWithPermutation(modulEig, perm, neig);
   
   if (!m_lambdaN)
@@ -442,7 +439,7 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const Matri
     m_lambdaN = (std::max)(modulEig.maxCoeff(), m_lambdaN);
   }
   //Count the real number of extracted eigenvalues (with complex conjugates)
-  int nbrEig = 0; 
+  Index nbrEig = 0; 
   while (nbrEig < neig)
   {
     if(eig(perm(it-nbrEig-1)).imag() == RealScalar(0)) nbrEig++; 
@@ -451,7 +448,7 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const Matri
   // Extract the  Schur vectors corresponding to the smallest Ritz values
   DenseMatrix Sr(it, nbrEig); 
   Sr.setZero();
-  for (int j = 0; j < nbrEig; j++)
+  for (Index j = 0; j < nbrEig; j++)
   {
     Sr.col(j) = schurofH.matrixU().col(perm(it-j-1));
   }
@@ -462,8 +459,8 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const Matri
   if (m_r)
   {
    // Orthogonalize X against m_U using modified Gram-Schmidt
-   for (int j = 0; j < nbrEig; j++)
-     for (int k =0; k < m_r; k++)
+   for (Index j = 0; j < nbrEig; j++)
+     for (Index k =0; k < m_r; k++)
       X.col(j) = X.col(j) - (m_U.col(k).dot(X.col(j)))*m_U.col(k); 
   }
   
@@ -473,7 +470,7 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const Matri
     dgmresInitDeflation(m); 
   DenseMatrix MX(m, nbrEig);
   DenseVector tv1(m);
-  for (int j = 0; j < nbrEig; j++)
+  for (Index j = 0; j < nbrEig; j++)
   {
     tv1 = mat * X.col(j);
     MX.col(j) = precond.solve(tv1);
@@ -488,8 +485,8 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const Matri
   }
   
   // Save X into m_U and m_MX in m_MU
-  for (int j = 0; j < nbrEig; j++) m_U.col(m_r+j) = X.col(j);
-  for (int j = 0; j < nbrEig; j++) m_MU.col(m_r+j) = MX.col(j);
+  for (Index j = 0; j < nbrEig; j++) m_U.col(m_r+j) = X.col(j);
+  for (Index j = 0; j < nbrEig; j++) m_MU.col(m_r+j) = MX.col(j);
   // Increase the size of the invariant subspace
   m_r += nbrEig; 
   
@@ -502,7 +499,7 @@ int DGMRES<_MatrixType, _Preconditioner>::dgmresComputeDeflationData(const Matri
 }
 template<typename _MatrixType, typename _Preconditioner>
 template<typename RhsType, typename DestType>
-int DGMRES<_MatrixType, _Preconditioner>::dgmresApplyDeflation(const RhsType &x, DestType &y) const
+Index DGMRES<_MatrixType, _Preconditioner>::dgmresApplyDeflation(const RhsType &x, DestType &y) const
 {
   DenseVector x1 = m_U.leftCols(m_r).transpose() * x; 
   y = x + m_U.leftCols(m_r) * ( m_lambdaN * m_luT.solve(x1) - x1);
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h
index 5a82b0df6..92618b107 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h
@@ -21,7 +21,7 @@ namespace internal {
 *
 * Parameters:
 *  \param mat       matrix of linear system of equations
-*  \param Rhs       right hand side vector of linear system of equations
+*  \param rhs       right hand side vector of linear system of equations
 *  \param x         on input: initial guess, on output: solution
 *  \param precond   preconditioner used
 *  \param iters     on input: maximum number of iterations to perform
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
index bb6d9e1fe..e5ebbcf23 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
@@ -234,12 +234,13 @@ struct matrix_exp_computeUV<MatrixType, float>
 template <typename MatrixType>
 struct matrix_exp_computeUV<MatrixType, double>
 {
+  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
   template <typename ArgType>
   static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
   {
     using std::frexp;
     using std::pow;
-    const double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
+    const RealScalar l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
     squarings = 0;
     if (l1norm < 1.495585217958292e-002) {
       matrix_exp_pade3(arg, U, V);
@@ -250,10 +251,10 @@ struct matrix_exp_computeUV<MatrixType, double>
     } else if (l1norm < 2.097847961257068e+000) {
       matrix_exp_pade9(arg, U, V);
     } else {
-      const double maxnorm = 5.371920351148152;
+      const RealScalar maxnorm = 5.371920351148152;
       frexp(l1norm / maxnorm, &squarings);
       if (squarings < 0) squarings = 0;
-      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<double>(squarings));
+      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<RealScalar>(squarings));
       matrix_exp_pade13(A, U, V);
     }
   }
@@ -326,6 +327,7 @@ struct matrix_exp_computeUV<MatrixType, long double>
     } else if (l1norm < 1.125358383453143065081397882891878e+000L) {
       matrix_exp_pade13(arg, U, V);
     } else {
+      const long double maxnorm = 2.884233277829519311757165057717815L;
       frexp(l1norm / maxnorm, &squarings);
       if (squarings < 0) squarings = 0;
       MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
@@ -342,6 +344,27 @@ struct matrix_exp_computeUV<MatrixType, long double>
   }
 };
 
+template<typename T> struct is_exp_known_type : false_type {};
+template<> struct is_exp_known_type<float> : true_type {};
+template<> struct is_exp_known_type<double> : true_type {};
+#if LDBL_MANT_DIG <= 112
+template<> struct is_exp_known_type<long double> : true_type {};
+#endif
+
+template <typename ArgType, typename ResultType>
+void matrix_exp_compute(const ArgType& arg, ResultType &result, true_type) // natively supported scalar type
+{
+  typedef typename ArgType::PlainObject MatrixType;
+  MatrixType U, V;
+  int squarings;
+  matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings); // Pade approximant is (U+V) / (-U+V)
+  MatrixType numer = U + V;
+  MatrixType denom = -U + V;
+  result = denom.partialPivLu().solve(numer);
+  for (int i=0; i<squarings; i++)
+    result *= result;   // undo scaling by repeated squaring
+}
+
 
 /* Computes the matrix exponential
  *
@@ -349,26 +372,13 @@ struct matrix_exp_computeUV<MatrixType, long double>
  * \param result variable in which result will be stored
  */
 template <typename ArgType, typename ResultType>
-void matrix_exp_compute(const ArgType& arg, ResultType &result)
+void matrix_exp_compute(const ArgType& arg, ResultType &result, false_type) // default
 {
   typedef typename ArgType::PlainObject MatrixType;
-#if LDBL_MANT_DIG > 112 // rarely happens
   typedef typename traits<MatrixType>::Scalar Scalar;
   typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef typename std::complex<RealScalar> ComplexScalar;
-  if (sizeof(RealScalar) > 14) {
-    result = arg.matrixFunction(internal::stem_function_exp<ComplexScalar>);
-    return;
-  }
-#endif
-  MatrixType U, V;
-  int squarings; 
-  matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings); // Pade approximant is (U+V) / (-U+V)
-  MatrixType numer = U + V;
-  MatrixType denom = -U + V;
-  result = denom.partialPivLu().solve(numer);
-  for (int i=0; i<squarings; i++)
-    result *= result;   // undo scaling by repeated squaring
+  result = arg.matrixFunction(internal::stem_function_exp<ComplexScalar>);
 }
 
 } // end namespace Eigen::internal
@@ -402,7 +412,7 @@ template<typename Derived> struct MatrixExponentialReturnValue
     inline void evalTo(ResultType& result) const
     {
       const typename internal::nested_eval<Derived, 10>::type tmp(m_src);
-      internal::matrix_exp_compute(tmp, result);
+      internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type<typename Derived::Scalar>());
     }
 
     Index rows() const { return m_src.rows(); }
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
index 3f7d77710..3df82394c 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
@@ -7,8 +7,8 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-#ifndef EIGEN_MATRIX_FUNCTION
-#define EIGEN_MATRIX_FUNCTION
+#ifndef EIGEN_MATRIX_FUNCTION_H
+#define EIGEN_MATRIX_FUNCTION_H
 
 #include "StemFunction.h"
 
@@ -577,4 +577,4 @@ const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
 
 } // end namespace Eigen
 
-#endif // EIGEN_MATRIX_FUNCTION
+#endif // EIGEN_MATRIX_FUNCTION_H
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
index ff8f6e732..cf5fffad3 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
@@ -324,7 +324,7 @@ public:
   
   /** \brief Compute the matrix logarithm.
     *
-    * \param[out]  result  Logarithm of \p A, where \A is as specified in the constructor.
+    * \param[out]  result  Logarithm of \c A, where \c A is as specified in the constructor.
     */
   template <typename ResultType>
   inline void evalTo(ResultType& result) const
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
index ebc433d89..a3273da4e 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
@@ -57,8 +57,8 @@ class MatrixPowerParenthesesReturnValue : public ReturnByValue< MatrixPowerParen
      * \param[out] result
      */
     template<typename ResultType>
-    inline void evalTo(ResultType& res) const
-    { m_pow.compute(res, m_p); }
+    inline void evalTo(ResultType& result) const
+    { m_pow.compute(result, m_p); }
 
     Index rows() const { return m_pow.rows(); }
     Index cols() const { return m_pow.cols(); }
@@ -618,8 +618,8 @@ class MatrixPowerReturnValue : public ReturnByValue< MatrixPowerReturnValue<Deri
      * constructor.
      */
     template<typename ResultType>
-    inline void evalTo(ResultType& res) const
-    { MatrixPower<PlainObject>(m_A.eval()).compute(res, m_p); }
+    inline void evalTo(ResultType& result) const
+    { MatrixPower<PlainObject>(m_A.eval()).compute(result, m_p); }
 
     Index rows() const { return m_A.rows(); }
     Index cols() const { return m_A.cols(); }
@@ -669,8 +669,8 @@ class MatrixComplexPowerReturnValue : public ReturnByValue< MatrixComplexPowerRe
      * constructor.
      */
     template<typename ResultType>
-    inline void evalTo(ResultType& res) const
-    { res = (m_p * m_A.log()).exp(); }
+    inline void evalTo(ResultType& result) const
+    { result = (m_p * m_A.log()).exp(); }
 
     Index rows() const { return m_A.rows(); }
     Index cols() const { return m_A.cols(); }
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
index afd88ec4d..2e5abda38 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
@@ -120,7 +120,6 @@ template <typename MatrixType, typename ResultType>
 void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT)
 {
   using std::sqrt;
-  typedef typename MatrixType::Index Index;
   const Index size = T.rows();
   for (Index i = 0; i < size; i++) {
     if (i == size - 1 || T.coeff(i+1, i) == 0) {
@@ -139,7 +138,6 @@ void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrt
 template <typename MatrixType, typename ResultType>
 void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT)
 {
-  typedef typename MatrixType::Index Index;
   const Index size = T.rows();
   for (Index j = 1; j < size; j++) {
       if (T.coeff(j, j-1) != 0)  // if T(j-1:j, j-1:j) is a 2-by-2 block
@@ -206,7 +204,6 @@ template <typename MatrixType, typename ResultType>
 void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
 {
   using std::sqrt;
-  typedef typename MatrixType::Index Index;
       typedef typename MatrixType::Scalar Scalar;
 
   eigen_assert(arg.rows() == arg.cols());
@@ -318,7 +315,6 @@ template<typename Derived> class MatrixSquareRootReturnValue
 : public ReturnByValue<MatrixSquareRootReturnValue<Derived> >
 {
   protected:
-    typedef typename Derived::Index Index;
     typedef typename internal::ref_selector<Derived>::type DerivedNested;
 
   public:
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialUtils.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialUtils.h
index 40ba65b7e..394e857ac 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialUtils.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialUtils.h
@@ -20,8 +20,8 @@ namespace Eigen {
  *  e.g. \f$ 1 + 3x^2 \f$ is stored as a vector \f$ [ 1, 0, 3 ] \f$.
  * \param[in] x : the value to evaluate the polynomial at.
  *
- * <i><b>Note for stability:</b></i>
- *  <dd> \f$ |x| \le 1 \f$ </dd>
+ * \note for stability:
+ *   \f$ |x| \le 1 \f$
  */
 template <typename Polynomials, typename T>
 inline
@@ -67,8 +67,8 @@ T poly_eval( const Polynomials& poly, const T& x )
  *  by degrees i.e. poly[i] is the coefficient of degree i of the polynomial
  *  e.g. \f$ 1 + 3x^2 \f$ is stored as a vector \f$ [ 1, 0, 3 ] \f$.
  *
- *  <i><b>Precondition:</b></i>
- *  <dd> the leading coefficient of the input polynomial poly must be non zero </dd>
+ *  \pre
+ *   the leading coefficient of the input polynomial poly must be non zero
  */
 template <typename Polynomial>
 inline
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/BlockSparseMatrix.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/BlockSparseMatrix.h
index 0e8350a7d..536a0c320 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/BlockSparseMatrix.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/BlockSparseMatrix.h
@@ -931,7 +931,7 @@ class BlockSparseMatrix : public SparseMatrixBase<BlockSparseMatrix<_Scalar,_Blo
     }
 
     /**
-      * \returns the starting position of the block <id> in the array of values
+      * \returns the starting position of the block \p id in the array of values
       */
     Index blockPtr(Index id) const
     {
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h
index 037a13f86..0ffbc43d2 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h
@@ -228,6 +228,9 @@ template<typename _Scalar, int _Options, typename _StorageIndex>
     EIGEN_DEPRECATED inline DynamicSparseMatrix()
       : m_innerSize(0), m_data(0)
     {
+      #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+        EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+      #endif
       eigen_assert(innerSize()==0 && outerSize()==0);
     }
 
@@ -235,6 +238,9 @@ template<typename _Scalar, int _Options, typename _StorageIndex>
     EIGEN_DEPRECATED inline DynamicSparseMatrix(Index rows, Index cols)
       : m_innerSize(0)
     {
+      #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+        EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+      #endif
       resize(rows, cols);
     }
 
@@ -243,12 +249,18 @@ template<typename _Scalar, int _Options, typename _StorageIndex>
     EIGEN_DEPRECATED explicit inline DynamicSparseMatrix(const SparseMatrixBase<OtherDerived>& other)
       : m_innerSize(0)
     {
-    Base::operator=(other.derived());
+      #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+        EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+      #endif
+      Base::operator=(other.derived());
     }
 
     inline DynamicSparseMatrix(const DynamicSparseMatrix& other)
       : Base(), m_innerSize(0)
     {
+      #ifdef EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+        EIGEN_SPARSE_CREATE_TEMPORARY_PLUGIN
+      #endif
       *this = other.derived();
     }
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h
index cdc14f86e..04b7d69ac 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h
@@ -17,8 +17,8 @@ namespace Eigen {
 
 namespace internal 
 {
-  template <typename Scalar>
-  inline bool GetMarketLine (std::stringstream& line, Index& M, Index& N, Index& i, Index& j, Scalar& value)
+  template <typename Scalar,typename IndexType>
+  inline bool GetMarketLine (std::stringstream& line, IndexType& M, IndexType& N, IndexType& i, IndexType& j, Scalar& value)
   {
     line >> i >> j >> value;
     i--;
@@ -30,8 +30,8 @@ namespace internal
     else
       return false;
   }
-  template <typename Scalar>
-  inline bool GetMarketLine (std::stringstream& line, Index& M, Index& N, Index& i, Index& j, std::complex<Scalar>& value)
+  template <typename Scalar,typename IndexType>
+  inline bool GetMarketLine (std::stringstream& line, IndexType& M, IndexType& N, IndexType& i, IndexType& j, std::complex<Scalar>& value)
   {
     Scalar valR, valI;
     line >> i >> j >> valR >> valI;
@@ -109,6 +109,7 @@ namespace internal
 inline bool getMarketHeader(const std::string& filename, int& sym, bool& iscomplex, bool& isvector)
 {
   sym = 0; 
+  iscomplex = false;
   isvector = false;
   std::ifstream in(filename.c_str(),std::ios::in);
   if(!in)
@@ -133,7 +134,7 @@ template<typename SparseMatrixType>
 bool loadMarket(SparseMatrixType& mat, const std::string& filename)
 {
   typedef typename SparseMatrixType::Scalar Scalar;
-  typedef typename SparseMatrixType::Index Index;
+  typedef typename SparseMatrixType::StorageIndex StorageIndex;
   std::ifstream input(filename.c_str(),std::ios::in);
   if(!input)
     return false;
@@ -143,11 +144,11 @@ bool loadMarket(SparseMatrixType& mat, const std::string& filename)
   
   bool readsizes = false;
 
-  typedef Triplet<Scalar,Index> T;
+  typedef Triplet<Scalar,StorageIndex> T;
   std::vector<T> elements;
   
-  Index M(-1), N(-1), NNZ(-1);
-  Index count = 0;
+  StorageIndex M(-1), N(-1), NNZ(-1);
+  StorageIndex count = 0;
   while(input.getline(buffer, maxBuffersize))
   {
     // skip comments   
@@ -170,7 +171,7 @@ bool loadMarket(SparseMatrixType& mat, const std::string& filename)
     }
     else
     { 
-      Index i(-1), j(-1);
+      StorageIndex i(-1), j(-1);
       Scalar value; 
       if( internal::GetMarketLine(line, M, N, i, j, value) ) 
       {
diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/Spline.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/Spline.h
index 627f6e482..57788c84a 100644
--- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/Spline.h
+++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/Spline.h
@@ -249,8 +249,6 @@ namespace Eigen
     DenseIndex degree,
     const typename Spline<_Scalar, _Dim, _Degree>::KnotVectorType& knots)
   {
-    typedef typename Spline<_Scalar, _Dim, _Degree>::BasisVectorType BasisVectorType;
-
     const DenseIndex p = degree;
     const DenseIndex i = Spline::Span(u, degree, knots);
 
@@ -380,9 +378,6 @@ namespace Eigen
     typedef Spline<_Scalar, _Dim, _Degree> SplineType;
     enum { Order = SplineTraits<SplineType>::OrderAtCompileTime };
 
-    typedef typename SplineTraits<SplineType>::Scalar Scalar;
-    typedef typename SplineTraits<SplineType>::BasisVectorType BasisVectorType;
-  
     const DenseIndex span = SplineType::Span(u, p, U);
 
     const DenseIndex n = (std::min)(p, order);
diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/examples/FFT.cpp b/gtsam/3rdparty/Eigen/unsupported/doc/examples/FFT.cpp
index fcbf81276..85e8a0241 100644
--- a/gtsam/3rdparty/Eigen/unsupported/doc/examples/FFT.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/doc/examples/FFT.cpp
@@ -61,14 +61,14 @@ template <typename T>
 void RandomFill(std::vector<T> & vec)
 {
     for (size_t k=0;k<vec.size();++k)
-        vec[k] = T( rand() )/T(RAND_MAX) - .5;
+        vec[k] = T( rand() )/T(RAND_MAX) - T(.5);
 }
 
 template <typename T>
 void RandomFill(std::vector<std::complex<T> > & vec)
 {
     for (size_t k=0;k<vec.size();++k)
-        vec[k] = std::complex<T> ( T( rand() )/T(RAND_MAX) - .5, T( rand() )/T(RAND_MAX) - .5);
+        vec[k] = std::complex<T> ( T( rand() )/T(RAND_MAX) - T(.5), T( rand() )/T(RAND_MAX) - T(.5));
 }
 
 template <typename T_time,typename T_freq>
@@ -85,7 +85,7 @@ void fwd_inv(size_t nfft)
     vector<T_time> timebuf2;
     fft.inv(timebuf2,freqbuf);
 
-    long double rmse = mag2(timebuf - timebuf2) / mag2(timebuf);
+    T_time rmse = mag2(timebuf - timebuf2) / mag2(timebuf);
     cout << "roundtrip rmse: " << rmse << endl;
 }
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt
index b5fa1c845..3a8775a1c 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/unsupported/test/CMakeLists.txt
@@ -30,11 +30,16 @@ else(GOOGLEHASH_FOUND)
   ei_add_property(EIGEN_MISSING_BACKENDS  "GoogleHash, ")
 endif(GOOGLEHASH_FOUND)
 
+
 find_package(Adolc)
 if(ADOLC_FOUND)
   include_directories(${ADOLC_INCLUDES})
   ei_add_property(EIGEN_TESTED_BACKENDS "Adolc, ")
-  ei_add_test(forward_adolc "" ${ADOLC_LIBRARIES})
+  if(EIGEN_TEST_CXX11)
+    ei_add_test(forward_adolc "" ${ADOLC_LIBRARIES})
+  else()
+    message(STATUS "Adolc found, but tests require C++11 mode")
+  endif()
 else(ADOLC_FOUND)
   ei_add_property(EIGEN_MISSING_BACKENDS "Adolc, ")
 endif(ADOLC_FOUND)
@@ -63,7 +68,7 @@ ei_add_test(EulerAngles)
 
 find_package(MPFR 2.3.0)
 find_package(GMP)
-if(MPFR_FOUND AND EIGEN_COMPILER_SUPPORT_CXX11)
+if(MPFR_FOUND AND EIGEN_COMPILER_SUPPORT_CPP11)
   include_directories(${MPFR_INCLUDES} ./mpreal)
   ei_add_property(EIGEN_TESTED_BACKENDS "MPFR C++, ")
   set(EIGEN_MPFR_TEST_LIBRARIES ${MPFR_LIBRARIES} ${GMP_LIBRARIES})
@@ -149,7 +154,8 @@ if(EIGEN_TEST_CXX11)
   endif(EIGEN_TEST_SYCL)
   # It should be safe to always run these tests as there is some fallback code for
   # older compiler that don't support cxx11.
-  set(CMAKE_CXX_STANDARD 11)
+  # This is already set if EIGEN_TEST_CXX11 is enabled:
+  # set(CMAKE_CXX_STANDARD 11)
 
   ei_add_test(cxx11_eventcount "-pthread" "${CMAKE_THREAD_LIBS_INIT}")
   ei_add_test(cxx11_runqueue "-pthread" "${CMAKE_THREAD_LIBS_INIT}")
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp b/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp
index 1d682dd83..f0c336c15 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/test/NonLinearOptimization.cpp
@@ -565,7 +565,7 @@ void testLmdif1()
 
   // do the computation
   lmdif_functor functor;
-  DenseIndex nfev;
+  DenseIndex nfev = -1; // initialize to avoid maybe-uninitialized warning
   info = LevenbergMarquardt<lmdif_functor>::lmdif1(functor, x, &nfev);
 
   // check return value
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp
index 85743137e..1c5e0dc66 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp
@@ -306,6 +306,8 @@ double bug_1222() {
   return denom.value();
 }
 
+#ifdef EIGEN_TEST_PART_5
+
 double bug_1223() {
   using std::min;
   typedef Eigen::AutoDiffScalar<Eigen::Vector3d> AD;
@@ -326,8 +328,8 @@ double bug_1223() {
 
 // regression test for some compilation issues with specializations of ScalarBinaryOpTraits
 void bug_1260() {
-  Matrix4d A;
-  Vector4d v;
+  Matrix4d A = Matrix4d::Ones();
+  Vector4d v = Vector4d::Ones();
   A*v;
 }
 
@@ -336,7 +338,7 @@ double bug_1261() {
   typedef AutoDiffScalar<Matrix2d> AD;
   typedef Matrix<AD,2,1> VectorAD;
 
-  VectorAD v;
+  VectorAD v(0.,0.);
   const AD maxVal = v.maxCoeff();
   const AD minVal = v.minCoeff();
   return maxVal.value() + minVal.value();
@@ -344,12 +346,14 @@ double bug_1261() {
 
 double bug_1264() {
   typedef AutoDiffScalar<Vector2d> AD;
-  const AD s;
-  const Matrix<AD, 3, 1> v1;
+  const AD s = 0.;
+  const Matrix<AD, 3, 1> v1(0.,0.,0.);
   const Matrix<AD, 3, 1> v2 = (s + 3.0) * v1;
   return v2(0).value();
 }
 
+#endif
+
 void test_autodiff()
 {
   for(int i = 0; i < g_repeat; i++) {
@@ -359,9 +363,9 @@ void test_autodiff()
     CALL_SUBTEST_4( test_autodiff_hessian<1>() );
   }
 
-  bug_1222();
-  bug_1223();
-  bug_1260();
-  bug_1261();
+  CALL_SUBTEST_5( bug_1222() );
+  CALL_SUBTEST_5( bug_1223() );
+  CALL_SUBTEST_5( bug_1260() );
+  CALL_SUBTEST_5( bug_1261() );
 }
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/autodiff_scalar.cpp b/gtsam/3rdparty/Eigen/unsupported/test/autodiff_scalar.cpp
index 9cf11280c..a917ec344 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/autodiff_scalar.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/test/autodiff_scalar.cpp
@@ -81,6 +81,9 @@ void check_limits_specialization()
   typedef std::numeric_limits<AD> A;
   typedef std::numeric_limits<Scalar> B;
 
+  // workaround "unsed typedef" warning:
+  VERIFY(!bool(internal::is_same<B, A>::value));
+
 #if EIGEN_HAS_CXX11
   VERIFY(bool(std::is_base_of<B, A>::value));
 #endif
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_argmax_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_argmax_cuda.cu
index 653443dc5..3d73d491a 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_argmax_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_argmax_cuda.cu
@@ -12,9 +12,6 @@
 #define EIGEN_TEST_FUNC cxx11_tensor_cuda
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cast_float16_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cast_float16_cuda.cu
index 88c233994..816e03220 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cast_float16_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cast_float16_cuda.cu
@@ -13,9 +13,6 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE int
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cuda.cu
index d4e111f5d..916f12a84 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cuda.cu
@@ -11,9 +11,6 @@
 #define EIGEN_TEST_FUNC cxx11_tensor_complex
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cwise_ops_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cwise_ops_cuda.cu
index 2baf5eaad..aac780905 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cwise_ops_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_complex_cwise_ops_cuda.cu
@@ -11,9 +11,6 @@
 #define EIGEN_TEST_FUNC cxx11_tensor_complex_cwise_ops
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_contract_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_contract_cuda.cu
index dd68430ce..e821ccf0c 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_contract_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_contract_cuda.cu
@@ -14,9 +14,6 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE int
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cuda.cu
index 0ba9d52e9..9584a539f 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_cuda.cu
@@ -12,9 +12,6 @@
 #define EIGEN_TEST_FUNC cxx11_tensor_cuda
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_device.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_device.cu
index fde20ddf2..cbb43e210 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_device.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_device.cu
@@ -13,9 +13,6 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE int
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_of_float16_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_of_float16_cuda.cu
index 2f86980a2..e296bf991 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_of_float16_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_of_float16_cuda.cu
@@ -13,9 +13,6 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE int
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_random_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_random_cuda.cu
index b3be199e1..fa1a46732 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_random_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_random_cuda.cu
@@ -13,9 +13,6 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE int
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_reduction_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_reduction_cuda.cu
index 6858b43a7..ec0669704 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_reduction_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_reduction_cuda.cu
@@ -12,9 +12,6 @@
 #define EIGEN_TEST_FUNC cxx11_tensor_reduction_cuda
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_scan_cuda.cu b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_scan_cuda.cu
index 5f146f3c9..de1c0ac95 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_scan_cuda.cu
+++ b/gtsam/3rdparty/Eigen/unsupported/test/cxx11_tensor_scan_cuda.cu
@@ -13,9 +13,6 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE int
 #define EIGEN_USE_GPU
 
-#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
-#include <cuda_fp16.h>
-#endif
 #include "main.h"
 #include <unsupported/Eigen/CXX11/Tensor>
 
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp b/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp
index 7c9b68a3c..6a2b2194a 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/test/matrix_function.cpp
@@ -25,7 +25,6 @@ inline bool test_isApprox_abs(const Type1& a, const Type2& b)
 template<typename MatrixType>
 MatrixType randomMatrixWithRealEivals(const typename MatrixType::Index size)
 {
-  typedef typename MatrixType::Index Index;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
   MatrixType diag = MatrixType::Zero(size, size);
@@ -51,7 +50,6 @@ struct randomMatrixWithImagEivals<MatrixType, 0>
 {
   static MatrixType run(const typename MatrixType::Index size)
   {
-    typedef typename MatrixType::Index Index;
     typedef typename MatrixType::Scalar Scalar;
     MatrixType diag = MatrixType::Zero(size, size);
     Index i = 0;
@@ -79,7 +77,6 @@ struct randomMatrixWithImagEivals<MatrixType, 1>
 {
   static MatrixType run(const typename MatrixType::Index size)
   {
-    typedef typename MatrixType::Index Index;
     typedef typename MatrixType::Scalar Scalar;
     typedef typename MatrixType::RealScalar RealScalar;
     const Scalar imagUnit(0, 1);
@@ -171,7 +168,6 @@ void testMatrixType(const MatrixType& m)
 {
   // Matrices with clustered eigenvalue lead to different code paths
   // in MatrixFunction.h and are thus useful for testing.
-  typedef typename MatrixType::Index Index;
 
   const Index size = m.rows();
   for (int i = 0; i < g_repeat; i++) {
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/openglsupport.cpp b/gtsam/3rdparty/Eigen/unsupported/test/openglsupport.cpp
index 706a816f7..5f6343427 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/openglsupport.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/test/openglsupport.cpp
@@ -318,10 +318,6 @@ void test_openglsupport()
         
       GLint prg_id = createShader(vtx,frg);
       
-      typedef Vector2d Vector2d;
-      typedef Vector3d Vector3d;
-      typedef Vector4d Vector4d;
-      
       VERIFY_UNIFORM(dv,v2d, Vector2d);
       VERIFY_UNIFORM(dv,v3d, Vector3d);
       VERIFY_UNIFORM(dv,v4d, Vector4d);
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp
index 0c87478dd..4cfc46b41 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/test/polynomialsolver.cpp
@@ -30,7 +30,6 @@ struct increment_if_fixed_size
 template<int Deg, typename POLYNOMIAL, typename SOLVER>
 bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve )
 {
-  typedef typename POLYNOMIAL::Index Index;
   typedef typename POLYNOMIAL::Scalar Scalar;
 
   typedef typename SOLVER::RootsType    RootsType;
@@ -107,7 +106,6 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const
     // 1) the roots found are correct
     // 2) the roots have distinct moduli
 
-    typedef typename POLYNOMIAL::Scalar                 Scalar;
     typedef typename REAL_ROOTS::Scalar                 Real;
 
     //Test realRoots
diff --git a/gtsam/3rdparty/Eigen/unsupported/test/sparse_extra.cpp b/gtsam/3rdparty/Eigen/unsupported/test/sparse_extra.cpp
index a010ceb93..7a049c870 100644
--- a/gtsam/3rdparty/Eigen/unsupported/test/sparse_extra.cpp
+++ b/gtsam/3rdparty/Eigen/unsupported/test/sparse_extra.cpp
@@ -8,10 +8,10 @@
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 
-// import basic and product tests for deprectaed DynamicSparseMatrix
+// import basic and product tests for deprecated DynamicSparseMatrix
 #define EIGEN_NO_DEPRECATED_WARNING
-#include "sparse_basic.cpp"
 #include "sparse_product.cpp"
+#include "sparse_basic.cpp"
 #include <Eigen/SparseExtra>
 
 template<typename SetterType,typename DenseType, typename Scalar, int Options>

From 0295f1e2406a825752e14af144e6765d05f12039 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Tue, 11 Dec 2018 21:00:43 -0800
Subject: [PATCH 065/281] Fix testScenario in quaternion mode

---
 gtsam/navigation/tests/testScenario.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp
index f814f1710..9c080929d 100644
--- a/gtsam/navigation/tests/testScenario.cpp
+++ b/gtsam/navigation/tests/testScenario.cpp
@@ -96,7 +96,11 @@ TEST(Scenario, Loop) {
   const double R = v / w;
   const Pose3 T30 = scenario.pose(30);
   EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
+#ifdef GTSAM_USE_QUATERNIONS
+  EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz()));
+#else
   EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz()));
+#endif
   EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
 }
 

From 24712f674e25e7f2e82bc81c9921be2dc1015f7b Mon Sep 17 00:00:00 2001
From: chrisbeall <chrisbeall@gmail.com>
Date: Wed, 12 Dec 2018 09:12:17 -0800
Subject: [PATCH 066/281] Upgrade to Eigen 3.3.7

---
 .../Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h   | 4 ++--
 gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h             | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
index 6be1b49c6..e3980f6ff 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
@@ -1199,8 +1199,8 @@ void gebp_kernel<LhsScalar,RhsScalar,Index,DataMapper,mr,nr,ConjugateLhs,Conjuga
 
           // NOTE: the begin/end asm comments below work around bug 935!
           // but they are not enough for gcc>=6 without FMA (bug 1637)
-          #if EIGEN_GNUC_AT_LEAST(6,0)
-            #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND __asm__  ("" : [a0] "+rm" (A0),[a1] "+rm" (A1));
+          #if EIGEN_GNUC_AT_LEAST(6,0) && defined(EIGEN_VECTORIZE_SSE)
+            #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND __asm__  ("" : [a0] "+x,m" (A0),[a1] "+x,m" (A1));
           #else
             #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND
           #endif
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
index eef845c5f..aa054a0b7 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
@@ -13,7 +13,7 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 3
-#define EIGEN_MINOR_VERSION 6
+#define EIGEN_MINOR_VERSION 7
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \

From e550f4f2aec423cb3f2791b81cb5858b8826ebac Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Tue, 11 Dec 2018 12:04:34 -0800
Subject: [PATCH 067/281] Delete unused DerivedValue.h

---
 gtsam/base/DerivedValue.h    | 143 -----------------------------------
 gtsam/nonlinear/Values-inl.h |   1 -
 gtsam/precompiled_header.h   |   1 -
 3 files changed, 145 deletions(-)
 delete mode 100644 gtsam/base/DerivedValue.h

diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h
deleted file mode 100644
index f01156bd6..000000000
--- a/gtsam/base/DerivedValue.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/*
- * @file DerivedValue.h
- * @date Jan 26, 2012
- * @author Duy Nguyen Ta
- */
-
-#pragma once
-
-#include <gtsam/base/Value.h>
-#include <boost/make_shared.hpp>
-
-//////////////////
-// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
-#include <boost/pool/singleton_pool.hpp>
-
-#ifdef min
-#undef min
-#endif
-
-#ifdef max
-#undef max
-#endif
-
-#ifdef ERROR
-#undef ERROR
-#endif
-//////////////////
-
-
-namespace gtsam {
-
-template<class DERIVED>
-class DerivedValue : public Value {
-
-protected:
-  DerivedValue() {}
-
-public:
-
-  virtual ~DerivedValue() {}
-
-  /**
-   * Create a duplicate object returned as a pointer to the generic Value interface.
-   * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
-   * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
-   */
-  virtual Value* clone_() const {
-    void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
-    DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
-    return ptr;
-  }
-
-  /**
-   * Destroy and deallocate this object, only if it was originally allocated using clone_().
-   */
-  virtual void deallocate_() const {
-    this->~DerivedValue(); // Virtual destructor cleans up the derived object
-    boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this); // Release memory from pool
-  }
-
-  /**
-   * Clone this value (normal clone on the heap, delete with 'delete' operator)
-   */
-  virtual boost::shared_ptr<Value> clone() const {
-    return boost::make_shared<DERIVED>(static_cast<const DERIVED&>(*this));
-  }
-
-  /// equals implementing generic Value interface
-  virtual bool equals_(const Value& p, double tol = 1e-9) const {
-    // Cast the base class Value pointer to a derived class pointer
-    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);
-
-    // Return the result of calling equals on the derived class
-    return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
-  }
-
-  /// Generic Value interface version of retract
-  virtual Value* retract_(const Vector& delta) const {
-    // Call retract on the derived class
-    const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
-
-    // Create a Value pointer copy of the result
-    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
-    Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
-
-    // Return the pointer to the Value base class
-    return resultAsValue;
-  }
-
-  /// Generic Value interface version of localCoordinates
-  virtual Vector localCoordinates_(const Value& value2) const {
-    // Cast the base class Value pointer to a derived class pointer
-    const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(value2);
-
-    // Return the result of calling localCoordinates on the derived class
-    return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
-  }
-
-  /// Assignment operator
-  virtual Value& operator=(const Value& rhs) {
-    // Cast the base class Value pointer to a derived class pointer
-    const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);
-
-    // Do the assignment and return the result
-    return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
-  }
-
-  /// Conversion to the derived class
-  operator const DERIVED& () const {
-    return static_cast<const DERIVED&>(*this);
-  }
-
-  /// Conversion to the derived class
-  operator DERIVED& () {
-    return static_cast<DERIVED&>(*this);
-  }
-
-protected:
-  /// Assignment operator, protected because only the Value or DERIVED
-  /// assignment operators should be used.
-  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& /*rhs*/) {
-    // Nothing to do, do not call base class assignment operator
-    return *this;
-  }
-
-private:
-  /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
-  struct PoolTag { };
-
-};
-
-} /* namespace gtsam */
diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h
index 40979b86c..7e14578c3 100644
--- a/gtsam/nonlinear/Values-inl.h
+++ b/gtsam/nonlinear/Values-inl.h
@@ -26,7 +26,6 @@
 
 #include <utility>
 
-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
 
 namespace gtsam {
diff --git a/gtsam/precompiled_header.h b/gtsam/precompiled_header.h
index 37ab01cf2..5ff2a55c5 100644
--- a/gtsam/precompiled_header.h
+++ b/gtsam/precompiled_header.h
@@ -28,7 +28,6 @@
 #include <gtsam/base/concepts.h>
 #include <gtsam/base/ConcurrentMap.h>
 #include <gtsam/base/debug.h>
-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/DSFVector.h>
 #include <gtsam/base/FastDefaultAllocator.h>
 #include <gtsam/base/FastList.h>

From 28a6c39b85ec077e2f92d3adff5e1ad0fa751f27 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Thu, 13 Dec 2018 14:03:52 -0800
Subject: [PATCH 068/281] Clean up documentation in Value.h

---
 gtsam/base/Value.h | 59 +++++-----------------------------------------
 1 file changed, 6 insertions(+), 53 deletions(-)

diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index 2d5b9d879..1cccf0319 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -11,7 +11,7 @@
 
 /**
  * @file Value.h
- * @brief The interface class for any variable that can be optimized or used in a factor.
+ * @brief The base class for any variable that can be optimized or used in a factor.
  * @author Richard Roberts
  * @date Jan 14, 2012
  */
@@ -27,58 +27,11 @@
 namespace gtsam {
 
   /**
-   * This is the interface class for any value that may be used as a variable
-   * assignment in a factor graph, and which you must derive to create new
-   * variable types to use with gtsam.  Examples of built-in classes
-   * implementing this are mainly in geometry, including Rot3, Pose2, etc.
-   *
-   * This interface specifies pure virtual retract_(), localCoordinates_() and
-   * equals_() functions that work with pointers and references to this interface
-   * class, i.e. the base class.  These functions allow containers, such as
-   * Values can operate generically on Value objects, retracting or computing
-   * local coordinates for many Value objects of different types.
-   *
-   * Inheriting from the DerivedValue class template provides a generic implementation of
-   * the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating
-   * the need to implement these functions in your class. Note that you must inherit from
-   * DerivedValue templated on the class you are defining. For example you cannot define
-   * the following
-   * \code
-   * class Rot3 : public DerivedValue<Point3>{ \\classdef }
-   * \endcode
-   *
-   * Using the above practice, here is an example of implementing a typical
-   * class derived from Value:
-   * \code
-     class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
-     public:
-       // Constructor, there is never a need to call the Value base class constructor.
-       Rot3() { ... }
-
-       // Print for unit tests and debugging (virtual, implements Value::print())
-       virtual void print(const std::string& str = "") const;
-
-       // Equals working directly with Rot3 objects (non-virtual, non-overriding!)
-       bool equals(const Rot3& other, double tol = 1e-9) const;
-
-       // Tangent space dimensionality (virtual, implements Value::dim())
-       virtual size_t dim() const {
-         return 3;
-       }
-
-       // retract working directly with Rot3 objects (non-virtual, non-overriding!)
-       Rot3 retract(const Vector& delta) const {
-         // Math to implement a 3D rotation retraction e.g. exponential map
-         return Rot3(result);
-       }
-
-       // localCoordinates working directly with Rot3 objects (non-virtual, non-overriding!)
-       Vector localCoordinates(const Rot3& r2) const {
-         // Math to implement 3D rotation localCoordinates, e.g. logarithm map
-         return Vector(result);
-       }
-     };
-     \endcode
+   * This is the base class for any type to be stored in Values.
+   * Note: As of GTSAM 4.0, Value types should no longer derive from Value or
+   * DerivedValue. Use type traits instead.
+   * See https://bitbucket.org/gtborg/gtsam/wiki/Migrating%20from%20GTSAM%203.X%20to%20GTSAM%204.0#markdown-header-custom-value-types
+   * for current usage and migration details.
    */
   class GTSAM_EXPORT Value {
   public:

From ff05eee0ca8062221c88ab01b42154ff2a89ba86 Mon Sep 17 00:00:00 2001
From: Nghia Ho <nghia@prenav.com>
Date: Thu, 13 Dec 2018 17:03:31 -0800
Subject: [PATCH 069/281] fix for valgrind invalid read warning

---
 gtsam/nonlinear/ISAM2Clique.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp
index c55ca7959..48295f338 100644
--- a/gtsam/nonlinear/ISAM2Clique.cpp
+++ b/gtsam/nonlinear/ISAM2Clique.cpp
@@ -172,7 +172,7 @@ bool ISAM2Clique::valuesChanged(const KeySet& replaced,
                                 double threshold) const {
   auto frontals = conditional_->frontals();
   if (replaced.exists(frontals.front())) return true;
-  auto diff = originalValues - delta.vector(frontals);
+  Vector diff = originalValues - delta.vector(frontals);
   return diff.lpNorm<Eigen::Infinity>() >= threshold;
 }
 

From 224af650bb337f40ac5e7d3a998353dc30991640 Mon Sep 17 00:00:00 2001
From: Nghia Ho <nghia@prenav.com>
Date: Thu, 13 Dec 2018 17:28:52 -0800
Subject: [PATCH 070/281] add test code

---
 examples/Data/ISAM2_SmartFactor_valgrind.txt | 812 +++++++++++++++++++
 examples/ISAM2_SmartFactor_valgrind.cpp      | 222 +++++
 2 files changed, 1034 insertions(+)
 create mode 100644 examples/Data/ISAM2_SmartFactor_valgrind.txt
 create mode 100644 examples/ISAM2_SmartFactor_valgrind.cpp

diff --git a/examples/Data/ISAM2_SmartFactor_valgrind.txt b/examples/Data/ISAM2_SmartFactor_valgrind.txt
new file mode 100644
index 000000000..feee7bc12
--- /dev/null
+++ b/examples/Data/ISAM2_SmartFactor_valgrind.txt
@@ -0,0 +1,812 @@
+i 1 0.485693 0.334961 10.0536 0.00585324 0.00159634 0.00532113
+i 1 0.449805 0.346924 10.0536 0.00744958 0.00159634 0.00478901
+i 1 0.430664 0.351709 10.0249 0.00904591 0.00159634 0.0042569
+i 1 0.442627 0.346924 9.99858 0.0106423 0.00159634 0.00478901
+i 1 0.449805 0.344531 9.99619 0.0117065 0.00159634 0.00478901
+i 1 0.44502 0.330176 10.0034 0.0127707 0.00159634 0.00478901
+i 1 0.411523 0.325391 9.99141 0.0138349 0.00159634 0.0042569
+i 1 0.411523 0.322998 9.98184 0.014367 0.00106423 0.00372479
+i 1 0.413916 0.325391 9.97944 0.0148992 0.000532113 0.00266056
+i 1 0.413916 0.332568 9.9938 0.0159634 0 0.00212845
+i 1 0.385205 0.327783 9.97944 0.0175597 0.000532113 0.00159634
+i 1 0.358887 0.332568 9.93398 0.0186239 0.00106423 0.00106423
+i 1 0.368457 0.327783 9.8981 0.0196882 0.00212845 0.00159634
+i 1 0.37085 0.342139 9.88613 0.0212845 0.00266056 0.00212845
+i 1 0.361279 0.346924 9.88374 0.0223487 0.00319268 0.00266056
+i 1 0.337354 0.356494 9.86221 0.023413 0.00266056 0.00266056
+i 1 0.330176 0.37085 9.82632 0.0244772 0.00212845 0.00266056
+i 1 0.349316 0.38042 9.80957 0.0250093 0.00159634 0.00266056
+i 1 0.361279 0.392383 9.81675 0.0255414 0.00159634 0.00266056
+i 1 0.344531 0.399561 9.80239 0.0266056 0.00159634 0.00266056
+i 1 0.339746 0.394775 9.78086 0.0276699 0.00212845 0.00212845
+i 1 0.330176 0.387598 9.74736 0.028202 0.00266056 0.00212845
+i 1 0.332568 0.382812 9.74736 0.028202 0.00319268 0.00212845
+i 1 0.313428 0.373242 9.75933 0.028202 0.00319268 0.00212845
+i 1 0.282324 0.361279 9.74497 0.0276699 0.00319268 0.00266056
+i 1 0.284717 0.363672 9.74497 0.028202 0.00319268 0.00319268
+i 1 0.303857 0.358887 9.74497 0.0287341 0.00266056 0.00372479
+i 1 0.327783 0.363672 9.77129 0.0303304 0.00319268 0.00478901
+i 2 0.313428 0.368457 9.77129 0.0313946 0.00372479 0.00532113
+i 2 0.313428 0.37085 9.74019 0.0324589 0.00478901 0.00585324
+i 2 0.322998 0.368457 9.71387 0.0335231 0.00532113 0.00691746
+i 2 0.344531 0.382812 9.70669 0.032991 0.00585324 0.00744958
+i 2 0.342139 0.38999 9.72344 0.032991 0.00585324 0.0085138
+i 2 0.322998 0.399561 9.71147 0.0324589 0.00532113 0.0085138
+i 2 0.322998 0.409131 9.69233 0.0324589 0.00478901 0.0085138
+i 2 0.349316 0.409131 9.69233 0.0324589 0.0042569 0.0085138
+i 2 0.358887 0.416309 9.71865 0.0324589 0.00372479 0.0085138
+i 2 0.361279 0.418701 9.73779 0.032991 0.00372479 0.0085138
+i 2 0.361279 0.430664 9.73779 0.0335231 0.0042569 0.00904591
+i 2 0.382812 0.437842 9.72822 0.0340552 0.00478901 0.00904591
+i 2 0.411523 0.452197 9.73301 0.0340552 0.00532113 0.00957803
+i 2 0.416309 0.449805 9.74258 0.0340552 0.00585324 0.00957803
+i 2 0.399561 0.45459 9.72583 0.0340552 0.00638535 0.00957803
+i 2 0.406738 0.452197 9.7043 0.0335231 0.00691746 0.00904591
+i 2 0.433057 0.45459 9.69233 0.0335231 0.00744958 0.0085138
+i 2 0.456982 0.452197 9.7019 0.032991 0.00744958 0.0085138
+i 2 0.461768 0.449805 9.69473 0.032991 0.00798169 0.00798169
+i 2 0.452197 0.437842 9.65884 0.032991 0.00798169 0.00744958
+i 2 0.466553 0.430664 9.62534 0.0335231 0.0085138 0.00691746
+i 2 0.483301 0.428271 9.62534 0.032991 0.0085138 0.00585324
+i 2 0.480908 0.433057 9.64448 0.032991 0.0085138 0.00532113
+i 2 0.46416 0.437842 9.64688 0.032991 0.0085138 0.00478901
+i 2 0.45459 0.449805 9.6397 0.032991 0.0085138 0.00478901
+i 2 0.478516 0.461768 9.6397 0.032991 0.0085138 0.00478901
+i 2 0.483301 0.471338 9.65405 0.032991 0.00904591 0.0042569
+s 2 1 885 772.028 190.341
+s 2 5 467 386.865 243.455
+s 2 8 814 710.317 241.333
+s 2 9 820 715.802 246.233
+s 2 10 815 711.209 245.21
+s 2 11 883 770.583 198.045
+s 2 12 877 764.741 195.175
+s 2 14 877 764.741 195.175
+s 2 17 941 820.999 242.057
+s 2 19 948 827.372 235.193
+s 2 20 948 827.445 241.199
+s 2 23 557 473.955 298.317
+s 2 28 760 663.371 287.116
+s 2 33 813 709.517 280.212
+s 2 34 818 713.39 277.34
+s 2 36 815 711.652 286.066
+s 2 37 893 779.04 309.272
+s 2 39 899 784.538 308.196
+s 2 40 908 793.502 319.35
+s 2 41 902 787.564 319.381
+s 2 42 938 819.201 257.197
+s 2 43 989 863.847 282.826
+s 2 44 972 848.495 314.307
+s 2 46 975 852.26 260.734
+s 2 51 232 144.289 321.996
+s 2 53 598 513.866 338.268
+s 2 54 693 603.198 367.966
+s 2 58 815 710.993 377.494
+s 2 59 799 697.398 356.37
+s 2 63 893 779.706 351.987
+s 2 67 900 786.329 357.122
+s 2 68 898 784.262 353.125
+s 2 70 916 800.114 348.207
+s 2 71 986 861.517 326.212
+s 2 72 986 861.517 326.212
+s 2 73 976 854.046 367.861
+s 2 79 565 493.761 419.421
+s 2 80 560 488.614 422.453
+s 2 81 575 501.453 445.367
+s 2 83 602 525.726 413.384
+s 2 86 701 611.86 434.26
+s 2 87 701 611.86 434.26
+s 2 88 653 570.815 405.361
+s 2 91 747 652.271 432.316
+s 2 92 754 658.581 441.421
+s 2 93 708 618.271 430.162
+s 2 94 744 649.626 434.257
+s 2 95 823 718.362 385.033
+s 2 96 797 696.56 426.269
+s 2 97 801 700.128 396.737
+s 2 98 773 675.088 390.936
+s 2 101 885 773.221 414.264
+s 2 102 880 768.822 387.314
+s 2 103 888 776.039 410.211
+s 2 105 950 831.12 397.844
+s 2 106 907 792.351 408.117
+s 2 116 405 331.508 456.012
+s 2 118 402 328.617 500.451
+s 2 119 400 326.365 502.275
+s 2 121 476 406.683 478.349
+s 2 127 585 511.319 485.324
+s 2 128 617 538.861 453.39
+s 2 130 582 508.155 452.366
+s 2 131 643 561.073 503.265
+s 2 132 703 613.588 462.253
+s 2 134 766 669.159 502.063
+s 2 136 772 674.493 495.248
+s 2 137 781 682.123 471.232
+s 2 138 784 685.118 468.173
+s 2 139 781 682.191 474.374
+s 2 141 885 773.615 494.269
+s 2 145 888 776.431 492.348
+s 2 147 904 790.471 502.254
+s 2 148 902 788.507 504.272
+s 2 149 938 820.465 502.391
+s 2 152 963 842.721 503.376
+s 2 156 242 148.008 545.764
+s 2 158 248 154.208 545.888
+s 2 162 314 224.505 546.204
+s 2 163 315 225.905 530.765
+s 2 164 315 225.261 552.166
+s 2 166 368 291.55 564.057
+s 2 171 571 498.553 561.333
+s 2 172 572 499.545 563.335
+s 2 173 638 556.502 563.852
+s 2 175 636 554.844 552.416
+s 2 176 641 559.993 556.383
+s 2 178 692 604.702 533.137
+s 2 180 722 630.968 528.786
+s 2 181 716 624.803 518.298
+s 2 182 764 666.755 536.361
+s 2 185 769 671.642 537.317
+s 2 187 810 708.021 538.302
+s 2 189 841 735.447 558.082
+s 2 190 881 770.988 555.105
+s 2 192 860 754.238 555.186
+s 2 193 938 820.657 574.383
+s 2 194 948 829.586 514.034
+s 2 195 915 800.701 574.39
+s 2 196 904 790.859 538.891
+s 2 197 976 854.831 525.328
+s 2 204 321 230.814 584.287
+s 2 205 322 231.43 590.824
+s 2 210 507 413.694 594.058
+s 2 216 586 512.591 601.786
+s 2 217 585 511.221 598.227
+s 2 218 657 574.472 587.268
+s 2 219 675 589.664 581.33
+s 2 220 656 573.502 582.378
+s 2 221 651 568.578 581.394
+s 2 223 715 624.955 635.236
+s 2 224 828 724.063 600.264
+s 2 225 813 710.869 596.413
+s 2 227 882 771.755 624.292
+s 2 231 977 855.606 577.181
+s 2 233 252 132.188 646.925
+s 2 234 253 133.288 649.949
+s 2 236 264 144.633 647.915
+s 2 237 335 226.123 646.194
+s 2 240 448 356.089 665.294
+s 2 241 465 362.168 676.569
+s 2 242 481 383.624 673.992
+s 2 244 478 380.89 673.024
+s 2 245 562 458.491 663.422
+s 2 246 565 461.494 669.205
+s 2 247 548 446.265 666.974
+s 2 249 607 530.299 640.373
+s 2 250 613 512.347 667.362
+s 2 252 687 591.589 664.392
+s 2 253 698 601.698 663.19
+s 2 254 700 603.327 667.232
+s 2 255 656 563.305 657.902
+s 2 256 694 598.036 667.268
+s 2 257 714 618.145 662.355
+s 2 258 708 618.512 645.366
+s 2 259 726 634.945 643.204
+s 2 260 713 616.887 667.415
+s 2 261 730 638.995 646.386
+s 2 264 776 678.805 643.356
+s 2 265 769 672.092 642.262
+s 2 267 895 781.553 683.16
+s 2 268 895 781.553 683.16
+s 2 269 891 777.506 679.182
+s 2 271 888 774.288 683.233
+s 2 272 898 784.251 690.38
+s 2 273 956 836.692 655.37
+s 2 274 947 828.873 673.098
+s 2 275 359 196.329 730.773
+s 2 276 359 196.329 730.773
+s 2 277 444 291.606 761.777
+s 2 278 463 357.487 706.022
+s 2 282 617 476.167 725.153
+s 2 285 664 515.317 729.1
+s 2 286 661 513.039 724.11
+s 2 288 659 511.731 743.188
+s 2 289 744 604.533 735.338
+s 2 290 740 601.769 711.221
+s 2 291 742 601.937 737.397
+s 2 292 740 600.916 732.141
+s 2 295 775 641.709 727.255
+s 2 301 313 146.314 779.858
+s 2 302 291 111.606 782.816
+s 2 304 376 193.757 790.556
+s 2 305 373 191.601 783.684
+s 2 306 385 200.302 789.599
+s 2 307 406 218.92 809.926
+s 2 310 455 295.709 773.864
+s 2 311 472 310.994 772.922
+s 2 312 544 356.24 799.68
+s 2 316 758 564.263 809.259
+s 2 321 801 633.92 774.402
+s 2 323 446 219.679 857.011
+s 2 328 668 430.1 872.24
+s 2 329 750 498.621 887.234
+s 2 330 765 511.029 890.334
+s 2 331 745 494.602 886.002
+s 2 333 862 641.477 846.36
+s 2 335 872 644.41 859.881
+s 2 339 404 147.16 898.548
+s 2 345 589 323.535 907.722
+i 3 0.471338 0.478516 9.65645 0.032991 0.0101101 0.0042569
+i 3 0.471338 0.490479 9.62773 0.0324589 0.0111744 0.00319268
+i 3 0.480908 0.492871 9.58706 0.0319268 0.0117065 0.00266056
+i 3 0.502441 0.504834 9.56553 0.0313946 0.0117065 0.00212845
+i 3 0.502441 0.502441 9.55596 0.0313946 0.0122386 0.00266056
+i 3 0.46416 0.507227 9.53203 0.0313946 0.0122386 0.00319268
+i 3 0.456982 0.509619 9.50332 0.0319268 0.0122386 0.00372479
+i 3 0.45459 0.516797 9.49136 0.0313946 0.0127707 0.00372479
+i 3 0.466553 0.526367 9.5105 0.0308625 0.0133028 0.00372479
+i 3 0.452197 0.52876 9.52485 0.0303304 0.014367 0.00319268
+i 3 0.425879 0.535937 9.50571 0.0292662 0.0154313 0.00266056
+i 3 0.428271 0.533545 9.50571 0.0292662 0.0164955 0.00212845
+i 3 0.425879 0.545508 9.51528 0.0287341 0.0170276 0.00159634
+i 3 0.411523 0.5479 9.54399 0.028202 0.0170276 0.00159634
+i 3 0.382812 0.559863 9.56074 0.0276699 0.0170276 0.00106423
+i 3 0.375635 0.571826 9.54878 0.0276699 0.0159634 0.000532113
+i 3 0.387598 0.581396 9.54878 0.0266056 0.0148992 0.000532113
+i 3 0.401953 0.586182 9.55835 0.0250093 0.014367 0.000532113
+i 3 0.382812 0.590967 9.56074 0.0239451 0.0138349 0.000532113
+i 3 0.363672 0.593359 9.52725 0.0228808 0.0138349 0.000532113
+i 3 0.366064 0.598145 9.50571 0.0223487 0.0138349 0.000532113
+i 3 0.37085 0.617285 9.5105 0.0223487 0.0138349 0
+i 3 0.356494 0.624463 9.53921 0.0228808 0.014367 0
+i 3 0.327783 0.634033 9.55596 0.0223487 0.0148992 0
+i 3 0.308643 0.629248 9.56074 0.0218166 0.0159634 0
+i 3 0.320605 0.638818 9.57031 0.0212845 0.0164955 0
+i 3 0.322998 0.641211 9.59185 0.0202203 0.0170276 0
+s 3 1 885 771.886 190.394
+s 3 8 814 710.222 241.36
+s 3 10 815 711.117 244.276
+s 3 11 883 770.656 198.02
+s 3 13 874 761.801 193.21
+s 3 15 882 769.349 194.094
+s 3 16 952 831.218 238.215
+s 3 17 940 820.031 242.02
+s 3 19 948 827.354 235.194
+s 3 20 948 827.391 241.213
+s 3 28 760 663.312 287.142
+s 3 33 813 709.44 279.254
+s 3 34 819 714.246 276.35
+s 3 36 815 711.636 286.046
+s 3 37 893 779.028 309.293
+s 3 39 898 783.513 307.182
+s 3 40 908 793.523 318.373
+s 3 42 938 819.064 256.113
+s 3 43 989 863.966 281.804
+s 3 45 967 843.818 310.173
+s 3 46 975 852.424 259.949
+s 3 51 231 143.198 321.116
+s 3 52 229 140.636 323.223
+s 3 53 598 513.854 337.163
+s 3 58 815 711.008 377.495
+s 3 59 798 696.404 357.311
+s 3 61 798 696.404 357.311
+s 3 63 893 779.621 352
+s 3 67 900 786.305 357.088
+s 3 68 898 784.227 353.135
+s 3 70 917 801.17 347.212
+s 3 71 986 861.382 325.185
+s 3 72 985 860.395 327.196
+s 3 79 564 492.659 419.338
+s 3 80 560 488.554 421.427
+s 3 81 575 501.51 445.356
+s 3 83 603 526.725 413.46
+s 3 84 586 511.864 447.453
+s 3 85 650 567.813 402.398
+s 3 86 700 611.102 433.159
+s 3 87 703 613.898 434.235
+s 3 88 652 569.851 405.336
+s 3 90 706 616.823 433.228
+s 3 91 747 652.473 432.3
+s 3 92 754 658.648 441.454
+s 3 93 708 618.268 429.213
+s 3 94 743 648.861 434.243
+s 3 95 823 718.34 385.05
+s 3 96 796 695.581 425.304
+s 3 97 800 699.235 396.68
+s 3 98 773 675.006 390.852
+s 3 101 884 772.387 414.266
+s 3 102 879 768.099 387.4
+s 3 103 888 775.703 409.171
+s 3 106 907 793.163 407.121
+s 3 109 224 130.501 479.026
+s 3 110 232 140.654 474.298
+s 3 112 353 276.953 460.195
+s 3 113 326 247.913 479.275
+s 3 114 323 244.999 481.419
+s 3 116 405 331.557 456.078
+s 3 118 402 328.474 500.448
+s 3 121 476 406.808 478.331
+s 3 124 560 488.874 453.428
+s 3 125 559 487.866 451.429
+s 3 127 585 511.21 485.366
+s 3 130 582 508.117 452.34
+s 3 131 642 560.191 503.303
+s 3 132 703 613.512 462.274
+s 3 134 766 669.178 501.123
+s 3 137 780 681.115 471.213
+s 3 138 785 686.111 468.15
+s 3 140 766 669.178 501.123
+s 3 147 904 790.478 501.294
+s 3 149 938 820.46 502.412
+s 3 152 963 842.7 502.39
+s 3 156 242 148.04 545.744
+s 3 158 248 154.13 544.882
+s 3 162 314 224.573 545.257
+s 3 163 315 225.942 530.779
+s 3 164 315 225.25 552.16
+s 3 165 320 230.24 575.172
+s 3 166 368 292.361 563.739
+s 3 167 411 337.518 559.362
+s 3 169 456 385.345 575.417
+s 3 170 576 503.558 560.339
+s 3 171 572 499.542 562.338
+s 3 172 572 499.542 562.338
+s 3 173 638 556.345 563.543
+s 3 175 636 554.93 552.498
+s 3 178 692 604.626 533.07
+s 3 179 763 665.69 529.344
+s 3 181 716 624.895 518.341
+s 3 182 764 666.738 535.365
+s 3 185 769 671.706 536.364
+s 3 186 769 671.706 536.364
+s 3 189 841 735.519 558.219
+s 3 190 881 771.578 554.18
+s 3 191 839 733.85 555.19
+s 3 193 938 820.706 573.366
+s 3 194 948 829.543 513.025
+s 3 196 904 790.951 538.836
+s 3 197 976 854.393 525.427
+s 3 198 236 140.322 603.995
+s 3 199 235 139.053 600.975
+s 3 203 290 208.701 584.181
+s 3 204 321 230.851 584.311
+s 3 205 322 231.422 590.784
+s 3 207 425 352.676 598.285
+s 3 210 507 413.915 594.08
+s 3 216 586 512.672 602.104
+s 3 217 585 511.513 598.4
+s 3 218 657 574.43 587.301
+s 3 219 675 589.734 581.331
+s 3 220 656 573.443 582.387
+s 3 221 650 567.724 580.446
+s 3 223 715 624.944 635.149
+s 3 224 828 723.983 600.259
+s 3 225 813 710.87 596.451
+s 3 231 977 855.78 576.339
+s 3 233 252 132.151 646.953
+s 3 235 270 151.285 641.899
+s 3 236 263 143.885 646.853
+s 3 237 335 226.072 646.176
+s 3 240 448 356.109 665.276
+s 3 241 465 362.114 676.566
+s 3 242 481 383.735 674.003
+s 3 243 472 384.478 660.065
+s 3 244 478 381.18 672.03
+s 3 246 564 460.41 668.205
+s 3 247 548 446.541 666.983
+s 3 249 607 530.254 640.36
+s 3 251 615 515.283 663.243
+s 3 252 687 591.632 664.369
+s 3 253 698 601.651 663.156
+s 3 254 700 603.327 667.192
+s 3 256 693 597.206 667.286
+s 3 257 714 618.171 662.386
+s 3 258 708 618.448 645.308
+s 3 259 726 634.919 642.193
+s 3 260 713 616.921 667.437
+s 3 261 730 638.986 646.352
+s 3 264 776 678.909 643.341
+s 3 268 893 779.545 683.224
+s 3 271 888 774.345 683.25
+s 3 272 898 784.392 690.3
+s 3 273 956 836.616 654.36
+s 3 274 947 828.918 673.067
+s 3 275 359 196.307 730.769
+s 3 276 356 193.389 729.874
+s 3 277 444 291.736 761.796
+s 3 278 463 357.497 706.028
+s 3 279 610 473.389 731.03
+s 3 281 602 465.988 723.373
+s 3 285 665 516.633 728.092
+s 3 286 661 513.323 723.09
+s 3 288 659 511.572 743.182
+s 3 290 738 599.252 709.299
+s 3 292 740 600.731 731.179
+s 3 301 313 146.474 779.852
+s 3 304 375 193.685 790.593
+s 3 305 373 191.613 783.699
+s 3 306 385 200.249 789.607
+s 3 307 406 218.946 809.916
+s 3 310 455 295.762 773.851
+s 3 311 472 311.008 772.914
+s 3 312 544 356.234 798.671
+s 3 316 757 563.508 808.199
+s 3 323 445 218.732 856.915
+s 3 328 668 427.886 871.306
+s 3 329 750 498.489 887.267
+s 3 330 765 510.975 890.36
+s 3 331 744 494.093 885.015
+s 3 333 863 642.396 846.332
+s 3 334 857 636.646 843.358
+s 3 343 514 229.103 934.981
+i 4 0.294287 0.660352 9.6062 0.0191561 0.0175597 -0.000532113
+i 4 0.265576 0.674707 9.57988 0.0180918 0.0175597 0
+i 4 0.256006 0.698633 9.56074 0.0170276 0.0170276 0
+i 4 0.265576 0.715381 9.56553 0.0164955 0.0164955 0.000532113
+i 4 0.265576 0.727344 9.58467 0.0164955 0.0154313 0.000532113
+i 4 0.239258 0.732129 9.59424 0.0164955 0.0148992 0.00106423
+i 4 0.22251 0.732129 9.57749 0.0164955 0.0138349 0.00159634
+i 4 0.23208 0.729736 9.57988 0.0164955 0.0133028 0.00159634
+i 4 0.227295 0.727344 9.58945 0.0159634 0.0133028 0.00212845
+i 4 0.203369 0.736914 9.60142 0.0148992 0.0127707 0.00159634
+i 4 0.179443 0.734522 9.59902 0.0138349 0.0122386 0.00159634
+i 4 0.172266 0.741699 9.59424 0.0133028 0.0117065 0.00159634
+i 4 0.193799 0.741699 9.63013 0.0127707 0.0111744 0.00212845
+i 4 0.186621 0.741699 9.66841 0.0122386 0.0106423 0.00266056
+i 4 0.172266 0.741699 9.68994 0.0122386 0.0101101 0.00319268
+i 4 0.160303 0.748877 9.67798 0.0122386 0.0101101 0.00372479
+i 4 0.181836 0.756055 9.67798 0.0117065 0.00957803 0.0042569
+i 4 0.200977 0.775195 9.69951 0.0117065 0.00904591 0.00478901
+i 4 0.186621 0.77998 9.7043 0.0111744 0.00957803 0.00478901
+i 4 0.162695 0.777588 9.68755 0.0106423 0.0101101 0.00478901
+i 4 0.155518 0.768018 9.66123 0.0106423 0.0111744 0.00532113
+i 4 0.16748 0.748877 9.67319 0.0111744 0.0122386 0.00638535
+i 4 0.162695 0.739307 9.7043 0.0111744 0.0127707 0.00798169
+i 4 0.153125 0.732129 9.70908 0.0117065 0.0122386 0.00904591
+i 4 0.150732 0.727344 9.69233 0.0122386 0.0117065 0.00957803
+i 4 0.179443 0.739307 9.66841 0.0133028 0.0111744 0.0101101
+s 4 1 885 771.994 190.366
+s 4 8 814 710.223 241.363
+s 4 10 815 711.068 244.306
+s 4 11 882 769.488 198.098
+s 4 12 876 763.708 195.193
+s 4 13 874 761.731 192.256
+s 4 15 882 769.341 194.117
+s 4 16 952 831.207 237.222
+s 4 17 940 820.094 241.983
+s 4 18 945 824.618 239.15
+s 4 19 948 827.346 234.198
+s 4 22 432 359.221 264.591
+s 4 28 760 663.293 287.118
+s 4 33 813 709.512 279.215
+s 4 34 818 713.314 276.343
+s 4 36 815 711.529 285.057
+s 4 37 893 778.938 308.242
+s 4 39 898 783.55 307.164
+s 4 40 907 792.695 318.382
+s 4 41 902 787.654 318.396
+s 4 42 938 819.106 256.202
+s 4 43 989 863.908 281.834
+s 4 44 971 847.662 314.346
+s 4 45 968 844.579 310.273
+s 4 46 975 852.254 260.043
+s 4 52 229 140.68 323.217
+s 4 53 598 514.121 337.175
+s 4 58 815 710.937 376.471
+s 4 59 798 696.344 357.345
+s 4 61 796 694.481 358.445
+s 4 63 893 779.58 351.02
+s 4 67 899 785.244 357.125
+s 4 68 898 784.179 353.158
+s 4 70 916 800.164 347.205
+s 4 71 987 862.374 323.193
+s 4 72 984 859.517 326.195
+s 4 78 349 274.104 444.811
+s 4 79 564 492.65 419.414
+s 4 81 575 501.478 444.409
+s 4 83 601 524.76 412.446
+s 4 84 586 511.86 446.439
+s 4 85 650 567.837 402.416
+s 4 86 700 611.156 433.123
+s 4 87 702 612.913 434.209
+s 4 90 706 616.807 433.199
+s 4 91 746 651.408 432.284
+s 4 93 707 617.559 430.146
+s 4 94 743 648.795 434.231
+s 4 95 823 718.332 385.052
+s 4 96 796 695.579 425.228
+s 4 97 800 699.055 396.852
+s 4 98 773 674.916 390.759
+s 4 102 879 768.454 386.971
+s 4 103 888 776.252 409.162
+s 4 106 907 793.277 407.058
+s 4 109 224 130.613 478.998
+s 4 110 232 140.621 473.277
+s 4 112 353 277.014 460.171
+s 4 114 323 244.961 480.36
+s 4 115 349 272.766 463.341
+s 4 116 405 331.398 455.088
+s 4 118 401 327.557 499.323
+s 4 119 401 327.557 499.323
+s 4 120 395 321.112 493.435
+s 4 121 476 406.743 478.377
+s 4 127 586 512.557 483.379
+s 4 130 582 508.157 452.348
+s 4 131 642 560.134 502.312
+s 4 132 703 613.486 461.235
+s 4 134 764 667.34 501.126
+s 4 136 773 675.456 494.29
+s 4 137 780 681.045 471.208
+s 4 138 784 685.045 467.118
+s 4 139 781 682.087 473.259
+s 4 140 766 669.129 501.112
+s 4 141 884 772.588 493.247
+s 4 143 860 752.712 506.715
+s 4 149 938 820.417 502.393
+s 4 152 963 842.708 502.408
+s 4 156 242 148.092 545.745
+s 4 158 248 154.161 544.898
+s 4 162 314 224.577 545.216
+s 4 163 315 225.946 530.744
+s 4 164 315 225.235 552.134
+s 4 165 320 230.327 575.153
+s 4 167 411 337.774 559.343
+s 4 169 456 385.542 575.339
+s 4 170 575 502.563 557.259
+s 4 171 572 499.587 562.29
+s 4 172 572 499.587 562.29
+s 4 173 638 556.429 563.579
+s 4 175 636 554.961 551.494
+s 4 177 641 559.492 561.262
+s 4 178 692 604.626 533.116
+s 4 179 763 665.738 529.302
+s 4 181 716 624.827 517.256
+s 4 182 764 666.839 535.368
+s 4 185 769 671.733 536.347
+s 4 186 769 671.733 536.347
+s 4 187 810 708.024 538.294
+s 4 189 841 735.558 558.172
+s 4 190 881 772.053 554.336
+s 4 191 840 734.563 554.241
+s 4 192 860 753.225 554.14
+s 4 193 938 820.664 573.398
+s 4 194 948 829.589 513.021
+s 4 196 904 790.866 537.836
+s 4 197 976 854.583 524.319
+s 4 198 236 140.38 603.999
+s 4 201 288 206.836 586.271
+s 4 204 321 230.91 583.331
+s 4 207 425 352.585 598.27
+s 4 210 507 413.476 594.048
+s 4 216 586 512.578 601.8
+s 4 217 585 511.154 598.205
+s 4 218 657 574.497 587.3
+s 4 220 656 573.483 582.382
+s 4 221 651 568.555 580.399
+s 4 223 715 624.942 635.151
+s 4 224 827 722.978 600.292
+s 4 231 977 855.254 576.338
+s 4 234 252 132.144 646.968
+s 4 240 448 356.099 665.285
+s 4 242 481 383.757 674.017
+s 4 244 478 381.174 672.047
+s 4 246 564 460.379 668.196
+s 4 247 548 446.621 666.998
+s 4 249 607 530.219 640.38
+s 4 250 613 512.601 667.348
+s 4 251 616 516.659 662.172
+s 4 252 687 591.584 663.361
+s 4 253 698 601.767 663.178
+s 4 254 700 603.389 667.207
+s 4 256 693 597.195 667.276
+s 4 258 708 618.612 644.382
+s 4 259 726 634.907 642.21
+s 4 260 713 616.921 667.416
+s 4 261 730 638.979 646.36
+s 4 267 895 781.565 682.132
+s 4 268 893 779.549 683.206
+s 4 272 898 784.406 690.256
+s 4 273 956 836.687 654.365
+s 4 274 947 828.881 671.937
+s 4 275 359 196.281 730.773
+s 4 276 356 193.356 729.874
+s 4 277 444 291.69 761.788
+s 4 278 463 357.45 704.999
+s 4 281 602 466.206 723.378
+s 4 283 621 486.721 706.163
+s 4 286 661 513.021 723.096
+s 4 288 657 510.171 742.231
+s 4 290 738 599.306 709.267
+s 4 292 740 600.83 731.158
+s 4 301 313 146.359 779.839
+s 4 303 308 140.181 777.88
+s 4 310 454 294.447 773.873
+i 5 0.200977 0.746484 9.66602 0.0133028 0.0106423 0.00957803
+i 5 0.196191 0.756055 9.66602 0.0133028 0.0101101 0.00957803
+i 5 0.189014 0.765625 9.65166 0.0122386 0.00957803 0.00957803
+i 5 0.193799 0.772803 9.64209 0.0106423 0.00957803 0.00957803
+i 5 0.217725 0.777588 9.66123 0.00904591 0.00904591 0.00957803
+i 5 0.24165 0.777588 9.69233 0.00744958 0.00904591 0.0106423
+i 5 0.234473 0.77041 9.7019 0.00691746 0.00957803 0.0111744
+i 5 0.248828 0.765625 9.68276 0.00638535 0.00957803 0.0122386
+i 5 0.275146 0.75127 9.67319 0.00691746 0.00957803 0.0122386
+i 5 0.291895 0.748877 9.67319 0.00744958 0.00904591 0.0117065
+i 5 0.270361 0.744092 9.68276 0.00798169 0.0085138 0.0106423
+i 5 0.246436 0.746484 9.66602 0.00904591 0.00798169 0.0101101
+i 5 0.234473 0.748877 9.64448 0.00957803 0.00798169 0.00904591
+i 5 0.248828 0.758447 9.64209 0.0101101 0.00798169 0.00904591
+i 5 0.253613 0.765625 9.6397 0.0106423 0.00798169 0.00904591
+i 5 0.246436 0.753662 9.61816 0.0106423 0.00904591 0.00904591
+i 5 0.248828 0.744092 9.58706 0.0106423 0.00904591 0.00957803
+i 5 0.282324 0.720166 9.57271 0.0106423 0.0085138 0.0101101
+i 5 0.30625 0.710596 9.58945 0.0101101 0.00744958 0.0106423
+i 5 0.30625 0.708203 9.60142 0.00904591 0.00638535 0.0111744
+i 5 0.294287 0.710596 9.59663 0.00798169 0.00585324 0.0117065
+i 5 0.287109 0.720166 9.57749 0.00744958 0.00585324 0.0111744
+i 5 0.30625 0.724951 9.59185 0.00691746 0.00585324 0.0111744
+i 5 0.299072 0.722559 9.61577 0.00691746 0.00585324 0.0106423
+i 5 0.267969 0.712988 9.61338 0.00638535 0.00532113 0.0101101
+i 5 0.260791 0.703418 9.57988 0.00585324 0.00478901 0.00957803
+i 5 0.272754 0.693848 9.55356 0.00585324 0.0042569 0.00957803
+s 5 1 885 771.967 190.374
+s 5 8 814 710.258 241.348
+s 5 11 882 769.5 198.11
+s 5 12 876 763.725 195.196
+s 5 13 874 761.761 192.265
+s 5 14 876 763.725 195.196
+s 5 15 882 769.333 194.13
+s 5 16 952 831.211 238.222
+s 5 17 941 820.874 241.088
+s 5 19 948 827.331 234.199
+s 5 28 760 663.304 287.152
+s 5 33 813 709.417 279.246
+s 5 34 818 713.31 276.339
+s 5 36 815 711.553 285.069
+s 5 37 893 778.921 308.279
+s 5 39 898 783.503 307.195
+s 5 40 908 793.456 318.342
+s 5 41 902 787.598 318.335
+s 5 42 938 819.203 256.15
+s 5 43 989 863.912 281.803
+s 5 44 972 848.245 313.371
+s 5 45 968 844.392 308.24
+s 5 46 975 852.111 259.768
+s 5 52 229 140.643 323.25
+s 5 58 815 711.009 376.479
+s 5 59 797 695.427 357.337
+s 5 61 797 695.427 357.337
+s 5 63 893 779.641 350.98
+s 5 67 899 785.25 357.122
+s 5 70 916 800.144 347.212
+s 5 71 986 861.363 325.258
+s 5 72 986 861.363 325.258
+s 5 78 349 272.718 444.977
+s 5 79 564 492.645 419.352
+s 5 81 575 501.478 444.458
+s 5 83 601 524.669 412.369
+s 5 84 586 511.845 446.414
+s 5 85 650 567.806 402.399
+s 5 86 701 611.648 433.295
+s 5 87 701 611.648 433.295
+s 5 90 706 616.746 433.269
+s 5 91 747 652.381 432.302
+s 5 93 708 618.322 429.229
+s 5 94 744 649.616 434.254
+s 5 95 823 718.328 385.098
+s 5 96 796 695.506 425.257
+s 5 97 800 699.015 396.835
+s 5 98 773 674.898 390.815
+s 5 103 888 776.053 409.196
+s 5 106 907 793.174 407.16
+s 5 109 224 130.598 479.009
+s 5 112 353 277.059 460.178
+s 5 113 326 247.919 479.3
+s 5 114 323 245.04 481.416
+s 5 118 402 328.417 500.439
+s 5 119 400 326.248 502.231
+s 5 121 476 406.735 478.368
+s 5 127 585 511.229 485.269
+s 5 130 582 508.086 452.315
+s 5 131 643 561.152 502.328
+s 5 132 703 613.454 461.25
+s 5 134 766 669.174 501.069
+s 5 136 773 675.449 494.288
+s 5 137 781 682.128 471.206
+s 5 138 784 685.119 467.122
+s 5 139 781 682.162 473.269
+s 5 140 766 669.174 501.069
+s 5 145 888 776.3 491.33
+s 5 149 938 820.54 502.447
+s 5 152 963 842.697 502.391
+s 5 156 242 148.049 545.748
+s 5 158 248 154.189 544.861
+s 5 162 314 224.58 545.228
+s 5 163 315 225.943 530.706
+s 5 164 315 225.254 552.161
+s 5 165 320 230.248 573.933
+s 5 167 411 337.603 559.374
+s 5 169 456 385.477 575.333
+s 5 170 575 502.564 557.297
+s 5 171 572 499.585 562.324
+s 5 172 572 499.585 562.324
+s 5 173 638 556.357 563.566
+s 5 177 641 559.473 561.27
+s 5 178 693 605.572 532.124
+s 5 179 763 665.768 529.292
+s 5 181 716 624.878 517.287
+s 5 182 764 666.826 535.34
+s 5 185 769 671.754 536.33
+s 5 186 769 671.754 536.33
+s 5 187 810 707.981 538.318
+s 5 189 841 735.367 557.132
+s 5 190 881 771.284 554.167
+s 5 191 840 734.433 554.139
+s 5 192 860 752.832 554.234
+s 5 193 938 820.661 573.411
+s 5 194 948 829.548 513.027
+s 5 196 904 790.811 537.922
+s 5 197 976 854.459 524.408
+s 5 198 236 140.438 604.965
+s 5 199 235 139.069 600.972
+s 5 201 288 206.856 586.255
+s 5 203 290 208.734 584.196
+s 5 204 321 230.86 584.302
+s 5 205 322 231.412 590.747
+s 5 207 425 352.658 598.284
+s 5 208 430 357.846 600.368
+s 5 209 430 357.846 600.368
+s 5 210 507 413.862 594.141
+s 5 216 586 512.524 601.846
+s 5 217 585 511.371 598.24
+s 5 220 656 573.388 582.4
+s 5 221 650 567.67 580.444
+s 5 223 715 625.005 635.16
+s 5 224 828 724.003 600.283
+s 5 231 977 856.036 576.324
+s 5 233 252 132.27 646.898
+s 5 235 270 151.147 641.895
+s 5 237 335 226.3 646.265
+s 5 240 448 356.101 665.311
+s 5 241 465 362.419 675.526
+s 5 242 482 384.778 674.024
+s 5 244 478 381.143 672.035
+s 5 246 564 460.402 668.186
+s 5 247 548 446.693 666.997
+s 5 250 613 512.473 667.358
+s 5 251 615 515.389 663.256
+s 5 252 687 591.542 663.355
+s 5 253 698 601.734 663.164
+s 5 254 700 603.352 667.193
+s 5 257 714 618.128 662.357
+s 5 258 708 618.561 644.403
+s 5 259 726 634.882 642.196
+s 5 260 713 616.859 667.397
+s 5 261 730 638.896 646.366
+s 5 264 776 678.964 643.331
+s 5 268 893 779.594 683.216
+s 5 271 888 774.41 682.262
+s 5 273 956 836.775 654.374
+s 5 274 947 828.877 671.995
+s 5 275 359 196.336 730.802
+s 5 276 356 193.409 729.896
+s 5 277 444 291.551 761.797
+s 5 278 464 358.548 705.089
+s 5 279 610 473.571 731.066
+s 5 281 602 466.135 723.367
+s 5 282 617 475.86 725.172
+s 5 285 665 516.486 728.097
+s 5 286 661 513.146 723.099
+s 5 289 743 603.491 734.339
+s 5 290 738 599.323 709.259
+s 5 292 740 600.943 731.142
+s 5 301 313 146.284 779.851
+s 5 304 375 193.624 790.587
+s 5 306 385 200.303 789.594
+s 5 307 406 218.914 809.947
+s 5 310 455 295.69 773.843
+s 5 311 472 310.886 772.897
+s 5 312 544 356.329 798.683
+s 5 329 750 498.867 886.161
+s 5 330 765 511.01 889.347
+s 5 331 745 495.029 884.943
+s 5 333 862 641.525 845.353
+s 5 334 858 637.569 843.345
+s 5 345 589 322.892 907.696
+s 5 349 765 431.497 988.976
diff --git a/examples/ISAM2_SmartFactor_valgrind.cpp b/examples/ISAM2_SmartFactor_valgrind.cpp
new file mode 100644
index 000000000..2d5c6c15b
--- /dev/null
+++ b/examples/ISAM2_SmartFactor_valgrind.cpp
@@ -0,0 +1,222 @@
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
+
+#include <iostream>
+#include <vector>
+#include <sstream>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+using symbol_shorthand::V;
+using symbol_shorthand::B;
+
+struct IMUHelper
+{
+    IMUHelper()
+    {
+        gtsam::Matrix33 measured_acc_cov      = gtsam::Matrix33::Identity(3,3) * pow(0.113, 2.0);
+        gtsam::Matrix33 measured_omega_cov    = gtsam::Matrix33::Identity(3,3) * pow(4.0e-5, 2.0);
+        gtsam::Matrix33 bias_acc_cov          = gtsam::Matrix33::Identity(3,3) * pow( 0.00002, 2.0);
+        gtsam::Matrix33 bias_omega_cov        = gtsam::Matrix33::Identity(3,3) * pow(0.001, 2.0);
+        gtsam::Matrix33 integration_error_cov = gtsam::Matrix33::Identity(3,3)*1e-9; // error committed in integrating position from velocities
+        gtsam::Matrix bias_acc_omega_int      = gtsam::Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
+
+        {
+            auto gaussian = gtsam::noiseModel::Diagonal::Sigmas(
+                (gtsam::Vector(6) <<
+                gtsam::Vector3::Constant(5.0e-2),
+                gtsam::Vector3::Constant(5.0e-3)).finished());
+
+            auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
+
+            bias_noise_model = huber;
+        }
+
+        {
+            auto gaussian = gtsam::noiseModel::Isotropic::Sigma(3,0.01);
+            auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
+
+            velocity_noise_model = huber;
+        }
+
+        // expect IMU to be rotated in image space co-ords
+        auto p = boost::make_shared<gtsam::PreintegratedCombinedMeasurements::Params>(gtsam::Vector3(0.0, 9.8, 0.0));
+
+        p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
+        p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
+        p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
+        p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
+        p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
+        p->biasAccOmegaInt = bias_acc_omega_int;
+
+         // from SVD comparing gyro and vision estimates, no bias modelled
+        gtsam::Rot3 body_to_imu_rot(
+            3.612861008216737e-02,  -9.987267865568920e-01,   3.520695026944293e-02,
+            4.541686330383411e-02,  -3.355264881270600e-02,  -9.984044913186698e-01,
+            9.983145957368207e-01,   3.766995581886975e-02,   4.414682737675374e-02);
+
+        gtsam::Point3 body_to_imu_trans(0.03, -0.025, -0.06);
+
+        p->body_P_sensor = gtsam::Pose3(body_to_imu_rot, body_to_imu_trans);
+
+        gtsam::Rot3 prior_rotation = gtsam::Rot3(gtsam::Matrix33::Identity());
+        gtsam::Pose3 prior_pose(prior_rotation, gtsam::Point3(0,0,0));
+
+        gtsam::Vector3 acc_bias(2.05998e-18, -0.0942015, 1.17663e-16);
+        gtsam::Vector3 gyro_bias(0,0,0);
+
+        prior_imu_bias = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias);
+
+        prev_state = gtsam::NavState(prior_pose, gtsam::Vector3(0,0,0));
+        prop_state = prev_state;
+        prev_bias = prior_imu_bias;
+
+        preintegrated = new gtsam::PreintegratedCombinedMeasurements(p, prior_imu_bias);
+    }
+
+    gtsam::imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
+    gtsam::noiseModel::Robust::shared_ptr velocity_noise_model;
+    gtsam::noiseModel::Robust::shared_ptr bias_noise_model;
+    gtsam::NavState prev_state;
+    gtsam::NavState prop_state;
+    gtsam::imuBias::ConstantBias prev_bias;
+    gtsam::PreintegratedCombinedMeasurements *preintegrated;
+};
+
+int main(int argc, char* argv[]) {
+    if (argc != 2) {
+        cout << "./main [data.txt]\n";
+        return 0;
+    }
+
+    double fx = 8.2237229542137766e+02;
+    double fy = fx;
+    double cx = 5.3872524261474609e+02;
+    double cy = 5.7909587860107422e+02;
+    double baseline = 7.4342307430851651e-01;
+
+    Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
+
+    ISAM2Params parameters;
+    parameters.relinearizeThreshold = 0.1;
+    ISAM2 isam(parameters);
+
+    // Create a factor graph
+    std::map<size_t, gtsam::SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
+    NonlinearFactorGraph graph;
+    Values initialEstimate;
+    IMUHelper imu;
+
+    // Pose prior
+    auto priorPoseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
+    graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(), priorPoseNoise);
+    initialEstimate.insert(X(0), Pose3::identity());
+
+    // Bias prior
+    graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.prior_imu_bias, imu.bias_noise_model));
+    initialEstimate.insert(B(0), imu.prior_imu_bias);
+
+    // Velocity prior
+    graph.add(PriorFactor<Vector3>(V(1), Vector3(0,0,0), imu.velocity_noise_model));
+    initialEstimate.insert(V(0), Vector3(0,0,0));
+
+    ifstream in(argv[1]);
+
+    if (!in) {
+        cerr << "error opening: " << argv[1] << "\n";
+        return 1;
+    }
+
+    int last_frame = 1;
+    int frame;
+
+    while (true) {
+        char line[1024];
+
+        in.getline(line, sizeof(line));
+        stringstream ss(line);
+        char type;
+
+        ss >> type;
+        ss >> frame;
+
+        if (frame != last_frame || in.eof()) {
+            cout << "Running isam for frame: " << last_frame << "\n";
+
+            initialEstimate.insert(X(last_frame), Pose3::identity());
+            initialEstimate.insert(V(last_frame), Vector3(0,0,0));
+            initialEstimate.insert(B(last_frame), imu.prev_bias);
+
+            CombinedImuFactor imu_factor(
+                X(last_frame-1), V(last_frame-1),
+                X(last_frame), V(last_frame),
+                B(last_frame-1), B(last_frame),
+                *imu.preintegrated);
+
+            graph.add(imu_factor);
+
+            isam.update(graph, initialEstimate);
+
+            Values currentEstimate = isam.calculateEstimate();
+            //currentEstimate.print("Current estimate: ");
+
+            imu.prop_state = imu.preintegrated->predict(imu.prev_state, imu.prev_bias);
+            imu.prev_state = NavState(currentEstimate.at<Pose3>(X(last_frame)), currentEstimate.at<Vector3>(V(last_frame)));
+            imu.prev_bias = currentEstimate.at<imuBias::ConstantBias>(B(last_frame));
+            imu.preintegrated->resetIntegrationAndSetBias(imu.prev_bias);
+
+            graph.resize(0);
+            initialEstimate.clear();
+
+            if (in.eof()) {
+                break;
+            }
+        }
+
+        if (type == 'i') {
+            double ax, ay, az;
+            double gx, gy, gz;
+            double dt = 1/800.0;
+
+            ss >> ax;
+            ss >> ay;
+            ss >> az;
+
+            ss >> gx;
+            ss >> gy;
+            ss >> gz;
+
+            Vector3 acc(ax, ay, az);
+            Vector3 gyr(gx, gy, gz);
+
+            imu.preintegrated->integrateMeasurement(acc, gyr, dt);
+        } else if (type == 's') {
+            int landmark;
+            double xl, xr, y;
+
+            ss >> landmark;
+            ss >> xl;
+            ss >> xr;
+            ss >> y;
+
+            if (smartFactors.count(landmark) == 0) {
+                auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
+
+                SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
+
+                smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(new SmartStereoProjectionPoseFactor(gaussian, params));
+                graph.push_back(smartFactors[landmark]);
+            }
+
+            smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
+        }
+
+        last_frame = frame;
+    }
+
+    return 0;
+}

From 937cdcf4d98c14ab45de3a879c0ad351b8fbb315 Mon Sep 17 00:00:00 2001
From: Nghia Ho <nghia@prenav.com>
Date: Fri, 14 Dec 2018 12:12:33 -0800
Subject: [PATCH 071/281] shorten parameter values

---
 ...nd.txt => ISAM2_SmartFactorStereo_IMU.txt} |   0
 examples/ISAM2_SmartFactorStereo_IMU.cpp      | 236 ++++++++++++++++++
 examples/ISAM2_SmartFactor_valgrind.cpp       | 222 ----------------
 3 files changed, 236 insertions(+), 222 deletions(-)
 rename examples/Data/{ISAM2_SmartFactor_valgrind.txt => ISAM2_SmartFactorStereo_IMU.txt} (100%)
 create mode 100644 examples/ISAM2_SmartFactorStereo_IMU.cpp
 delete mode 100644 examples/ISAM2_SmartFactor_valgrind.cpp

diff --git a/examples/Data/ISAM2_SmartFactor_valgrind.txt b/examples/Data/ISAM2_SmartFactorStereo_IMU.txt
similarity index 100%
rename from examples/Data/ISAM2_SmartFactor_valgrind.txt
rename to examples/Data/ISAM2_SmartFactorStereo_IMU.txt
diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp
new file mode 100644
index 000000000..25675abf8
--- /dev/null
+++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp
@@ -0,0 +1,236 @@
+/**
+ * @file ISAM2_SmartFactorStereo_IMU.cpp
+ * @brief test of iSAM2 with smart stereo factors and IMU preintegration,
+ * originally used to debug valgrind invalid reads with Eigen
+ * @author Nghia Ho
+ *
+ * Setup is a stationary stereo camera with an IMU attached.
+ * The data file is at examples/Data/ISAM2_SmartFactorStereo_IMU.txt
+ * It contains 5 frames of stereo matches and IMU data.
+ */
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+using symbol_shorthand::V;
+using symbol_shorthand::B;
+
+struct IMUHelper {
+  IMUHelper() {
+    {
+      auto gaussian = noiseModel::Diagonal::Sigmas(
+          (Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3))
+              .finished());
+
+      auto huber = noiseModel::Robust::Create(
+          noiseModel::mEstimator::Huber::Create(1.345), gaussian);
+
+      biasNoiseModel = huber;
+    }
+
+    {
+      auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);
+      auto huber = noiseModel::Robust::Create(
+          noiseModel::mEstimator::Huber::Create(1.345), gaussian);
+
+      velocityNoiseModel = huber;
+    }
+
+    // expect IMU to be rotated in image space co-ords
+    auto p = boost::make_shared<PreintegratedCombinedMeasurements::Params>(
+        Vector3(0.0, 9.8, 0.0));
+
+    p->accelerometerCovariance =
+        I_3x3 * pow(0.0565, 2.0);  // acc white noise in continuous
+    p->integrationCovariance =
+        I_3x3 * 1e-9;  // integration uncertainty continuous
+    p->gyroscopeCovariance =
+        I_3x3 * pow(4.0e-5, 2.0);  // gyro white noise in continuous
+    p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0);  // acc bias in continuous
+    p->biasOmegaCovariance =
+        I_3x3 * pow(0.001, 2.0);  // gyro bias in continuous
+    p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
+
+    // body to IMU rotation
+    Rot3 iRb(
+       0.036129, -0.998727,  0.035207,
+       0.045417, -0.033553, -0.99840,4
+       0.998315,  0.037670,  0.044147);
+
+    // body to IMU translation (meters)
+    Point3 iTb(0.03, -0.025, -0.06);
+
+    // body in this example is the left camera
+    p->body_P_sensor = Pose3(iRb, iTb);
+
+    Rot3 prior_rotation = Rot3(I_3x3);
+    Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));
+
+    Vector3 acc_bias(0.0, -0.0942015, 0.0);  // in camera frame
+    Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
+
+    priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);
+
+    prevState = NavState(prior_pose, Vector3(0, 0, 0));
+    propState = prevState;
+    prevBias = priorImuBias;
+
+    preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);
+  }
+
+  imuBias::ConstantBias priorImuBias;  // assume zero initial bias
+  noiseModel::Robust::shared_ptr velocityNoiseModel;
+  noiseModel::Robust::shared_ptr biasNoiseModel;
+  NavState prevState;
+  NavState propState;
+  imuBias::ConstantBias prevBias;
+  PreintegratedCombinedMeasurements* preintegrated;
+};
+
+int main(int argc, char* argv[]) {
+  if (argc != 2) {
+    cout << "./main [data.txt]\n";
+    return 0;
+  }
+
+  ifstream in(argv[1]);
+
+  if (!in) {
+    cerr << "error opening: " << argv[1] << "\n";
+    return 1;
+  }
+
+  // Camera parameters
+  double fx = 822.37;
+  double fy = 822.37;
+  double cx = 538.73;
+  double cy = 579.10;
+  double baseline = 0.372; // meters
+
+  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
+
+  ISAM2Params parameters;
+  parameters.relinearizeThreshold = 0.1;
+  ISAM2 isam(parameters);
+
+  // Create a factor graph
+  std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
+  NonlinearFactorGraph graph;
+  Values initialEstimate;
+  IMUHelper imu;
+
+  // Pose prior - at identity
+  auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
+  graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(),
+                                           priorPoseNoise);
+  initialEstimate.insert(X(0), Pose3::identity());
+
+  // Bias prior
+  graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.priorImuBias,
+                                               imu.biasNoiseModel));
+  initialEstimate.insert(B(0), imu.priorImuBias);
+
+  // Velocity prior - assume stationary
+  graph.add(
+      PriorFactor<Vector3>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel));
+  initialEstimate.insert(V(0), Vector3(0, 0, 0));
+
+  int lastFrame = 1;
+  int frame;
+
+  while (true) {
+    char line[1024];
+
+    in.getline(line, sizeof(line));
+    stringstream ss(line);
+    char type;
+
+    ss >> type;
+    ss >> frame;
+
+    if (frame != lastFrame || in.eof()) {
+      cout << "Running iSAM for frame: " << lastFrame << "\n";
+
+      initialEstimate.insert(X(lastFrame), Pose3::identity());
+      initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
+      initialEstimate.insert(B(lastFrame), imu.prevBias);
+
+      CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1),
+                                  X(lastFrame), V(lastFrame), B(lastFrame - 1),
+                                  B(lastFrame), *imu.preintegrated);
+
+      graph.add(imuFactor);
+
+      isam.update(graph, initialEstimate);
+
+      Values currentEstimate = isam.calculateEstimate();
+
+      imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias);
+      imu.prevState = NavState(currentEstimate.at<Pose3>(X(lastFrame)),
+                               currentEstimate.at<Vector3>(V(lastFrame)));
+      imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(B(lastFrame));
+      imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias);
+
+      graph.resize(0);
+      initialEstimate.clear();
+
+      if (in.eof()) {
+        break;
+      }
+    }
+
+    if (type == 'i') {  // Process IMU measurement
+      double ax, ay, az;
+      double gx, gy, gz;
+      double dt = 1 / 800.0;  // IMU at ~800Hz
+
+      ss >> ax;
+      ss >> ay;
+      ss >> az;
+
+      ss >> gx;
+      ss >> gy;
+      ss >> gz;
+
+      Vector3 acc(ax, ay, az);
+      Vector3 gyr(gx, gy, gz);
+
+      imu.preintegrated->integrateMeasurement(acc, gyr, dt);
+    } else if (type == 's') {  // Process stereo measurement
+      int landmark;
+      double xl, xr, y;
+
+      ss >> landmark;
+      ss >> xl;
+      ss >> xr;
+      ss >> y;
+
+      if (smartFactors.count(landmark) == 0) {
+        auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
+
+        SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
+
+        smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(
+            new SmartStereoProjectionPoseFactor(gaussian, params));
+        graph.push_back(smartFactors[landmark]);
+      }
+
+      smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
+    } else {
+      throw runtime_error("unexpected data type: " + type);
+    }
+
+    lastFrame = frame;
+  }
+
+  return 0;
+}
diff --git a/examples/ISAM2_SmartFactor_valgrind.cpp b/examples/ISAM2_SmartFactor_valgrind.cpp
deleted file mode 100644
index 2d5c6c15b..000000000
--- a/examples/ISAM2_SmartFactor_valgrind.cpp
+++ /dev/null
@@ -1,222 +0,0 @@
-#include <gtsam/nonlinear/ISAM2.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
-#include <gtsam/navigation/CombinedImuFactor.h>
-
-#include <iostream>
-#include <vector>
-#include <sstream>
-#include <fstream>
-
-using namespace std;
-using namespace gtsam;
-using symbol_shorthand::X;
-using symbol_shorthand::V;
-using symbol_shorthand::B;
-
-struct IMUHelper
-{
-    IMUHelper()
-    {
-        gtsam::Matrix33 measured_acc_cov      = gtsam::Matrix33::Identity(3,3) * pow(0.113, 2.0);
-        gtsam::Matrix33 measured_omega_cov    = gtsam::Matrix33::Identity(3,3) * pow(4.0e-5, 2.0);
-        gtsam::Matrix33 bias_acc_cov          = gtsam::Matrix33::Identity(3,3) * pow( 0.00002, 2.0);
-        gtsam::Matrix33 bias_omega_cov        = gtsam::Matrix33::Identity(3,3) * pow(0.001, 2.0);
-        gtsam::Matrix33 integration_error_cov = gtsam::Matrix33::Identity(3,3)*1e-9; // error committed in integrating position from velocities
-        gtsam::Matrix bias_acc_omega_int      = gtsam::Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
-
-        {
-            auto gaussian = gtsam::noiseModel::Diagonal::Sigmas(
-                (gtsam::Vector(6) <<
-                gtsam::Vector3::Constant(5.0e-2),
-                gtsam::Vector3::Constant(5.0e-3)).finished());
-
-            auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
-
-            bias_noise_model = huber;
-        }
-
-        {
-            auto gaussian = gtsam::noiseModel::Isotropic::Sigma(3,0.01);
-            auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian);
-
-            velocity_noise_model = huber;
-        }
-
-        // expect IMU to be rotated in image space co-ords
-        auto p = boost::make_shared<gtsam::PreintegratedCombinedMeasurements::Params>(gtsam::Vector3(0.0, 9.8, 0.0));
-
-        p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
-        p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
-        p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
-        p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
-        p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
-        p->biasAccOmegaInt = bias_acc_omega_int;
-
-         // from SVD comparing gyro and vision estimates, no bias modelled
-        gtsam::Rot3 body_to_imu_rot(
-            3.612861008216737e-02,  -9.987267865568920e-01,   3.520695026944293e-02,
-            4.541686330383411e-02,  -3.355264881270600e-02,  -9.984044913186698e-01,
-            9.983145957368207e-01,   3.766995581886975e-02,   4.414682737675374e-02);
-
-        gtsam::Point3 body_to_imu_trans(0.03, -0.025, -0.06);
-
-        p->body_P_sensor = gtsam::Pose3(body_to_imu_rot, body_to_imu_trans);
-
-        gtsam::Rot3 prior_rotation = gtsam::Rot3(gtsam::Matrix33::Identity());
-        gtsam::Pose3 prior_pose(prior_rotation, gtsam::Point3(0,0,0));
-
-        gtsam::Vector3 acc_bias(2.05998e-18, -0.0942015, 1.17663e-16);
-        gtsam::Vector3 gyro_bias(0,0,0);
-
-        prior_imu_bias = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias);
-
-        prev_state = gtsam::NavState(prior_pose, gtsam::Vector3(0,0,0));
-        prop_state = prev_state;
-        prev_bias = prior_imu_bias;
-
-        preintegrated = new gtsam::PreintegratedCombinedMeasurements(p, prior_imu_bias);
-    }
-
-    gtsam::imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
-    gtsam::noiseModel::Robust::shared_ptr velocity_noise_model;
-    gtsam::noiseModel::Robust::shared_ptr bias_noise_model;
-    gtsam::NavState prev_state;
-    gtsam::NavState prop_state;
-    gtsam::imuBias::ConstantBias prev_bias;
-    gtsam::PreintegratedCombinedMeasurements *preintegrated;
-};
-
-int main(int argc, char* argv[]) {
-    if (argc != 2) {
-        cout << "./main [data.txt]\n";
-        return 0;
-    }
-
-    double fx = 8.2237229542137766e+02;
-    double fy = fx;
-    double cx = 5.3872524261474609e+02;
-    double cy = 5.7909587860107422e+02;
-    double baseline = 7.4342307430851651e-01;
-
-    Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
-
-    ISAM2Params parameters;
-    parameters.relinearizeThreshold = 0.1;
-    ISAM2 isam(parameters);
-
-    // Create a factor graph
-    std::map<size_t, gtsam::SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
-    NonlinearFactorGraph graph;
-    Values initialEstimate;
-    IMUHelper imu;
-
-    // Pose prior
-    auto priorPoseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
-    graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(), priorPoseNoise);
-    initialEstimate.insert(X(0), Pose3::identity());
-
-    // Bias prior
-    graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.prior_imu_bias, imu.bias_noise_model));
-    initialEstimate.insert(B(0), imu.prior_imu_bias);
-
-    // Velocity prior
-    graph.add(PriorFactor<Vector3>(V(1), Vector3(0,0,0), imu.velocity_noise_model));
-    initialEstimate.insert(V(0), Vector3(0,0,0));
-
-    ifstream in(argv[1]);
-
-    if (!in) {
-        cerr << "error opening: " << argv[1] << "\n";
-        return 1;
-    }
-
-    int last_frame = 1;
-    int frame;
-
-    while (true) {
-        char line[1024];
-
-        in.getline(line, sizeof(line));
-        stringstream ss(line);
-        char type;
-
-        ss >> type;
-        ss >> frame;
-
-        if (frame != last_frame || in.eof()) {
-            cout << "Running isam for frame: " << last_frame << "\n";
-
-            initialEstimate.insert(X(last_frame), Pose3::identity());
-            initialEstimate.insert(V(last_frame), Vector3(0,0,0));
-            initialEstimate.insert(B(last_frame), imu.prev_bias);
-
-            CombinedImuFactor imu_factor(
-                X(last_frame-1), V(last_frame-1),
-                X(last_frame), V(last_frame),
-                B(last_frame-1), B(last_frame),
-                *imu.preintegrated);
-
-            graph.add(imu_factor);
-
-            isam.update(graph, initialEstimate);
-
-            Values currentEstimate = isam.calculateEstimate();
-            //currentEstimate.print("Current estimate: ");
-
-            imu.prop_state = imu.preintegrated->predict(imu.prev_state, imu.prev_bias);
-            imu.prev_state = NavState(currentEstimate.at<Pose3>(X(last_frame)), currentEstimate.at<Vector3>(V(last_frame)));
-            imu.prev_bias = currentEstimate.at<imuBias::ConstantBias>(B(last_frame));
-            imu.preintegrated->resetIntegrationAndSetBias(imu.prev_bias);
-
-            graph.resize(0);
-            initialEstimate.clear();
-
-            if (in.eof()) {
-                break;
-            }
-        }
-
-        if (type == 'i') {
-            double ax, ay, az;
-            double gx, gy, gz;
-            double dt = 1/800.0;
-
-            ss >> ax;
-            ss >> ay;
-            ss >> az;
-
-            ss >> gx;
-            ss >> gy;
-            ss >> gz;
-
-            Vector3 acc(ax, ay, az);
-            Vector3 gyr(gx, gy, gz);
-
-            imu.preintegrated->integrateMeasurement(acc, gyr, dt);
-        } else if (type == 's') {
-            int landmark;
-            double xl, xr, y;
-
-            ss >> landmark;
-            ss >> xl;
-            ss >> xr;
-            ss >> y;
-
-            if (smartFactors.count(landmark) == 0) {
-                auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
-
-                SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
-
-                smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(new SmartStereoProjectionPoseFactor(gaussian, params));
-                graph.push_back(smartFactors[landmark]);
-            }
-
-            smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
-        }
-
-        last_frame = frame;
-    }
-
-    return 0;
-}

From 8b8947b95c5f3721069036fc831f13f1eddb13e0 Mon Sep 17 00:00:00 2001
From: Nghia Ho <nghia@prenav.com>
Date: Fri, 14 Dec 2018 12:15:14 -0800
Subject: [PATCH 072/281] fixed typo

---
 examples/ISAM2_SmartFactorStereo_IMU.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp
index 25675abf8..43be2bd4d 100644
--- a/examples/ISAM2_SmartFactorStereo_IMU.cpp
+++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp
@@ -29,7 +29,6 @@ struct IMUHelper {
       auto gaussian = noiseModel::Diagonal::Sigmas(
           (Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3))
               .finished());
-
       auto huber = noiseModel::Robust::Create(
           noiseModel::mEstimator::Huber::Create(1.345), gaussian);
 
@@ -62,7 +61,7 @@ struct IMUHelper {
     // body to IMU rotation
     Rot3 iRb(
        0.036129, -0.998727,  0.035207,
-       0.045417, -0.033553, -0.99840,4
+       0.045417, -0.033553, -0.998404,
        0.998315,  0.037670,  0.044147);
 
     // body to IMU translation (meters)

From 4da1b7189d78708d62fae91de46b674eb002da3b Mon Sep 17 00:00:00 2001
From: Nghia Ho <nghia@prenav.com>
Date: Fri, 14 Dec 2018 12:16:19 -0800
Subject: [PATCH 073/281] change help output text

---
 examples/ISAM2_SmartFactorStereo_IMU.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp
index 43be2bd4d..e376cfcff 100644
--- a/examples/ISAM2_SmartFactorStereo_IMU.cpp
+++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp
@@ -96,7 +96,7 @@ struct IMUHelper {
 
 int main(int argc, char* argv[]) {
   if (argc != 2) {
-    cout << "./main [data.txt]\n";
+    cout << "./ISAM2_SmartFactorStereo_IMU [data.txt]\n";
     return 0;
   }
 

From e8da58725f6767e4c41656e6521c6a39f633ab0d Mon Sep 17 00:00:00 2001
From: Nghia Ho <nghia@prenav.com>
Date: Fri, 14 Dec 2018 12:18:43 -0800
Subject: [PATCH 074/281] formatting

---
 examples/ISAM2_SmartFactorStereo_IMU.cpp | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp
index e376cfcff..968f1edc7 100644
--- a/examples/ISAM2_SmartFactorStereo_IMU.cpp
+++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp
@@ -59,10 +59,9 @@ struct IMUHelper {
     p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
 
     // body to IMU rotation
-    Rot3 iRb(
-       0.036129, -0.998727,  0.035207,
-       0.045417, -0.033553, -0.998404,
-       0.998315,  0.037670,  0.044147);
+    Rot3 iRb(0.036129, -0.998727, 0.035207,
+             0.045417, -0.033553, -0.998404,
+             0.998315, 0.037670, 0.044147);
 
     // body to IMU translation (meters)
     Point3 iTb(0.03, -0.025, -0.06);
@@ -112,7 +111,7 @@ int main(int argc, char* argv[]) {
   double fy = 822.37;
   double cx = 538.73;
   double cy = 579.10;
-  double baseline = 0.372; // meters
+  double baseline = 0.372;  // meters
 
   Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
 

From f21a431427e076192120400b09563cc1c5339cbe Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Fri, 14 Dec 2018 14:53:03 -0800
Subject: [PATCH 075/281] Make gtsam_unstable/partition compile again, and
 actually build when GTSAM_SUPPORT_NESTED_DISSECTION=ON

---
 gtsam_unstable/CMakeLists.txt                        | 8 ++++----
 gtsam_unstable/partition/CMakeLists.txt              | 2 +-
 gtsam_unstable/partition/FindSeparator-inl.h         | 2 +-
 gtsam_unstable/partition/GenericGraph.cpp            | 9 ++++-----
 gtsam_unstable/partition/GenericGraph.h              | 1 +
 gtsam_unstable/partition/tests/testFindSeparator.cpp | 8 +++++---
 package_scripts/toolbox_package_unix.sh              | 2 +-
 7 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 448e8b00e..9ce9d9a22 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -10,9 +10,9 @@ set (gtsam_unstable_subdirs
     slam
 )
 
-if(GTSAM_BUILD_METIS) # Only build partition if metis is built
+if(GTSAM_SUPPORT_NESTED_DISSECTION) # Only build partition if metis is built
     set (gtsam_unstable_subdirs ${gtsam_unstable_subdirs} partition)
-endif(GTSAM_BUILD_METIS)
+endif(GTSAM_SUPPORT_NESTED_DISSECTION)
 
 set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES})
 
@@ -55,9 +55,9 @@ set(gtsam_unstable_srcs
     ${slam_srcs}
 )
 
-if(GTSAM_BUILD_METIS) # Only build partition if metis is built
+if(GTSAM_SUPPORT_NESTED_DISSECTION) # Only build partition if metis is built
     set (gtsam_unstable_srcs ${gtsam_unstable_srcs} ${partition_srcs})
-endif(GTSAM_BUILD_METIS)
+endif(GTSAM_SUPPORT_NESTED_DISSECTION)
 
 # Versions - same as core gtsam library
 set(gtsam_unstable_version   ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH})
diff --git a/gtsam_unstable/partition/CMakeLists.txt b/gtsam_unstable/partition/CMakeLists.txt
index fa5c336c3..74951bf93 100644
--- a/gtsam_unstable/partition/CMakeLists.txt
+++ b/gtsam_unstable/partition/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Install headers
 file(GLOB partition_headers "*.h")
-install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/parition)
+install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/partition)
 
 add_subdirectory(tests)
diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h
index a65e3f1ca..15411264f 100644
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@@ -474,7 +474,7 @@ namespace gtsam { namespace partition {
 
     if (!result.is_initialized()) {
       std::cout << "metis failed!" << std::endl;
-      return 0;
+      return boost::none;
     }
 
     if (reduceGraph) {
diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp
index 3ac77c213..7200f6168 100644
--- a/gtsam_unstable/partition/GenericGraph.cpp
+++ b/gtsam_unstable/partition/GenericGraph.cpp
@@ -6,7 +6,6 @@
  *  Description: generic graph types used in partitioning
  */
 #include <iostream>
-#include <boost/tuple/tuple.hpp>
 #include <boost/make_shared.hpp>
 #include <boost/lexical_cast.hpp>
 
@@ -105,9 +104,8 @@ namespace gtsam { namespace partition {
 
     list<vector<size_t> > islands;
     map<size_t, vector<size_t> > arrays = dsf.arrays();
-    size_t key; vector<size_t> array;
-    for(boost::tie(key, array): arrays)
-      islands.push_back(array);
+    for(const auto& kv : arrays)
+      islands.push_back(kv.second);
     return islands;
   }
 
@@ -305,7 +303,8 @@ namespace gtsam { namespace partition {
     // regenerating islands
     map<size_t, vector<size_t> > labelIslands = dsf.arrays();
     size_t label; vector<size_t> island;
-    for(boost::tie(label, island): labelIslands) {
+    for(const auto& li: labelIslands) {
+      tie(label, island) = li;
       vector<size_t> filteredIsland; // remove singular cameras from array
       filteredIsland.reserve(island.size());
       for(const size_t key: island) {
diff --git a/gtsam_unstable/partition/GenericGraph.h b/gtsam_unstable/partition/GenericGraph.h
index 803a79719..ec0027635 100644
--- a/gtsam_unstable/partition/GenericGraph.h
+++ b/gtsam_unstable/partition/GenericGraph.h
@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include <set>
 #include <list>
 #include <vector>
 #include <stdexcept>
diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp
index 8c8f12d74..9706e4cc4 100644
--- a/gtsam_unstable/partition/tests/testFindSeparator.cpp
+++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp
@@ -179,9 +179,11 @@ TEST ( Partition, findSeparator2 )
 }
 
 /* *************************************************************************/
-//  l1-l8   l9-l16    l17-l24
-//   / |     /    \    |  \
-// x25 x26             x27 x28
+/**
+ *  l1-l8   l9-l16    l17-l24
+ *   / |     /    \    |  \
+ * x25 x26             x27 x28
+ */
 TEST ( Partition, findSeparator3_with_reduced_camera )
 {
   GenericGraph3D graph;
diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh
index bb5845a3c..28de2572a 100755
--- a/package_scripts/toolbox_package_unix.sh
+++ b/package_scripts/toolbox_package_unix.sh
@@ -42,7 +42,7 @@ cmake -DCMAKE_BUILD_TYPE=Release \
 -DGTSAM_BUILD_TIMING:BOOL=OFF \
 -DGTSAM_BUILD_EXAMPLES_ALWAYS:BOOL=OFF \
 -DGTSAM_WITH_TBB:BOOL=OFF \
--DGTSAM_BUILD_METIS:BOOL=OFF \
+-DGTSAM_SUPPORT_NESTED_DISSECTION:BOOL=OFF \
 -DGTSAM_INSTALL_GEOGRAPHICLIB:BOOL=OFF \
 -DGTSAM_BUILD_UNSTABLE:BOOL=OFF \
 -DGTSAM_MEX_BUILD_STATIC_MODULE:BOOL=ON ..

From 7b1bd997f481ed5b198b76023128262e6dbe84c7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 15 Dec 2018 14:33:18 -0500
Subject: [PATCH 076/281] Fixed comments about order of things in covariance

---
 gtsam/navigation/CombinedImuFactor.h | 2 +-
 gtsam/navigation/ImuFactor.h         | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 7ca7fe463..a7802120c 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -111,7 +111,7 @@ public:
 
  protected:
   /* Covariance matrix of the preintegrated measurements
-   * COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
+   * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
    * (first-order propagation from *measurementCovariance*).
    * PreintegratedCombinedMeasurements also include the biases and keep the correlation
    * between the preintegrated measurements and the biases
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index ee1369f6b..8e3f8f0a4 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -75,7 +75,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
 
 protected:
 
-  Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
+  Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY]
   ///< (first-order propagation from *measurementCovariance*).
 
 public:

From 7371097d6d8e6b88ab03e044e2f9f780c2c3cbe3 Mon Sep 17 00:00:00 2001
From: chrisbeall <chrisbeall@gmail.com>
Date: Sat, 15 Dec 2018 15:16:49 -0800
Subject: [PATCH 077/281] Make FindSeparator-inl.h compile with recent versions
 of boost

---
 gtsam_unstable/partition/FindSeparator-inl.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h
index 15411264f..0e22edf0f 100644
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@@ -284,7 +284,7 @@ namespace gtsam { namespace partition {
       throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!");
     }
 
-    return boost::make_optional<MetisResult >(result);
+    return result;
   }
 
   /* *************************************************************************/ 
@@ -359,7 +359,7 @@ namespace gtsam { namespace partition {
       std::cout << "edgeCut: " << edgeCut << std::endl;
     }
 
-    return boost::make_optional<MetisResult >(result);
+    return result;
   }
 
   /* ************************************************************************* */

From e58ba2d2afb5de8d6a677dba01a0a337f1ce44af Mon Sep 17 00:00:00 2001
From: AndreiCostinescu <andrei.costinescu@yahoo.com>
Date: Sun, 16 Dec 2018 17:47:00 -0500
Subject: [PATCH 078/281] Fixed possibly uninitialized warnings in
 ImuFactorExample2.cpp

---
 examples/ImuFactorExample2.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp
index b4ad5d574..25a6adf51 100644
--- a/examples/ImuFactorExample2.cpp
+++ b/examples/ImuFactorExample2.cpp
@@ -108,9 +108,9 @@ int main(int argc, char* argv[]) {
         initialEstimate.insert(biasKey, imuBias::ConstantBias());
       }
       // Predict acceleration and gyro measurements in (actual) body frame
-      auto measuredAcc = scenario.acceleration_b(t) -
-                         scenario.rotation(t).transpose() * params->n_gravity;
-      auto measuredOmega = scenario.omega_b(t);
+      Vector3 measuredAcc = scenario.acceleration_b(t) -
+                            scenario.rotation(t).transpose() * params->n_gravity;
+      Vector3 measuredOmega = scenario.omega_b(t);
       accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
 
       // Add Imu Factor

From 63d7d7c54bfd6319e8dcfaa43186c31924aedf29 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Wed, 28 Nov 2018 16:01:54 -0800
Subject: [PATCH 079/281] Attempt to fix GenericValue assignment operator for
 windows

---
 gtsam/base/GenericValue.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 67cc2646e..52899fe45 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -164,11 +164,11 @@ public:
 
   protected:
 
-    // implicit assignment operator for (const GenericValue& rhs) works fine here
     /// Assignment operator, protected because only the Value or DERIVED
     /// assignment operators should be used.
     GenericValue<T>& operator=(const GenericValue<T>& rhs) {
-      // Nothing to do, do not call base class assignment operator
+      Value::operator=(static_cast<Value const&>(rhs));
+      value_ = rhs.value_;
       return *this;
     }
 

From ab0fb0d4a50dc43f212a32efe54c6ee4242f7a47 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 18 Dec 2018 13:56:38 -0500
Subject: [PATCH 080/281] Added iSAM2 test to check issue #412

---
 tests/testVisualISAM2.cpp | 125 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 125 insertions(+)
 create mode 100644 tests/testVisualISAM2.cpp

diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp
new file mode 100644
index 000000000..cd04c4b31
--- /dev/null
+++ b/tests/testVisualISAM2.cpp
@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testVisualISAM2.cpp
+ * @brief   Test convergence of visualSLAM example.
+ * @author  Duy-Nguyen Ta
+ * @author  Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <examples/SFMdata.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(testVisualISAM2, all)
+{
+    Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+
+    auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+
+    vector<Point3> points = createPoints();
+
+    // Create the set of ground-truth poses
+    vector<Pose3> poses = createPoses();
+
+    // Set the parameters
+    ISAM2Params parameters;
+    parameters.relinearizeThreshold = 0.01;
+    parameters.relinearizeSkip = 1;
+    ISAM2 isam(parameters);
+
+    // Create a Factor Graph and Values to hold the new data
+    NonlinearFactorGraph graph;
+    Values initialEstimate;
+
+    // Loop over the poses, adding the observations to iSAM incrementally
+    for (size_t i = 0; i < poses.size(); ++i)
+    {
+        // Add factors for each landmark observation
+        for (size_t j = 0; j < points.size(); ++j)
+        {
+            SimpleCamera camera(poses[i], *K);
+            Point2 measurement = camera.project(points[j]);
+            graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
+                measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+        }
+
+        // Add an initial guess for the current pose
+        // Intentionally initialize the variables off from the ground truth
+        static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                Point3(0.05, -0.10, 0.20));
+        initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
+
+        // Treat first iteration as special case
+        if (i == 0)
+        {
+            // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
+            static auto kPosePrior = noiseModel::Diagonal::Sigmas(
+                (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+                    .finished());
+            graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
+                                                     kPosePrior);
+
+            // Add a prior on landmark l0
+            static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
+            graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0],
+                                                      kPointPrior);
+
+            // Add initial guesses to all observed landmarks
+            // Intentionally initialize the variables off from the ground truth
+            static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
+            for (size_t j = 0; j < points.size(); ++j)
+                initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
+        }
+        else
+        {
+            // Update iSAM with the new factors
+            isam.update(graph, initialEstimate);
+
+            // Optimize
+            Values currentEstimate = isam.calculateEstimate();
+
+            // reset for next iteration
+            graph.resize(0);
+            initialEstimate.clear();
+        }
+    } // for loop
+
+    auto result = isam.calculateEstimate();
+    EXPECT_LONGS_EQUAL(16, result.size());
+    for (size_t j = 0; j < points.size(); ++j)
+    {
+        Symbol key('l', j);
+        EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01));
+    }
+}
+
+/* ************************************************************************* */
+int main()
+{
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

From 77f3a9195c4eb990b0b58dd12fb492b2ede9b3da Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 18 Dec 2018 14:22:44 -0500
Subject: [PATCH 081/281] Added an iSAM2 convergence test.

---
 tests/testVisualISAM2.cpp | 127 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 127 insertions(+)
 create mode 100644 tests/testVisualISAM2.cpp

diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp
new file mode 100644
index 000000000..1ced2af23
--- /dev/null
+++ b/tests/testVisualISAM2.cpp
@@ -0,0 +1,127 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testVisualISAM2.cpp
+ * @brief   Test convergence of visualSLAM example.
+ * @author  Duy-Nguyen Ta
+ * @author  Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <examples/SFMdata.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(testVisualISAM2, all)
+{
+    Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+
+    auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+
+    // Create ground truth data
+    vector<Point3> points = createPoints();
+    vector<Pose3> poses = createPoses();
+
+    // Set the parameters
+    ISAM2Params parameters;
+    parameters.relinearizeThreshold = 0.01;
+    parameters.relinearizeSkip = 1;
+    ISAM2 isam(parameters);
+
+    // Create a Factor Graph and Values to hold the new data
+    NonlinearFactorGraph graph;
+    Values initialEstimate;
+
+    // Loop over the poses, adding the observations to iSAM incrementally
+    for (size_t i = 0; i < poses.size(); ++i)
+    {
+        // Add factors for each landmark observation
+        for (size_t j = 0; j < points.size(); ++j)
+        {
+            SimpleCamera camera(poses[i], *K);
+            Point2 measurement = camera.project(points[j]);
+            graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
+                measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+        }
+
+        // Add an initial guess for the current pose
+        // Intentionally initialize the variables off from the ground truth
+        static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                Point3(0.05, -0.10, 0.20));
+        initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
+
+        // Treat first iteration as special case
+        if (i == 0)
+        {
+            // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
+            static auto kPosePrior = noiseModel::Diagonal::Sigmas(
+                (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+                    .finished());
+            graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
+                                                     kPosePrior);
+
+            // Add a prior on landmark l0
+            static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
+            graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0],
+                                                      kPointPrior);
+
+            // Add initial guesses to all observed landmarks
+            // Intentionally initialize the variables off from the ground truth
+            static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
+            for (size_t j = 0; j < points.size(); ++j)
+                initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
+        }
+        else
+        {
+            // Update iSAM with the new factors
+            isam.update(graph, initialEstimate);
+
+            // Do an extra update to converge withing these 8 iterations
+            isam.update();
+
+            // Optimize
+            Values currentEstimate = isam.calculateEstimate();
+
+            // reset for next iteration
+            graph.resize(0);
+            initialEstimate.clear();
+        }
+    } // for loop
+
+    auto result = isam.calculateEstimate();
+    EXPECT_LONGS_EQUAL(16, result.size());
+    for (size_t j = 0; j < points.size(); ++j)
+    {
+        Symbol key('l', j);
+        EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01));
+    }
+}
+
+/* ************************************************************************* */
+int main()
+{
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

From 1468250a0d51d8e0c4d6f2a9b0c833c8082c8ee2 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 8 Nov 2018 22:09:05 -0500
Subject: [PATCH 082/281] Added ccache support as shown in Issue #390

---
 CMakeLists.txt | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d471e3fd6..fd35d9b18 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -193,6 +193,13 @@ endif()
 # Find Google perftools
 find_package(GooglePerfTools)
 
+###############################################################################
+# Support ccache, if installed
+find_program(CCACHE_FOUND ccache)  
+if(CCACHE_FOUND)  
+    set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)  
+    set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)  
+endif(CCACHE_FOUND)  
 
 ###############################################################################
 # Find MKL

From f17b12bbfd5f6ddd0f7c1f51de26612bb9cc9724 Mon Sep 17 00:00:00 2001
From: cbeall <cbeall@nuro.ai>
Date: Tue, 18 Dec 2018 15:03:33 -0800
Subject: [PATCH 083/281] Add cmake flag to toggle use of ccache. On by
 default.

---
 CMakeLists.txt | 31 +++++++++++++++++++++++++------
 1 file changed, 25 insertions(+), 6 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd35d9b18..5930742ea 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,6 @@
 
 project(GTSAM CXX C)
-cmake_minimum_required(VERSION 2.6)
+cmake_minimum_required(VERSION 2.8.4)
 
 # new feature to Cmake Version > 2.8.12
 # Mac ONLY. Define Relative Path on Mac OS
@@ -69,6 +69,9 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions depreca
 option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
+if(NOT MSVC AND NOT XCODE_VERSION)
+    option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
+endif()
 
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
@@ -195,11 +198,18 @@ find_package(GooglePerfTools)
 
 ###############################################################################
 # Support ccache, if installed
-find_program(CCACHE_FOUND ccache)  
-if(CCACHE_FOUND)  
-    set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)  
-    set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)  
-endif(CCACHE_FOUND)  
+if(NOT MSVC AND NOT XCODE_VERSION)
+	find_program(CCACHE_FOUND ccache)
+	if(CCACHE_FOUND)
+		if(GTSAM_BUILD_WITH_CCACHE)
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
+		else()
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
+		endif()
+	endif(CCACHE_FOUND)
+endif()
 
 ###############################################################################
 # Find MKL
@@ -529,6 +539,15 @@ else()
 endif()
 message(STATUS "  Default allocator              : ${GTSAM_DEFAULT_ALLOCATOR}")
 
+if(NOT MSVC AND NOT XCODE_VERSION)
+	if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
+		message(STATUS "  Build with ccache              : Yes")
+	elseif(CCACHE_FOUND)
+		message(STATUS "  Build with ccache              : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
+	else()
+		message(STATUS "  Build with ccache              : No")
+	endif()
+endif()
 
 message(STATUS "Packaging flags                                               ")
 message(STATUS "  CPack Source Generator         : ${CPACK_SOURCE_GENERATOR}")

From f4d1fec558b024f690dc28ef083e74fa5c6d0ea8 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Wed, 19 Dec 2018 11:08:52 -0800
Subject: [PATCH 084/281] Fix compiler error in GenericGraph when
 GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF

---
 gtsam_unstable/partition/GenericGraph.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp
index 7200f6168..f941407e9 100644
--- a/gtsam_unstable/partition/GenericGraph.cpp
+++ b/gtsam_unstable/partition/GenericGraph.cpp
@@ -47,15 +47,15 @@ namespace gtsam { namespace partition {
           toErase.push_back(itFactor);  nrFactors--; continue;
         }
 
-        size_t label1 = dsf.findSet(key1.index);
-        size_t label2 = dsf.findSet(key2.index);
+        size_t label1 = dsf.find(key1.index);
+        size_t label2 = dsf.find(key2.index);
         if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }
 
         // merge two trees if the connection is strong enough, otherwise cache it
         // an odometry factor always merges two islands
         if (key1.type == NODE_POSE_2D && key2.type  == NODE_POSE_2D) {
           toErase.push_back(itFactor); nrFactors--;
-          dsf.makeUnionInPlace(label1, label2);
+          dsf.merge(label1, label2);
           succeed = true;
           break;
         }
@@ -64,7 +64,7 @@ namespace gtsam { namespace partition {
         if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
             (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
           toErase.push_back(itFactor); nrFactors--;
-          dsf.makeUnionInPlace(label1, label2);
+          dsf.merge(label1, label2);
           succeed = true;
           break;
         }
@@ -87,7 +87,7 @@ namespace gtsam { namespace partition {
           } else {
             toErase.push_back(itFactor); nrFactors--;
             toErase.push_back(itCached->second); nrFactors--;
-            dsf.makeUnionInPlace(label1, label2);
+            dsf.merge(label1, label2);
             connections.erase(itCached);
             succeed = true;
             break;
@@ -150,8 +150,8 @@ namespace gtsam { namespace partition {
         }
 
         if (graph.size() == 178765) cout << "kai22" << endl;
-        size_t label1 = dsf.findSet(key1.index);
-        size_t label2 = dsf.findSet(key2.index);
+        size_t label1 = dsf.find(key1.index);
+        size_t label2 = dsf.find(key2.index);
         if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }
 
         if (graph.size() == 178765) cout << "kai23" << endl;
@@ -160,7 +160,7 @@ namespace gtsam { namespace partition {
         if ((key1.type == NODE_POSE_3D && key2.type  == NODE_LANDMARK_3D) ||
             (key1.type == NODE_POSE_3D && key2.type  == NODE_POSE_3D)) {
           toErase.push_back(itFactor); nrFactors--;
-          dsf.makeUnionInPlace(label1, label2);
+          dsf.merge(label1, label2);
           succeed = true;
           break;
         }

From 88bfbceb213d4be1a72c3abefeedd3f6e4484ec3 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Fri, 21 Dec 2018 13:04:15 +0100
Subject: [PATCH 085/281] Use standard BUILD_SHARED_LIBS

Instead of custom GTSAM_BUILD_STATIC_LIBRARY
---
 CMakeLists.txt       | 10 +++----
 cmake/dllexport.h.in |  7 +++--
 gtsam/CMakeLists.txt | 64 ++++++++++++++++++++++----------------------
 3 files changed, 42 insertions(+), 39 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d471e3fd6..3c399316e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -55,7 +55,7 @@ endif()
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
 endif()
-option(GTSAM_BUILD_STATIC_LIBRARY        "Build a static gtsam library, instead of shared" OFF)
+option(BUILD_SHARED_LIBS                 "Build shared gtsam library, instead of static" ON)
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
 option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
@@ -84,8 +84,8 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
 	message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
 endif()
 
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
-	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled.  The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
+if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
+	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
 endif()
 
 if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
@@ -270,7 +270,7 @@ else()
 endif()
 
 if (MSVC)
-	if (NOT GTSAM_BUILD_STATIC_LIBRARY)
+	if (BUILD_SHARED_LIBS)
 		# mute eigen static assert to avoid errors in shared lib
 		add_definitions(-DEIGEN_NO_STATIC_ASSERT)
 	endif()
@@ -479,7 +479,7 @@ print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS}         "Build timing scripts wit
 if (DOXYGEN_FOUND)
     print_config_flag(${GTSAM_BUILD_DOCS}              "Build Docs                     ")
 endif()
-print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY}        "Build static GTSAM library instead of shared")
+print_config_flag(${BUILD_SHARED_LIBS}                 "Build shared GTSAM libraries   ")
 print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build type in library name ")
 if(GTSAM_UNSTABLE_AVAILABLE)
     print_config_flag(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable        ")
diff --git a/cmake/dllexport.h.in b/cmake/dllexport.h.in
index 55683a496..9a0a344b7 100644
--- a/cmake/dllexport.h.in
+++ b/cmake/dllexport.h.in
@@ -31,10 +31,10 @@
 
 // Whether GTSAM is compiled as static or DLL in windows. 
 // This will be used to decide whether include __declspec(dllimport) or not in headers
-#cmakedefine GTSAM_BUILD_STATIC_LIBRARY
+#cmakedefine BUILD_SHARED_LIBS
 
 #ifdef _WIN32
-#  ifdef @library_name@_BUILD_STATIC_LIBRARY
+#  ifndef BUILD_SHARED_LIBS
 #    define @library_name@_EXPORT
 #    define @library_name@_EXTERN_EXPORT extern
 #  else
@@ -50,3 +50,6 @@
 #  define @library_name@_EXPORT
 #  define @library_name@_EXTERN_EXPORT extern
 #endif
+
+#undef BUILD_SHARED_LIBS
+
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index a6814a422..09c95fd12 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -97,48 +97,48 @@ message(STATUS "GTSAM Version: ${gtsam_version}")
 message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
 
 # build shared and static versions of the library
-if (GTSAM_BUILD_STATIC_LIBRARY)
-    message(STATUS "Building GTSAM - static")
-    add_library(gtsam STATIC ${gtsam_srcs})
-	target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
-    set_target_properties(gtsam PROPERTIES
-        OUTPUT_NAME         gtsam
-        CLEAN_DIRECT_OUTPUT 1
-    	VERSION             ${gtsam_version}
-     	SOVERSION           ${gtsam_soversion})
-	if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
+
+# BUILD_SHARED_LIBS automatically defines static/shared libs:
+add_library(gtsam ${gtsam_srcs})
+target_link_libraries(gtsam
+    PUBLIC
+    ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
+set_target_properties(gtsam PROPERTIES
+    OUTPUT_NAME         gtsam
+    CLEAN_DIRECT_OUTPUT 1
+    VERSION             ${gtsam_version}
+    SOVERSION           ${gtsam_soversion})
+
+if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+	if (NOT BUILD_SHARED_LIBS)
 		set_target_properties(gtsam PROPERTIES
 			PREFIX "lib"
 			COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
-	endif()
-    install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
-    list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
-    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-else()
-    message(STATUS "Building GTSAM - shared")
-    add_library(gtsam SHARED ${gtsam_srcs})
-    target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
-    set_target_properties(gtsam PROPERTIES
-        OUTPUT_NAME         gtsam
-        CLEAN_DIRECT_OUTPUT 1
-    	VERSION             ${gtsam_version}
-     	SOVERSION           ${gtsam_soversion})
-	if(WIN32)
+	else()
 		set_target_properties(gtsam PROPERTIES
 			PREFIX ""
 			DEFINE_SYMBOL GTSAM_EXPORTS
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
 	endif()
-	if (APPLE)
-		set_target_properties(gtsam PROPERTIES
-			INSTALL_NAME_DIR
-			"${CMAKE_INSTALL_PREFIX}/lib")
-	endif()
-    install(TARGETS gtsam EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-    list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
-    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 endif()
 
+if (APPLE AND BUILD_SHARED_LIBS)
+	set_target_properties(gtsam PROPERTIES
+		INSTALL_NAME_DIR
+		"${CMAKE_INSTALL_PREFIX}/lib")
+endif()
+
+install(
+	TARGETS gtsam
+	EXPORT GTSAM-exports
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	RUNTIME DESTINATION bin)
+
+list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+
 # make sure that ccolamd compiles even in face of warnings
 if(WIN32)
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")

From 617040f503d8f09f73e6c60f13da8e0b75021d52 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 22 Dec 2018 17:09:14 -0500
Subject: [PATCH 086/281] count method (better than filter, then size).

---
 gtsam/nonlinear/Values.h             | 11 ++++++++
 gtsam/nonlinear/tests/testValues.cpp |  1 +
 gtsam/slam/dataset.cpp               | 39 ++++++++++++++--------------
 3 files changed, 31 insertions(+), 20 deletions(-)

diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index bf17f1f0d..676d30787 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -385,6 +385,17 @@ namespace gtsam {
     ConstFiltered<ValueType>
     filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
 
+    // Count values of given type \c ValueType
+    template<class ValueType>
+    bool count() const {
+      size_t i = 0;
+      for (const auto& key_value : *this) {
+        if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
+          ++i;
+      }
+      return i;
+    }
+
   private:
     // Filters based on ValueType (if not Value) and also based on the user-
     // supplied \c filter function.
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index bcf01eff5..0dee52570 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -383,6 +383,7 @@ TEST(Values, filter) {
     ++ i;
   }
   EXPECT_LONGS_EQUAL(2, (long)i);
+  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose3>());
 
   // construct a values with the view
   Values actualSubValues2(pose_filtered);
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index ba51864f1..4f52f3c40 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -432,14 +432,14 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   fstream stream(filename.c_str(), fstream::out);
 
   // save 2D & 3D poses
-  Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
-  for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2) {
+  const auto viewPose2 = estimate.filter<Pose2>();
+  for(const auto& key_value: viewPose2) {
     stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
         << key_value.value.y() << " " << key_value.value.theta() << endl;
   }
 
-  Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
-  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: viewPose3) {
+  const auto viewPose3 = estimate.filter<Pose3>();
+  for(const auto& key_value: viewPose3) {
       Point3 p = key_value.value.translation();
       Rot3 R = key_value.value.rotation();
       stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
@@ -448,7 +448,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   }
 
   // save edges (2D or 3D)
-  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
+  for(const auto& factor_: graph) {
     boost::shared_ptr<BetweenFactor<Pose2> > factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
     if (factor){
@@ -857,48 +857,47 @@ bool writeBAL(const string& filename, SfM_data &data) {
 
 bool writeBALfromValues(const string& filename, const SfM_data &data,
     Values& values) {
-
+  using Camera = PinholeCamera<Cal3Bundler>;
   SfM_data dataValues = data;
 
   // Store poses or cameras in SfM_data
-  Values valuesPoses = values.filter<Pose3>();
-  if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses
+  size_t nrPoses = values.count<Pose3>();
+  if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
     for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
       Key poseKey = symbol('x', i);
       Pose3 pose = values.at<Pose3>(poseKey);
       Cal3Bundler K = dataValues.cameras[i].calibration();
-      PinholeCamera<Cal3Bundler> camera(pose, K);
+      Camera camera(pose, K);
       dataValues.cameras[i] = camera;
     }
   } else {
-    Values valuesCameras = values.filter<PinholeCamera<Cal3Bundler> >();
-    if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
-      for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
+    size_t nrCameras = values.count<Camera>();
+    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration
+      for (size_t i = 0; i < nrCameras; i++) { // for each camera
         Key cameraKey = i; // symbol('c',i);
-        PinholeCamera<Cal3Bundler> camera =
-            values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
+        Camera camera = values.at<Camera>(cameraKey);
         dataValues.cameras[i] = camera;
       }
     } else {
       cout
           << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
           << dataValues.number_cameras() << ") and values (#cameras "
-          << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
+          << nrPoses << ", #poses " << nrCameras << ")!!"
           << endl;
       return false;
     }
   }
 
   // Store 3D points in SfM_data
-  Values valuesPoints = values.filter<Point3>();
-  if (valuesPoints.size() != dataValues.number_tracks()) {
+  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
+  if (nrPoints != nrTracks) {
     cout
         << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
-        << dataValues.number_tracks() << ") and values (#points "
-        << valuesPoints.size() << ")!!" << endl;
+        << nrTracks << ") and values (#points "
+        << nrPoints << ")!!" << endl;
   }
 
-  for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point
+  for (size_t j = 0; j < nrTracks; j++) { // for each point
     Key pointKey = P(j);
     if (values.exists(pointKey)) {
       Point3 point = values.at<Point3>(pointKey);

From 94010aee9d4507ee76ee86c0e6464c32f4811c1a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 22 Dec 2018 17:10:25 -0500
Subject: [PATCH 087/281] Removed redundant (and troublesome on Windows)
 assignment.

---
 gtsam/base/GenericValue.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 52899fe45..1ee1a27ba 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -168,7 +168,6 @@ public:
     /// assignment operators should be used.
     GenericValue<T>& operator=(const GenericValue<T>& rhs) {
       Value::operator=(static_cast<Value const&>(rhs));
-      value_ = rhs.value_;
       return *this;
     }
 

From e2363e90bd8cdec9d2ef052a23594040502069b1 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 22 Dec 2018 18:07:00 -0500
Subject: [PATCH 088/281] Fixed issue with 'count'

---
 gtsam/nonlinear/Values.h             | 2 +-
 gtsam/nonlinear/tests/testValues.cpp | 1 +
 gtsam/slam/dataset.cpp               | 2 +-
 3 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 676d30787..16f8eba16 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -387,7 +387,7 @@ namespace gtsam {
 
     // Count values of given type \c ValueType
     template<class ValueType>
-    bool count() const {
+    size_t count() const {
       size_t i = 0;
       for (const auto& key_value : *this) {
         if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index 0dee52570..b3c557b32 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -384,6 +384,7 @@ TEST(Values, filter) {
   }
   EXPECT_LONGS_EQUAL(2, (long)i);
   EXPECT_LONGS_EQUAL(2, (long)values.count<Pose3>());
+  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose2>());
 
   // construct a values with the view
   Values actualSubValues2(pose_filtered);
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 4f52f3c40..fbb198265 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -880,7 +880,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data,
       }
     } else {
       cout
-          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
+          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras "
           << dataValues.number_cameras() << ") and values (#cameras "
           << nrPoses << ", #poses " << nrCameras << ")!!"
           << endl;

From 70e2534cc283e4ced31aa0af276fcd59adaa8a8d Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 22 Dec 2018 18:07:22 -0500
Subject: [PATCH 089/281] Restored assignment

---
 gtsam/base/GenericValue.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 1ee1a27ba..52899fe45 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -168,6 +168,7 @@ public:
     /// assignment operators should be used.
     GenericValue<T>& operator=(const GenericValue<T>& rhs) {
       Value::operator=(static_cast<Value const&>(rhs));
+      value_ = rhs.value_;
       return *this;
     }
 

From 6d0a76aec980ccad2bd5be535466857723a0a8c8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 29 Dec 2018 15:11:24 -0500
Subject: [PATCH 090/281] Got rid of filter because of compile issues on
 Windows

---
 gtsam/slam/dataset.cpp | 22 +++++++++++++---------
 1 file changed, 13 insertions(+), 9 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index fbb198265..6efd01feb 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -432,17 +432,21 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   fstream stream(filename.c_str(), fstream::out);
 
   // save 2D & 3D poses
-  const auto viewPose2 = estimate.filter<Pose2>();
-  for(const auto& key_value: viewPose2) {
-    stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
-        << key_value.value.y() << " " << key_value.value.theta() << endl;
+  for (const auto& key_value : estimate) {
+    auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
+    if (!p) continue;
+    const Pose2& pose = p->value();
+    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
+        << pose.y() << " " << pose.theta() << endl;
   }
 
-  const auto viewPose3 = estimate.filter<Pose3>();
-  for(const auto& key_value: viewPose3) {
-      Point3 p = key_value.value.translation();
-      Rot3 R = key_value.value.rotation();
-      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
+  for(const auto& key_value: estimate) {
+      auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+      if (!p) continue;
+      const Pose3& pose = p->value();
+      Point3 t = pose.translation();
+      Rot3 R = pose.rotation();
+      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "  << t.y() << " " << t.z()
         << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
         << " " << R.toQuaternion().w() << endl;
   }

From 2166dc23fe300ff07d7b258a6399c35f419e6a00 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 30 Dec 2018 13:01:43 -0500
Subject: [PATCH 091/281] Added virtual destructors to avoid warnings (on Mac)
 and fixed some lint warnings.

---
 gtsam/navigation/PreintegrationBase.h | 47 +++++++++++++++++----------
 gtsam/navigation/Scenario.h           | 11 ++++---
 2 files changed, 36 insertions(+), 22 deletions(-)

diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 3c22a1d00..e0792f873 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -27,6 +27,8 @@
 #include <gtsam/linear/NoiseModel.h>
 
 #include <iosfwd>
+#include <string>
+#include <utility>
 
 namespace gtsam {
 
@@ -61,7 +63,6 @@ class GTSAM_EXPORT PreintegrationBase {
   typedef PreintegrationParams Params;
 
  protected:
-
   /// Parameters. Declared mutable only for deprecated predict method.
   /// TODO(frank): make const once deprecated method is removed
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@@ -78,7 +79,10 @@ class GTSAM_EXPORT PreintegrationBase {
   /// Default constructor for serialization
   PreintegrationBase() {}
 
-public:
+  /// Virtual destructor for serialization
+  virtual ~PreintegrationBase() {}
+
+ public:
   /// @name Constructors
   /// @{
 
@@ -95,7 +99,7 @@ public:
   /// @name Basic utilities
   /// @{
   /// Re-initialize PreintegratedMeasurements
-  virtual void resetIntegration()=0;
+  virtual void resetIntegration() = 0;
 
   /// @name Basic utilities
   /// @{
@@ -129,10 +133,10 @@ public:
   const imuBias::ConstantBias& biasHat() const { return biasHat_; }
   double deltaTij() const { return deltaTij_; }
 
-  virtual Vector3  deltaPij() const=0;
-  virtual Vector3  deltaVij() const=0;
-  virtual Rot3     deltaRij() const=0;
-  virtual NavState deltaXij() const=0;
+  virtual Vector3  deltaPij() const = 0;
+  virtual Vector3  deltaVij() const = 0;
+  virtual Rot3     deltaRij() const = 0;
+  virtual NavState deltaXij() const = 0;
 
   // Exposed for MATLAB
   Vector6 biasHatVector() const { return biasHat_.vector(); }
@@ -147,20 +151,24 @@ public:
   /// @name Main functionality
   /// @{
 
-  /// Subtract estimate and correct for sensor pose
-  /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
-  /// Ignore D_correctedOmega_measuredAcc as it is trivially zero
+  /** 
+   * Subtract estimate and correct for sensor pose
+   * Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
+   * Ignore D_correctedOmega_measuredAcc as it is trivially zero
+   */
   std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
       const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
       OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
       OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
       OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
 
-  /// Update preintegrated measurements and get derivatives
-  /// It takes measured quantities in the j frame
-  /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
+  /**
+   *  Update preintegrated measurements and get derivatives
+   * It takes measured quantities in the j frame
+   * Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
+   */
   virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
-      const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
+      const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0;
 
   /// Version without derivatives
   virtual void integrateMeasurement(const Vector3& measuredAcc,
@@ -169,7 +177,7 @@ public:
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
   virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
-      OptionalJacobian<9, 6> H = boost::none) const=0;
+      OptionalJacobian<9, 6> H = boost::none) const = 0;
 
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
@@ -182,7 +190,10 @@ public:
                        OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
                        OptionalJacobian<9, 6> H3) const;
 
-  /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
+  /** 
+   * Compute errors w.r.t. preintegrated measurements and jacobians 
+   * wrt pose_i, vel_i, bias_i, pose_j, bias_j
+   */
   Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
       const Pose3& pose_j, const Vector3& vel_j,
       const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
@@ -202,8 +213,8 @@ public:
   /// @}
 #endif
 
-public:
+ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-} /// namespace gtsam
+}  /// namespace gtsam
diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h
index acb8f46f5..fff7e7e50 100644
--- a/gtsam/navigation/Scenario.h
+++ b/gtsam/navigation/Scenario.h
@@ -24,12 +24,15 @@ namespace gtsam {
 /// Simple trajectory simulator.
 class Scenario {
  public:
+  /// virtual destructor
+  virtual ~Scenario() {}
+
   // Quantities a Scenario needs to specify:
 
-  virtual Pose3 pose(double t) const = 0;
-  virtual Vector3 omega_b(double t) const = 0;
-  virtual Vector3 velocity_n(double t) const = 0;
-  virtual Vector3 acceleration_n(double t) const = 0;
+  virtual Pose3 pose(double t) const = 0;  ///< pose at time t
+  virtual Vector3 omega_b(double t) const = 0;  ///< angular velocity in body frame
+  virtual Vector3 velocity_n(double t) const = 0;  ///< velocity at time t, in nav frame
+  virtual Vector3 acceleration_n(double t) const = 0;  ///< acceleration in nav frame
 
   // Derived quantities:
 

From 66959f8423cabd8be4ffc4b0fae2157f4056e773 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 30 Dec 2018 14:34:06 -0500
Subject: [PATCH 092/281] Added noise model to make test succeed

---
 matlab/gtsam_tests/testJacobianFactor.m | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/matlab/gtsam_tests/testJacobianFactor.m b/matlab/gtsam_tests/testJacobianFactor.m
index bba6ca5ac..1c214c3bc 100644
--- a/matlab/gtsam_tests/testJacobianFactor.m
+++ b/matlab/gtsam_tests/testJacobianFactor.m
@@ -54,7 +54,8 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13);
+unit2 = noiseModel.Unit.Create(2);
+expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,unit2);
 % check if the result matches
 CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4));
 

From 2c8f55a2d8ff6a54dae83490b63e6a251000de44 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 30 Dec 2018 14:34:55 -0500
Subject: [PATCH 093/281] Adding GenericValue for Values serialization, as
 suggested by Callum Robinson and Mike Sheffler in issue #398

---
 gtsam.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 24a717c3c..01b23e6cc 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -228,6 +228,12 @@ virtual class Value {
   size_t dim() const;
 };
 
+#include <gtsam/base/GenericValue.h>
+template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Pose2, gtsam::Pose3, gtsam::Rot2}>
+virtual class GenericValue : gtsam::Value {
+  void serializable() const;
+};
+
 #include <gtsam/base/deprecated/LieScalar.h>
 class LieScalar {
   // Standard constructors

From cdf2a6335b288e4282f74855039dfe5020e9e4a9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 30 Dec 2018 16:23:43 -0500
Subject: [PATCH 094/281] Added serialization for two more factors

---
 gtsam.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 01b23e6cc..d681ddb0d 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2265,6 +2265,9 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NoiseModelFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
@@ -2281,6 +2284,9 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactor
 template<POSE, POINT>
 virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
   RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransformPosePoint2;

From fe1daec086aa5b20966d4ab9615be137e665adc9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 30 Dec 2018 16:24:19 -0500
Subject: [PATCH 095/281] Changed include error as in
 http://boost.2283326.n4.nabble.com/boost-serialization-Serializing-Dynamically-Loaded-Libraries-quot-Unregistered-Void-Cast-quot-td2570981.html

---
 gtsam/base/serialization.h               | 2 +-
 wrap/Module.cpp                          | 4 ++--
 wrap/tests/expected/geometry_wrapper.cpp | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h
index ebd893ad1..954d3e86b 100644
--- a/gtsam/base/serialization.h
+++ b/gtsam/base/serialization.h
@@ -24,7 +24,6 @@
 #include <string>
 
 // includes for standard serialization types
-#include <boost/serialization/export.hpp>
 #include <boost/serialization/optional.hpp>
 #include <boost/serialization/shared_ptr.hpp>
 #include <boost/serialization/vector.hpp>
@@ -39,6 +38,7 @@
 #include <boost/archive/xml_oarchive.hpp>
 #include <boost/archive/binary_iarchive.hpp>
 #include <boost/archive/binary_oarchive.hpp>
+#include <boost/serialization/export.hpp>
 
 namespace gtsam {
 
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 9eee686cb..5cf3b5d6c 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -273,9 +273,9 @@ void Module::generate_matlab_wrapper(const string& toolboxPath) const {
 
   // Include boost.serialization archive headers before other class headers
   if (hasSerialiable) {
-    wrapperFile.oss << "#include <boost/serialization/export.hpp>\n";
     wrapperFile.oss << "#include <boost/archive/text_iarchive.hpp>\n";
-    wrapperFile.oss << "#include <boost/archive/text_oarchive.hpp>\n\n";
+    wrapperFile.oss << "#include <boost/archive/text_oarchive.hpp>\n";
+    wrapperFile.oss << "#include <boost/serialization/export.hpp>\n\n";
   }
 
   // Generate includes while avoiding redundant includes
diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp
index 7e0cb0e47..dec78b80c 100644
--- a/wrap/tests/expected/geometry_wrapper.cpp
+++ b/wrap/tests/expected/geometry_wrapper.cpp
@@ -1,9 +1,9 @@
 #include <wrap/matlab.h>
 #include <map>
 
-#include <boost/serialization/export.hpp>
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
+#include <boost/serialization/export.hpp>
 
 #include <folder/path/to/Test.h>
 #include <gtsam/geometry/Point2.h>

From 63acd1a50c9362190d351279c671d76cd9d82f80 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 30 Dec 2018 18:35:33 -0500
Subject: [PATCH 096/281] Add bearing and range factor tests

---
 matlab/gtsam_tests/testSerialization.m | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m
index 9f49328cd..baacb198b 100644
--- a/matlab/gtsam_tests/testSerialization.m
+++ b/matlab/gtsam_tests/testSerialization.m
@@ -52,14 +52,26 @@ priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 
-% Between Factors - FAIL: unregistered class
+% Between Factors
 odometry = Pose2(2.0, 0.0, 0.0);
 odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
  
-% BearingRange Factors - FAIL: unregistered class
+% Range Factors
+rNoise = noiseModel.Diagonal.Sigmas([0.2]);
+graph.add(RangeFactorPosePoint2(i1, j1, sqrt(4+4), rNoise));
+graph.add(RangeFactorPosePoint2(i2, j1, 2, rNoise));
+graph.add(RangeFactorPosePoint2(i3, j2, 2, rNoise));
+
+% Bearing Factors
 degrees = pi/180;
+bNoise = noiseModel.Diagonal.Sigmas([0.1]);
+graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise));
+graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise));
+graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise));
+
+% BearingRange Factors
 brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));

From 3b3e39381712d69f3252a27af6fa0da65acec68b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 30 Dec 2018 18:37:38 -0500
Subject: [PATCH 097/281] Add serialization functions that call base classes,
 to avoid "unregistered void cast" errors (in MATLAB, as flagged by issue
 #398), as mentioned in
 http://tb-nguyen.blogspot.com/2009/08/more-on-using-boost-serialization-and.html

---
 gtsam/nonlinear/ExpressionFactor.h | 7 +++++++
 gtsam/sam/BearingFactor.h          | 8 ++++++++
 gtsam/sam/BearingRangeFactor.h     | 8 ++++++++
 gtsam/sam/RangeFactor.h            | 8 ++++++++
 4 files changed, 31 insertions(+)

diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 34ba8e1ff..04d82fe9a 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -266,6 +266,13 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
   virtual Expression<T> expression() const {
     return expression(this->keys_[0], this->keys_[1]);
   }
+
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
+  }
 };
 // ExpressionFactor2
 
diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h
index f190e683c..a9ed5ef4b 100644
--- a/gtsam/sam/BearingFactor.h
+++ b/gtsam/sam/BearingFactor.h
@@ -60,6 +60,14 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
     std::cout << s << "BearingFactor" << std::endl;
     Base::print(s, kf);
   }
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
 };  // BearingFactor
 
 /// traits
diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h
index 2dd1fecb8..44740f8ff 100644
--- a/gtsam/sam/BearingRangeFactor.h
+++ b/gtsam/sam/BearingRangeFactor.h
@@ -73,6 +73,14 @@ class BearingRangeFactor
     Base::print(s, kf);
   }
 
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
 };  // BearingRangeFactor
 
 /// traits
diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h
index a5bcac822..40a9cf758 100644
--- a/gtsam/sam/RangeFactor.h
+++ b/gtsam/sam/RangeFactor.h
@@ -65,6 +65,14 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
     std::cout << s << "RangeFactor" << std::endl;
     Base::print(s, kf);
   }
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
 };  // \ RangeFactor
 
 /// traits

From e1e8de7cedf78b1a69dd459113db868a11fff7a7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 10:24:48 -0500
Subject: [PATCH 098/281] Fixed issue with GTSAM 4 deprecated retract

---
 matlab/gtsam_examples/SFMExample.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/matlab/gtsam_examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m
index 4115fa6e3..6700e90d2 100644
--- a/matlab/gtsam_examples/SFMExample.m
+++ b/matlab/gtsam_examples/SFMExample.m
@@ -58,7 +58,7 @@ for i=1:size(truth.cameras,2)
     initialEstimate.insert(symbol('x',i), pose_i);
 end
 for j=1:size(truth.points,2)
-    point_j = truth.points{j}.retract(0.1*randn(3,1));
+    point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
     initialEstimate.insert(symbol('p',j), point_j);
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));

From f54b078447284a7d436fdc9d8489dd85b81eaaf7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 11:06:32 -0500
Subject: [PATCH 099/281] Fixed retract for SBA

---
 matlab/gtsam_examples/SBAExample.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m
index b0f754044..7f50f2db8 100644
--- a/matlab/gtsam_examples/SBAExample.m
+++ b/matlab/gtsam_examples/SBAExample.m
@@ -64,7 +64,7 @@ for i=1:size(truth.cameras,2)
     initialEstimate.insert(symbol('c',i), camera_i);
 end
 for j=1:size(truth.points,2)
-    point_j = truth.points{j}.retract(0.1*randn(3,1));
+    point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
     initialEstimate.insert(symbol('p',j), point_j);
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));

From fbcfbf0cdddef17ccf9dfe26b236fa7f4ba71fdf Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 11:19:46 -0500
Subject: [PATCH 100/281] Made naming convention in wrapper uniform. 2D means
 Pose2 + Point2 3D means Pose3 + Point3

---
 gtsam.h                                       | 10 ++++---
 gtsam_unstable/slam/serialization.cpp         |  8 +++---
 gtsampy.h                                     |  4 +--
 matlab/+gtsam/Contents.m                      |  4 +--
 .../gtsam_examples/RangeISAMExample_plaza.m   |  6 ++--
 .../gtsam_examples/RangeSLAMExample_plaza.m   |  2 +-
 matlab/gtsam_tests/testSerialization.m        |  6 ++--
 .../SmartRangeFactorExample.m                 | 20 ++++++-------
 tests/testSerializationSLAM.cpp               | 28 +++++++++----------
 wrap/Module.cpp                               |  2 +-
 10 files changed, 46 insertions(+), 44 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index d681ddb0d..629fd70cf 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2270,8 +2270,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
-typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
+typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
@@ -2289,8 +2289,8 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransformPosePoint2;
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransformPosePoint3;
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransform2D;
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransform3D;
 typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Pose2> RangeFactorWithTransformPose2;
 typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Pose3> RangeFactorWithTransformPose3;
 
@@ -2304,6 +2304,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 };
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
+typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
 
 #include <gtsam/sam/BearingRangeFactor.h>
 template<POSE, POINT, BEARING, RANGE>
@@ -2317,6 +2318,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
 };
 
 typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double> BearingRangeFactorPose2;
 
 
 #include <gtsam/slam/ProjectionFactor.h>
diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp
index 7928a2aac..17c95e614 100644
--- a/gtsam_unstable/slam/serialization.cpp
+++ b/gtsam_unstable/slam/serialization.cpp
@@ -70,8 +70,8 @@ typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
 typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
 typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactorPosePoint2;
-typedef RangeFactor<Pose3, Point3>                      RangeFactorPosePoint3;
+typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
+typedef RangeFactor<Pose3, Point3>                      RangeFactor3D;
 typedef RangeFactor<Pose2, Pose2>                       RangeFactorPose2;
 typedef RangeFactor<Pose3, Pose3>                       RangeFactorPose3;
 typedef RangeFactor<CalibratedCamera, Point3>           RangeFactorCalibratedCameraPoint;
@@ -172,8 +172,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
 
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2");
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3");
+BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
+BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
diff --git a/gtsampy.h b/gtsampy.h
index 9cadf6be3..27af74e74 100644
--- a/gtsampy.h
+++ b/gtsampy.h
@@ -2258,8 +2258,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
 };
 
-typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
+typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m
index 023c61dbe..10fd5e142 100644
--- a/matlab/+gtsam/Contents.m
+++ b/matlab/+gtsam/Contents.m
@@ -142,8 +142,8 @@
 %   RangeFactorCalibratedCameraPoint  - class RangeFactorCalibratedCameraPoint, see Doxygen page for details
 %   RangeFactorPose2                  - class RangeFactorPose2, see Doxygen page for details
 %   RangeFactorPose3                  - class RangeFactorPose3, see Doxygen page for details
-%   RangeFactorPosePoint2             - class RangeFactorPosePoint2, see Doxygen page for details
-%   RangeFactorPosePoint3             - class RangeFactorPosePoint3, see Doxygen page for details
+%   RangeFactor2D                     - class RangeFactor2D, see Doxygen page for details
+%   RangeFactor3D                     - class RangeFactor3D, see Doxygen page for details
 %   RangeFactorSimpleCamera           - class RangeFactorSimpleCamera, see Doxygen page for details
 %   RangeFactorSimpleCameraPoint      - class RangeFactorSimpleCameraPoint, see Doxygen page for details
 %   VisualISAMGenerateData            - VisualISAMGenerateData creates data for viusalSLAM::iSAM examples
diff --git a/matlab/gtsam_examples/RangeISAMExample_plaza.m b/matlab/gtsam_examples/RangeISAMExample_plaza.m
index cffe81c30..e31fb18a8 100644
--- a/matlab/gtsam_examples/RangeISAMExample_plaza.m
+++ b/matlab/gtsam_examples/RangeISAMExample_plaza.m
@@ -125,7 +125,7 @@ for i=1:M % M
     j = TD(k,3);
     range = TD(k,4);
     if addRange
-      factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+      factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range);
       % Throw out obvious outliers based on current landmark estimates
       error=factor.unwhitenedError(landmarkEstimates);
       if k<=minK || abs(error)<5
@@ -146,14 +146,14 @@ for i=1:M % M
     end
     isam.update(newFactors, initial);
     result = isam.calculateEstimate();
-    lastPose = result.at(i);
+    lastPose = result.atPose2(i);
     % update landmark estimates
     if addRange
       landmarkEstimates = Values;
       for jj=1:size(TL,1)
         j=TL(jj,1);
         key = symbol('L',j);
-        landmarkEstimates.insert(key,result.at(key));
+        landmarkEstimates.insert(key,result.atPoint2(key));
       end
     end
     newFactors = NonlinearFactorGraph;
diff --git a/matlab/gtsam_examples/RangeSLAMExample_plaza.m b/matlab/gtsam_examples/RangeSLAMExample_plaza.m
index bd643d854..d2d1e9d70 100644
--- a/matlab/gtsam_examples/RangeSLAMExample_plaza.m
+++ b/matlab/gtsam_examples/RangeSLAMExample_plaza.m
@@ -76,7 +76,7 @@ for i=1:M
   while k<=K && t>=TD(k,1)
     j = TD(k,3);
     range = TD(k,4);
-    factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+    factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range);
     graph.add(factor);
     k=k+1;
   end
diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m
index baacb198b..f8b21b7ad 100644
--- a/matlab/gtsam_tests/testSerialization.m
+++ b/matlab/gtsam_tests/testSerialization.m
@@ -60,9 +60,9 @@ graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
  
 % Range Factors
 rNoise = noiseModel.Diagonal.Sigmas([0.2]);
-graph.add(RangeFactorPosePoint2(i1, j1, sqrt(4+4), rNoise));
-graph.add(RangeFactorPosePoint2(i2, j1, 2, rNoise));
-graph.add(RangeFactorPosePoint2(i3, j2, 2, rNoise));
+graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise));
+graph.add(RangeFactor2D(i2, j1, 2, rNoise));
+graph.add(RangeFactor2D(i3, j2, 2, rNoise));
 
 % Bearing Factors
 degrees = pi/180;
diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m
index 7535447df..a192a1f5e 100644
--- a/matlab/unstable_examples/SmartRangeFactorExample.m
+++ b/matlab/unstable_examples/SmartRangeFactorExample.m
@@ -103,9 +103,9 @@ for ind_pose = 2:7
       r2 = curr_pose.range(lmk1); % range of lmk1 wrt x2
       srf1.addRange(key_curr, r2);
       
-      rangef1 = RangeFactorPosePoint2(key_prev, lmkKey(1), r1, noiseRange);
+      rangef1 = RangeFactor2D(key_prev, lmkKey(1), r1, noiseRange);
       fullGraph.add(rangef1); 
-      rangef2 = RangeFactorPosePoint2(key_curr, lmkKey(1), r2, noiseRange);
+      rangef2 = RangeFactor2D(key_curr, lmkKey(1), r2, noiseRange);
       fullGraph.add(rangef2); 
       
       if goodInitFlag_lmk1==1
@@ -123,9 +123,9 @@ for ind_pose = 2:7
       r4 = curr_pose.range(lmk2); % range of lmk2 wrt x3
       srf2.addRange(key_curr, r4);    
       
-      rangef3 = RangeFactorPosePoint2(key_curr, lmkKey(1), r3, noiseRange);
+      rangef3 = RangeFactor2D(key_curr, lmkKey(1), r3, noiseRange);
       fullGraph.add(rangef3); 
-      rangef4 = RangeFactorPosePoint2(key_curr, lmkKey(2), r4, noiseRange);
+      rangef4 = RangeFactor2D(key_curr, lmkKey(2), r4, noiseRange);
       % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef4);  
     %====================================================================  
     case 4
@@ -138,9 +138,9 @@ for ind_pose = 2:7
       
       % DELAYED INITIALIZATION: 
       fullGraph.add(rangef4); 
-      rangef5 = RangeFactorPosePoint2(key_curr, lmkKey(2), r5, noiseRange);
+      rangef5 = RangeFactor2D(key_curr, lmkKey(2), r5, noiseRange);
       fullGraph.add(rangef5); 
-      rangef6 = RangeFactorPosePoint2(key_curr, lmkKey(3), r6, noiseRange);
+      rangef6 = RangeFactor2D(key_curr, lmkKey(3), r6, noiseRange);
       % IF WE ADD FACTOR HERE IT CRASHES:  fullGraph.add(rangef6); 
       
       if goodInitFlag_lmk2==1
@@ -160,9 +160,9 @@ for ind_pose = 2:7
 
       % DELAYED INITIALIZATION: 
       fullGraph.add(rangef6); 
-      rangef7 = RangeFactorPosePoint2(key_curr, lmkKey(2), r7, noiseRange);
+      rangef7 = RangeFactor2D(key_curr, lmkKey(2), r7, noiseRange);
       fullGraph.add(rangef7); 
-      rangef8 = RangeFactorPosePoint2(key_curr, lmkKey(3), r8, noiseRange);
+      rangef8 = RangeFactor2D(key_curr, lmkKey(3), r8, noiseRange);
       fullGraph.add(rangef8); 
       
       if goodInitFlag_lmk3==1
@@ -176,7 +176,7 @@ for ind_pose = 2:7
       r9 = curr_pose.range(lmk3); % range of lmk3 wrt x6
       srf3.addRange(key_curr, r9);
       
-      rangef9 = RangeFactorPosePoint2(key_curr, lmkKey(3), r9, noiseRange);
+      rangef9 = RangeFactor2D(key_curr, lmkKey(3), r9, noiseRange);
       fullGraph.add(rangef9);
     case 7
       % x6-lmk3
@@ -184,7 +184,7 @@ for ind_pose = 2:7
       srf3.addRange(key_curr, r10);
       smartGraph.add(srf3);
       
-      rangef10 = RangeFactorPosePoint2(key_curr, lmkKey(3), r10, noiseRange);
+      rangef10 = RangeFactor2D(key_curr, lmkKey(3), r10, noiseRange);
       fullGraph.add(rangef10);
   end
   
diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp
index 33453d7d3..6bc155214 100644
--- a/tests/testSerializationSLAM.cpp
+++ b/tests/testSerializationSLAM.cpp
@@ -96,8 +96,8 @@ typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
 typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
 typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactorPosePoint2;
-typedef RangeFactor<Pose3, Point3>                      RangeFactorPosePoint3;
+typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
+typedef RangeFactor<Pose3, Point3>                      RangeFactor3D;
 typedef RangeFactor<Pose2, Pose2>                       RangeFactorPose2;
 typedef RangeFactor<Pose3, Pose3>                       RangeFactorPose3;
 typedef RangeFactor<CalibratedCamera, Point3>           RangeFactorCalibratedCameraPoint;
@@ -204,8 +204,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
 
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2");
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3");
+BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
+BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
@@ -378,8 +378,8 @@ TEST (testSerializationSLAM, factors) {
   NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera);
   NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera);
 
-  RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1);
-  RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1);
+  RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1);
+  RangeFactor3D rangeFactor3D(a09, a05, 2.0, model1);
   RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1);
   RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1);
   RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1);
@@ -439,8 +439,8 @@ TEST (testSerializationSLAM, factors) {
   graph += nonlinearEqualitySimpleCamera;
   graph += nonlinearEqualityStereoCamera;
 
-  graph += rangeFactorPosePoint2;
-  graph += rangeFactorPosePoint3;
+  graph += rangeFactor2D;
+  graph += rangeFactor3D;
   graph += rangeFactorPose2;
   graph += rangeFactorPose3;
   graph += rangeFactorCalibratedCameraPoint;
@@ -505,8 +505,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsObj<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
-  EXPECT(equalsObj<RangeFactorPosePoint2>(rangeFactorPosePoint2));
-  EXPECT(equalsObj<RangeFactorPosePoint3>(rangeFactorPosePoint3));
+  EXPECT(equalsObj<RangeFactor2D>(rangeFactor2D));
+  EXPECT(equalsObj<RangeFactor3D>(rangeFactor3D));
   EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
@@ -571,8 +571,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsXML<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
-  EXPECT(equalsXML<RangeFactorPosePoint2>(rangeFactorPosePoint2));
-  EXPECT(equalsXML<RangeFactorPosePoint3>(rangeFactorPosePoint3));
+  EXPECT(equalsXML<RangeFactor2D>(rangeFactor2D));
+  EXPECT(equalsXML<RangeFactor3D>(rangeFactor3D));
   EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
@@ -637,8 +637,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsBinary<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
-  EXPECT(equalsBinary<RangeFactorPosePoint2>(rangeFactorPosePoint2));
-  EXPECT(equalsBinary<RangeFactorPosePoint3>(rangeFactorPosePoint3));
+  EXPECT(equalsBinary<RangeFactor2D>(rangeFactor2D));
+  EXPECT(equalsBinary<RangeFactor3D>(rangeFactor3D));
   EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index 5cf3b5d6c..a3b8df630 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -127,7 +127,7 @@ void Module::parseMarkup(const std::string& data) {
   TemplateInstantiationTypedef singleInstantiation, singleInstantiation0;
   TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList);
 
-  // typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
+  // typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
   TypeGrammar instantiationClass_g(singleInstantiation.class_);
   Rule templateSingleInstantiation_p =
     (str_p("typedef") >> instantiationClass_g >>

From 609019b5851379969a151e4c8acca6c958a44b2e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 12:30:53 -0500
Subject: [PATCH 101/281] Fixed warning

---
 examples/ISAM2_SmartFactorStereo_IMU.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp
index 968f1edc7..f39e9f4eb 100644
--- a/examples/ISAM2_SmartFactorStereo_IMU.cpp
+++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp
@@ -15,6 +15,7 @@
 #include <fstream>
 #include <iostream>
 #include <sstream>
+#include <string>
 #include <vector>
 
 using namespace std;
@@ -224,7 +225,7 @@ int main(int argc, char* argv[]) {
 
       smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
     } else {
-      throw runtime_error("unexpected data type: " + type);
+      throw runtime_error("unexpected data type: " + string(1, type));
     }
 
     lastFrame = frame;

From 6a58e886313eeb09485567c76f7264dd1effb099 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 12:33:13 -0500
Subject: [PATCH 102/281] Added example by Wenqiang Zhou given in issue #369

---
 examples/Pose2SLAMStressTest.cpp | 89 ++++++++++++++++++++++++++++++++
 1 file changed, 89 insertions(+)
 create mode 100644 examples/Pose2SLAMStressTest.cpp

diff --git a/examples/Pose2SLAMStressTest.cpp b/examples/Pose2SLAMStressTest.cpp
new file mode 100644
index 000000000..030834894
--- /dev/null
+++ b/examples/Pose2SLAMStressTest.cpp
@@ -0,0 +1,89 @@
+/**
+ * @file Pose2SLAMStressTest.cpp
+ * @brief Test GTSAM on large open-loop chains
+ * @date May 23, 2018
+ * @author Wenqiang Zhou
+ */
+
+// Create N 3D poses, add relative motion between each consecutive poses. (The
+// relative motion is simply a unit translation(1, 0, 0), no rotation). For each
+// each pose, add some random noise to the x value of the translation part.
+// Use gtsam to create a prior factor for the first pose and N-1 between factors
+// and run optimization.
+
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/StereoFactor.h>
+
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+void testGtsam(int numberNodes) {
+  std::random_device rd;
+  std::mt19937 e2(rd());
+  std::uniform_real_distribution<> dist(0, 1);
+
+  vector<Pose3> poses;
+  for (int i = 0; i < numberNodes; ++i) {
+    Matrix4 M;
+    double r = dist(e2);
+    r = (r - 0.5) / 10 + i;
+    M << 1, 0, 0, r, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
+    poses.push_back(Pose3(M));
+  }
+
+  // prior factor for the first pose
+  auto priorModel = noiseModel::Isotropic::Variance(6, 1e-4);
+  Matrix4 first_M;
+  first_M << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
+  Pose3 first = Pose3(first_M);
+
+  NonlinearFactorGraph graph;
+  graph.add(PriorFactor<Pose3>(0, first, priorModel));
+
+  // vo noise model
+  auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3);
+
+  // relative VO motion
+  Matrix4 vo_M;
+  vo_M << 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
+  Pose3 relativeMotion(vo_M);
+  for (int i = 0; i < numberNodes - 1; ++i) {
+    graph.add(
+        BetweenFactor<Pose3>(i, i + 1, relativeMotion, VOCovarianceModel));
+  }
+
+  // inital values
+  Values initial;
+  for (int i = 0; i < numberNodes; ++i) {
+    initial.insert(i, poses[i]);
+  }
+
+  LevenbergMarquardtParams params;
+  params.verbosity = NonlinearOptimizerParams::ERROR;
+  // params.setLinearSolverType("MULTIFRONTAL_QR");
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+
+  // GaussNewtonParams params_gn;
+  // params_gn.setVerbosity("ERROR");
+  // params_gn.setMaxIterations(20);
+  // params_gn.setLinearSolverType("MULTIFRONTAL_QR");
+  // GaussNewtonOptimizer optimizer(graph, initial, params_gn );
+  auto result = optimizer.optimize();
+}
+
+int main(int args, char* argv[]) {
+  int numberNodes = stoi(argv[1]);
+  cout << "number of_nodes: " << numberNodes << endl;
+  testGtsam(numberNodes);
+  return 0;
+}

From 3c3f6d2b7c42f9cb289bf53e891a456f1fdf1a37 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 13:08:41 -0500
Subject: [PATCH 103/281] Switching to METIS ordering fixes out of memory error
 for large examples.

---
 examples/Pose2SLAMStressTest.cpp | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/examples/Pose2SLAMStressTest.cpp b/examples/Pose2SLAMStressTest.cpp
index 030834894..0f306b7f4 100644
--- a/examples/Pose2SLAMStressTest.cpp
+++ b/examples/Pose2SLAMStressTest.cpp
@@ -69,15 +69,10 @@ void testGtsam(int numberNodes) {
   }
 
   LevenbergMarquardtParams params;
-  params.verbosity = NonlinearOptimizerParams::ERROR;
-  // params.setLinearSolverType("MULTIFRONTAL_QR");
+  params.setVerbosity("ERROR");
+  params.setOrderingType("METIS");
+  params.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
   LevenbergMarquardtOptimizer optimizer(graph, initial, params);
-
-  // GaussNewtonParams params_gn;
-  // params_gn.setVerbosity("ERROR");
-  // params_gn.setMaxIterations(20);
-  // params_gn.setLinearSolverType("MULTIFRONTAL_QR");
-  // GaussNewtonOptimizer optimizer(graph, initial, params_gn );
   auto result = optimizer.optimize();
 }
 

From eada1ee505aecc2e6dc4bb4ddf55ae8502c1d257 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 14:48:22 -0500
Subject: [PATCH 104/281] Adding adjoint and adjoint transpose functions

---
 gtsam/geometry/Pose2.h | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index f03e0852e..079c649f3 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -132,6 +132,8 @@ public:
    * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
    */
   Matrix3 AdjointMap() const;
+
+  /// Apply AdjointMap to twist xi
   inline Vector3 Adjoint(const Vector3& xi) const {
     return AdjointMap()*xi;
   }
@@ -141,6 +143,20 @@ public:
    */
   static Matrix3 adjointMap(const Vector3& v);
 
+  /**
+   * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
+   */
+  Vector3 adjoint(const Vector3& xi, const Vector3& y) {
+    return adjointMap(xi) * y;
+  }
+
+  /**
+   * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
+   */
+  Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
+    return adjointMap(xi).transpose() * y;
+  }
+
   /**
    * wedge for SE(2):
    * @param xi 3-dim twist (v,omega) where

From 409a0215b88d548524d39cdddbee73de63889bb9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 15:37:31 -0500
Subject: [PATCH 105/281] Added adjoint operators etc.

---
 gtsam.h | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 629fd70cf..d9052f65b 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -573,8 +573,13 @@ class Pose2 {
   // Lie Group
   static gtsam::Pose2 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose2& p);
+  static Matrix ExpmapDerivative(Vector v);
+  static Matrix LogmapDerivative(const gtsam::Pose2& v);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
+  static Matrix adjointMap(Vector v);
+  Vector adjoint(Vector xi, Vector y);
+  Vector adjointTranspose(Vector xi, Vector y);
   static Matrix wedge(double vx, double vy, double w);
 
   // Group Actions on Point2
@@ -623,6 +628,11 @@ class Pose3 {
   static Vector Logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
+  static Matrix adjointMap(Vector xi);
+  static Vector adjoint(Vector xi, Vector y);
+  static Vector adjointTranspose(Vector xi, Vector y);
+  static Matrix ExpmapDerivative(Vector xi);
+  static Matrix LogmapDerivative(const gtsam::Pose3& xi);
   static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
 
   // Group Action on Point3

From 1999fba7ae06957ec0f941307fcb71e9714cb1e8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 31 Dec 2018 15:38:12 -0500
Subject: [PATCH 106/281] Cleaned up Pose3 unit test, added unit test for
 adjoint.

---
 cython/gtsam/tests/test_Pose3.py | 36 ++++++++++++++++++++++----------
 1 file changed, 25 insertions(+), 11 deletions(-)

diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py
index 8fa50b90c..4752a4b02 100644
--- a/cython/gtsam/tests/test_Pose3.py
+++ b/cython/gtsam/tests/test_Pose3.py
@@ -1,25 +1,32 @@
+"""Pose3 unit tests."""
 import math
 import unittest
 
-from gtsam import Point3, Rot3, Pose3
+import numpy as np
+
+from gtsam import Point3, Pose3, Rot3
 
 
 class TestPose3(unittest.TestCase):
+    """Test selected Pose3 methods."""
 
-    def test__between(self):
-        T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2))
+    def test_between(self):
+        """Test between method."""
+        T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
         T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
         expected = T2.inverse().compose(T3)
         actual = T2.between(T3)
         self.assertTrue(actual.equals(expected, 1e-6))
 
     def test_transform_to(self):
-        transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0))
-        actual = transform.transform_to(Point3(3,2,10))
-        expected = Point3 (2,1,10)
+        """Test transform_to method."""
+        transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
+        actual = transform.transform_to(Point3(3, 2, 10))
+        expected = Point3(2, 1, 10)
         self.assertTrue(actual.equals(expected, 1e-6))
 
     def test_range(self):
+        """Test range method."""
         l1 = Point3(1, 0, 0)
         l2 = Point3(1, 1, 0)
         x1 = Pose3()
@@ -28,16 +35,23 @@ class TestPose3(unittest.TestCase):
         xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
 
         # establish range is indeed zero
-        self.assertEqual(1,x1.range(point=l1))
+        self.assertEqual(1, x1.range(point=l1))
 
         # establish range is indeed sqrt2
-        self.assertEqual(math.sqrt(2.0),x1.range(point=l2))
+        self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
 
         # establish range is indeed zero
-        self.assertEqual(1,x1.range(pose=xl1))
-        
+        self.assertEqual(1, x1.range(pose=xl1))
+
         # establish range is indeed sqrt2
-        self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2))
+        self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
+
+    def test_adjoint(self):
+        """Test adjoint method."""
+        xi = np.array([1, 2, 3, 4, 5, 6])
+        expected = np.dot(Pose3.adjointMap(xi), xi)
+        actual = Pose3.adjoint(xi, xi)
+        np.testing.assert_array_equal(actual, expected)
 
 
 if __name__ == "__main__":

From 0a2e4e34e89f4f84cd31ee9c97600b667e43d609 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 2 Jan 2019 14:32:49 -0500
Subject: [PATCH 107/281] Added extra types included by Jacob Thomson in
 (declined) PR #269

---
 gtsam.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam.h b/gtsam.h
index 629fd70cf..2741799d7 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -229,7 +229,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Pose2, gtsam::Pose3, gtsam::Rot2}>
+template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };

From f558ccbb2de2d25c93705b96d482b07e0c7a19d2 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Wed, 2 Jan 2019 13:26:53 -0800
Subject: [PATCH 108/281] Update LICENSE to enumerate all dependencies in
 gtsam/3rdparty

---
 LICENSE | 27 +++++++++++++++++----------
 1 file changed, 17 insertions(+), 10 deletions(-)

diff --git a/LICENSE b/LICENSE
index e1c3be202..d828deb55 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1,18 +1,25 @@
 GTSAM is released under the simplified BSD license, reproduced in the file 
 LICENSE.BSD in this directory.
 
-GTSAM contains two third party libraries, with documentation of licensing and 
-modifications as follows:
+GTSAM contains several third party libraries, with documentation of licensing
+and modifications as follows:
 
-- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree 
+- CCOLAMD 2.9.6: Tim Davis' constrained column approximate minimum degree
 ordering library
-    - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
+    - Included unmodified in gtsam/3rdparty/CCOLAMD and
+      gtsam/3rdparty/SuiteSparse_config
     - http://faculty.cse.tamu.edu/davis/suitesparse.html
     - Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt
-- Eigen 3.2:  General C++ matrix and linear algebra library
-    - Modified with 3 patches that have been contributed back to the Eigen team:
-        - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
-        - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
-        - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
+- ceres: Google's nonlinear least-squares optimization library
+    - Includes only auto-diff/jet code, with minor modifications to includes
+    - http://ceres-solver.org/license.html
+- Eigen 3.3.7:  General C++ matrix and linear algebra library
     - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README 
-    - Some code that is 3rd-party to Eigen is BSD and LGPL
\ No newline at end of file
+    - Some code that is 3rd-party to Eigen is BSD and LGPL
+- GeographicLib 1.35: Charles Karney's geographic conversion utility library
+    - Included unmodified in gtsam/3rdparty/GeographicLib
+    - Licenced under MIT, provided in gtsam/3rdparty/GeographicLib/LICENSE.txt
+- METIS 5.1.0: Graph partitioning and fill-reducing matrix ordering library
+    - Included unmodified in gtsam/3rdparty/metis
+    - Licenced under Apache License v 2.0, provided in
+      gtsam/3rdparty/metis/LICENSE.txt

From d4398fb0928d3fafddac28d758dceb7219cb7a7e Mon Sep 17 00:00:00 2001
From: Thomas Horstink <thomashorstink@gmail.com>
Date: Fri, 4 Jan 2019 11:31:41 +0100
Subject: [PATCH 109/281] expression example of estimating trajectory,
 landmarks and sensor-body-transform simultaneously

---
 ...leExpressions_BearinRangeWithTransform.cpp | 122 ++++++++++++++++++
 1 file changed, 122 insertions(+)
 create mode 100644 examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp

diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
new file mode 100644
index 000000000..a78328c48
--- /dev/null
+++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
@@ -0,0 +1,122 @@
+/**
+ * @file    Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
+ * @brief   A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
+ * @author  Thomas Horstink
+ * @date    January 4th, 2019
+ */
+
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/slam/expressions.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+
+using namespace gtsam;
+
+typedef BearingRange<Pose3, Point3> BearingRange3D;
+
+// These functions are very similar to those in the SFM example, except that it you can give it a fixed delta in between poses.
+/* ************************************************************************* */
+std::vector<Point3> createPoints() {
+
+  // Create the set of ground-truth landmarks
+  std::vector<Point3> points;
+  points.push_back(Point3(10.0,10.0,10.0));
+  points.push_back(Point3(-10.0,10.0,10.0));
+  points.push_back(Point3(-10.0,-10.0,10.0));
+  points.push_back(Point3(10.0,-10.0,10.0));
+  points.push_back(Point3(10.0,10.0,-10.0));
+  points.push_back(Point3(-10.0,10.0,-10.0));
+  points.push_back(Point3(-10.0,-10.0,-10.0));
+  points.push_back(Point3(10.0,-10.0,-10.0));
+
+  return points;
+}
+
+/* ************************************************************************* */
+std::vector<Pose3> createPoses(Pose3 delta, int steps) {
+
+  // Create the set of ground-truth poses
+  std::vector<Pose3> poses;
+  Pose3 pose = Pose3();
+  int i = 0;
+  for(; i < steps; ++i) {
+    poses.push_back(pose);
+    pose = pose.compose(delta);
+  }
+  poses.push_back(pose);
+
+  return poses;
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  // Move around so the whole state (including the sensor tf) is observable
+  Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0));
+  Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0));
+  Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0));
+
+  int steps = 4;
+  auto poses = createPoses(delta_pose1, steps);
+  auto poses2 = createPoses(delta_pose2, steps);
+  auto poses3 = createPoses(delta_pose3, steps);
+
+  // concatenate poses to create trajectory
+  poses.insert( poses.end(), poses2.begin(), poses2.end() );
+  poses.insert( poses.end(), poses3.begin(), poses3.end() );  // std::vector of Pose3
+  auto points = createPoints();                               // std::vector of Point3 
+
+  // (ground-truth) sensor pose in body frame, further an unknown variable
+  Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+  // a graph
+  ExpressionFactorGraph graph;
+  // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
+  auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
+  // Uncertainty bearing range measurement;
+  auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
+  // Expressions for body-frame at key 0 and sensor-tf
+  Pose3_ x_('x', 0);
+  Pose3_ body_T_sensor_('T', 0);
+  // add a prior on the body-pose and sensor-tf.
+  graph.addExpressionFactor(x_, poses[0], poseNoise); 
+  // Simulated measurements from pose, adding them to the factor graph
+  for (size_t i = 0; i < poses.size(); ++i) {
+    auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
+    for (size_t j = 0; j < points.size(); ++j) {
+      // Create the expression
+      auto prediction = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
+      // Create a *perfect* measurement
+      auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
+      graph.addExpressionFactor(prediction, measurement, bearingRangeNoise);
+    }
+    // and add a between factor 
+    if (i > 0)
+    {
+      // And also we have a nice measurement for the between factor.
+      graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); 
+    }
+  }
+
+  // Create perturbed initial
+  Values initial;
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  for (size_t i = 0; i < poses.size(); ++i)
+    initial.insert(Symbol('x', i), poses[i].compose(delta)); 
+  for (size_t j = 0; j < points.size(); ++j)
+    initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+  
+  // initialize body_T_sensor wrongly (because we do not know!)
+  initial.insert<Pose3>(Symbol('T',0), Pose3()); 
+
+  std::cout << "initial error: " << graph.error(initial) << std::endl;
+  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+  std::cout << "final error: " << graph.error(result) << std::endl;
+
+  initial.at<Pose3>(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */
+  result.at<Pose3>(Symbol('T',0)).print("\nFinal estimate body_T_sensor\n");    /* optimized sensor_P_body estimate */
+  body_T_sensor_gt.print("\nGround truth body_T_sensor\n");                     /* sensor_P_body ground truth */
+
+  return 0;
+}
+/* ************************************************************************* */
\ No newline at end of file

From 7bb6863e7567a4f72d1f011d3505ca1339e62112 Mon Sep 17 00:00:00 2001
From: Thomas Horstink <thomashorstink@gmail.com>
Date: Fri, 4 Jan 2019 11:50:20 +0100
Subject: [PATCH 110/281] little typo in a comment

---
 .../Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
index a78328c48..69409e87b 100644
--- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
+++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
@@ -16,7 +16,7 @@ using namespace gtsam;
 
 typedef BearingRange<Pose3, Point3> BearingRange3D;
 
-// These functions are very similar to those in the SFM example, except that it you can give it a fixed delta in between poses.
+// These functions are very similar to those in the SFM example, except that you can give it a fixed delta in between poses for n steps.
 /* ************************************************************************* */
 std::vector<Point3> createPoints() {
 

From 986346f2b9c43918a9d4f626f4f5f0859e25dd3d Mon Sep 17 00:00:00 2001
From: Thomas Horstink <thomashorstink@gmail.com>
Date: Fri, 4 Jan 2019 11:53:50 +0100
Subject: [PATCH 111/281] another comment update

---
 ...xampleExpressions_BearinRangeWithTransform.cpp | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
index 69409e87b..6ba7caca3 100644
--- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
+++ b/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
@@ -72,28 +72,29 @@ int main(int argc, char* argv[]) {
   // a graph
   ExpressionFactorGraph graph;
   // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
-  auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
+  auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());ExpressiExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampon examp
   // Uncertainty bearing range measurement;
   auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
   // Expressions for body-frame at key 0 and sensor-tf
-  Pose3_ x_('x', 0);
+  Pose3_ x_('x', 0);nice
   Pose3_ body_T_sensor_('T', 0);
-  // add a prior on the body-pose and sensor-tf.
+  // add a prior on the body-pose.
   graph.addExpressionFactor(x_, poses[0], poseNoise); 
-  // Simulated measurements from pose, adding them to the factor graph
+  // Simulated measurements from pose
   for (size_t i = 0; i < poses.size(); ++i) {
     auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
     for (size_t j = 0; j < points.size(); ++j) {
       // Create the expression
       auto prediction = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
-      // Create a *perfect* measurement
+      // Create a *perfect* measurementExpression exampExpreExpression exampExpression exampExpression exampssion examp
       auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
+      // Add factor
       graph.addExpressionFactor(prediction, measurement, bearingRangeNoise);
     }
-    // and add a between factor 
+    // and add a between factor to the graph
     if (i > 0)
     {
-      // And also we have a nice measurement for the between factor.
+      // And also we have a *perfect* measurement for the between factor.
       graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); 
     }
   }

From ba03b398f4d0c6290de1d6cbaac85dbed9726b6e Mon Sep 17 00:00:00 2001
From: Thomas Horstink <thomashorstink@gmail.com>
Date: Fri, 4 Jan 2019 11:55:26 +0100
Subject: [PATCH 112/281] type in filename....

---
 ...> Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp} | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)
 rename examples/{Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp => Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp} (98%)

diff --git a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
similarity index 98%
rename from examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
rename to examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
index 6ba7caca3..8faa6b182 100644
--- a/examples/Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
+++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
@@ -1,5 +1,5 @@
 /**
- * @file    Pose3SLAMExampleExpressions_BearinRangeWithTransform.cpp
+ * @file    Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
  * @brief   A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
  * @author  Thomas Horstink
  * @date    January 4th, 2019

From 9c382b6c144fd778dec1ecc81c166bfe15a69ffb Mon Sep 17 00:00:00 2001
From: Thomas Horstink <thomashorstink@gmail.com>
Date: Fri, 4 Jan 2019 16:17:33 +0100
Subject: [PATCH 113/281] changed the SFMdata functions so that it allows the
 passage of function arguments to generate a trajectory; default arguments
 result in the original behaviour (described in header). In the range bearing
 examples: fixed weirdo text-artifacts, add newline for readability, added
 underscore the prediction expression.

---
 ...eExpressions_BearingRangeWithTransform.cpp | 73 +++++++------------
 examples/SFMdata.h                            | 28 +++----
 2 files changed, 40 insertions(+), 61 deletions(-)

diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
index 8faa6b182..39157a4f6 100644
--- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
+++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
@@ -11,86 +11,65 @@
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Values.h>
+#include <examples/SFMdata.h>
 
 using namespace gtsam;
 
 typedef BearingRange<Pose3, Point3> BearingRange3D;
 
-// These functions are very similar to those in the SFM example, except that you can give it a fixed delta in between poses for n steps.
-/* ************************************************************************* */
-std::vector<Point3> createPoints() {
-
-  // Create the set of ground-truth landmarks
-  std::vector<Point3> points;
-  points.push_back(Point3(10.0,10.0,10.0));
-  points.push_back(Point3(-10.0,10.0,10.0));
-  points.push_back(Point3(-10.0,-10.0,10.0));
-  points.push_back(Point3(10.0,-10.0,10.0));
-  points.push_back(Point3(10.0,10.0,-10.0));
-  points.push_back(Point3(-10.0,10.0,-10.0));
-  points.push_back(Point3(-10.0,-10.0,-10.0));
-  points.push_back(Point3(10.0,-10.0,-10.0));
-
-  return points;
-}
-
-/* ************************************************************************* */
-std::vector<Pose3> createPoses(Pose3 delta, int steps) {
-
-  // Create the set of ground-truth poses
-  std::vector<Pose3> poses;
-  Pose3 pose = Pose3();
-  int i = 0;
-  for(; i < steps; ++i) {
-    poses.push_back(pose);
-    pose = pose.compose(delta);
-  }
-  poses.push_back(pose);
-
-  return poses;
-}
-
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
+
   // Move around so the whole state (including the sensor tf) is observable
+  Pose3 init_pose = Pose3();
   Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0));
   Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0));
   Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0));
 
   int steps = 4;
-  auto poses = createPoses(delta_pose1, steps);
-  auto poses2 = createPoses(delta_pose2, steps);
-  auto poses3 = createPoses(delta_pose3, steps);
+  auto poses  = createPoses(init_pose, delta_pose1, steps);
+  auto poses2 = createPoses(init_pose, delta_pose2, steps);
+  auto poses3 = createPoses(init_pose, delta_pose3, steps);
 
-  // concatenate poses to create trajectory
+  // Concatenate poses to create trajectory
   poses.insert( poses.end(), poses2.begin(), poses2.end() );
   poses.insert( poses.end(), poses3.begin(), poses3.end() );  // std::vector of Pose3
   auto points = createPoints();                               // std::vector of Point3 
 
   // (ground-truth) sensor pose in body frame, further an unknown variable
   Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
-  // a graph
+  
+  // The graph
   ExpressionFactorGraph graph;
+  
   // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
-  auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());ExpressiExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampExpression exampon examp
+  auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
+  
   // Uncertainty bearing range measurement;
   auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
+  
   // Expressions for body-frame at key 0 and sensor-tf
-  Pose3_ x_('x', 0);nice
+  Pose3_ x_('x', 0);
   Pose3_ body_T_sensor_('T', 0);
-  // add a prior on the body-pose.
+  
+  // Add a prior on the body-pose
   graph.addExpressionFactor(x_, poses[0], poseNoise); 
+  
   // Simulated measurements from pose
   for (size_t i = 0; i < poses.size(); ++i) {
     auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
     for (size_t j = 0; j < points.size(); ++j) {
-      // Create the expression
-      auto prediction = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
-      // Create a *perfect* measurementExpression exampExpreExpression exampExpression exampExpression exampssion examp
+      
+      // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform.
+      auto prediction_ = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
+      
+      // Create a *perfect* measurement
       auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
+      
       // Add factor
-      graph.addExpressionFactor(prediction, measurement, bearingRangeNoise);
+      graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise);
     }
+    
     // and add a between factor to the graph
     if (i > 0)
     {
@@ -107,7 +86,7 @@ int main(int argc, char* argv[]) {
   for (size_t j = 0; j < points.size(); ++j)
     initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
   
-  // initialize body_T_sensor wrongly (because we do not know!)
+  // Initialize body_T_sensor wrongly (because we do not know!)
   initial.insert<Pose3>(Symbol('T',0), Pose3()); 
 
   std::cout << "initial error: " << graph.error(initial) << std::endl;
diff --git a/examples/SFMdata.h b/examples/SFMdata.h
index 25442d527..af1f761ee 100644
--- a/examples/SFMdata.h
+++ b/examples/SFMdata.h
@@ -16,9 +16,10 @@
  */
 
 /**
- * A structure-from-motion example with landmarks
+ * A structure-from-motion example with landmarks, default function arguments give
  *  - The landmarks form a 10 meter cube
  *  - The robot rotates around the landmarks, always facing towards the cube
+ * Passing function argument allows to specificy an initial position, a pose increment and step count.
  */
 
 // As this is a full 3D problem, we will use Pose3 variables to represent the camera
@@ -49,20 +50,19 @@ std::vector<gtsam::Point3> createPoints() {
 }
 
 /* ************************************************************************* */
-std::vector<gtsam::Pose3> createPoses() {
-
+std::vector<gtsam::Pose3> createPoses(
+            const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)),
+            const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), 
+            int steps = 8) {
+  
   // Create the set of ground-truth poses
+  // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
   std::vector<gtsam::Pose3> poses;
-  double radius = 30.0;
-  int i = 0;
-  double theta = 0.0;
-  gtsam::Point3 up(0,0,1);
-  gtsam::Point3 target(0,0,0);
-  for(; i < 8; ++i, theta += 2*M_PI/8) {
-    gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0);
-    gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up);
-    poses.push_back(camera.pose());
+  int i = 1;
+  poses.push_back(init);
+  for(; i < steps; ++i) {
+    poses.push_back(poses[i-1].compose(delta));
   }
+  
   return poses;
-}
-/* ************************************************************************* */
+}
\ No newline at end of file

From e7d6cd4faf4ecfe1bbf1b247379a8cdd39d55b35 Mon Sep 17 00:00:00 2001
From: Thomas Horstink <thomashorstink@gmail.com>
Date: Fri, 4 Jan 2019 17:12:04 +0100
Subject: [PATCH 114/281] fixed typo in description

---
 .../Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
index 39157a4f6..68a3fd7e7 100644
--- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
+++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
@@ -1,6 +1,6 @@
 /**
  * @file    Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
- * @brief   A simultanious optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
+ * @brief   A simultaneous optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
  * @author  Thomas Horstink
  * @date    January 4th, 2019
  */

From ba5ef236e4b6a4233617f18654edaff28561b351 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Mon, 7 Jan 2019 23:38:03 +0100
Subject: [PATCH 115/281] git ignore qtcreator IDE files

---
 .gitignore | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.gitignore b/.gitignore
index 42bd27466..ad0e08aa1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,3 +19,5 @@ cython/gtsam_wrapper.pxd
 .env
 /.vs/
 /CMakeSettings.json
+# for QtCreator:
+CMakeLists.txt.user*

From 87888f7bc34d259c9385033b7a00bc3ad9ea5574 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Tue, 8 Jan 2019 00:11:08 +0100
Subject: [PATCH 116/281] Imported TBB targets; update gtsam_unstable cmake

---
 CMakeLists.txt                |  18 +-
 cmake/FindTBB.cmake           | 532 +++++++++++++++++-----------------
 gtsam/CMakeLists.txt          |   1 -
 gtsam_unstable/CMakeLists.txt |  52 ++--
 4 files changed, 296 insertions(+), 307 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5479225c1..972c40650 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -163,24 +163,14 @@ endif()
 
 ###############################################################################
 # Find TBB
-find_package(TBB)
+find_package(TBB COMPONENTS tbb tbbmalloc)
 
 # Set up variables if we're using TBB
 if(TBB_FOUND AND GTSAM_WITH_TBB)
 	set(GTSAM_USE_TBB 1)  # This will go into config.h
-	include_directories(BEFORE ${TBB_INCLUDE_DIRS})
-	set(GTSAM_TBB_LIBRARIES "")
-	if(TBB_DEBUG_LIBRARIES)
-		foreach(lib ${TBB_LIBRARIES})
-			list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}")
-		endforeach()
-		foreach(lib ${TBB_DEBUG_LIBRARIES})
-			list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}")
-		endforeach()
-	else()
-		set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES})
-	endif()
-	list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${GTSAM_TBB_LIBRARIES})
+  # all definitions and link requisites will go via imported targets:
+  # tbb & tbbmalloc
+  list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
 else()
 	set(GTSAM_USE_TBB 0)  # This will go into config.h
 endif()
diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake
index f39d64601..76fe944f5 100644
--- a/cmake/FindTBB.cmake
+++ b/cmake/FindTBB.cmake
@@ -1,13 +1,6 @@
-# Locate Intel Threading Building Blocks include paths and libraries
-# FindTBB.cmake can be found at https://code.google.com/p/findtbb/
-# Written by Hannes Hofmann <hannes.hofmann _at_ informatik.uni-erlangen.de>
-# Improvements by Gino van den Bergen <gino _at_ dtecta.com>,
-#   Florian Uhlig <F.Uhlig _at_ gsi.de>,
-#   Jiri Marsik <jiri.marsik89 _at_ gmail.com>
-
-# The MIT License
+# The MIT License (MIT)
 #
-# Copyright (c) 2011 Hannes Hofmann
+# Copyright (c) 2015 Justus Calvin
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
@@ -16,295 +9,306 @@
 # copies of the Software, and to permit persons to whom the Software is
 # furnished to do so, subject to the following conditions:
 #
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
 #
 # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
 
-# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler.
-#   e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21"
-#   TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found
-#   in the TBB installation directory (TBB_INSTALL_DIR).
 #
-# GvdB: Mac OS X distribution places libraries directly in lib directory.
+# FindTBB
+# -------
 #
-# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER.
-# TBB_ARCHITECTURE [ ia32 | em64t | itanium ]
-#   which architecture to use
-# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9
-#   which compiler to use (detected automatically on Windows)
+# Find TBB include directories and libraries.
+#
+# Usage:
+#
+#  find_package(TBB [major[.minor]] [EXACT]
+#               [QUIET] [REQUIRED]
+#               [[COMPONENTS] [components...]]
+#               [OPTIONAL_COMPONENTS components...])
+#
+# where the allowed components are tbbmalloc and tbb_preview. Users may modify
+# the behavior of this module with the following variables:
+#
+# * TBB_ROOT_DIR          - The base directory the of TBB installation.
+# * TBB_INCLUDE_DIR       - The directory that contains the TBB headers files.
+# * TBB_LIBRARY           - The directory that contains the TBB library files.
+# * TBB_<library>_LIBRARY - The path of the TBB the corresponding TBB library.
+#                           These libraries, if specified, override the
+#                           corresponding library search results, where <library>
+#                           may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug,
+#                           tbb_preview, or tbb_preview_debug.
+# * TBB_USE_DEBUG_BUILD   - The debug version of tbb libraries, if present, will
+#                           be used instead of the release version.
+#
+# Users may modify the behavior of this module with the following environment
+# variables:
+#
+# * TBB_INSTALL_DIR
+# * TBBROOT
+# * LIBRARY_PATH
+#
+# This module will set the following variables:
+#
+# * TBB_FOUND             - Set to false, or undefined, if we haven’t found, or
+#                           don’t want to use TBB.
+# * TBB_<component>_FOUND - If False, optional <component> part of TBB sytem is
+#                           not available.
+# * TBB_VERSION           - The full version string
+# * TBB_VERSION_MAJOR     - The major version
+# * TBB_VERSION_MINOR     - The minor version
+# * TBB_INTERFACE_VERSION - The interface version number defined in
+#                           tbb/tbb_stddef.h.
+# * TBB_<library>_LIBRARY_RELEASE - The path of the TBB release version of
+#                           <library>, where <library> may be tbb, tbb_debug,
+#                           tbbmalloc, tbbmalloc_debug, tbb_preview, or
+#                           tbb_preview_debug.
+# * TBB_<library>_LIBRARY_DEGUG - The path of the TBB release version of
+#                           <library>, where <library> may be tbb, tbb_debug,
+#                           tbbmalloc, tbbmalloc_debug, tbb_preview, or
+#                           tbb_preview_debug.
+#
+# The following varibles should be used to build and link with TBB:
+#
+# * TBB_INCLUDE_DIRS        - The include directory for TBB.
+# * TBB_LIBRARIES           - The libraries to link against to use TBB.
+# * TBB_LIBRARIES_RELEASE   - The release libraries to link against to use TBB.
+# * TBB_LIBRARIES_DEBUG     - The debug libraries to link against to use TBB.
+# * TBB_DEFINITIONS         - Definitions to use when compiling code that uses
+#                             TBB.
+# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that
+#                             uses TBB.
+# * TBB_DEFINITIONS_DEBUG   - Definitions to use when compiling debug code that
+#                             uses TBB.
+#
+# This module will also create the "tbb" target that may be used when building
+# executables and libraries.
 
-# This module respects
-# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR}
+include(FindPackageHandleStandardArgs)
 
-# This module defines
-# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc.
-# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc
-# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug
-# TBB_INSTALL_DIR, the base TBB install directory
-# TBB_LIBRARIES, the libraries to link against to use TBB.
-# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols.
-# TBB_FOUND, If false, don't try to use TBB.
-# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h
+if(NOT TBB_FOUND)
 
+  ##################################
+  # Check the build type
+  ##################################
 
-if (WIN32)
-    # has em64t/vc8 em64t/vc9
-    # has ia32/vc7.1 ia32/vc8 ia32/vc9
-    set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB")
-    set(_TBB_LIB_NAME "tbb")
-    set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
-    set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
-    set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
-    if (MSVC71)
-        set (_TBB_COMPILER "vc7.1")
-        set (TBB_COMPILER "vc7.1")
-    endif(MSVC71)
-    if (MSVC80)
-        set(_TBB_COMPILER "vc8")
-        set(TBB_COMPILER "vc8")
-    endif(MSVC80)
-    if (MSVC90)
-        set(_TBB_COMPILER "vc9")
-        set(TBB_COMPILER "vc9")
-    endif(MSVC90)
-    if(MSVC10)
-        set(_TBB_COMPILER "vc10")
-        set(TBB_COMPILER "vc10")
-    endif(MSVC10)
-    if(MSVC11)
-        set(_TBB_COMPILER "vc11")
-        set(TBB_COMPILER "vc11")
-    endif(MSVC11)
-    if(MSVC14)
-        set(_TBB_COMPILER "vc14")
-        set(TBB_COMPILER "vc14")
-    endif(MSVC14)
-    # Todo: add other Windows compilers such as ICL.
-	if(TBB_ARCHITECTURE)
-		set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
-	elseif("$ENV{TBB_ARCH_PLATFORM}" STREQUAL "")
-		# Try to guess the architecture
-		if(CMAKE_CL_64)
-			set(_TBB_ARCHITECTURE intel64)
-			set(TBB_ARCHITECTURE intel64)
-		else()
-			set(_TBB_ARCHITECTURE ia32)
-			set(TBB_ARCHITECTURE ia32)
-		endif()
-	endif()
-endif (WIN32)
+  if(NOT DEFINED TBB_USE_DEBUG_BUILD)
+    if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
+      set(TBB_BUILD_TYPE DEBUG)
+    else()
+      set(TBB_BUILD_TYPE RELEASE)
+    endif()
+  elseif(TBB_USE_DEBUG_BUILD)
+    set(TBB_BUILD_TYPE DEBUG)
+  else()
+    set(TBB_BUILD_TYPE RELEASE)
+  endif()
 
-if (UNIX)
-    if (APPLE)
-        # MAC
-        set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions")
-        # libs: libtbb.dylib, libtbbmalloc.dylib, *_debug
-        set(_TBB_LIB_NAME "tbb")
-        set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
-        set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
-        set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
-        # default flavor on apple: ia32/cc4.0.1_os10.4.9
-        # Jiri: There is no reason to presume there is only one flavor and
-        #       that user's setting of variables should be ignored.
-        if(NOT TBB_COMPILER)
-            set(_TBB_COMPILER "cc4.0.1_os10.4.9")
-        elseif (NOT TBB_COMPILER)
-            set(_TBB_COMPILER ${TBB_COMPILER})
-        endif(NOT TBB_COMPILER)
-        if(NOT TBB_ARCHITECTURE)
-            set(_TBB_ARCHITECTURE "ia32")
-        elseif(NOT TBB_ARCHITECTURE)
-            set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
-        endif(NOT TBB_ARCHITECTURE)
-    else (APPLE)
-        # LINUX
-        set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include")
-        set(_TBB_LIB_NAME "tbb")
-        set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
-        set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
-        set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
-        # has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21
-        # has ia32/*
-        # has itanium/*
-        set(_TBB_COMPILER ${TBB_COMPILER})
-        set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
-    endif (APPLE)
-endif (UNIX)
+  ##################################
+  # Set the TBB search directories
+  ##################################
 
-if (CMAKE_SYSTEM MATCHES "SunOS.*")
-# SUN
-# not yet supported
-# has em64t/cc3.4.3_kernel5.10
-# has ia32/*
-endif (CMAKE_SYSTEM MATCHES "SunOS.*")
+  # Define search paths based on user input and environment variables
+  set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT})
 
+  # Define the search directories based on the current platform
+  if(CMAKE_SYSTEM_NAME STREQUAL "Windows")
+    set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB"
+                               "C:/Program Files (x86)/Intel/TBB")
 
-#-- Clear the public variables
-set (TBB_FOUND "NO")
+    # Set the target architecture
+    if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+      set(TBB_ARCHITECTURE "intel64")
+    else()
+      set(TBB_ARCHITECTURE "ia32")
+    endif()
 
+    # Set the TBB search library path search suffix based on the version of VC
+    if(WINDOWS_STORE)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui")
+    elseif(MSVC14)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14")
+    elseif(MSVC12)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12")
+    elseif(MSVC11)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11")
+    elseif(MSVC10)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10")
+    endif()
 
-#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR}
-# first: use CMake variable TBB_INSTALL_DIR
-if (TBB_INSTALL_DIR)
-    set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR})
-endif (TBB_INSTALL_DIR)
-# second: use environment variable
-if (NOT _TBB_INSTALL_DIR)
-    if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB_INSTALL_DIR})
-    endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
-    # Intel recommends setting TBB21_INSTALL_DIR
-    if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB21_INSTALL_DIR})
-    endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
-    if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB22_INSTALL_DIR})
-    endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
-    if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB30_INSTALL_DIR})
-    endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
-endif (NOT _TBB_INSTALL_DIR)
-# third: try to find path automatically
-if (NOT _TBB_INSTALL_DIR)
-    if (_TBB_DEFAULT_INSTALL_DIR)
-        set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR})
-    endif (_TBB_DEFAULT_INSTALL_DIR)
-endif (NOT _TBB_INSTALL_DIR)
-# sanity check
-if (NOT _TBB_INSTALL_DIR)
-    message (STATUS "TBB: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}")
-else (NOT _TBB_INSTALL_DIR)
-# finally: set the cached CMake variable TBB_INSTALL_DIR
-if (NOT TBB_INSTALL_DIR)
-    set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory")
-    mark_as_advanced(TBB_INSTALL_DIR)
-endif (NOT TBB_INSTALL_DIR)
+    # Add the library path search suffix for the VC independent version of TBB
+    list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt")
 
+  elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
+    # OS X
+    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
 
-#-- A macro to rewrite the paths of the library. This is necessary, because
-#   find_library() always found the em64t/vc9 version of the TBB libs
-macro(TBB_CORRECT_LIB_DIR var_name)
-#    if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
-        string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
-#    endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
-    string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
-    string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
-    string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
-    string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
-    string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
-    string(REPLACE vc11 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
-endmacro(TBB_CORRECT_LIB_DIR var_content)
+    # TODO: Check to see which C++ library is being used by the compiler.
+    if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
+      # The default C++ library on OS X 10.9 and later is libc++
+      set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib")
+    else()
+      set(TBB_LIB_PATH_SUFFIX "lib")
+    endif()
+  elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux")
+    # Linux
+    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
 
+    # TODO: Check compiler version to see the suffix should be <arch>/gcc4.1 or
+    #       <arch>/gcc4.1. For now, assume that the compiler is more recent than
+    #       gcc 4.4.x or later.
+    if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
+      set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4")
+    elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$")
+      set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4")
+    endif()
+  endif()
 
-#-- Look for include directory and set ${TBB_INCLUDE_DIR}
-set (TBB_INC_SEARCH_DIR ${_TBB_INSTALL_DIR}/include)
-# Jiri: tbbvars now sets the CPATH environment variable to the directory
-#       containing the headers.
-find_path(TBB_INCLUDE_DIR
-    tbb/task_scheduler_init.h
-    PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH
-)
-mark_as_advanced(TBB_INCLUDE_DIR)
+  ##################################
+  # Find the TBB include dir
+  ##################################
 
+  find_path(TBB_INCLUDE_DIRS tbb/tbb.h
+      HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR}
+      PATHS ${TBB_DEFAULT_SEARCH_DIR}
+      PATH_SUFFIXES include)
 
-#-- Look for libraries
-# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh]
-if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
-    set (_TBB_LIBRARY_DIR 
-         ${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM}
-         ${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib
-        )
-endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
-# Jiri: This block isn't mutually exclusive with the previous one
-#       (hence no else), instead I test if the user really specified
-#       the variables in question.
-if ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
-    # HH: deprecated
-    message(STATUS "[Warning] FindTBB.cmake: The use of TBB_ARCHITECTURE and TBB_COMPILER is deprecated and may not be supported in future versions. Please set \$ENV{TBB_ARCH_PLATFORM} (using tbbvars.[bat|csh|sh]).")
-    # Jiri: It doesn't hurt to look in more places, so I store the hints from
-    #       ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER
-    #       variables and search them both.
-    set (
-		_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR}
-		_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/lib/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}" ${_TBB_LIBRARY_DIR}
-		)
-endif ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
+  ##################################
+  # Set version strings
+  ##################################
 
-# GvdB: Mac OS X distribution places libraries directly in lib directory.
-list(APPEND _TBB_LIBRARY_DIR ${_TBB_INSTALL_DIR}/lib)
+  if(TBB_INCLUDE_DIRS)
+    file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file)
+    string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
+        TBB_VERSION_MAJOR "${_tbb_version_file}")
+    string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
+        TBB_VERSION_MINOR "${_tbb_version_file}")
+    string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
+        TBB_INTERFACE_VERSION "${_tbb_version_file}")
+    set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}")
+  endif()
 
-# Jiri: No reason not to check the default paths. From recent versions,
-#       tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH
-#       variables, which now point to the directories of the lib files.
-#       It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS
-#       argument instead of the implicit PATHS as it isn't hard-coded
-#       but computed by system introspection. Searching the LIBRARY_PATH
-#       and LD_LIBRARY_PATH environment variables is now even more important
-#       that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates
-#       the use of TBB built from sources.
-find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR}
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
-find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR}
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
+  ##################################
+  # Find TBB components
+  ##################################
 
-#Extract path from TBB_LIBRARY name
-get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH)
+  if(TBB_VERSION VERSION_LESS 4.3)
+    set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb)
+  else()
+    set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb)
+  endif()
 
-#TBB_CORRECT_LIB_DIR(TBB_LIBRARY)
-#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY)
-mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY)
+  # Find each component
+  foreach(_comp ${TBB_SEARCH_COMPOMPONENTS})
+    if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};")
 
-#-- Look for debug libraries
-# Jiri: Changed the same way as for the release libraries.
-find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
-find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
+      # Search for the libraries
+      find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp}
+          HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
+          PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
+          PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
 
-# Jiri: Self-built TBB stores the debug libraries in a separate directory.
-#       Extract path from TBB_LIBRARY_DEBUG name
-get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH)
+      find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug
+          HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
+          PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
+          PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
 
-#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG)
-#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG)
-mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG)
+      if(TBB_${_comp}_LIBRARY_DEBUG)
+        list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}")
+      endif()
+      if(TBB_${_comp}_LIBRARY_RELEASE)
+        list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}")
+      endif()
+      if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY)
+        set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}")
+      endif()
 
+      if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}")
+        set(TBB_${_comp}_FOUND TRUE)
+      else()
+        set(TBB_${_comp}_FOUND FALSE)
+      endif()
 
-if (TBB_INCLUDE_DIR)
-    if (TBB_LIBRARY)
-        set (TBB_FOUND "YES")
-        set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES})
-        set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES})
-        set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE)
-        set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE)
-        # Jiri: Self-built TBB stores the debug libraries in a separate directory.
-        set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE)
-        mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES)
-        message(STATUS "Found Intel TBB")
-    endif (TBB_LIBRARY)
-endif (TBB_INCLUDE_DIR)
+      # Mark internal variables as advanced
+      mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE)
+      mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG)
+      mark_as_advanced(TBB_${_comp}_LIBRARY)
 
-if (NOT TBB_FOUND)
-    message(STATUS "TBB: Intel TBB NOT found!")
-    message(STATUS "TBB: Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}")
-    # do only throw fatal, if this pkg is REQUIRED
-    if (TBB_FIND_REQUIRED)
-        message(FATAL_ERROR "Could NOT find TBB library.")
-    endif (TBB_FIND_REQUIRED)
-endif (NOT TBB_FOUND)
+    endif()
+  endforeach()
 
-endif (NOT _TBB_INSTALL_DIR)
+  ##################################
+  # Set compile flags and libraries
+  ##################################
 
-if (TBB_FOUND)
-	set(TBB_INTERFACE_VERSION 0)
-	FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS)
-	STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}")
-	set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}")
-endif (TBB_FOUND)
\ No newline at end of file
+  set(TBB_DEFINITIONS_RELEASE "")
+  set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1")
+
+  if(TBB_LIBRARIES_${TBB_BUILD_TYPE})
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}")
+  elseif(TBB_LIBRARIES_RELEASE)
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}")
+  elseif(TBB_LIBRARIES_DEBUG)
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}")
+  endif()
+
+  find_package_handle_standard_args(TBB
+      REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES
+      HANDLE_COMPONENTS
+      VERSION_VAR TBB_VERSION)
+
+  ##################################
+  # Create targets
+  ##################################
+
+  if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND)
+    # Start fix to support different targets for tbb, tbbmalloc, etc.
+    # (Jose Luis Blanco, Jan 2019)
+    # Iterate over tbb, tbbmalloc, etc.
+    foreach(libname ${TBB_SEARCH_COMPOMPONENTS})
+      if ((NOT TBB_${libname}_LIBRARY_RELEASE) AND (NOT TBB_${libname}_LIBRARY_DEBUG))
+        continue()
+      endif()
+
+      add_library(${libname} SHARED IMPORTED)
+
+      set_target_properties(${libname} PROPERTIES
+            INTERFACE_INCLUDE_DIRECTORIES  ${TBB_INCLUDE_DIRS}
+            IMPORTED_LOCATION              ${TBB_${libname}_LIBRARY_RELEASE})
+      if(TBB_${libname}_LIBRARY_RELEASE AND TBB_${libname}_LIBRARY_DEBUG)
+        set_target_properties(${libname} PROPERTIES
+            INTERFACE_COMPILE_DEFINITIONS "$<$<OR:$<CONFIG:Debug>,$<CONFIG:RelWithDebInfo>>:TBB_USE_DEBUG=1>"
+            IMPORTED_LOCATION_DEBUG          ${TBB_${libname}_LIBRARY_DEBUG}
+            IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_${libname}_LIBRARY_DEBUG}
+            IMPORTED_LOCATION_RELEASE        ${TBB_${libname}_LIBRARY_RELEASE}
+            IMPORTED_LOCATION_MINSIZEREL     ${TBB_${libname}_LIBRARY_RELEASE}
+            )
+      elseif(TBB_${libname}_LIBRARY_RELEASE)
+        set_target_properties(${libname} PROPERTIES IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
+      else()
+        set_target_properties(${libname} PROPERTIES
+            INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}"
+            IMPORTED_LOCATION              ${TBB_${libname}_LIBRARY_DEBUG}
+            )
+      endif()
+    endforeach()
+    # End of fix to support different targets
+  endif()
+
+  mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES)
+
+  unset(TBB_ARCHITECTURE)
+  unset(TBB_BUILD_TYPE)
+  unset(TBB_LIB_PATH_SUFFIX)
+  unset(TBB_DEFAULT_SEARCH_DIR)
+
+endif()
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 09c95fd12..c3ffa4eff 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -160,4 +160,3 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Wrap
     wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
 endif ()
-
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 9ce9d9a22..2d334df0f 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -65,44 +65,40 @@ set(gtsam_unstable_soversion ${GTSAM_VERSION_MAJOR})
 message(STATUS "GTSAM_UNSTABLE Version: ${gtsam_unstable_version}")
 message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
 
-# build shared or static versions of the library
-if (GTSAM_BUILD_STATIC_LIBRARY)
-	message(STATUS "Building GTSAM_UNSTABLE - static")
-	add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs})
-	set_target_properties(gtsam_unstable PROPERTIES
-		OUTPUT_NAME         gtsam_unstable
-		CLEAN_DIRECT_OUTPUT 1
-		VERSION             ${gtsam_unstable_version}
-		SOVERSION           ${gtsam_unstable_soversion})
-	if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+# BUILD_SHARED_LIBS automatically defines static/shared libs:
+add_library(gtsam_unstable ${gtsam_unstable_srcs})
+set_target_properties(gtsam_unstable PROPERTIES
+  OUTPUT_NAME         gtsam_unstable
+  CLEAN_DIRECT_OUTPUT 1
+  VERSION             ${gtsam_unstable_version}
+  SOVERSION           ${gtsam_unstable_soversion})
+target_link_libraries(gtsam_unstable
+    PUBLIC
+    gtsam
+    ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
+
+if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+	if (NOT BUILD_SHARED_LIBS)
 		set_target_properties(gtsam_unstable PROPERTIES
 			PREFIX "lib"
 			COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC)
-	endif()
-	target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
-	install(TARGETS gtsam_unstable EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
-	list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
-	set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-else()
-    message(STATUS "Building GTSAM_UNSTABLE - shared")
-    add_library(gtsam_unstable SHARED ${gtsam_unstable_srcs})
-    set_target_properties(gtsam_unstable PROPERTIES
-        OUTPUT_NAME         gtsam_unstable
-        CLEAN_DIRECT_OUTPUT 1
-    	VERSION             ${gtsam_unstable_version}
-     	SOVERSION           ${gtsam_unstable_soversion})
-	if(WIN32)
+	else()
 		set_target_properties(gtsam_unstable PROPERTIES
 			PREFIX ""
 			DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
 	endif()
-    target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
-    install(TARGETS gtsam_unstable EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-    list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
-    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 endif()
 
+install(
+	TARGETS gtsam_unstable
+	EXPORT GTSAM-exports
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	RUNTIME DESTINATION bin)
+list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+
 # Wrap version for gtsam_unstable
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Set up codegen

From c29090c427a565266da05ada7df8b67e300cabc7 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Wed, 9 Jan 2019 17:03:26 -0800
Subject: [PATCH 117/281] Fix alignment crash in numerical derivative with
 march=native

---
 gtsam/base/numericalDerivative.h | 46 ++++++++++++++++----------------
 1 file changed, 23 insertions(+), 23 deletions(-)

diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index 2fa6e33c6..cc1cbdb51 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -177,7 +177,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -202,7 +202,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::func
 //       "Template argument X1 must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
        "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1), x2, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -230,7 +230,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
@@ -258,7 +258,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
@@ -286,7 +286,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
-  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1), x3, delta);
+  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
@@ -314,7 +314,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -341,7 +341,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -368,7 +368,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
-  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4), x3, delta);
+  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -395,7 +395,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
       "Template argument X4 must be a manifold type.");
-  return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1), x4, delta);
+  return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -423,7 +423,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4, x5), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -451,7 +451,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4, x5), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -479,7 +479,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4, x5), x3, delta);
+  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -507,7 +507,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1, x5), x4, delta);
+  return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -535,7 +535,7 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X5>(boost::bind(h, x1, x2, x3, x4, _1), x5, delta);
+  return numericalDerivative11<Y, X5>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -587,7 +587,7 @@ public:
       f_(f), x1_(x1), delta_(delta) {
   }
   Vector operator()(const X2& x2) {
-    return numericalGradient<X1>(boost::bind(f_, _1, x2), x1_, delta_);
+    return numericalGradient<X1>(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_);
   }
 };
 
@@ -618,7 +618,7 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
 
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(boost::bind(f, _1, x2));
+  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2)));
 
   return numericalDerivative11<Vector, X1>(
       boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -639,7 +639,7 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(boost::bind(f, x1, _1));
+  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1));
 
   return numericalDerivative11<Vector, X2>(
       boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -664,7 +664,7 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(boost::bind(f, _1, x2, x3));
+  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3)));
 
   return numericalDerivative11<Vector, X1>(
       boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -687,7 +687,7 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(boost::bind(f, x1, _1, x3));
+  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3)));
 
   return numericalDerivative11<Vector, X2>(
       boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -710,7 +710,7 @@ inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
   typedef typename internal::FixedSizeMatrix<X3>::type Vector;
   Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
       double) = &numericalGradient<X3>;
-  boost::function<double(const X3&)> f2(boost::bind(f, x1, x2, _1));
+  boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1));
 
   return numericalDerivative11<Vector, X3>(
       boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -731,7 +731,7 @@ inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X2>(
-      boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, x3)),
+      boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, boost::cref(x3))),
       x1, x2, delta);
 }
 
@@ -740,7 +740,7 @@ inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X3>(
-      boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, x2, _2)),
+      boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
       x1, x3, delta);
 }
 
@@ -749,7 +749,7 @@ inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X2, X3>(
-      boost::function<double(const X2&, const X3&)>(boost::bind(f, x1, _1, _2)),
+      boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _2)),
       x2, x3, delta);
 }
 

From d2cedc6c967f61da87c1452c4b48a93a44bab7fa Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Wed, 9 Jan 2019 17:58:52 -0800
Subject: [PATCH 118/281] attempt to fix alignment error in expression factors

---
 gtsam/nonlinear/internal/ExecutionTrace.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h
index a147f505e..ace0aaea8 100644
--- a/gtsam/nonlinear/internal/ExecutionTrace.h
+++ b/gtsam/nonlinear/internal/ExecutionTrace.h
@@ -35,7 +35,7 @@ template<int T> struct CallRecord;
 /// Storage type for the execution trace.
 /// It enforces the proper alignment in a portable way.
 /// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
-static const unsigned TraceAlignment = 16;
+static const unsigned TraceAlignment = 32;
 typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
 
 template<bool UseBlock, typename Derived>

From 704f302129da0ef17312f857541a850695f8abe0 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Thu, 10 Jan 2019 14:04:11 -0800
Subject: [PATCH 119/281] Fix testExpression

---
 gtsam/nonlinear/tests/testExpression.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index bde79e780..7a06179b4 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -196,6 +196,7 @@ TEST(Expression, BinaryDimensions) {
 TEST(Expression, BinaryTraceSize) {
   typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t expectedTraceSize = sizeof(Binary::Record);
+  internal::upAlign(expectedTraceSize);
   EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize());
 }
 

From 0b1791583f5d3683e855401982da0968516b5563 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>
Date: Thu, 10 Jan 2019 23:37:12 +0100
Subject: [PATCH 120/281] missing GTSAM_BUILD_STATIC_LIBRARY flags

---
 gtsam/CMakeLists.txt          | 6 +++---
 gtsam_unstable/CMakeLists.txt | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index c3ffa4eff..7fa9859ea 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -153,9 +153,9 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 
     # Generate, build and install toolbox
     set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-	if(GTSAM_BUILD_STATIC_LIBRARY)
-		list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
-	endif()
+        if(NOT BUILD_SHARED_LIBS)
+                list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
+        endif()
 
     # Wrap
     wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 2d334df0f..1494dcf41 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -106,9 +106,9 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 
     # Generate, build and install toolbox
     set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-	if(GTSAM_BUILD_STATIC_LIBRARY)
-		list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
-	endif()
+        if(NOT BUILD_SHARED_LIBS)
+                list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
+        endif()
 
     # Wrap
     wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")

From 2505ef6623fc8c440e8ac62d4b5315c90e6eefa0 Mon Sep 17 00:00:00 2001
From: jlblancoc <joseluisblancoc@gmail.com>
Date: Wed, 9 Jan 2019 00:58:13 +0100
Subject: [PATCH 121/281] metis: rely on global BUILD_SHARED_LIBS"

---
 gtsam/3rdparty/metis/CMakeLists.txt | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt
index dd21338a4..8fca88e26 100644
--- a/gtsam/3rdparty/metis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/CMakeLists.txt
@@ -20,7 +20,6 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
 endif()
 
 set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib")
-set(METIS_SHARED TRUE CACHE BOOL "build a shared library")
 
 if(MSVC)
   set(METIS_INSTALL FALSE)
@@ -29,11 +28,11 @@ else()
 endif()
 
 # Configure libmetis library.
-if(METIS_SHARED)
+if(BUILD_SHARED_LIBS)
   set(METIS_LIBRARY_TYPE SHARED)
 else()
   set(METIS_LIBRARY_TYPE STATIC)
-endif(METIS_SHARED)
+endif()
 
 include(${GKLIB_PATH}/GKlibSystem.cmake)
 # Add include directories.
@@ -48,4 +47,3 @@ if(GTSAM_BUILD_METIS_EXECUTABLES)
 endif()
 
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-

From 223020ec8200ed6e0aed771f8ee1959180aa8c4f Mon Sep 17 00:00:00 2001
From: Vikrant Shah <vikrantshah@gmail.com>
Date: Fri, 18 Jan 2019 22:05:57 -0500
Subject: [PATCH 122/281] Adding wrapper for Pose3 for reading g2o files and
 examples for Pose2 and Pose3 slam using g2o file

---
 cython/gtsam/examples/Pose2SLAMExample_g2o.py | 85 +++++++++++++++++++
 cython/gtsam/examples/Pose3SLAMExample_g2o.py | 56 ++++++++++++
 gtsam.h                                       |  2 +
 3 files changed, 143 insertions(+)
 create mode 100644 cython/gtsam/examples/Pose2SLAMExample_g2o.py
 create mode 100644 cython/gtsam/examples/Pose3SLAMExample_g2o.py

diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
new file mode 100644
index 000000000..7920a4aa2
--- /dev/null
+++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
@@ -0,0 +1,85 @@
+"""
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the optimization. Output is written on a file, in g2o format 
+
+Syntax for the script is "python ./Pose2SLAMExample_g2o.py input.g2o output.g2o"
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+import math
+import numpy as np
+import gtsam
+import sys
+
+
+def vector3(x, y, z):
+    """Create 3d double numpy array."""
+    return np.array([x, y, z], dtype=np.float)
+
+kernelType = "none"
+maxIterations = 100 # default
+g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") # default
+
+if len(sys.argv) > 1:
+    g2ofile = str(sys.argv[1])
+    print ("Input file: ",g2ofile)
+if len(sys.argv) > 3:
+    maxIterations = int(sys.argv[3])
+    print("Sepficied max iterations: ", maxIterations)
+if len(sys.argv) > 4:
+    kernelType = sys.argv[4]
+
+is3D = False # readG2o 3d parameter not available at time of this writing
+
+if kernelType is "none":
+    graph, initial = gtsam.readG2o(g2oFile)
+if kernelType is "huber":
+    print("Using robust kernel: huber - NOT CURRENTLY IMPLEMENTED IN PYTHON")
+    #graph, initial = gtsam.readG2o(g2oFile,is3D, KernelFunctionTypeHUBER)
+if kernelType is "tukey":
+    print("Using robust kernel: tukey - NOT CURRENTLY IMPLEMENTED IN PYTHON")
+    #graph, initial = gtsam.readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY)
+
+# Add prior on the pose having index (key) = 0
+graphWithPrior = graph
+priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
+graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
+print("Adding prior on pose 0 ")
+
+print("\nFactor Graph:\n{}".format(graph)) 
+
+print("\nInitial Estimate:\n{}".format(initial))
+
+params = gtsam.GaussNewtonParams()
+params.setVerbosity("Termination")
+
+if (sys.argv > 3):
+    params.setMaxIterations(maxIterations)
+    print("User setting: required to perform maximum ", maxIterations," iterations ")
+
+#parameters.setRelativeErrorTol(1e-5)
+
+# Create the optimizer ...
+optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+# ... and optimize
+result = optimizer.optimize()
+
+print("Optimization complete")
+print("initial error = ", graph.error(initial))
+print("final error = ", graph.error(result))
+
+if len(sys.argv) < 3:
+    print("Final Result:\n{}".format(result))
+else:
+    outputFile = sys.argv[2]
+    print("Writing results to file: ", outputFile)
+    graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
+    gtsam.writeG2o(graphNoKernel, result, outputFile)
+    print ("Done!")
diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
new file mode 100644
index 000000000..efafe0ecf
--- /dev/null
+++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
@@ -0,0 +1,56 @@
+"""
+ * @file Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * @date Jan 17, 2019 
+ * @author Vikrant Shah based on CPP example by Luca Carlone
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import math
+
+import numpy as np
+
+import gtsam
+
+import sys
+
+def vector6(x, y, z, a, b, c):
+    """Create 6d double numpy array."""
+    return np.array([x, y, z, a, b, c], dtype=np.float)
+
+if len(sys.argv) < 2:
+    g2oFile = gtsam.findExampleDataFile("pose3example.txt")
+else:
+    g2oFile = str(sys.argv[1])
+is3D = True
+graph, initial = gtsam.readG2o(g2oFile,is3D)
+
+# Add Prior on the first key
+priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 
+                                                         1e-4, 1e-4, 1e-4))
+
+print("Adding prior to g2o file ")
+graphWithPrior = graph
+firstKey = initial.keys().at(0)
+graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
+
+params = gtsam.GaussNewtonParams()
+params.setVerbosity("Termination") # this will show info about stopping conditions
+optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+result = optimizer.optimize()
+print("Optimization complete")
+
+print("initial error = ",graphWithPrior.error(initial))
+print("final error = ",graphWithPrior.error(result))
+
+if len(sys.argv) < 3:
+    print("Final Result:\n{}".format(result))
+else:
+    outputFile = sys.argv[2]
+    print("Writing results to file: ", outputFile)
+    graphNoKernel, initial2 = gtsam.readG2o(g2oFile,is3D)
+    gtsam.writeG2o(graphNoKernel, result, outputFile)
+    print ("Done!")
diff --git a/gtsam.h b/gtsam.h
index 8097cc6c9..bcf8f90f4 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2449,6 +2449,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/dataset.h>
+string findExampleDataFile(string name);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
@@ -2465,6 +2466,7 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
     string filename);
 
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& estimate, string filename);
 

From edb94a6e938d2166aa30fe5adcec552930bcd299 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 20 Jan 2019 16:52:44 -0500
Subject: [PATCH 123/281] Better error message.

---
 wrap/Method.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/wrap/Method.cpp b/wrap/Method.cpp
index 1a58692b6..8f0ef17b9 100644
--- a/wrap/Method.cpp
+++ b/wrap/Method.cpp
@@ -200,6 +200,6 @@ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const {
     file.oss << "            pass\n";
   }
   file.oss
-      << "        raise TypeError('Incorrect arguments for method call.')\n\n";
+      << "        raise TypeError('Incorrect arguments or types for method call.')\n\n";
 }
 /* ************************************************************************* */

From b3b69bfd21c7d1dffcee5bc595ec5f4e13e4a265 Mon Sep 17 00:00:00 2001
From: Vikrant Shah <vikrantshah@gmail.com>
Date: Mon, 21 Jan 2019 00:46:57 -0500
Subject: [PATCH 124/281] Addressing Duy's comments about argparse and PEP8. 
 Adding plot flag to the examples

---
 cython/gtsam/examples/Pose2SLAMExample_g2o.py | 82 ++++++++++---------
 cython/gtsam/examples/Pose3SLAMExample_g2o.py | 54 +++++++-----
 2 files changed, 78 insertions(+), 58 deletions(-)

diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
index 7920a4aa2..0ec80d169 100644
--- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py
+++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
@@ -6,66 +6,61 @@ Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 See LICENSE for the license information
 
-A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the optimization. Output is written on a file, in g2o format 
-
-Syntax for the script is "python ./Pose2SLAMExample_g2o.py input.g2o output.g2o"
+A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph
+and does the optimization. Output is written on a file, in g2o format
 """
 # pylint: disable=invalid-name, E1101
 
 from __future__ import print_function
+import argparse
 import math
 import numpy as np
+import matplotlib.pyplot as plt
+
 import gtsam
-import sys
+from utils import plot
 
 
 def vector3(x, y, z):
     """Create 3d double numpy array."""
     return np.array([x, y, z], dtype=np.float)
 
-kernelType = "none"
-maxIterations = 100 # default
-g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") # default
 
-if len(sys.argv) > 1:
-    g2ofile = str(sys.argv[1])
-    print ("Input file: ",g2ofile)
-if len(sys.argv) > 3:
-    maxIterations = int(sys.argv[3])
-    print("Sepficied max iterations: ", maxIterations)
-if len(sys.argv) > 4:
-    kernelType = sys.argv[4]
+parser = argparse.ArgumentParser(
+    description="A 2D Pose SLAM example that reads input from g2o, "
+                "converts it to a factor graph and does the optimization. "
+                "Output is written on a file, in g2o format")
+parser.add_argument('-i', '--input', help='input file g2o format')
+parser.add_argument('-o', '--output',
+                    help="the path to the output file with optimized graph")
+parser.add_argument('-m', '--maxiter', type=int,
+                    help="maximum number of iterations for optimizer")
+parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'],
+                    default="none", help="Type of kernel used")
+parser.add_argument("-p", "--plot", action="store_true",
+                    help="Flag to plot results")
+args = parser.parse_args()
 
-is3D = False # readG2o 3d parameter not available at time of this writing
+g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
+    else args.input
 
-if kernelType is "none":
-    graph, initial = gtsam.readG2o(g2oFile)
-if kernelType is "huber":
-    print("Using robust kernel: huber - NOT CURRENTLY IMPLEMENTED IN PYTHON")
-    #graph, initial = gtsam.readG2o(g2oFile,is3D, KernelFunctionTypeHUBER)
-if kernelType is "tukey":
-    print("Using robust kernel: tukey - NOT CURRENTLY IMPLEMENTED IN PYTHON")
-    #graph, initial = gtsam.readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY)
+maxIterations = 100 if args.maxiter is None else args.maxiter
+
+is3D = False
+
+graph, initial = gtsam.readG2o(g2oFile, is3D)
+
+assert args.kernel == "none", "Supplied kernel type is not yet implemented"
 
 # Add prior on the pose having index (key) = 0
 graphWithPrior = graph
 priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
 graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
-print("Adding prior on pose 0 ")
-
-print("\nFactor Graph:\n{}".format(graph)) 
-
-print("\nInitial Estimate:\n{}".format(initial))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")
-
-if (sys.argv > 3):
-    params.setMaxIterations(maxIterations)
-    print("User setting: required to perform maximum ", maxIterations," iterations ")
-
-#parameters.setRelativeErrorTol(1e-5)
-
+params.setMaxIterations(maxIterations)
+# parameters.setRelativeErrorTol(1e-5)
 # Create the optimizer ...
 optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
 # ... and optimize
@@ -75,11 +70,20 @@ print("Optimization complete")
 print("initial error = ", graph.error(initial))
 print("final error = ", graph.error(result))
 
-if len(sys.argv) < 3:
+
+if args.output is None:
+    print("\nFactor Graph:\n{}".format(graph))
+    print("\nInitial Estimate:\n{}".format(initial))
     print("Final Result:\n{}".format(result))
 else:
-    outputFile = sys.argv[2]
+    outputFile = args.output
     print("Writing results to file: ", outputFile)
-    graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
+    graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
     gtsam.writeG2o(graphNoKernel, result, outputFile)
     print ("Done!")
+
+if args.plot:
+    resultPoses = gtsam.extractPose2(result)
+    for i in range(resultPoses.shape[0]):
+        plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
+    plt.show()
diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
index efafe0ecf..787dc5d99 100644
--- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py
+++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
@@ -1,35 +1,45 @@
 """
  * @file Pose3SLAMExample_initializePose3.cpp
- * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
- * @date Jan 17, 2019 
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the
+ *  Pose3 using InitializePose3
+ * @date Jan 17, 2019
  * @author Vikrant Shah based on CPP example by Luca Carlone
 """
 # pylint: disable=invalid-name, E1101
 
 from __future__ import print_function
-
-import math
-
+import argparse
 import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
 
 import gtsam
+from utils import plot
 
-import sys
 
 def vector6(x, y, z, a, b, c):
     """Create 6d double numpy array."""
     return np.array([x, y, z, a, b, c], dtype=np.float)
 
-if len(sys.argv) < 2:
-    g2oFile = gtsam.findExampleDataFile("pose3example.txt")
-else:
-    g2oFile = str(sys.argv[1])
+
+parser = argparse.ArgumentParser(
+    description="A 3D Pose SLAM example that reads input from g2o, and "
+                "initializes Pose3")
+parser.add_argument('-i', '--input', help='input file g2o format')
+parser.add_argument('-o', '--output',
+                    help="the path to the output file with optimized graph")
+parser.add_argument("-p", "--plot", action="store_true",
+                    help="Flag to plot results")
+args = parser.parse_args()
+
+g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
+    else args.input
+
 is3D = True
-graph, initial = gtsam.readG2o(g2oFile,is3D)
+graph, initial = gtsam.readG2o(g2oFile, is3D)
 
 # Add Prior on the first key
-priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 
+priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
                                                          1e-4, 1e-4, 1e-4))
 
 print("Adding prior to g2o file ")
@@ -38,19 +48,25 @@ firstKey = initial.keys().at(0)
 graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
 params = gtsam.GaussNewtonParams()
-params.setVerbosity("Termination") # this will show info about stopping conditions
+params.setVerbosity("Termination")  # this will show info about stopping conds
 optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
 result = optimizer.optimize()
 print("Optimization complete")
 
-print("initial error = ",graphWithPrior.error(initial))
-print("final error = ",graphWithPrior.error(result))
+print("initial error = ", graphWithPrior.error(initial))
+print("final error = ", graphWithPrior.error(result))
 
-if len(sys.argv) < 3:
+if args.output is None:
     print("Final Result:\n{}".format(result))
 else:
-    outputFile = sys.argv[2]
+    outputFile = args.output
     print("Writing results to file: ", outputFile)
-    graphNoKernel, initial2 = gtsam.readG2o(g2oFile,is3D)
+    graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
     gtsam.writeG2o(graphNoKernel, result, outputFile)
     print ("Done!")
+
+if args.plot:
+    resultPoses = gtsam.allPose3s(result)
+    for i in range(resultPoses.size()):
+        plot.plot_pose3(1, resultPoses.atPose3(i))
+    plt.show()

From ddf0c839181bf5b3e284a9fbe92e4af04ecb44de Mon Sep 17 00:00:00 2001
From: Vikrant Shah <vikrantshah@gmail.com>
Date: Mon, 21 Jan 2019 13:29:46 -0500
Subject: [PATCH 125/281] Fixing gtsam.utils import

---
 cython/gtsam/examples/Pose2SLAMExample_g2o.py | 2 +-
 cython/gtsam/examples/Pose3SLAMExample_g2o.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
index 0ec80d169..3aee7daff 100644
--- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py
+++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py
@@ -18,7 +18,7 @@ import numpy as np
 import matplotlib.pyplot as plt
 
 import gtsam
-from utils import plot
+from gtsam.utils import plot
 
 
 def vector3(x, y, z):
diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
index 787dc5d99..38c5db275 100644
--- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py
+++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py
@@ -14,7 +14,7 @@ import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
 
 import gtsam
-from utils import plot
+from gtsam.utils import plot
 
 
 def vector6(x, y, z, a, b, c):

From 1aa36b2ea9f60848d4d25d7d2517d5d36b5d2c6e Mon Sep 17 00:00:00 2001
From: chrisbeall <chrisbeall@gmail.com>
Date: Mon, 21 Jan 2019 23:27:58 -0800
Subject: [PATCH 126/281] Fix testPose2 failure by forcing eval in wedge
 expression. Always align Pose2.

---
 gtsam/geometry/Pose2.h | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 079c649f3..603c3a421 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -295,17 +295,16 @@ private:
     ar & BOOST_SERIALIZATION_NVP(r_);
   }
 
-#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
 public:
-  // Make sure Pose2 is aligned if it contains a Vector2
+  // Align for Point2, which is either derived from, or is typedef, of Vector2
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-#endif
 }; // Pose2
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */
 template <>
 inline Matrix wedge<Pose2>(const Vector& xi) {
-  return Pose2::wedge(xi(0),xi(1),xi(2));
+  // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
+  return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
 }
 
 /**

From 9ea90b5b451ada9d5282c8c9e4661cb5f9e53b44 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <jlblanco@ual.es>
Date: Tue, 15 Jan 2019 00:38:14 +0100
Subject: [PATCH 127/281] port to target-based compile options and definitions

This also fixes CMake warnings on Windows
---
 CMakeLists.txt                       | 38 +++++++++++++++-------------
 cmake/example_project/CMakeLists.txt |  2 +-
 gtsam/CMakeLists.txt                 |  7 ++---
 gtsam_unstable/CMakeLists.txt        |  8 ++----
 4 files changed, 28 insertions(+), 27 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 972c40650..673f9a45f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,6 @@
 
 project(GTSAM CXX C)
-cmake_minimum_required(VERSION 2.8.4)
+cmake_minimum_required(VERSION 3.0)
 
 # new feature to Cmake Version > 2.8.12
 # Mac ONLY. Define Relative Path on Mac OS
@@ -121,12 +121,11 @@ if(MSVC)
 	# If we use Boost shared libs, disable auto linking.
 	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
 	if(NOT Boost_USE_STATIC_LIBS)
-		add_definitions(-DBOOST_ALL_NO_LIB)
-	  add_definitions(-DBOOST_ALL_DYN_LINK)
+		list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
 	endif()
 	# Virtual memory range for PCH exceeded on VS2015
 	if(MSVC_VERSION LESS 1910) # older than VS2017
-	  add_definitions(-Zm295)
+	  list(APPEND GTSAM_COMPILE_OPTIONS -Zm295)
 	endif()
 endif()
 
@@ -141,14 +140,19 @@ endif()
 option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
 # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
 set(GTSAM_BOOST_LIBRARIES
-    ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
-	${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
+  Boost::serialization
+  Boost::system
+  Boost::filesystem
+  Boost::thread
+  Boost::date_time
+  Boost::regex
+)
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
-    add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
+    list(APPEND GTSAM_COMPILE_DEFINITIONS DGTSAM_DISABLE_NEW_TIMERS)
 else()
     if(Boost_TIMER_LIBRARY)
-      list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
+      list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
     else()
       list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
       message("WARNING:  GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
@@ -158,7 +162,7 @@ endif()
 
 if(NOT (${Boost_VERSION} LESS 105600))
 	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
-	add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
+	list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
 endif()
 
 ###############################################################################
@@ -279,9 +283,9 @@ endif()
 if (MSVC)
 	if (BUILD_SHARED_LIBS)
 		# mute eigen static assert to avoid errors in shared lib
-		add_definitions(-DEIGEN_NO_STATIC_ASSERT)
+		list(APPEND GTSAM_COMPILE_DEFINITIONS DEIGEN_NO_STATIC_ASSERT)
 	endif()
-	add_definitions(/wd4244) # Disable loss of precision which is thrown all over our Eigen
+	list(APPEND GTSAM_COMPILE_OPTIONS "/wd4244") # Disable loss of precision which is thrown all over our Eigen
 endif()
 
 ###############################################################################
@@ -347,27 +351,27 @@ include_directories(BEFORE
   CppUnitLite)
 
 if(MSVC)
-	add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
-	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
-	add_definitions(/bigobj) # Allow large object files for template-based code
+	list(APPEND GTSAM_COMPILE_DEFINITIONS _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
+	list(APPEND GTSAM_COMPILE_OPTIONS /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
+	list(APPEND GTSAM_COMPILE_OPTIONS /bigobj) # Allow large object files for template-based code
 endif()
 
 # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
-    add_definitions(-Wno-unused-local-typedefs)
+    list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs)
   endif()
 endif()
 
 # As of XCode 7, clang also complains about this
 if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
   if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0)
-    add_definitions(-Wno-unused-local-typedefs)
+    list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs)
   endif()
 endif()
 
 if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
-  add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
+  list(APPEND GTSAM_COMPILE_DEFINITIONS GTSAM_EXTRA_CONSISTENCY_CHECKS)
 endif()
 
 ###############################################################################
diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt
index 548d56acd..e287087d1 100644
--- a/cmake/example_project/CMakeLists.txt
+++ b/cmake/example_project/CMakeLists.txt
@@ -7,7 +7,7 @@
 
 ###################################################################################
 # To create your own project, replace "example" with the actual name of your project
-cmake_minimum_required(VERSION 2.6)
+cmake_minimum_required(VERSION 3.0)
 project(example CXX C)
 
 # Include GTSAM CMake tools
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 7fa9859ea..d369009c0 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -101,9 +101,10 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
 
 # BUILD_SHARED_LIBS automatically defines static/shared libs:
 add_library(gtsam ${gtsam_srcs})
-target_link_libraries(gtsam
-    PUBLIC
-    ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
+target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
+target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
+target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS})
+target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS})
 set_target_properties(gtsam PROPERTIES
     OUTPUT_NAME         gtsam
     CLEAN_DIRECT_OUTPUT 1
diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt
index 1494dcf41..327326201 100644
--- a/gtsam_unstable/CMakeLists.txt
+++ b/gtsam_unstable/CMakeLists.txt
@@ -14,8 +14,6 @@ if(GTSAM_SUPPORT_NESTED_DISSECTION) # Only build partition if metis is built
     set (gtsam_unstable_subdirs ${gtsam_unstable_subdirs} partition)
 endif(GTSAM_SUPPORT_NESTED_DISSECTION)
 
-set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES})
-
 add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
 
 # To exclude a source from the library build (in any subfolder)
@@ -72,10 +70,8 @@ set_target_properties(gtsam_unstable PROPERTIES
   CLEAN_DIRECT_OUTPUT 1
   VERSION             ${gtsam_unstable_version}
   SOVERSION           ${gtsam_unstable_soversion})
-target_link_libraries(gtsam_unstable
-    PUBLIC
-    gtsam
-    ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
+target_link_libraries(gtsam_unstable PUBLIC gtsam)
+# No need to link against Boost here, it's inherited from gtsam PUBLIC interface
 
 if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
 	if (NOT BUILD_SHARED_LIBS)

From ca9c9720281eee008c3ad4a6e9b061482ba844a9 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 25 Jan 2019 15:20:54 -0500
Subject: [PATCH 128/281] Wrapping BearingRange

---
 gtsam.h                       | 15 +++++++++++++++
 gtsam/geometry/BearingRange.h | 24 +++++++++++++++++-------
 2 files changed, 32 insertions(+), 7 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 8097cc6c9..64f025972 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2316,6 +2316,21 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
 
+#include <gtsam/geometry/BearingRange.h>
+template <POSE, POINT, BEARING, RANGE>
+class BearingRange {
+  BearingRange(const BEARING& b, const RANGE& r);
+  BEARING bearing() const;
+  RANGE range() const;
+  // TODO(frank): can't class instance itself?
+  // static gtsam::BearingRange Measure(const POSE& pose, const POINT& point);
+  static BEARING MeasureBearing(const POSE& pose, const POINT& point);
+  static RANGE MeasureRange(const POSE& pose, const POINT& point);
+  void print(string s) const;
+};
+
+typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D;
+
 #include <gtsam/sam/BearingRangeFactor.h>
 template<POSE, POINT, BEARING, RANGE>
 virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index ffa373027..8214b0727 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -44,11 +44,11 @@ struct Range;
  * For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double>
  * and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
  */
-template <typename A1, typename A2>
+template <typename A1, typename A2,
+          typename B = typename Bearing<A1, A2>::result_type,
+          typename R = typename Range<A1, A2>::result_type>
 struct BearingRange {
 private:
-  typedef typename Bearing<A1, A2>::result_type B;
-  typedef typename Range<A1, A2>::result_type R;
   B bearing_;
   R range_;
 
@@ -91,6 +91,16 @@ public:
     return BearingRange(b, r);
   }
 
+  /// Predict bearing
+  static B MeasureBearing(const A1& a1, const A2& a2) {
+    return Bearing<A1, A2>()(a1, a2);
+  }
+
+  /// Predict range
+  static R MeasureRange(const A1& a1, const A2& a2) {
+    return Range<A1, A2>()(a1, a2);
+  }
+
   /// @}
   /// @name Testable
   /// @{
@@ -170,8 +180,8 @@ struct HasBearing {
   typedef RT result_type;
   RT operator()(
       const A1& a1, const A2& a2,
-      OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
-      OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
+      OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none,
+      OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
     return a1.bearing(a2, H1, H2);
   }
 };
@@ -184,8 +194,8 @@ struct HasRange {
   typedef RT result_type;
   RT operator()(
       const A1& a1, const A2& a2,
-      OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
-      OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
+      OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1=boost::none,
+      OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
     return a1.range(a2, H1, H2);
   }
 };

From 5acd89f6344cd0fa81520c2f7c4ddc1c3eb3768a Mon Sep 17 00:00:00 2001
From: Mike Sheffler <msheffler@toyon.com>
Date: Tue, 29 Jan 2019 00:28:54 -0800
Subject: [PATCH 129/281] Non-include of <iostream> caused compilation errors
 related to std::cout usage

---
 gtsam/base/serializationTestHelpers.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h
index 4ef09beb1..06b3462e9 100644
--- a/gtsam/base/serializationTestHelpers.h
+++ b/gtsam/base/serializationTestHelpers.h
@@ -19,6 +19,7 @@
 
 #pragma once
 
+#include <iostream>
 #include <sstream>
 #include <string>
 

From ca7866811336baa8212b3403989cd9951df02824 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 11 Feb 2019 10:33:16 -0500
Subject: [PATCH 130/281] Deleted legacy files

---
 cython/gtsam/tests/experiments.py | 112 -----
 cython/gtsam/tests/gtsam_test.h   | 772 ------------------------------
 2 files changed, 884 deletions(-)
 delete mode 100644 cython/gtsam/tests/experiments.py
 delete mode 100644 cython/gtsam/tests/gtsam_test.h

diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py
deleted file mode 100644
index 425180173..000000000
--- a/cython/gtsam/tests/experiments.py
+++ /dev/null
@@ -1,112 +0,0 @@
-"""
-This file is not a real python unittest. It contains small experiments
-to test the wrapper with gtsam_test, a short version of gtsam.h.
-Its name convention is different from other tests so it won't be discovered.
-"""
-import gtsam
-import numpy as np
-
-r = gtsam.Rot3()
-print(r)
-print(r.pitch())
-r2 = gtsam.Rot3()
-r3 = r.compose(r2)
-print("r3 pitch:", r3.pitch())
-
-v = np.array([1, 1, 1]) 
-print("v = ", v)
-r4 = r3.retract(v)
-print("r4 pitch:", r4.pitch())
-r4.print_(b'r4: ')
-r3.print_(b"r3: ")
-
-v = r3.localCoordinates(r4)
-print("localCoordinates:", v)
-
-Rmat = np.array([
-    [0.990074, -0.0942928, 0.104218], 
-    [0.104218,  0.990074, -0.0942928], 
-    [-0.0942928, 0.104218, 0.990074]
-    ])
-r5 = gtsam.Rot3(Rmat)
-r5.print_(b"r5: ")
-
-l = gtsam.Rot3.Logmap(r5)
-print("l = ", l)
-
-
-noise = gtsam.noiseModel_Gaussian.Covariance(Rmat)
-noise.print_(b"noise:")
-
-D = np.array([1.,2.,3.])
-diag = gtsam.noiseModel_Diagonal.Variances(D)
-print("diag:", diag)
-diag.print_(b"diag:")
-print("diag R:", diag.R())
-
-p = gtsam.Point3()
-p.print_("p:")
-factor = gtsam.BetweenFactorPoint3(1,2,p, noise)
-factor.print_(b"factor:")
-
-vv = gtsam.VectorValues()
-vv.print_(b"vv:")
-vv.insert(1, np.array([1.,2.,3.]))
-vv.insert(2, np.array([3.,4.]))
-vv.insert(3, np.array([5.,6.,7.,8.]))
-vv.print_(b"vv:")
-
-vv2 = gtsam.VectorValues(vv)
-vv2.insert(4, np.array([4.,2.,1]))
-vv2.print_(b"vv2:")
-vv.print_(b"vv:")
-
-vv.insert(4, np.array([1.,2.,4.]))
-vv.print_(b"vv:")
-vv3 = vv.add(vv2)
-
-vv3.print_(b"vv3:")
-
-values = gtsam.Values()
-values.insert(1, gtsam.Point3())
-values.insert(2, gtsam.Rot3())
-values.print_(b"values:")
-
-factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag)
-print "Prior factor vector: ", factor
-
-
-
-keys = gtsam.KeyVector()
-
-keys.push_back(1)
-keys.push_back(2)
-print 'size: ', keys.size()
-print keys.at(0)
-print keys.at(1)
-
-noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
-noise.print_('noise:')
-print 'noise print:', noise
-f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise,  b=np.ones(2))
-print 'JacobianFactor(7):\n', f
-print "A = ", f.getA()
-print "b = ", f.getb()
-
-f = gtsam.JacobianFactor(np.ones(2))
-f.print_('jacoboian b_in:')
-
-
-print "JacobianFactor initalized with b_in:", f
-
-diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
-fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag)
-print "priorfactorvector: ", fv
-
-print "base noise: ", fv.get_noiseModel()
-print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel())
-
-X = gtsam.symbol(65, 19)
-print X 
-print gtsam.symbolChr(X)
-print gtsam.symbolIndex(X)
diff --git a/cython/gtsam/tests/gtsam_test.h b/cython/gtsam/tests/gtsam_test.h
deleted file mode 100644
index 659a7cd0c..000000000
--- a/cython/gtsam/tests/gtsam_test.h
+++ /dev/null
@@ -1,772 +0,0 @@
-namespace gtsam {
-
-#include <gtsam/inference/Key.h>
-typedef size_t Key;
-
-#include <gtsam/base/FastVector.h>
-template<T> class FastVector {
-  FastVector();
-  FastVector(const This& f);
-  void push_back(const T& e);
-  //T& operator[](int);
-  T at(int i);
-  size_t size() const;
-};
-
-typedef gtsam::FastVector<gtsam::Key> KeyVector;
-
-//*************************************************************************
-// geometry
-//*************************************************************************
-
-#include <gtsam/geometry/Point2.h>
-class Point2 {
-  // Standard Constructors
-  Point2();
-  Point2(double x, double y);
-  Point2(Vector v);
-  //Point2(const gtsam::Point2& l);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Point2& pose, double tol) const;
-
-  // Group
-  static gtsam::Point2 identity();
-
-  // Standard Interface
-  double x() const;
-  double y() const;
-  Vector vector() const;
-  double distance(const gtsam::Point2& p2) const;
-  double norm() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/geometry/Point3.h>
-class Point3 {
-  // Standard Constructors
-  Point3();
-  Point3(double x, double y, double z);
-  Point3(Vector v);
-  //Point3(const gtsam::Point3& l);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Point3& p, double tol) const;
-
-  // Group
-  static gtsam::Point3 identity();
-
-  // Standard Interface
-  Vector vector() const;
-  double x() const;
-  double y() const;
-  double z() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/geometry/Rot2.h>
-class Rot2 {
-  // Standard Constructors and Named Constructors
-  Rot2();
-  Rot2(double theta);
-  //Rot2(const gtsam::Rot2& l);
-
-  static gtsam::Rot2 fromAngle(double theta);
-  static gtsam::Rot2 fromDegrees(double theta);
-  static gtsam::Rot2 fromCosSin(double c, double s);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Rot2& rot, double tol) const;
-
-  // Group
-  static gtsam::Rot2 identity();
-  gtsam::Rot2 inverse();
-  gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
-  gtsam::Rot2 between(const gtsam::Rot2& p2) const;
-
-  // Manifold
-  gtsam::Rot2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Rot2& p) const;
-
-  // Lie Group
-  static gtsam::Rot2 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Rot2& p);
-
-  // Group Action on Point2
-  gtsam::Point2 rotate(const gtsam::Point2& point) const;
-  gtsam::Point2 unrotate(const gtsam::Point2& point) const;
-
-  // Standard Interface
-  static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative
-  static gtsam::Rot2 atan2(double y, double x);
-  double theta() const;
-  double degrees() const;
-  double c() const;
-  double s() const;
-  Matrix matrix() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/geometry/Rot3.h>
-class Rot3 {
-  // Standard Constructors and Named Constructors
-  Rot3();
-  Rot3(Matrix R);
-  //Rot3(const gtsam::Rot3& l);
-
-  static gtsam::Rot3 Rx(double t);
-  static gtsam::Rot3 Ry(double t);
-  static gtsam::Rot3 Rz(double t);
-  static gtsam::Rot3 RzRyRx(double x, double y, double z);
-  static gtsam::Rot3 RzRyRx(Vector xyz);
-  static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading)
-  static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude)
-  static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
-  static gtsam::Rot3 Ypr(double y, double p, double r);
-  static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
-  static gtsam::Rot3 Rodrigues(Vector v);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Rot3& rot, double tol) const;
-
-  // Group
-  static gtsam::Rot3 identity();
-    gtsam::Rot3 inverse() const;
-  gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
-  gtsam::Rot3 between(const gtsam::Rot3& p2) const;
-
-  // Manifold
-  //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
-  gtsam::Rot3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Rot3& p) const;
-
-  // Group Action on Point3
-  gtsam::Point3 rotate(const gtsam::Point3& p) const;
-  gtsam::Point3 unrotate(const gtsam::Point3& p) const;
-
-  // Standard Interface
-  static gtsam::Rot3 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Rot3& p);
-  Matrix matrix() const;
-  Matrix transpose() const;
-  gtsam::Point3 column(size_t index) const;
-  Vector xyz() const;
-  Vector ypr() const;
-  Vector rpy() const;
-  double roll() const;
-  double pitch() const;
-  double yaw() const;
-//  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
-  Vector quaternion() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/geometry/Pose2.h>
-class Pose2 {
-  // Standard Constructor
-  Pose2();
-  //Pose2(const gtsam::Pose2& pose);
-  Pose2(double x, double y, double theta);
-  Pose2(double theta, const gtsam::Point2& t);
-  Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
-  Pose2(Vector v);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Pose2& pose, double tol) const;
-
-  // Group
-  static gtsam::Pose2 identity();
-  gtsam::Pose2 inverse() const;
-  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
-  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
-
-  // Manifold
-  gtsam::Pose2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Pose2& p) const;
-
-  // Lie Group
-  static gtsam::Pose2 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Pose2& p);
-  Matrix AdjointMap() const;
-  Vector Adjoint(const Vector& xi) const;
-  static Matrix wedge(double vx, double vy, double w);
-
-  // Group Actions on Point2
-  gtsam::Point2 transform_from(const gtsam::Point2& p) const;
-  gtsam::Point2 transform_to(const gtsam::Point2& p) const;
-
-  // Standard Interface
-  double x() const;
-  double y() const;
-  double theta() const;
-  gtsam::Rot2 bearing(const gtsam::Point2& point) const;
-  double range(const gtsam::Point2& point) const;
-  gtsam::Point2 translation() const;
-  gtsam::Rot2 rotation() const;
-  Matrix matrix() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-
-#include <gtsam/geometry/Pose3.h>
-class Pose3 {
-  // Standard Constructors
-  Pose3();
-  //Pose3(const gtsam::Pose3& pose);
-  Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
-  Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose)
-  Pose3(Matrix t);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::Pose3& pose, double tol) const;
-
-  // Group
-  static gtsam::Pose3 identity();
-  gtsam::Pose3 inverse() const;
-  gtsam::Pose3 compose(const gtsam::Pose3& p2) const;
-  gtsam::Pose3 between(const gtsam::Pose3& p2) const;
-
-  // Manifold
-  gtsam::Pose3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Pose3& T2) const;
-
-  // Lie Group
-  static gtsam::Pose3 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Pose3& p);
-  Matrix AdjointMap() const;
-  Vector Adjoint(Vector xi) const;
-  static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
-
-  // Group Action on Point3
-  gtsam::Point3 transform_from(const gtsam::Point3& p) const;
-  gtsam::Point3 transform_to(const gtsam::Point3& p) const;
-
-  // Standard Interface
-  gtsam::Rot3 rotation() const;
-  gtsam::Point3 translation() const;
-  double x() const;
-  double y() const;
-  double z() const;
-  Matrix matrix() const;
-  gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
-  double range(const gtsam::Point3& point);
-  double range(const gtsam::Pose3& pose);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-//*************************************************************************
-// noise
-//*************************************************************************
-
-namespace noiseModel {
-#include <gtsam/linear/NoiseModel.h>
-virtual class Base {
-};
-
-virtual class Gaussian : gtsam::noiseModel::Base {
-  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
-  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
-  Matrix R() const;
-  bool equals(gtsam::noiseModel::Base& expected, double tol);
-  void print(string s) const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Diagonal : gtsam::noiseModel::Gaussian {
-  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
-  static gtsam::noiseModel::Diagonal* Variances(Vector variances);
-  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
-  Matrix R() const;
-  void print(string s) const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Constrained : gtsam::noiseModel::Diagonal {
-    static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas);
-    static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas);
-    static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances);
-    static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances);
-    static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions);
-    static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions);
-
-    static gtsam::noiseModel::Constrained* All(size_t dim);
-    static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
-
-    gtsam::noiseModel::Constrained* unit() const;
-
-    // enabling serialization functionality
-    void serializable() const;
-};
-
-virtual class Isotropic : gtsam::noiseModel::Diagonal {
-  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
-  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
-  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
-  void print(string s) const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Unit : gtsam::noiseModel::Isotropic {
-  static gtsam::noiseModel::Unit* Create(size_t dim);
-  void print(string s) const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-namespace mEstimator {
-virtual class Base {
-};
-
-virtual class Null: gtsam::noiseModel::mEstimator::Base {
-  Null();
-  //Null(const gtsam::noiseModel::mEstimator::Null& other);
-  void print(string s) const;
-  static gtsam::noiseModel::mEstimator::Null* Create();
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Fair: gtsam::noiseModel::mEstimator::Base {
-  Fair(double c);
-  //Fair(const gtsam::noiseModel::mEstimator::Fair& other);
-  void print(string s) const;
-  static gtsam::noiseModel::mEstimator::Fair* Create(double c);
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Huber: gtsam::noiseModel::mEstimator::Base {
-  Huber(double k);
-  //Huber(const gtsam::noiseModel::mEstimator::Huber& other);
-
-  void print(string s) const;
-  static gtsam::noiseModel::mEstimator::Huber* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
-  Tukey(double k);
-  //Tukey(const gtsam::noiseModel::mEstimator::Tukey& other);
-
-  void print(string s) const;
-  static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-}///\namespace mEstimator
-
-virtual class Robust : gtsam::noiseModel::Base {
-  Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
-  //Robust(const gtsam::noiseModel::Robust& other);
-
-  static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
-  void print(string s) const;
-
-  // enabling serialization functionality
-  void serializable() const;
-};
-
-}///\namespace noiseModel
-
-#include <gtsam/linear/Sampler.h>
-class Sampler {
-    //Constructors
-  Sampler(gtsam::noiseModel::Diagonal* model, int seed);
-  Sampler(Vector sigmas, int seed);
-  Sampler(int seed);
-  //Sampler(const gtsam::Sampler& other);
-
-
-    //Standard Interface
-  size_t dim() const;
-  Vector sigmas() const;
-  gtsam::noiseModel::Diagonal* model() const;
-  Vector sample();
-  Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
-};
-
-#include <gtsam/linear/VectorValues.h>
-class VectorValues {
-    //Constructors
-  VectorValues();
-  VectorValues(const gtsam::VectorValues& other);
-
-  //Named Constructors
-  static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
-
-  //Standard Interface
-  size_t size() const;
-  size_t dim(size_t j) const;
-  bool exists(size_t j) const;
-  void print(string s) const;
-  bool equals(const gtsam::VectorValues& expected, double tol) const;
-  void insert(size_t j, Vector value);
-  Vector vector() const;
-  Vector at(size_t j) const;
-  void update(const gtsam::VectorValues& values);
-
-  //Advanced Interface
-  void setZero();
-
-  gtsam::VectorValues add(const gtsam::VectorValues& c) const;
-  void addInPlace(const gtsam::VectorValues& c);
-  gtsam::VectorValues subtract(const gtsam::VectorValues& c) const;
-  gtsam::VectorValues scale(double a) const;
-  void scaleInPlace(double a);
-
-  bool hasSameStructure(const gtsam::VectorValues& other)  const;
-  double dot(const gtsam::VectorValues& V) const;
-  double norm() const;
-  double squaredNorm() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/linear/GaussianFactor.h>
-virtual class GaussianFactor {
-  gtsam::KeyVector keys() const;
-  void print(string s) const;
-  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
-  double error(const gtsam::VectorValues& c) const;
-  gtsam::GaussianFactor* clone() const;
-  gtsam::GaussianFactor* negate() const;
-  Matrix augmentedInformation() const;
-  Matrix information() const;
-  Matrix augmentedJacobian() const;
-  pair<Matrix, Vector> jacobian() const;
-  size_t size() const;
-  bool empty() const;
-};
-
-#include <gtsam/linear/JacobianFactor.h>
-virtual class JacobianFactor : gtsam::GaussianFactor {
-  //Constructors
-  JacobianFactor();
-  JacobianFactor(const gtsam::GaussianFactor& factor);
-  JacobianFactor(Vector b_in);
-  JacobianFactor(size_t i1, Matrix A1, Vector b,
-      const gtsam::noiseModel::Diagonal* model);
-  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
-      const gtsam::noiseModel::Diagonal* model);
-  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
-      Vector b, const gtsam::noiseModel::Diagonal* model);
-  //JacobianFactor(const gtsam::GaussianFactorGraph& graph);
-  //JacobianFactor(const gtsam::JacobianFactor& other);
-
-  //Testable
-  void print(string s) const;
-  void printKeys(string s) const;
-  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
-  size_t size() const;
-  Vector unweighted_error(const gtsam::VectorValues& c) const;
-  Vector error_vector(const gtsam::VectorValues& c) const;
-  double error(const gtsam::VectorValues& c) const;
-
-  //Standard Interface
-  Matrix getA() const;
-  Vector getb() const;
-  size_t rows() const;
-  size_t cols() const;
-  bool isConstrained() const;
-  pair<Matrix, Vector> jacobianUnweighted() const;
-  Matrix augmentedJacobianUnweighted() const;
-
-  void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
-  gtsam::JacobianFactor whiten() const;
-
-  //pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
-
-  void setModel(bool anyConstrained, const Vector& sigmas);
-
-  gtsam::noiseModel::Diagonal* get_model() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/linear/HessianFactor.h>
-virtual class HessianFactor : gtsam::GaussianFactor {
-  //Constructors
-  HessianFactor();
-  HessianFactor(const gtsam::GaussianFactor& factor);
-  HessianFactor(size_t j, Matrix G, Vector g, double f);
-  HessianFactor(size_t j, Vector mu, Matrix Sigma);
-  HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
-      Vector g2, double f);
-  HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
-      Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
-      double f);
-  //HessianFactor(const gtsam::GaussianFactorGraph& factors);
-  //HessianFactor(const gtsam::HessianFactor& other);
-
-  //Testable
-  size_t size() const;
-  void print(string s) const;
-  void printKeys(string s) const;
-  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
-  double error(const gtsam::VectorValues& c) const;
-
-  //Standard Interface
-  size_t rows() const;
-  Matrix information() const;
-  double constantTerm() const;
-  Vector linearTerm() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/nonlinear/Values.h>
-class Values {
-  Values();
-  //Values(const gtsam::Values& other);
-
-  size_t size() const;
-  bool empty() const;
-  void clear();
-  size_t dim() const;
-
-  void print(string s) const;
-  bool equals(const gtsam::Values& other, double tol) const;
-
-  void insert(const gtsam::Values& values);
-  void update(const gtsam::Values& values);
-  void erase(size_t j);
-  void swap(gtsam::Values& values);
-
-  bool exists(size_t j) const;
-  gtsam::KeyVector keys() const;
-
-  gtsam::VectorValues zeroVectors() const;
-
-  gtsam::Values retract(const gtsam::VectorValues& delta) const;
-  gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const;
-
-  // enabling serialization functionality
-  void serialize() const;
-
-  // New in 4.0, we have to specialize every insert/update/at to generate wrappers
-  // Instead of the old:
-  // void insert(size_t j, const gtsam::Value& value);
-  // void update(size_t j, const gtsam::Value& val);
-  // gtsam::Value at(size_t j) const;
-
-  // template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-  //                gtsam::Rot3, gtsam::Pose3}>
-  // void insert(size_t j, const T& t);
-
-  // template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-  //                gtsam::Rot3, gtsam::Pose3}>
-  // void update(size_t j, const T& t);
-  void insert(size_t j, const gtsam::Point2& t);
-  void insert(size_t j, const gtsam::Point3& t);
-  void insert(size_t j, const gtsam::Rot2& t);
-  void insert(size_t j, const gtsam::Pose2& t);
-  void insert(size_t j, const gtsam::Rot3& t);
-  void insert(size_t j, const gtsam::Pose3& t);
-  void insert(size_t j, Vector t);
-  void insert(size_t j, Matrix t);
-
-  void update(size_t j, const gtsam::Point2& t);
-  void update(size_t j, const gtsam::Point3& t);
-  void update(size_t j, const gtsam::Rot2& t);
-  void update(size_t j, const gtsam::Pose2& t);
-  void update(size_t j, const gtsam::Rot3& t);
-  void update(size_t j, const gtsam::Pose3& t);
-  void update(size_t j, Vector t);
-  void update(size_t j, Matrix t);
-
-  template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-                 gtsam::Rot3, gtsam::Pose3, Vector, Matrix}>
-  T at(size_t j);
-
-  /// version for double
-  void insertDouble(size_t j, double c);
-  double atDouble(size_t j) const;
-};
-
-#include <gtsam/nonlinear/NonlinearFactor.h>
-virtual class NonlinearFactor {
-  // Factor base class
-  size_t size() const;
-  gtsam::KeyVector keys() const;
-  void print(string s) const;
-  void printKeys(string s) const;
-  // NonlinearFactor
-  bool equals(const gtsam::NonlinearFactor& other, double tol) const;
-
-  double error(const gtsam::Values& c) const;
-  size_t dim() const;
-  bool active(const gtsam::Values& c) const;
-  gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
-  gtsam::NonlinearFactor* clone() const;
-  // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
-};
-
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-class NonlinearFactorGraph {
-  NonlinearFactorGraph();
-  //NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
-
-  // FactorGraph
-  void print(string s) const;
-  bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
-  size_t size() const;
-  bool empty() const;
-  void remove(size_t i);
-  size_t nrFactors() const;
-  gtsam::NonlinearFactor* at(size_t idx) const;
-  void push_back(const gtsam::NonlinearFactorGraph& factors);
-  void push_back(gtsam::NonlinearFactor* factor);
-  void add(gtsam::NonlinearFactor* factor);
-  bool exists(size_t idx) const;
-  // gtsam::KeySet keys() const;
-
-  // NonlinearFactorGraph
-  double error(const gtsam::Values& values) const;
-  double probPrime(const gtsam::Values& values) const;
-  //gtsam::Ordering orderingCOLAMD() const;
-  // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
-  //gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
-  gtsam::NonlinearFactorGraph clone() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/nonlinear/NonlinearFactor.h>
-virtual class NoiseModelFactor: gtsam::NonlinearFactor {
-  void equals(const gtsam::NoiseModelFactor& other, double tol) const;
-  gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
-  gtsam::noiseModel::Base* noiseModel() const;
-  Vector unwhitenedError(const gtsam::Values& x) const;
-  Vector whitenedError(const gtsam::Values& x) const;
-};
-
-#include <gtsam/slam/PriorFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
-virtual class PriorFactor : gtsam::NoiseModelFactor {
-  PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
-  //PriorFactor(const This& other);
-  T prior() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-
-#include <gtsam/slam/BetweenFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
-virtual class BetweenFactor : gtsam::NoiseModelFactor {
-  BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
-  //BetweenFactor(const This& other);
-  T measured() const;
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-#include <gtsam/inference/Symbol.h>
-size_t symbol(char chr, size_t index);
-char symbolChr(size_t key);
-size_t symbolIndex(size_t key);
-
-#include <gtsam/inference/Key.h>
-// Default keyformatter
-void PrintKeyVector(const gtsam::KeyVector& keys);
-void PrintKeyVector(const gtsam::KeyVector& keys, string s);
-
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
-bool checkConvergence(double relativeErrorTreshold,
-    double absoluteErrorTreshold, double errorThreshold,
-    double currentError, double newError);
-
-#include <gtsam/slam/dataset.h>
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model, int maxID);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
-    gtsam::noiseModel::Diagonal* model);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
-    gtsam::noiseModel::Base* model);
-void save2D(const gtsam::NonlinearFactorGraph& graph,
-    const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
-    string filename);
-
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
-void writeG2o(const gtsam::NonlinearFactorGraph& graph,
-    const gtsam::Values& estimate, string filename);
-
-//*************************************************************************
-// Utilities
-//*************************************************************************
-
-namespace utilities {
-
-  #include <gtsam/nonlinear/utilities.h>
-  // gtsam::KeyList createKeyList(Vector I);
-  // gtsam::KeyList createKeyList(string s, Vector I);
-  gtsam::KeyVector createKeyVector(Vector I);
-  gtsam::KeyVector createKeyVector(string s, Vector I);
-  // gtsam::KeySet createKeySet(Vector I);
-  // gtsam::KeySet createKeySet(string s, Vector I);
-  Matrix extractPoint2(const gtsam::Values& values);
-  Matrix extractPoint3(const gtsam::Values& values);
-  Matrix extractPose2(const gtsam::Values& values);
-  gtsam::Values allPose3s(gtsam::Values& values);
-  Matrix extractPose3(const gtsam::Values& values);
-  void perturbPoint2(gtsam::Values& values, double sigma, int seed);
-  void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
-  void perturbPoint3(gtsam::Values& values, double sigma, int seed);
-  // void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
-  // void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
-  // void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
-  Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
-  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
-  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
-
-} //\namespace utilities
-
-#include <gtsam/nonlinear/utilities.h>
-class RedirectCout {
-  RedirectCout();
-  string str();
-};
-
-} //\namespace gtsam

From c0686f1f09bedda09bdef26ddc4bbb05faf49a09 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 11 Feb 2019 10:43:48 -0500
Subject: [PATCH 131/281] Fixed findExampleDataFile issue. Note the wrapped
 version is not the one that will be available in MATLAB. Still have to test
 whether we can use that or not.

---
 gtsam/slam/dataset.cpp | 4 ----
 gtsam/slam/dataset.h   | 2 --
 2 files changed, 6 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 6efd01feb..34370d4e2 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -51,7 +51,6 @@ using namespace gtsam::symbol_shorthand;
 
 namespace gtsam {
 
-#ifndef MATLAB_MEX_FILE
 /* ************************************************************************* */
 string findExampleDataFile(const string& name) {
   // Search source tree and installed location
@@ -100,9 +99,6 @@ string createRewrittenFileName(const string& name) {
   return newpath.string();
 }
 
-/* ************************************************************************* */
-#endif
-
 /* ************************************************************************* */
 GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
     bool addNoise, bool smart, NoiseFormat noiseFormat,
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 929ada2c1..7c9b54a6f 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -37,7 +37,6 @@
 
 namespace gtsam {
 
-#ifndef MATLAB_MEX_FILE
 /**
  * Find the full path to an example dataset distributed with gtsam.  The name
  * may be specified with or without a file extension - if no extension is
@@ -56,7 +55,6 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
  * for checking read-write oprations
  */
 GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
-#endif
 
 /// Indicates how noise parameters are stored in file
 enum NoiseFormat {

From 37ed46cee1d8e4cf2a68eb4f6d61f1815aa82307 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 11 Feb 2019 10:44:24 -0500
Subject: [PATCH 132/281] Added underscore to case conflicts until wrap can
 handle this.

---
 cython/gtsam/tests/test_Pose2.py | 21 +++++++++++++++++++++
 cython/gtsam/tests/test_Pose3.py |  4 ++--
 gtsam.h                          |  8 ++++----
 gtsam/geometry/Pose2.h           |  8 ++++++--
 gtsam/geometry/Pose3.h           |  4 ++++
 5 files changed, 37 insertions(+), 8 deletions(-)
 create mode 100644 cython/gtsam/tests/test_Pose2.py

diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py
new file mode 100644
index 000000000..f43ec0683
--- /dev/null
+++ b/cython/gtsam/tests/test_Pose2.py
@@ -0,0 +1,21 @@
+"""Pose2 unit tests."""
+import unittest
+
+import numpy as np
+
+from gtsam import Pose2
+
+
+class TestPose2(unittest.TestCase):
+    """Test selected Pose2 methods."""
+
+    def test_adjoint(self):
+        """Test adjoint method."""
+        xi = np.array([1, 2, 3])
+        expected = np.dot(Pose2.adjointMap(xi), xi)
+        actual = Pose2.adjoint(xi, xi)
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py
index 4752a4b02..b878a9551 100644
--- a/cython/gtsam/tests/test_Pose3.py
+++ b/cython/gtsam/tests/test_Pose3.py
@@ -49,8 +49,8 @@ class TestPose3(unittest.TestCase):
     def test_adjoint(self):
         """Test adjoint method."""
         xi = np.array([1, 2, 3, 4, 5, 6])
-        expected = np.dot(Pose3.adjointMap(xi), xi)
-        actual = Pose3.adjoint(xi, xi)
+        expected = np.dot(Pose3.adjointMap_(xi), xi)
+        actual = Pose3.adjoint_(xi, xi)
         np.testing.assert_array_equal(actual, expected)
 
 
diff --git a/gtsam.h b/gtsam.h
index 3d4ee5c29..4720d9d6b 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -577,8 +577,8 @@ class Pose2 {
   static Matrix LogmapDerivative(const gtsam::Pose2& v);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
-  static Matrix adjointMap(Vector v);
-  Vector adjoint(Vector xi, Vector y);
+  static Matrix adjointMap_(Vector v);
+  Vector adjoint_(Vector xi, Vector y);
   Vector adjointTranspose(Vector xi, Vector y);
   static Matrix wedge(double vx, double vy, double w);
 
@@ -628,8 +628,8 @@ class Pose3 {
   static Vector Logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
-  static Matrix adjointMap(Vector xi);
-  static Vector adjoint(Vector xi, Vector y);
+  static Matrix adjointMap_(Vector xi);
+  static Vector adjoint_(Vector xi, Vector y);
   static Vector adjointTranspose(Vector xi, Vector y);
   static Matrix ExpmapDerivative(Vector xi);
   static Matrix LogmapDerivative(const gtsam::Pose3& xi);
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 603c3a421..6937ff78d 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -146,17 +146,21 @@ public:
   /**
    * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
    */
-  Vector3 adjoint(const Vector3& xi, const Vector3& y) {
+  static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
     return adjointMap(xi) * y;
   }
 
   /**
    * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
    */
-  Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
+  static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
     return adjointMap(xi).transpose() * y;
   }
 
+  // temporary fix for wrappers until case issue is resolved
+  static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
+  static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
+
   /**
    * wedge for SE(2):
    * @param xi 3-dim twist (v,omega) where
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index ca0fdff10..757c4fdd4 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -160,6 +160,10 @@ public:
   static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
       OptionalJacobian<6, 6> = boost::none);
 
+  // temporary fix for wrappers until case issue is resolved
+  static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
+  static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
+
   /**
    * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
    */

From 7138236f716f767d3401da2440f36b5e7cb35fa7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 11 Feb 2019 10:52:36 -0500
Subject: [PATCH 133/281] Fixed static issue

---
 cython/gtsam/tests/test_Pose2.py | 4 ++--
 gtsam.h                          | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py
index f43ec0683..afd0c5773 100644
--- a/cython/gtsam/tests/test_Pose2.py
+++ b/cython/gtsam/tests/test_Pose2.py
@@ -12,8 +12,8 @@ class TestPose2(unittest.TestCase):
     def test_adjoint(self):
         """Test adjoint method."""
         xi = np.array([1, 2, 3])
-        expected = np.dot(Pose2.adjointMap(xi), xi)
-        actual = Pose2.adjoint(xi, xi)
+        expected = np.dot(Pose2.adjointMap_(xi), xi)
+        actual = Pose2.adjoint_(xi, xi)
         np.testing.assert_array_equal(actual, expected)
 
 
diff --git a/gtsam.h b/gtsam.h
index 4720d9d6b..48768db40 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -578,8 +578,8 @@ class Pose2 {
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
   static Matrix adjointMap_(Vector v);
-  Vector adjoint_(Vector xi, Vector y);
-  Vector adjointTranspose(Vector xi, Vector y);
+  static Vector adjoint_(Vector xi, Vector y);
+  static Vector adjointTranspose(Vector xi, Vector y);
   static Matrix wedge(double vx, double vy, double w);
 
   // Group Actions on Point2

From 7625c2177710ef8a1b5624fcf8a94c74fce2bf86 Mon Sep 17 00:00:00 2001
From: jlblancoc <joseluisblancoc@gmail.com>
Date: Fri, 8 Feb 2019 22:18:21 +0100
Subject: [PATCH 134/281] Various fixes to cmake exported targets

List of changes:
 * -I boost is no longer required
   (Since the use of Boost::xxx imported targets)
 * fix missing Boost deps in imported gtsam by
   searching for Boost inside GTSAMConfig.cmake
 * Including the dirs for Eigen/MKL/SuiteSparse/Metis
   into exported targets public interface.
 * Fix missing cmake changes in wrap/*
 * Split build flags into private/public, not to
   expose to users flags that may be invasive.
 * Removed now useless include_dirs in "extra cmake"
 * Update cmake/example_project
 * Make cppunitlite to find boost headers via Boost::boost
 * Update README / INSTALL to reflect the updated minimum CMake >= 3.0
---
 CMakeLists.txt                       | 64 +++++++++-------------------
 CppUnitLite/CMakeLists.txt           |  1 +
 INSTALL                              | 64 ++++++++++++++--------------
 README.md                            | 10 ++---
 cmake/Config.cmake.in                |  8 +++-
 cmake/example_project/CMakeLists.txt |  5 ++-
 gtsam/CMakeLists.txt                 | 60 +++++++++++++++++++++++++-
 gtsam_extra.cmake.in                 | 12 ------
 wrap/CMakeLists.txt                  | 19 ++++++---
 9 files changed, 139 insertions(+), 104 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 673f9a45f..d084840be 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -121,15 +121,19 @@ if(MSVC)
 	# If we use Boost shared libs, disable auto linking.
 	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
 	if(NOT Boost_USE_STATIC_LIBS)
-		list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
+		list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
 	endif()
 	# Virtual memory range for PCH exceeded on VS2015
 	if(MSVC_VERSION LESS 1910) # older than VS2017
-	  list(APPEND GTSAM_COMPILE_OPTIONS -Zm295)
+	  list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
 	endif()
 endif()
 
-find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
+# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
+set(BOOST_FIND_MINIMUM_VERSION 1.43)
+set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
+
+find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
 
 # Required components
 if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
@@ -149,7 +153,7 @@ set(GTSAM_BOOST_LIBRARIES
 )
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
-    list(APPEND GTSAM_COMPILE_DEFINITIONS DGTSAM_DISABLE_NEW_TIMERS)
+    list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
 else()
     if(Boost_TIMER_LIBRARY)
       list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
@@ -162,7 +166,7 @@ endif()
 
 if(NOT (${Boost_VERSION} LESS 105600))
 	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
-	list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
+	list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
 endif()
 
 ###############################################################################
@@ -212,7 +216,6 @@ find_package(MKL)
 if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
     set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h
     set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
-    include_directories(${MKL_INCLUDE_DIR})
     list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
 
     # --no-as-needed is required with gcc according to the MKL link advisor
@@ -247,10 +250,9 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
 	find_package(Eigen3 REQUIRED)
-	include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
 
 	# Use generic Eigen include paths e.g. <Eigen/Core>
-    set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
+	set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
 
 	# check if MKL is also enabled - can have one or the other, but not both!
 	# Note: Eigen >= v3.2.5 includes our patches
@@ -270,22 +272,19 @@ else()
 	if(EIGEN3_INCLUDE_DIR)
 		set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
 	endif()
-	# Add the bundled version of eigen to the include path so that it can still be included
-	# with  #include <Eigen/Core>
-	include_directories(BEFORE "gtsam/3rdparty/Eigen/")
 
 	# set full path to be used by external projects
 	# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
-	set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
+	set(GTSAM_EIGEN_INCLUDE_PREFIX "include/gtsam/3rdparty/Eigen/")
 
 endif()
 
 if (MSVC)
 	if (BUILD_SHARED_LIBS)
 		# mute eigen static assert to avoid errors in shared lib
-		list(APPEND GTSAM_COMPILE_DEFINITIONS DEIGEN_NO_STATIC_ASSERT)
+		list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DEIGEN_NO_STATIC_ASSERT)
 	endif()
-	list(APPEND GTSAM_COMPILE_OPTIONS "/wd4244") # Disable loss of precision which is thrown all over our Eigen
+	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
 endif()
 
 ###############################################################################
@@ -326,52 +325,29 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
 	list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
 endif()
 
-# Include boost - use 'BEFORE' so that a specific boost specified to CMake
-# takes precedence over a system-installed one.
-include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR})
-
-if(GTSAM_SUPPORT_NESTED_DISSECTION)
-  set(METIS_INCLUDE_DIRECTORIES
-    gtsam/3rdparty/metis/include
-    gtsam/3rdparty/metis/libmetis
-    gtsam/3rdparty/metis/GKlib)
-else()
-  set(METIS_INCLUDE_DIRECTORIES)
-endif()
-
-# Add includes for source directories 'BEFORE' boost and any system include
-# paths so that the compiler uses GTSAM headers in our source directory instead
-# of any previously installed GTSAM headers.
-include_directories(BEFORE
-  gtsam/3rdparty/SuiteSparse_config
-  gtsam/3rdparty/CCOLAMD/Include
-  ${METIS_INCLUDE_DIRECTORIES}
-  ${PROJECT_SOURCE_DIR}
-  ${PROJECT_BINARY_DIR} # So we can include generated config header files
-  CppUnitLite)
-
 if(MSVC)
-	list(APPEND GTSAM_COMPILE_DEFINITIONS _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
-	list(APPEND GTSAM_COMPILE_OPTIONS /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
-	list(APPEND GTSAM_COMPILE_OPTIONS /bigobj) # Allow large object files for template-based code
+	list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
+	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
+	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
 endif()
 
 # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
-    list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs)
+    list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
   endif()
 endif()
 
 # As of XCode 7, clang also complains about this
 if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
   if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0)
-    list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs)
+    list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
   endif()
 endif()
 
 if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
-  list(APPEND GTSAM_COMPILE_DEFINITIONS GTSAM_EXTRA_CONSISTENCY_CHECKS)
+  # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h
+  list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS)
 endif()
 
 ###############################################################################
diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt
index 35ecd71f8..f52841274 100644
--- a/CppUnitLite/CMakeLists.txt
+++ b/CppUnitLite/CMakeLists.txt
@@ -6,6 +6,7 @@ file(GLOB cppunitlite_src "*.cpp")
 add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
 list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
 
 gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
 
diff --git a/INSTALL b/INSTALL
index c71dcd4f9..d2f0b68df 100644
--- a/INSTALL
+++ b/INSTALL
@@ -14,7 +14,7 @@ Important Installation Notes
 1)
 GTSAM requires the following libraries to be installed on your system:
  - BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
- - Cmake version 2.6 or higher
+ - Cmake version 3.0 or higher
  - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
 
 Optional dependent libraries:
@@ -33,20 +33,20 @@ Tested compilers:
 
 Tested systems:
 
-- Ubuntu 11.04 - 13.10
+- Ubuntu 11.04 - 18.04
 - MacOS 10.6 - 10.9
-- Windows 7, 8, 8.1
+- Windows 7, 8, 8.1, 10
 
 Known issues:
 
 - MSVC 2013 is not yet supported because it cannot build the serialization module
-  of Boost 1.55 (or earlier). 
+  of Boost 1.55 (or earlier).
 
 2)
 GTSAM makes extensive use of debug assertions, and we highly recommend you work
 in Debug mode while developing (enabled by default). Likewise, it is imperative
 that you switch to release mode when running finished code and for timing. GTSAM
-will run up to 10x faster in Release mode! See the end of this document for 
+will run up to 10x faster in Release mode! See the end of this document for
 additional debugging tips.
 
 3)
@@ -55,7 +55,7 @@ build directory.
 
 4)
 The instructions below install the library to the default system install path and
-build all components. From a terminal, starting in the root library folder, 
+build all components. From a terminal, starting in the root library folder,
 execute commands as follows for an out-of-source build:
 
 $] mkdir build
@@ -64,7 +64,7 @@ $] cmake ..
 $] make check (optional, runs unit tests)
 $] make install
 
-This will build the library and unit tests, run all of the unit tests, 
+This will build the library and unit tests, run all of the unit tests,
 and then install the library itself.
 
 - CMake Configuration Options and Details
@@ -75,12 +75,12 @@ one of the following:
   ccmake      the curses GUI for cmake
   cmake-gui   a real GUI for cmake
 
-Important Options: 
+Important Options:
 
 CMAKE_BUILD_TYPE: We support several build configurations for GTSAM (case insensitive)
   Debug (default)  All error checking options on, no optimization. Use for development.
-  Release          Optimizations turned on, no debug symbols.           
-  Timing           Adds ENABLE_TIMING flag to provide statistics on operation 
+  Release          Optimizations turned on, no debug symbols.
+  Timing           Adds ENABLE_TIMING flag to provide statistics on operation
   Profiling        Standard configuration for use during profiling
   RelWithDebInfo   Same as Release, but with the -g flag for debug symbols
 
@@ -92,42 +92,42 @@ GTSAM_TOOLBOX_INSTALL_PATH: The Matlab toolbox will be installed in a subdirecto
 of this folder, called 'gtsam'.
 $] cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox ..
 
-GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in 
-subfolders to be linked against convenience libraries rather than the full libgtsam. 
+GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in
+subfolders to be linked against convenience libraries rather than the full libgtsam.
 Set with the command line as follows:
 $] cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..
-  ON (Default)   This builds convenience libraries and links tests against them. This 
-  				 option is suggested for gtsam developers, as it is possible to build 
-  				 and run tests without first building the rest of the library, and 
-  				 speeds up compilation for a single test. The downside of this option 
-  				 is that it will build the entire library again to build the full 
+  ON (Default)   This builds convenience libraries and links tests against them. This
+  				 option is suggested for gtsam developers, as it is possible to build
+  				 and run tests without first building the rest of the library, and
+  				 speeds up compilation for a single test. The downside of this option
+  				 is that it will build the entire library again to build the full
   				 libgtsam library, so build/install will be slower.
-  OFF            This will build all of libgtsam before any of the tests, and then 
-  				 link all of the tests at once. This option is best for users of GTSAM, 
-  				 as it avoids rebuilding the entirety of gtsam an extra time. 
+  OFF            This will build all of libgtsam before any of the tests, and then
+  				 link all of the tests at once. This option is best for users of GTSAM,
+  				 as it avoids rebuilding the entirety of gtsam an extra time.
 
-GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library. 
+GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library.
 Set with the command line as follows:
 $] cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON ..
-  ON             When enabled, libgtsam_unstable will be built and installed with the 
-                 same options as libgtsam.  In addition, if tests are enabled, the 
+  ON             When enabled, libgtsam_unstable will be built and installed with the
+                 same options as libgtsam.  In addition, if tests are enabled, the
                  unit tests will be built as well.  The Matlab toolbox will also
-                 be generated if the matlab toolbox is enabled, installing into a 
+                 be generated if the matlab toolbox is enabled, installing into a
                  folder called "gtsam_unstable".
-  OFF (Default)  If disabled, no gtsam_unstable code will be included in build or install. 
+  OFF (Default)  If disabled, no gtsam_unstable code will be included in build or install.
 
 Check
 
-"make check" will build and run all of the tests. Note that the tests will only be 
+"make check" will build and run all of the tests. Note that the tests will only be
 built when using the "check" targets, to prevent "make install" from building the tests
-unnecessarily. You can also run "make timing" to build all of the timing scripts. 
+unnecessarily. You can also run "make timing" to build all of the timing scripts.
 To run check on a particular module only, run "make check.[subfolder]", so to run
 just the geometry tests, run "make check.geometry". Individual tests can be run by
-appending ".run" to the name of the test, for example, to run testMatrix, run 
-"make testMatrix.run". 
+appending ".run" to the name of the test, for example, to run testMatrix, run
+"make testMatrix.run".
 
-MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your 
-shell's PATH environment variable. mex is installed with matlab at 
+MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your
+shell's PATH environment variable. mex is installed with matlab at
 $MATLABROOT/bin/mex
 
 $MATLABROOT can be found by executing the command 'matlabroot' in MATLAB
@@ -143,4 +143,4 @@ it impossible to use _GLIBCXX_DEBUG.  MacPorts g++ compilers do work with it tho
 
 NOTE:  If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against
 gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of
-header-only Eigen.  
+header-only Eigen.
diff --git a/README.md b/README.md
index 02ed5149c..e815d7f4b 100644
--- a/README.md
+++ b/README.md
@@ -30,7 +30,7 @@ $ make install
 Prerequisites:
 
 - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
-- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
+- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
 - A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 
 Optional prerequisites - used automatically if findable by CMake:
@@ -54,7 +54,7 @@ GTSAM includes a state of the art IMU handling scheme based on
 
 Our implementation improves on this using integration on the manifold, as detailed in
 
-- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. 
+- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
 - Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015.
 
 If you are using the factor in academic work, please cite the publications above.
@@ -67,8 +67,8 @@ Additional Information
 
 There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion.
 
-Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions, 
-which support (superfast) automatic differentiation, 
+Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions,
+which support (superfast) automatic differentiation,
 can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home).
 
 See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
@@ -77,4 +77,4 @@ GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`L
 
 Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
 
-GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).
\ No newline at end of file
+GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).
diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in
index 5f3956f07..4323f760b 100644
--- a/cmake/Config.cmake.in
+++ b/cmake/Config.cmake.in
@@ -1,7 +1,7 @@
 # - Config file for @CMAKE_PROJECT_NAME@
 # It defines the following variables
 #  @PACKAGE_NAME@_INCLUDE_DIR - include directories for @CMAKE_PROJECT_NAME@
- 
+
 # Compute paths
 get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt")
@@ -11,7 +11,11 @@ else()
   # Find installed library
   set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory")
 endif()
-  
+
+# Find dependencies, required by cmake exported targets:
+include(CMakeFindDependencyMacro)
+find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
+
 # Load exports
 include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake)
 
diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt
index e287087d1..2afc6edc2 100644
--- a/cmake/example_project/CMakeLists.txt
+++ b/cmake/example_project/CMakeLists.txt
@@ -22,7 +22,10 @@ include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
 ###################################################################################
 # Find GTSAM components
 find_package(GTSAM REQUIRED) # Uses installed package
-include_directories(${GTSAM_INCLUDE_DIR})
+# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets
+# that automatically do include the include_directories() without the need
+# to call include_directories(), just target_link_libraries(NAME gtsam)
+#include_directories(${GTSAM_INCLUDE_DIR})
 
 ###################################################################################
 # Build static library from common sources
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index d369009c0..034268fdb 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -103,14 +103,70 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
 add_library(gtsam ${gtsam_srcs})
 target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
 target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
-target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS})
-target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS})
+target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
+target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})
+if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "")
+  target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC})
+endif()
+target_compile_options(gtsam PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE})
 set_target_properties(gtsam PROPERTIES
     OUTPUT_NAME         gtsam
     CLEAN_DIRECT_OUTPUT 1
     VERSION             ${gtsam_version}
     SOVERSION           ${gtsam_soversion})
 
+# Append Eigen include path, set in top-level CMakeLists.txt to either
+# system-eigen, or GTSAM eigen path
+if (GTSAM_USE_SYSTEM_EIGEN)
+  target_include_directories(gtsam PUBLIC
+    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+  )
+else()
+  target_include_directories(gtsam PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/Eigen/>
+  )
+endif()
+target_include_directories(gtsam PUBLIC
+  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_PREFIX}>
+)
+# MKL include dir:
+if (GTSAM_USE_EIGEN_MKL)
+  target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
+endif()
+
+if(GTSAM_USE_TBB)
+  target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS})
+endif()
+
+# Add includes for source directories 'BEFORE' boost and any system include
+# paths so that the compiler uses GTSAM headers in our source directory instead
+# of any previously installed GTSAM headers.
+target_include_directories(gtsam BEFORE PUBLIC
+  # SuiteSparse_config
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/SuiteSparse_config>
+  # CCOLAMD
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/CCOLAMD>
+  # main gtsam includes:
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/>
+  # config.h
+  $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
+  # unit tests:
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
+)
+if(GTSAM_SUPPORT_NESTED_DISSECTION)
+  target_include_directories(gtsam BEFORE PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/include>
+    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
+    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/metis/>
+  )
+endif()
+
+
+
 if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
 	if (NOT BUILD_SHARED_LIBS)
 		set_target_properties(gtsam PROPERTIES
diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in
index a1a8e3a90..b7990b3cc 100644
--- a/gtsam_extra.cmake.in
+++ b/gtsam_extra.cmake.in
@@ -9,18 +9,6 @@ set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@")
 set (GTSAM_USE_TBB @GTSAM_USE_TBB@)
 set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@)
 
-if("@GTSAM_USE_TBB@")
-  list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@")
-endif()
-
-# Append Eigen include path, set in top-level CMakeLists.txt to either
-# system-eigen, or GTSAM eigen path
-list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@")
-
-if("@GTSAM_USE_EIGEN_MKL@")
-  list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@")
-endif()
-
 if("@GTSAM_INSTALL_CYTHON_TOOLBOX@")
   list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@")
 endif()
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index 04d471b39..df8dc209f 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -1,22 +1,30 @@
 # Build/install Wrap
 
-set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY})
+set(WRAP_BOOST_LIBRARIES
+  Boost::system
+  Boost::filesystem
+  Boost::thread
+)
 
 # Allow for disabling serialization to handle errors related to Clang's linker
 option(GTSAM_WRAP_SERIALIZATION "If enabled, allows for wrapped objects to be saved via boost.serialization" ON)
-if (NOT GTSAM_WRAP_SERIALIZATION)
-    add_definitions(-DWRAP_DISABLE_SERIALIZE)
-endif()  
 
 # Build the executable itself
 file(GLOB wrap_srcs "*.cpp")
 file(GLOB wrap_headers "*.h")
 list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp)
 add_library(wrap_lib STATIC ${wrap_srcs} ${wrap_headers})
+target_include_directories(wrap_lib PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
+)
+if (NOT GTSAM_WRAP_SERIALIZATION)
+  target_compile_definitions(wrap_lib PUBLIC -DWRAP_DISABLE_SERIALIZE)
+endif()
+
 target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES})
 gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers})
 add_executable(wrap wrap.cpp)
-target_link_libraries(wrap wrap_lib ${WRAP_BOOST_LIBRARIES})
+target_link_libraries(wrap PRIVATE wrap_lib)
 
 # Set folder in Visual Studio
 file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
@@ -32,4 +40,3 @@ install(FILES matlab.h DESTINATION include/wrap)
 
 # Build tests
 add_subdirectory(tests)
-

From 6723b481a6e2985ed6204e4520b5bc57c2dd6727 Mon Sep 17 00:00:00 2001
From: jlblancoc <joseluisblancoc@gmail.com>
Date: Sun, 17 Feb 2019 00:58:35 +0100
Subject: [PATCH 135/281] fix missing Eigen in Cython wrapper

---
 CMakeLists.txt                      |  8 ++++++--
 cython/gtsam_eigency/CMakeLists.txt | 11 +++++++++++
 cython/gtsam_eigency/__init__.py.in |  2 +-
 gtsam/CMakeLists.txt                | 12 ++----------
 4 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d084840be..c1a421a31 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -252,7 +252,7 @@ if(GTSAM_USE_SYSTEM_EIGEN)
 	find_package(Eigen3 REQUIRED)
 
 	# Use generic Eigen include paths e.g. <Eigen/Core>
-	set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
+	set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
 
 	# check if MKL is also enabled - can have one or the other, but not both!
 	# Note: Eigen >= v3.2.5 includes our patches
@@ -266,6 +266,8 @@ if(GTSAM_USE_SYSTEM_EIGEN)
 		message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
 	endif()
 
+	# The actual include directory (for BUILD cmake target interface):
+	set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
 else()
 	# Use bundled Eigen include path.
 	# Clear any variables set by FindEigen3
@@ -275,8 +277,10 @@ else()
 
 	# set full path to be used by external projects
 	# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
-	set(GTSAM_EIGEN_INCLUDE_PREFIX "include/gtsam/3rdparty/Eigen/")
+	set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
 
+	# The actual include directory (for BUILD cmake target interface):
+	set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
 endif()
 
 if (MSVC)
diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt
index 54b7de9aa..4bff567eb 100644
--- a/cython/gtsam_eigency/CMakeLists.txt
+++ b/cython/gtsam_eigency/CMakeLists.txt
@@ -22,6 +22,17 @@ cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conv
   "${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "")
 cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core"
   ${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "" "")
+
+# Include Eigen headers:
+target_include_directories(cythonize_eigency_conversions PUBLIC
+  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
+)
+target_include_directories(cythonize_eigency_core PUBLIC
+  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
+)
+
 add_dependencies(cythonize_eigency_core cythonize_eigency_conversions)
 add_custom_target(cythonize_eigency)
 add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core)
diff --git a/cython/gtsam_eigency/__init__.py.in b/cython/gtsam_eigency/__init__.py.in
index dd278d128..a59d51eab 100644
--- a/cython/gtsam_eigency/__init__.py.in
+++ b/cython/gtsam_eigency/__init__.py.in
@@ -1,7 +1,7 @@
 import os
 import numpy as np
 
-__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_PREFIX}"
+__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}"
 
 def get_includes(include_eigen=True):
     root = os.path.dirname(__file__)
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 034268fdb..b4adc9978 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -117,17 +117,9 @@ set_target_properties(gtsam PROPERTIES
 
 # Append Eigen include path, set in top-level CMakeLists.txt to either
 # system-eigen, or GTSAM eigen path
-if (GTSAM_USE_SYSTEM_EIGEN)
-  target_include_directories(gtsam PUBLIC
-    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
-  )
-else()
-  target_include_directories(gtsam PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/Eigen/>
-  )
-endif()
 target_include_directories(gtsam PUBLIC
-  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_PREFIX}>
+  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
 )
 # MKL include dir:
 if (GTSAM_USE_EIGEN_MKL)

From dbc07997651b8bc7343d95ec661c1d10566d1ff9 Mon Sep 17 00:00:00 2001
From: Jeremy <jeraguilon@gmail.com>
Date: Wed, 27 Feb 2019 01:57:39 -0500
Subject: [PATCH 136/281] Init uncomment of fixed lag smoother

---
 gtsam_unstable/gtsam_unstable.h | 250 ++++++++++++++++----------------
 1 file changed, 125 insertions(+), 125 deletions(-)

diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index 39c910826..f1bb4005d 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -505,132 +505,132 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
 //*************************************************************************
 // nonlinear
 //*************************************************************************
-// #include <gtsam_unstable/nonlinear/sequentialSummarization.h>
-// gtsam::GaussianFactorGraph* summarizeGraphSequential(
-//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
-// gtsam::GaussianFactorGraph* summarizeGraphSequential(
-//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);
+#include <gtsam_unstable/nonlinear/sequentialSummarization.h>
+gtsam::GaussianFactorGraph* summarizeGraphSequential(
+    const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
+gtsam::GaussianFactorGraph* summarizeGraphSequential(
+    const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);
 
-// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-// class FixedLagSmootherKeyTimestampMapValue {
-//   FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp);
-//   FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
-// };
-// 
-// class FixedLagSmootherKeyTimestampMap {
-//   FixedLagSmootherKeyTimestampMap();
-//   FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
-// 
-//   // Note: no print function
-// 
-//   // common STL methods
-//   size_t size() const;
-//   bool empty() const;
-//   void clear();
-// 
-//   double at(const gtsam::Key& key) const;
-//   void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
-// };
-// 
-// class FixedLagSmootherResult {
-//   size_t getIterations() const;
-//   size_t getNonlinearVariables() const;
-//   size_t getLinearVariables() const;
-//   double getError() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-// virtual class FixedLagSmoother {
-//   void print(string s) const;
-//   bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
-// 
-//   gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
-//   double smootherLag() const;
-// 
-//   gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
-//   gtsam::Values calculateEstimate() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
-// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
-//   BatchFixedLagSmoother();
-//   BatchFixedLagSmoother(double smootherLag);
-//   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
-// 
-//   gtsam::LevenbergMarquardtParams params() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
-//   IncrementalFixedLagSmoother();
-//   IncrementalFixedLagSmoother(double smootherLag);
-//   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
-// 
-//   gtsam::ISAM2Params params() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
-// virtual class ConcurrentFilter {
-//   void print(string s) const;
-//   bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
-// };
-// 
-// virtual class ConcurrentSmoother {
-//   void print(string s) const;
-//   bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
-// };
-// 
-// // Synchronize function
-// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
-// 
-// #include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
-// class ConcurrentBatchFilterResult {
-//   size_t getIterations() const;
-//   size_t getLambdas() const;
-//   size_t getNonlinearVariables() const;
-//   size_t getLinearVariables() const;
-//   double getError() const;
-// };
-// 
-// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
-//   ConcurrentBatchFilter();
-//   ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
-// 
-//   gtsam::NonlinearFactorGraph getFactors() const;
-//   gtsam::Values getLinearizationPoint() const;
-//   gtsam::Ordering getOrdering() const;
-//   gtsam::VectorValues getDelta() const;
-// 
-//   gtsam::ConcurrentBatchFilterResult update();
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
-//   gtsam::Values calculateEstimate() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
-// class ConcurrentBatchSmootherResult {
-//   size_t getIterations() const;
-//   size_t getLambdas() const;
-//   size_t getNonlinearVariables() const;
-//   size_t getLinearVariables() const;
-//   double getError() const;
-// };
-// 
-// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
-//   ConcurrentBatchSmoother();
-//   ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
-// 
-//   gtsam::NonlinearFactorGraph getFactors() const;
-//   gtsam::Values getLinearizationPoint() const;
-//   gtsam::Ordering getOrdering() const;
-//   gtsam::VectorValues getDelta() const;
-// 
-//   gtsam::ConcurrentBatchSmootherResult update();
-//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
-//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-//   gtsam::Values calculateEstimate() const;
-// };
+#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+class FixedLagSmootherKeyTimestampMapValue {
+  FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp);
+  FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
+};
+
+class FixedLagSmootherKeyTimestampMap {
+  FixedLagSmootherKeyTimestampMap();
+  FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
+
+  // Note: no print function
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  double at(const gtsam::Key& key) const;
+  void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
+};
+
+class FixedLagSmootherResult {
+  size_t getIterations() const;
+  size_t getNonlinearVariables() const;
+  size_t getLinearVariables() const;
+  double getError() const;
+};
+
+#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+virtual class FixedLagSmoother {
+  void print(string s) const;
+  bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
+
+  gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
+  double smootherLag() const;
+
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
+  gtsam::Values calculateEstimate() const;
+};
+
+#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
+  BatchFixedLagSmoother();
+  BatchFixedLagSmoother(double smootherLag);
+  BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
+
+  gtsam::LevenbergMarquardtParams params() const;
+};
+
+#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
+  IncrementalFixedLagSmoother();
+  IncrementalFixedLagSmoother(double smootherLag);
+  IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
+
+  gtsam::ISAM2Params params() const;
+};
+
+#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
+virtual class ConcurrentFilter {
+  void print(string s) const;
+  bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
+};
+
+virtual class ConcurrentSmoother {
+  void print(string s) const;
+  bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
+};
+
+// Synchronize function
+void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
+
+#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
+class ConcurrentBatchFilterResult {
+  size_t getIterations() const;
+  size_t getLambdas() const;
+  size_t getNonlinearVariables() const;
+  size_t getLinearVariables() const;
+  double getError() const;
+};
+
+virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
+  ConcurrentBatchFilter();
+  ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::Ordering getOrdering() const;
+  gtsam::VectorValues getDelta() const;
+
+  gtsam::ConcurrentBatchFilterResult update();
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
+  gtsam::Values calculateEstimate() const;
+};
+
+#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
+class ConcurrentBatchSmootherResult {
+  size_t getIterations() const;
+  size_t getLambdas() const;
+  size_t getNonlinearVariables() const;
+  size_t getLinearVariables() const;
+  double getError() const;
+};
+
+virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
+  ConcurrentBatchSmoother();
+  ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::Ordering getOrdering() const;
+  gtsam::VectorValues getDelta() const;
+
+  gtsam::ConcurrentBatchSmootherResult update();
+  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
+  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+  gtsam::Values calculateEstimate() const;
+};
 
 //*************************************************************************
 // slam

From 93fd884aa74c5db01b95e7d11bcf3a595c97e4ae Mon Sep 17 00:00:00 2001
From: Jeremy <jeraguilon@gmail.com>
Date: Wed, 27 Feb 2019 04:37:02 -0500
Subject: [PATCH 137/281] Implement and add example

---
 .../examples/FixedLagSmootherExample.py       | 86 +++++++++++++++++++
 gtsam_unstable/gtsam_unstable.h               | 14 ++-
 2 files changed, 92 insertions(+), 8 deletions(-)
 create mode 100644 cython/gtsam_unstable/examples/FixedLagSmootherExample.py

diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
new file mode 100644
index 000000000..5d44dab64
--- /dev/null
+++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
@@ -0,0 +1,86 @@
+"""
+Demonstration of the fixed-lag smoothers using a planar robot example
+and multiple odometry-like sensors
+Author: Frank Dellaert (C++), Jeremy Aguilon (Python)
+"""
+
+import numpy as np
+
+import gtsam
+import gtsam_unstable
+
+# Create noise models
+PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
+
+def _timestamp_key_value(key, value):
+    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
+        key, value
+    )
+
+
+def BatchFixedLagSmootherExample():
+    # Define a batch fixed lag smoother, which uses
+    # Levenberg-Marquardt to perform the nonlinear optimization
+    lag = 2.0
+    smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
+
+
+    # Create containers to store the factors and linearization points
+    # that will be sent to the smoothers
+    new_factors = gtsam.NonlinearFactorGraph()
+    new_values = gtsam.Values()
+    new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
+
+
+    # Create  a prior on the first pose, placing it at the origin
+    prior_mean = gtsam.Pose2(0, 0, 0)
+    X1 = 0
+    new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, PRIOR_NOISE))
+    new_values.insert(X1, prior_mean)
+    new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+
+    delta_time = 0.25
+    time = 0.25
+
+    while time <= 3.0:
+        previous_key = 1000 * (time - delta_time)
+        current_key = 1000 * time
+
+        # assign current key to the current timestamp
+        new_timestamps.insert(_timestamp_key_value(current_key, time))
+
+        # Add a guess for this pose to the new values
+        # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
+        current_pose = gtsam.Pose2(time * 2, 0, 0)
+        new_values.insert(current_key, current_pose)
+
+        # Add odometry factors from two different sources with different error stats
+        odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
+        odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
+        new_factors.push_back(gtsam.BetweenFactorPose2(
+            previous_key, current_key, odometry_measurement_1, odometry_noise_1
+        ))
+
+        odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
+        odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05]))
+        new_factors.push_back(gtsam.BetweenFactorPose2(
+            previous_key, current_key, odometry_measurement_2, odometry_noise_2
+        ))
+
+        # Update the smoothers with the new factors
+        smoother_batch.update(new_factors, new_values, new_timestamps)
+
+        print("Timestamp = " + str(time) + ", Key = " + str(current_key))
+        print(smoother_batch.calculateEstimatePose2(current_key))
+
+        new_timestamps.clear()
+        new_values.clear()
+        new_factors.resize(0)
+
+
+        time += delta_time
+
+if __name__ == '__main__':
+    BatchFixedLagSmootherExample()
+    print("Example complete")
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index f1bb4005d..d77895d86 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -505,15 +505,9 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
 //*************************************************************************
 // nonlinear
 //*************************************************************************
-#include <gtsam_unstable/nonlinear/sequentialSummarization.h>
-gtsam::GaussianFactorGraph* summarizeGraphSequential(
-    const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
-gtsam::GaussianFactorGraph* summarizeGraphSequential(
-    const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);
-
 #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
 class FixedLagSmootherKeyTimestampMapValue {
-  FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp);
+  FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp);
   FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
 };
 
@@ -528,7 +522,7 @@ class FixedLagSmootherKeyTimestampMap {
   bool empty() const;
   void clear();
 
-  double at(const gtsam::Key& key) const;
+  double at(const size_t key) const;
   void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
 };
 
@@ -558,6 +552,10 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
 
   gtsam::LevenbergMarquardtParams params() const;
+  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+                     Vector, Matrix}>
+  VALUE calculateEstimate(size_t key) const;
 };
 
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>

From 42ac0e589e233f7f476e0240014df9a1d8ed38af Mon Sep 17 00:00:00 2001
From: Jeremy <jeraguilon@gmail.com>
Date: Wed, 27 Feb 2019 04:55:15 -0500
Subject: [PATCH 138/281] Implement unit test

---
 .../examples/FixedLagSmootherExample.py       |  6 +-
 cython/gtsam_unstable/examples/__init__.py    |  0
 cython/gtsam_unstable/tests/__init__.py       |  0
 .../tests/test_FixedLagSmootherExample.py     | 94 +++++++++++++++++++
 4 files changed, 96 insertions(+), 4 deletions(-)
 create mode 100644 cython/gtsam_unstable/examples/__init__.py
 create mode 100644 cython/gtsam_unstable/tests/__init__.py
 create mode 100644 cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py

diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
index 5d44dab64..b9777a07c 100644
--- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
@@ -9,9 +9,6 @@ import numpy as np
 import gtsam
 import gtsam_unstable
 
-# Create noise models
-PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
-MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
 
 def _timestamp_key_value(key, value):
     return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
@@ -35,8 +32,9 @@ def BatchFixedLagSmootherExample():
 
     # Create  a prior on the first pose, placing it at the origin
     prior_mean = gtsam.Pose2(0, 0, 0)
+    prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
     X1 = 0
-    new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, PRIOR_NOISE))
+    new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
     new_values.insert(X1, prior_mean)
     new_timestamps.insert(_timestamp_key_value(X1, 0.0))
 
diff --git a/cython/gtsam_unstable/examples/__init__.py b/cython/gtsam_unstable/examples/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/cython/gtsam_unstable/tests/__init__.py b/cython/gtsam_unstable/tests/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
new file mode 100644
index 000000000..64895f4d3
--- /dev/null
+++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
@@ -0,0 +1,94 @@
+import unittest
+import gtsam
+import gtsam_unstable
+import numpy as np
+
+def _timestamp_key_value(key, value):
+    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
+        key, value
+    )
+class TestFixedLagSmootherExample(unittest.TestCase):
+    # Simple test that checks for equality between C++ example
+    # file and the Python implementation
+    def test_FixedLagSmootherExample(self):
+        # Define a batch fixed lag smoother, which uses
+        # Levenberg-Marquardt to perform the nonlinear optimization
+        lag = 2.0
+        smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
+
+
+        # Create containers to store the factors and linearization points
+        # that will be sent to the smoothers
+        new_factors = gtsam.NonlinearFactorGraph()
+        new_values = gtsam.Values()
+        new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
+
+
+        # Create  a prior on the first pose, placing it at the origin
+        prior_mean = gtsam.Pose2(0, 0, 0)
+        prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+        X1 = 0
+        new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
+        new_values.insert(X1, prior_mean)
+        new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+
+        delta_time = 0.25
+        time = 0.25
+
+        i = 0
+
+        ground_truth = [
+            gtsam.Pose2(0.49792, 0.007802, 0.015),
+            gtsam.Pose2(0.99547, 0.023019, 0.03),
+            gtsam.Pose2(1.4928, 0.045725, 0.045),
+            gtsam.Pose2(1.9898, 0.075888, 0.06),
+            gtsam.Pose2(2.4863, 0.1135, 0.075),
+            gtsam.Pose2(2.9821, 0.15856, 0.09),
+            gtsam.Pose2(3.4772, 0.21105, 0.105),
+            gtsam.Pose2(3.9715, 0.27096, 0.12),
+            gtsam.Pose2(4.4648, 0.33827, 0.135),
+            gtsam.Pose2(4.957, 0.41298, 0.15),
+            gtsam.Pose2(5.4481, 0.49506, 0.165),
+            gtsam.Pose2(5.9379, 0.5845, 0.18),
+        ]
+        while time <= 3.0:
+            previous_key = 1000 * (time - delta_time)
+            current_key = 1000 * time
+
+            # assign current key to the current timestamp
+            new_timestamps.insert(_timestamp_key_value(current_key, time))
+
+            # Add a guess for this pose to the new values
+            # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
+            current_pose = gtsam.Pose2(time * 2, 0, 0)
+            new_values.insert(current_key, current_pose)
+
+            # Add odometry factors from two different sources with different error stats
+            odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
+            odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
+            new_factors.push_back(gtsam.BetweenFactorPose2(
+                previous_key, current_key, odometry_measurement_1, odometry_noise_1
+            ))
+
+            odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
+            odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05]))
+            new_factors.push_back(gtsam.BetweenFactorPose2(
+                previous_key, current_key, odometry_measurement_2, odometry_noise_2
+            ))
+
+            # Update the smoothers with the new factors
+            smoother_batch.update(new_factors, new_values, new_timestamps)
+
+            estimate = smoother_batch.calculateEstimatePose2(current_key)
+            self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
+            print("PASS")
+
+            new_timestamps.clear()
+            new_values.clear()
+            new_factors.resize(0)
+
+            time += delta_time
+            i += 1
+
+if __name__ == "__main__":
+    unittest.main()

From 5e97efa81518f3ecc83d83d43e425edbe951ceed Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Wed, 27 Feb 2019 13:04:38 -0500
Subject: [PATCH 139/281] Liner and update Cmakelists

---
 THANKS                                        |  1 +
 cython/CMakeLists.txt                         |  3 +--
 cython/gtsam_unstable/__init__.py.in          |  1 +
 .../examples/FixedLagSmootherExample.py       | 19 +++++++++++++++++++
 .../tests/test_FixedLagSmootherExample.py     | 12 +++++++++---
 5 files changed, 31 insertions(+), 5 deletions(-)
 create mode 100644 cython/gtsam_unstable/__init__.py.in

diff --git a/THANKS b/THANKS
index f84cfa185..689074b6f 100644
--- a/THANKS
+++ b/THANKS
@@ -1,5 +1,6 @@
 GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
 
+* Jeremy Aguilon
 * Sungtae An
 * Doru Balcan, Bank of America
 * Chris Beall
diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index bc21b91d1..b832a76a5 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -26,8 +26,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
                                     gtsam_unstable  # library to link with
                                     "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam"  # dependencies to be built before wrapping
                                     )
-    # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this
-    file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "")
+    configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py)
   endif()
 
   # Install the custom-generated __init__.py
diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in
new file mode 100644
index 000000000..2b0a28321
--- /dev/null
+++ b/cython/gtsam_unstable/__init__.py.in
@@ -0,0 +1 @@
+from gtsam_unstable import *
diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
index b9777a07c..5fc3bc98d 100644
--- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
@@ -1,4 +1,11 @@
 """
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
 Demonstration of the fixed-lag smoothers using a planar robot example
 and multiple odometry-like sensors
 Author: Frank Dellaert (C++), Jeremy Aguilon (Python)
@@ -11,12 +18,21 @@ import gtsam_unstable
 
 
 def _timestamp_key_value(key, value):
+    """
+
+    """
     return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
         key, value
     )
 
 
 def BatchFixedLagSmootherExample():
+    """
+    Runs a batch fixed smoother on an agent with two odometry
+    sensors that is simply moving along the x axis in constant
+    speed.
+    """
+
     # Define a batch fixed lag smoother, which uses
     # Levenberg-Marquardt to perform the nonlinear optimization
     lag = 2.0
@@ -41,6 +57,9 @@ def BatchFixedLagSmootherExample():
     delta_time = 0.25
     time = 0.25
 
+    # Iterates from 0.25s to 3.0s, adding 0.25s each loop
+    # In each iteration, the agent moves at a constant speed
+    # and its two odometers measure the change.
     while time <= 3.0:
         previous_key = 1000 * (time - delta_time)
         current_key = 1000 * time
diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
index 64895f4d3..8994913a2 100644
--- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
@@ -8,9 +8,16 @@ def _timestamp_key_value(key, value):
         key, value
     )
 class TestFixedLagSmootherExample(unittest.TestCase):
-    # Simple test that checks for equality between C++ example
-    # file and the Python implementation
+    '''
+    Tests the fixed lag smoother wrapper
+    '''
+
     def test_FixedLagSmootherExample(self):
+        '''
+        Simple test that checks for equality between C++ example
+        file and the Python implementation. See
+        gtsam_unstable/examples/FixedLagSmootherExample.cpp
+        '''
         # Define a batch fixed lag smoother, which uses
         # Levenberg-Marquardt to perform the nonlinear optimization
         lag = 2.0
@@ -81,7 +88,6 @@ class TestFixedLagSmootherExample(unittest.TestCase):
 
             estimate = smoother_batch.calculateEstimatePose2(current_key)
             self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
-            print("PASS")
 
             new_timestamps.clear()
             new_values.clear()

From 1ca14d516416be6efb460c9695efa2bf773460c8 Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Wed, 27 Feb 2019 13:08:49 -0500
Subject: [PATCH 140/281] Add comment

---
 THANKS                                                      | 2 +-
 cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py | 4 ++++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/THANKS b/THANKS
index 689074b6f..f2b51f61d 100644
--- a/THANKS
+++ b/THANKS
@@ -1,6 +1,6 @@
 GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
 
-* Jeremy Aguilon
+* Jeremy Aguilon, Facebook
 * Sungtae An
 * Doru Balcan, Bank of America
 * Chris Beall
diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
index 8994913a2..e26c450a0 100644
--- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
@@ -58,6 +58,10 @@ class TestFixedLagSmootherExample(unittest.TestCase):
             gtsam.Pose2(5.4481, 0.49506, 0.165),
             gtsam.Pose2(5.9379, 0.5845, 0.18),
         ]
+        # Iterates from 0.25s to 3.0s, adding 0.25s each loop
+        # In each iteration, the agent moves at a constant speed
+        # and its two odometers measure the change. The smoothed
+        # result is then compared to the ground truth
         while time <= 3.0:
             previous_key = 1000 * (time - delta_time)
             current_key = 1000 * time

From cca445711cf0daddb6863a75aeea0c7467a86457 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 27 Feb 2019 13:20:42 -0500
Subject: [PATCH 141/281] Updated INSTALL file to use Markdown syntax and
 applied correct formatting

---
 INSTALL    | 146 -----------------------------------------------------
 INSTALL.md | 143 +++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 143 insertions(+), 146 deletions(-)
 delete mode 100644 INSTALL
 create mode 100644 INSTALL.md

diff --git a/INSTALL b/INSTALL
deleted file mode 100644
index d2f0b68df..000000000
--- a/INSTALL
+++ /dev/null
@@ -1,146 +0,0 @@
-Quickstart
-
-In the root library folder execute:
-
-$] mkdir build
-$] cd build
-$] cmake ..
-$] make check (optional, runs unit tests)
-$] make install
-
-Important Installation Notes
-----------------------------
-
-1)
-GTSAM requires the following libraries to be installed on your system:
- - BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
- - Cmake version 3.0 or higher
- - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
-
-Optional dependent libraries:
- - If TBB is installed and detectable by CMake GTSAM will use it automatically.
-   Ensure that CMake prints "Use Intel TBB : Yes".  To disable the use of TBB,
-   disable the CMake flag GTSAM_WITH_TBB (enabled by default).  On Ubuntu, TBB
-   may be installed from the Ubuntu repositories, and for other platforms it
-   may be downloaded from https://www.threadingbuildingblocks.org/
-
-Tested compilers:
-
-- GCC 4.2-4.7
-- OSX Clang 2.9-5.0
-- OSX GCC 4.2
-- MSVC 2010, 2012
-
-Tested systems:
-
-- Ubuntu 11.04 - 18.04
-- MacOS 10.6 - 10.9
-- Windows 7, 8, 8.1, 10
-
-Known issues:
-
-- MSVC 2013 is not yet supported because it cannot build the serialization module
-  of Boost 1.55 (or earlier).
-
-2)
-GTSAM makes extensive use of debug assertions, and we highly recommend you work
-in Debug mode while developing (enabled by default). Likewise, it is imperative
-that you switch to release mode when running finished code and for timing. GTSAM
-will run up to 10x faster in Release mode! See the end of this document for
-additional debugging tips.
-
-3)
-GTSAM has Doxygen documentation. To generate, run 'make doc' from your
-build directory.
-
-4)
-The instructions below install the library to the default system install path and
-build all components. From a terminal, starting in the root library folder,
-execute commands as follows for an out-of-source build:
-
-$] mkdir build
-$] cd build
-$] cmake ..
-$] make check (optional, runs unit tests)
-$] make install
-
-This will build the library and unit tests, run all of the unit tests,
-and then install the library itself.
-
-- CMake Configuration Options and Details
-
-GTSAM has a number of options that can be configured, which is best done with
-one of the following:
-
-  ccmake      the curses GUI for cmake
-  cmake-gui   a real GUI for cmake
-
-Important Options:
-
-CMAKE_BUILD_TYPE: We support several build configurations for GTSAM (case insensitive)
-  Debug (default)  All error checking options on, no optimization. Use for development.
-  Release          Optimizations turned on, no debug symbols.
-  Timing           Adds ENABLE_TIMING flag to provide statistics on operation
-  Profiling        Standard configuration for use during profiling
-  RelWithDebInfo   Same as Release, but with the -g flag for debug symbols
-
-CMAKE_INSTALL_PREFIX: The install folder. The default is typically /usr/local/
-To configure to install to your home directory, you could execute:
-$] cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..
-
-GTSAM_TOOLBOX_INSTALL_PATH: The Matlab toolbox will be installed in a subdirectory
-of this folder, called 'gtsam'.
-$] cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox ..
-
-GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in
-subfolders to be linked against convenience libraries rather than the full libgtsam.
-Set with the command line as follows:
-$] cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..
-  ON (Default)   This builds convenience libraries and links tests against them. This
-  				 option is suggested for gtsam developers, as it is possible to build
-  				 and run tests without first building the rest of the library, and
-  				 speeds up compilation for a single test. The downside of this option
-  				 is that it will build the entire library again to build the full
-  				 libgtsam library, so build/install will be slower.
-  OFF            This will build all of libgtsam before any of the tests, and then
-  				 link all of the tests at once. This option is best for users of GTSAM,
-  				 as it avoids rebuilding the entirety of gtsam an extra time.
-
-GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library.
-Set with the command line as follows:
-$] cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON ..
-  ON             When enabled, libgtsam_unstable will be built and installed with the
-                 same options as libgtsam.  In addition, if tests are enabled, the
-                 unit tests will be built as well.  The Matlab toolbox will also
-                 be generated if the matlab toolbox is enabled, installing into a
-                 folder called "gtsam_unstable".
-  OFF (Default)  If disabled, no gtsam_unstable code will be included in build or install.
-
-Check
-
-"make check" will build and run all of the tests. Note that the tests will only be
-built when using the "check" targets, to prevent "make install" from building the tests
-unnecessarily. You can also run "make timing" to build all of the timing scripts.
-To run check on a particular module only, run "make check.[subfolder]", so to run
-just the geometry tests, run "make check.geometry". Individual tests can be run by
-appending ".run" to the name of the test, for example, to run testMatrix, run
-"make testMatrix.run".
-
-MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your
-shell's PATH environment variable. mex is installed with matlab at
-$MATLABROOT/bin/mex
-
-$MATLABROOT can be found by executing the command 'matlabroot' in MATLAB
-
-Debugging tips:
-
-Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks
-and safe containers in the standard C++ library and makes problems much easier
-to find.
-
-NOTE:  The native Snow Leopard g++ compiler/library contains a bug that makes
-it impossible to use _GLIBCXX_DEBUG.  MacPorts g++ compilers do work with it though.
-
-NOTE:  If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against
-gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of
-header-only Eigen.
diff --git a/INSTALL.md b/INSTALL.md
new file mode 100644
index 000000000..e19363c7e
--- /dev/null
+++ b/INSTALL.md
@@ -0,0 +1,143 @@
+# Quickstart
+
+In the root library folder execute:
+
+```sh
+$ mkdir build
+$ cd build
+$ cmake ..
+$ make check # (optional, runs unit tests)
+$ make install
+```
+
+## Important Installation Notes
+
+1. GTSAM requires the following libraries to be installed on your system:
+    - BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
+    - Cmake version 3.0 or higher
+    - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
+
+    Optional dependent libraries:
+     - If TBB is installed and detectable by CMake GTSAM will use it automatically.
+       Ensure that CMake prints "Use Intel TBB : Yes".  To disable the use of TBB,
+       disable the CMake flag GTSAM_WITH_TBB (enabled by default).  On Ubuntu, TBB
+       may be installed from the Ubuntu repositories, and for other platforms it
+       may be downloaded from https://www.threadingbuildingblocks.org/
+
+    Tested compilers:
+
+    - GCC 4.2-4.7
+    - OSX Clang 2.9-5.0
+    - OSX GCC 4.2
+    - MSVC 2010, 2012
+
+    Tested systems:
+
+    - Ubuntu 11.04 - 18.04
+    - MacOS 10.6 - 10.9
+    - Windows 7, 8, 8.1, 10
+
+    Known issues:
+
+    - MSVC 2013 is not yet supported because it cannot build the serialization module
+      of Boost 1.55 (or earlier).
+
+2. GTSAM makes extensive use of debug assertions, and we highly recommend you work
+in Debug mode while developing (enabled by default). Likewise, it is imperative
+that you switch to release mode when running finished code and for timing. GTSAM
+will run up to 10x faster in Release mode! See the end of this document for
+additional debugging tips.
+
+3. GTSAM has Doxygen documentation. To generate, run 'make doc' from your
+build directory.
+
+4. The instructions below install the library to the default system install path and
+build all components. From a terminal, starting in the root library folder,
+execute commands as follows for an out-of-source build:
+
+  ```sh
+  $ mkdir build
+  $ cd build
+  $ cmake ..
+  $ make check (optional, runs unit tests)
+  $ make install
+  ```
+
+  This will build the library and unit tests, run all of the unit tests,
+  and then install the library itself.
+
+## CMake Configuration Options and Details
+
+GTSAM has a number of options that can be configured, which is best done with
+one of the following:
+
+  - ccmake      the curses GUI for cmake
+  - cmake-gui   a real GUI for cmake
+
+### Important Options:
+
+#### CMAKE_BUILD_TYPE
+We support several build configurations for GTSAM (case insensitive)
+
+```cmake -DCMAKE_BUILD_TYPE=[Option] ..```
+
+  - Debug (default)  All error checking options on, no optimization. Use for development.
+  - Release          Optimizations turned on, no debug symbols.
+  - Timing           Adds ENABLE_TIMING flag to provide statistics on operation
+  - Profiling        Standard configuration for use during profiling
+  - RelWithDebInfo   Same as Release, but with the -g flag for debug symbols
+
+#### CMAKE_INSTALL_PREFIX
+
+The install folder. The default is typically `/usr/local/`. 
+To configure to install to your home directory, you could execute:
+
+```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..```
+
+#### GTSAM_TOOLBOX_INSTALL_PATH 
+
+The Matlab toolbox will be installed in a subdirectory
+of this folder, called 'gtsam'.
+
+```cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox ..```
+
+#### GTSAM_BUILD_CONVENIENCE_LIBRARIES
+
+This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. 
+Set with the command line as follows:
+
+```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..```
+  - ON (Default): This builds convenience libraries and links tests against them. This   				 option is suggested for gtsam developers, as it is possible to build and run tests without first building the rest of the library, and speeds up compilation for a single test. The downside of this option is that it will build the entire library again to build the full libgtsam library, so build/install will be slower.
+  - OFF: This will build all of libgtsam before any of the tests, and then link all of the tests at once. This option is best for users of GTSAM, as it avoids rebuilding the entirety of gtsam an extra time.
+
+#### GTSAM_BUILD_UNSTABLE
+
+Enable build and install for libgtsam_unstable library.
+Set with the command line as follows:
+
+```cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON ..```
+
+  ON:             When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam.  In addition, if tests are enabled, the unit tests will be built as well.  The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`.
+  OFF (Default)  If disabled, no `gtsam_unstable` code will be included in build or install.
+
+## Check
+
+`make check` will build and run all of the tests. Note that the tests will only be
+built when using the "check" targets, to prevent `make install` from building the tests
+unnecessarily. You can also run `make timing` to build all of the timing scripts.
+To run check on a particular module only, run `make check.[subfolder]`, so to run
+just the geometry tests, run `make check.geometry`. Individual tests can be run by
+appending `.run` to the name of the test, for example, to run testMatrix, run
+`make testMatrix.run`.
+
+MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your shell's PATH environment variable. mex is installed with matlab at `$MATLABROOT/bin/mex`
+
+$MATLABROOT can be found by executing the command `matlabroot` in MATLAB
+
+## Debugging tips
+
+Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find.
+
+NOTE:  The native Snow Leopard g++ compiler/library contains a bug that makes it impossible to use _GLIBCXX_DEBUG.  MacPorts g++ compilers do work with it though.
+
+NOTE:  If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of header-only Eigen.

From 2eee111960d46b5cf90bfd68dabb72210817a4a7 Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Wed, 27 Feb 2019 13:58:40 -0500
Subject: [PATCH 142/281] Forgotten docstring

---
 cython/gtsam_unstable/examples/FixedLagSmootherExample.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
index 5fc3bc98d..141162044 100644
--- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
@@ -19,7 +19,7 @@ import gtsam_unstable
 
 def _timestamp_key_value(key, value):
     """
-
+    Creates a key value pair for a FixedLagSmootherKeyTimeStampMap
     """
     return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
         key, value

From dae50c9a4a7702065e46e789b3bf4ef275991546 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 27 Feb 2019 14:23:18 -0500
Subject: [PATCH 143/281] updated link in README.md to point to correct
 INSTALL.md

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index e815d7f4b..7e03c81f2 100644
--- a/README.md
+++ b/README.md
@@ -71,7 +71,7 @@ Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTS
 which support (superfast) automatic differentiation,
 can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home).
 
-See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
+See the [`INSTALL`](INSTALL.md) file for more detailed installation instructions.
 
 GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
 

From 27d47c32bb65c22ac6929a23a33afeed1405c16e Mon Sep 17 00:00:00 2001
From: Jeremy <jeraguilon@gmail.com>
Date: Wed, 27 Feb 2019 23:38:59 -0500
Subject: [PATCH 144/281] Update cmaklists

---
 cython/CMakeLists.txt | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index b832a76a5..9acf8174c 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -27,7 +27,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
                                     "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam"  # dependencies to be built before wrapping
                                     )
     configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py)
+    install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable")
   endif()
+    install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
 
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable

From 5670c73158ac2a8c8a3360819d881d3b76889d44 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Tue, 8 Jan 2019 10:28:19 +0000
Subject: [PATCH 145/281] improved cython wrapper python3 support

---
 cmake/FindCython.cmake                  | 4 ++--
 cython/gtsam/__init__.py.in             | 2 +-
 wrap/Module.cpp                         | 4 ++++
 wrap/tests/expected-cython/geometry.pyx | 2 ++
 4 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake
index 23afb00e6..292d9d51e 100644
--- a/cmake/FindCython.cmake
+++ b/cmake/FindCython.cmake
@@ -32,7 +32,7 @@
 find_package( PythonInterp )
 if ( PYTHONINTERP_FOUND )
   execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c"
-      "import Cython; print Cython.__path__"
+      "import Cython; print(Cython.__path__[0])"
       RESULT_VARIABLE RESULT
       OUTPUT_VARIABLE CYTHON_PATH
       OUTPUT_STRIP_TRAILING_WHITESPACE
@@ -51,7 +51,7 @@ endif ()
 # RESULT=0 means ok
 if ( NOT RESULT )
   execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c"
-      "import Cython; print Cython.__version__"
+      "import Cython; print(Cython.__version__)"
       RESULT_VARIABLE RESULT
       OUTPUT_VARIABLE CYTHON_VAR_OUTPUT
       ERROR_VARIABLE CYTHON_VAR_OUTPUT
diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in
index 7d456023f..85451c680 100644
--- a/cython/gtsam/__init__.py.in
+++ b/cython/gtsam/__init__.py.in
@@ -1,2 +1,2 @@
-from gtsam import *
+from .gtsam import *
 ${GTSAM_UNSTABLE_IMPORT}
diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index a3b8df630..b2b1dc1dc 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -394,6 +394,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const {
 
 /* ************************************************************************* */
 void Module::emit_cython_pyx(FileWriter& pyxFile) const {
+  // directives...
+  // allow str to automatically coerce to std::string and back (for python3)
+  pyxFile.oss << "# cython: c_string_type=str, c_string_encoding=ascii\n\n";
+
   // headers...
   string pxdHeader = name;
   pyxFile.oss << "cimport numpy as np\n"
diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx
index 4bd14b130..7c20a500d 100644
--- a/wrap/tests/expected-cython/geometry.pyx
+++ b/wrap/tests/expected-cython/geometry.pyx
@@ -1,3 +1,5 @@
+# cython: c_string_type=str, c_string_encoding=ascii
+
 cimport numpy as np
 import numpy as npp
 cimport geometry

From 56ef410276e6dd8fb7ab101b543500300314df6e Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Tue, 8 Jan 2019 10:44:49 +0000
Subject: [PATCH 146/281] adding MKL installation instructions to README

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 7e03c81f2..64b36878e 100644
--- a/README.md
+++ b/README.md
@@ -36,7 +36,7 @@ Prerequisites:
 Optional prerequisites - used automatically if findable by CMake:
 
 - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
-- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
+- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo))
 
 GTSAM 4 Compatibility
 ---------------------

From b63537a47c8f7952a49f8f05fa669de537ada913 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Mon, 28 Jan 2019 16:30:29 +0000
Subject: [PATCH 147/281] fixed cython import for gtsam_unstable

---
 cython/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index bc21b91d1..e53c0b73e 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -19,7 +19,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   # wrap gtsam_unstable
   if(GTSAM_BUILD_UNSTABLE)
     add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h")
-    set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *")
+    set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *")
     wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
                                     "from gtsam.gtsam cimport *"                  # extra imports
                                     "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path

From e896ae1c43376b5fc576061ece1dbac79146d236 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 30 Jan 2019 09:41:28 +0000
Subject: [PATCH 148/281] compile cython compatible with the chosen python
 version

---
 cmake/GtsamCythonWrap.cmake | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index 73e2b63e0..520d72a32 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -35,6 +35,11 @@ function(set_up_required_cython_packages)
   include_directories(${NUMPY_INCLUDE_DIRS})
 endfunction()
 
+execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
+    "from __future__ import print_function;import sys;print(sys.version[0], end='')"
+    OUTPUT_VARIABLE PYTHON_MAJOR_VERSION
+)
+
 # Convert pyx to cpp by executing cython
 # This is the first step to compile cython from the command line
 # as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html
@@ -52,7 +57,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
   add_custom_command(
     OUTPUT ${generated_cpp}
     COMMAND
-      ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
+    ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus -${PYTHON_MAJOR_VERSION} ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
     VERBATIM)
   add_custom_target(${target} ALL DEPENDS ${generated_cpp})
 endfunction()

From 70470ff59ba9d1c318db506d7c1f023e627e42e5 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 30 Jan 2019 09:49:38 +0000
Subject: [PATCH 149/281] fixed more python 3 related import problems

---
 wrap/Module.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/wrap/Module.cpp b/wrap/Module.cpp
index b2b1dc1dc..a7db9e1f6 100644
--- a/wrap/Module.cpp
+++ b/wrap/Module.cpp
@@ -403,9 +403,9 @@ void Module::emit_cython_pyx(FileWriter& pyxFile) const {
   pyxFile.oss << "cimport numpy as np\n"
                  "import numpy as npp\n"
                  "cimport " << pxdHeader << "\n"
-                 "from "<< pxdHeader << " cimport shared_ptr\n"
-                 "from "<< pxdHeader << " cimport dynamic_pointer_cast\n"
-                 "from "<< pxdHeader << " cimport make_shared\n";
+                 "from ."<< pxdHeader << " cimport shared_ptr\n"
+                 "from ."<< pxdHeader << " cimport dynamic_pointer_cast\n"
+                 "from ."<< pxdHeader << " cimport make_shared\n";
 
   pyxFile.oss << "# C helper function that copies all arguments into a positional list.\n"
                  "cdef list process_args(list keywords, tuple args, dict kwargs):\n"

From 8df2c0a9a10d295d6cf6d821016e91296fb1ce19 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 30 Jan 2019 10:03:40 +0000
Subject: [PATCH 150/281] updated wrap test expected output

---
 wrap/tests/expected-cython/geometry.pyx | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx
index 7c20a500d..cae19d600 100644
--- a/wrap/tests/expected-cython/geometry.pyx
+++ b/wrap/tests/expected-cython/geometry.pyx
@@ -3,9 +3,9 @@
 cimport numpy as np
 import numpy as npp
 cimport geometry
-from geometry cimport shared_ptr
-from geometry cimport dynamic_pointer_cast
-from geometry cimport make_shared
+from .geometry cimport shared_ptr
+from .geometry cimport dynamic_pointer_cast
+from .geometry cimport make_shared
 # C helper function that copies all arguments into a positional list.
 cdef list process_args(list keywords, tuple args, dict kwargs):
    cdef str keyword

From a79e8654757fa72260956d98967deef7309124a7 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 14:48:24 +0000
Subject: [PATCH 151/281] added note about python interpreter version to README

---
 cython/README.md | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/cython/README.md b/cython/README.md
index 368d2a76d..3a7f65bca 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -2,20 +2,22 @@ This is the Cython/Python wrapper around the GTSAM C++ library.
 
 INSTALL
 =======
+- if you want to build the gtsam python library for a specific python version, use the `-DPYTHON_EXECUTABLE:FILEPATH=/path/to/python_interpreter` option when running `cmake` otherwise the interpreter at `$ which python` will be used.
+    - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam.
 - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows:
 
 ```bash
  pip install -r <gtsam_folder>/cython/requirements.txt
 ```
 
-- For compatiblity with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
+- For compatibility with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
 named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy.
 
-- Build and install gtsam using cmake with GTSAM_INSTALL_CYTHON_TOOLBOX enabled.
-The wrapped module will be installed to GTSAM_CYTHON_INSTALL_PATH, which is
-by default: <your CMAKE_INSTALL_PREFIX>/cython
+- Build and install gtsam using cmake with `GTSAM_INSTALL_CYTHON_TOOLBOX` enabled.
+The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is
+by default: `<your CMAKE_INSTALL_PREFIX>/cython`
 
-- Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH:
+- Modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
 ```bash
 export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
 ```

From 43ac8ad3438d619764c3b2c010a6d1a6fe90c0c3 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 15:54:33 +0000
Subject: [PATCH 152/281] made experiment script compatible with python 2 and 3

---
 cython/gtsam/tests/experiments.py | 113 ++++++++++++++++++++++++++++++
 1 file changed, 113 insertions(+)
 create mode 100644 cython/gtsam/tests/experiments.py

diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py
new file mode 100644
index 000000000..db3127a83
--- /dev/null
+++ b/cython/gtsam/tests/experiments.py
@@ -0,0 +1,113 @@
+"""
+This file is not a real python unittest. It contains small experiments
+to test the wrapper with gtsam_test, a short version of gtsam.h.
+Its name convention is different from other tests so it won't be discovered.
+"""
+from __future__ import print_function
+import gtsam
+import numpy as np
+
+r = gtsam.Rot3()
+print(r)
+print(r.pitch())
+r2 = gtsam.Rot3()
+r3 = r.compose(r2)
+print("r3 pitch:", r3.pitch())
+
+v = np.array([1, 1, 1]) 
+print("v = ", v)
+r4 = r3.retract(v)
+print("r4 pitch:", r4.pitch())
+r4.print_(b'r4: ')
+r3.print_(b"r3: ")
+
+v = r3.localCoordinates(r4)
+print("localCoordinates:", v)
+
+Rmat = np.array([
+    [0.990074, -0.0942928, 0.104218], 
+    [0.104218,  0.990074, -0.0942928], 
+    [-0.0942928, 0.104218, 0.990074]
+    ])
+r5 = gtsam.Rot3(Rmat)
+r5.print_(b"r5: ")
+
+l = gtsam.Rot3.Logmap(r5)
+print("l = ", l)
+
+
+noise = gtsam.noiseModel_Gaussian.Covariance(Rmat)
+noise.print_(b"noise:")
+
+D = np.array([1.,2.,3.])
+diag = gtsam.noiseModel_Diagonal.Variances(D)
+print("diag:", diag)
+diag.print_(b"diag:")
+print("diag R:", diag.R())
+
+p = gtsam.Point3()
+p.print_("p:")
+factor = gtsam.BetweenFactorPoint3(1,2,p, noise)
+factor.print_(b"factor:")
+
+vv = gtsam.VectorValues()
+vv.print_(b"vv:")
+vv.insert(1, np.array([1.,2.,3.]))
+vv.insert(2, np.array([3.,4.]))
+vv.insert(3, np.array([5.,6.,7.,8.]))
+vv.print_(b"vv:")
+
+vv2 = gtsam.VectorValues(vv)
+vv2.insert(4, np.array([4.,2.,1]))
+vv2.print_(b"vv2:")
+vv.print_(b"vv:")
+
+vv.insert(4, np.array([1.,2.,4.]))
+vv.print_(b"vv:")
+vv3 = vv.add(vv2)
+
+vv3.print_(b"vv3:")
+
+values = gtsam.Values()
+values.insert(1, gtsam.Point3())
+values.insert(2, gtsam.Rot3())
+values.print_(b"values:")
+
+factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag)
+print("Prior factor vector: ", factor)
+
+
+
+keys = gtsam.KeyVector()
+
+keys.push_back(1)
+keys.push_back(2)
+print('size: ', keys.size())
+print(keys.at(0))
+print(keys.at(1))
+
+noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
+noise.print_('noise:')
+print('noise print:', noise)
+f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise,  b=np.ones(2))
+print('JacobianFactor(7):\n', f)
+print("A = ", f.getA())
+print("b = ", f.getb())
+
+f = gtsam.JacobianFactor(np.ones(2))
+f.print_('jacoboian b_in:')
+
+
+print("JacobianFactor initalized with b_in:", f)
+
+diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
+fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag)
+print("priorfactorvector: ", fv)
+
+print("base noise: ", fv.get_noiseModel())
+print("casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel()))
+
+X = gtsam.symbol(65, 19)
+print(X)
+print(gtsam.symbolChr(X))
+print(gtsam.symbolIndex(X))

From 46eed55448fb753b7afc8d9ef8613f410faea8f0 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Feb 2019 09:17:55 +0000
Subject: [PATCH 153/281] removed MKL installation instructions

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 64b36878e..7e03c81f2 100644
--- a/README.md
+++ b/README.md
@@ -36,7 +36,7 @@ Prerequisites:
 Optional prerequisites - used automatically if findable by CMake:
 
 - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
-- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo))
+- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
 
 GTSAM 4 Compatibility
 ---------------------

From e251dbaebd95cae3463b62e7dcfbc4c33e8f589b Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Feb 2019 09:27:09 +0000
Subject: [PATCH 154/281] re-adding flags to pass to cmake to use correct
 python version

---
 cython/README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/README.md b/cython/README.md
index 3a7f65bca..896025124 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -2,7 +2,7 @@ This is the Cython/Python wrapper around the GTSAM C++ library.
 
 INSTALL
 =======
-- if you want to build the gtsam python library for a specific python version, use the `-DPYTHON_EXECUTABLE:FILEPATH=/path/to/python_interpreter` option when running `cmake` otherwise the interpreter at `$ which python` will be used.
+- if you want to build the gtsam python library for python 3, use the `-DPython_ADDITIONAL_VERSIONS=3` option when running `cmake` otherwise the interpreter at `$ which python` will be used.
     - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam.
 - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows:
 

From a1fba6a5b1e3f6627414b84201ca3d376cd7bcb4 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Feb 2019 09:28:34 +0000
Subject: [PATCH 155/281] removed experiments.py

---
 cython/gtsam/tests/experiments.py | 113 ------------------------------
 1 file changed, 113 deletions(-)
 delete mode 100644 cython/gtsam/tests/experiments.py

diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py
deleted file mode 100644
index db3127a83..000000000
--- a/cython/gtsam/tests/experiments.py
+++ /dev/null
@@ -1,113 +0,0 @@
-"""
-This file is not a real python unittest. It contains small experiments
-to test the wrapper with gtsam_test, a short version of gtsam.h.
-Its name convention is different from other tests so it won't be discovered.
-"""
-from __future__ import print_function
-import gtsam
-import numpy as np
-
-r = gtsam.Rot3()
-print(r)
-print(r.pitch())
-r2 = gtsam.Rot3()
-r3 = r.compose(r2)
-print("r3 pitch:", r3.pitch())
-
-v = np.array([1, 1, 1]) 
-print("v = ", v)
-r4 = r3.retract(v)
-print("r4 pitch:", r4.pitch())
-r4.print_(b'r4: ')
-r3.print_(b"r3: ")
-
-v = r3.localCoordinates(r4)
-print("localCoordinates:", v)
-
-Rmat = np.array([
-    [0.990074, -0.0942928, 0.104218], 
-    [0.104218,  0.990074, -0.0942928], 
-    [-0.0942928, 0.104218, 0.990074]
-    ])
-r5 = gtsam.Rot3(Rmat)
-r5.print_(b"r5: ")
-
-l = gtsam.Rot3.Logmap(r5)
-print("l = ", l)
-
-
-noise = gtsam.noiseModel_Gaussian.Covariance(Rmat)
-noise.print_(b"noise:")
-
-D = np.array([1.,2.,3.])
-diag = gtsam.noiseModel_Diagonal.Variances(D)
-print("diag:", diag)
-diag.print_(b"diag:")
-print("diag R:", diag.R())
-
-p = gtsam.Point3()
-p.print_("p:")
-factor = gtsam.BetweenFactorPoint3(1,2,p, noise)
-factor.print_(b"factor:")
-
-vv = gtsam.VectorValues()
-vv.print_(b"vv:")
-vv.insert(1, np.array([1.,2.,3.]))
-vv.insert(2, np.array([3.,4.]))
-vv.insert(3, np.array([5.,6.,7.,8.]))
-vv.print_(b"vv:")
-
-vv2 = gtsam.VectorValues(vv)
-vv2.insert(4, np.array([4.,2.,1]))
-vv2.print_(b"vv2:")
-vv.print_(b"vv:")
-
-vv.insert(4, np.array([1.,2.,4.]))
-vv.print_(b"vv:")
-vv3 = vv.add(vv2)
-
-vv3.print_(b"vv3:")
-
-values = gtsam.Values()
-values.insert(1, gtsam.Point3())
-values.insert(2, gtsam.Rot3())
-values.print_(b"values:")
-
-factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag)
-print("Prior factor vector: ", factor)
-
-
-
-keys = gtsam.KeyVector()
-
-keys.push_back(1)
-keys.push_back(2)
-print('size: ', keys.size())
-print(keys.at(0))
-print(keys.at(1))
-
-noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
-noise.print_('noise:')
-print('noise print:', noise)
-f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise,  b=np.ones(2))
-print('JacobianFactor(7):\n', f)
-print("A = ", f.getA())
-print("b = ", f.getb())
-
-f = gtsam.JacobianFactor(np.ones(2))
-f.print_('jacoboian b_in:')
-
-
-print("JacobianFactor initalized with b_in:", f)
-
-diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
-fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag)
-print("priorfactorvector: ", fv)
-
-print("base noise: ", fv.get_noiseModel())
-print("casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel()))
-
-X = gtsam.symbol(65, 19)
-print(X)
-print(gtsam.symbolChr(X))
-print(gtsam.symbolIndex(X))

From 27f87d340e1a24071c728a37c9a00eb5a837f41b Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Feb 2019 09:45:06 +0000
Subject: [PATCH 156/281] caching cmake variables

---
 cmake/GtsamCythonWrap.cmake | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index 520d72a32..bc6f230b0 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -3,6 +3,8 @@
 # in the current environment are different from the cached!
 unset(PYTHON_EXECUTABLE CACHE)
 unset(CYTHON_EXECUTABLE CACHE)
+unset(PYTHON_INCLUDE_DIR CACHE)
+unset(PYTHON_MAJOR_VERSION CACHE)
 find_package(Cython 0.25.2 REQUIRED)
 
 # User-friendly Cython wrapping and installing function.

From 09ac7f7c069a0c5f6d53d98b87a3e36e8c65d5e9 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Feb 2019 09:55:09 +0000
Subject: [PATCH 157/281] removed requirement for python 2.7 in cmake

---
 cmake/GtsamCythonWrap.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index bc6f230b0..0f4cb827e 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -31,7 +31,7 @@ endfunction()
 
 function(set_up_required_cython_packages)
   # Set up building of cython module
-  find_package(PythonLibs 2.7 REQUIRED)
+  find_package(PythonLibs REQUIRED)
   include_directories(${PYTHON_INCLUDE_DIRS})
   find_package(NumPy REQUIRED)
   include_directories(${NUMPY_INCLUDE_DIRS})

From e9e8ca39900c84b3c2e4c3986ff591cc494700c4 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Thu, 14 Feb 2019 09:45:48 +0000
Subject: [PATCH 158/281] added option to specify python version

---
 CMakeLists.txt              |  2 ++
 cmake/FindNumPy.cmake       |  8 ++++++++
 cmake/GtsamCythonWrap.cmake | 21 ++++++++++++++++-----
 cython/README.md            |  2 +-
 4 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c1a421a31..834ce732e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -78,6 +78,7 @@ endif()
 option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
 option(GTSAM_INSTALL_CYTHON_TOOLBOX      "Enable/Disable installation of Cython toolbox"  OFF)
 option(GTSAM_BUILD_WRAP                  "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON)
+set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper or python module for (or Default)")
 
 # Check / set dependent variables for MATLAB wrapper
 if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP)
@@ -554,6 +555,7 @@ endif()
 
 message(STATUS "Cython toolbox flags                                      ")
 print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX}      "Install Cython toolbox         ")
+message(STATUS "  Python version                 : ${GTSAM_PYTHON_VERSION}")
 print_config_flag(${GTSAM_BUILD_WRAP}                  "Build Wrap                     ")
 message(STATUS "===============================================================")
 
diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake
index eafed165e..4f5743aa6 100644
--- a/cmake/FindNumPy.cmake
+++ b/cmake/FindNumPy.cmake
@@ -40,9 +40,17 @@
 
 # Finding NumPy involves calling the Python interpreter
 if(NumPy_FIND_REQUIRED)
+  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
     find_package(PythonInterp REQUIRED)
+  else()
+      find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
+  endif()
 else()
+  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
     find_package(PythonInterp)
+  else()
+    find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT)
+  endif()
 endif()
 
 if(NOT PYTHONINTERP_FOUND)
diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index 0f4cb827e..f329d31ab 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -5,8 +5,19 @@ unset(PYTHON_EXECUTABLE CACHE)
 unset(CYTHON_EXECUTABLE CACHE)
 unset(PYTHON_INCLUDE_DIR CACHE)
 unset(PYTHON_MAJOR_VERSION CACHE)
+
+if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+  find_package(PythonLibs REQUIRED)
+else()
+  find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
+endif()
 find_package(Cython 0.25.2 REQUIRED)
 
+execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
+    "from __future__ import print_function;import sys;print(sys.version[0], end='')"
+    OUTPUT_VARIABLE PYTHON_MAJOR_VERSION
+)
+
 # User-friendly Cython wrapping and installing function.
 # Builds a Cython module from the provided interface_header.
 # For example, for the interface header gtsam.h,
@@ -31,16 +42,16 @@ endfunction()
 
 function(set_up_required_cython_packages)
   # Set up building of cython module
-  find_package(PythonLibs REQUIRED)
+  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+    find_package(PythonLibs REQUIRED)
+  else()
+    find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
+  endif()
   include_directories(${PYTHON_INCLUDE_DIRS})
   find_package(NumPy REQUIRED)
   include_directories(${NUMPY_INCLUDE_DIRS})
 endfunction()
 
-execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
-    "from __future__ import print_function;import sys;print(sys.version[0], end='')"
-    OUTPUT_VARIABLE PYTHON_MAJOR_VERSION
-)
 
 # Convert pyx to cpp by executing cython
 # This is the first step to compile cython from the command line
diff --git a/cython/README.md b/cython/README.md
index 896025124..8ba824f8d 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -2,7 +2,7 @@ This is the Cython/Python wrapper around the GTSAM C++ library.
 
 INSTALL
 =======
-- if you want to build the gtsam python library for python 3, use the `-DPython_ADDITIONAL_VERSIONS=3` option when running `cmake` otherwise the interpreter at `$ which python` will be used.
+- if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used.
     - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam.
 - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows:
 

From 9c1dfd244f2bbab5f1a9038d545612768cd527c6 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Thu, 14 Feb 2019 10:55:16 +0000
Subject: [PATCH 159/281] fixed a bug where unsetting the cached python version
 leads to different numpy/cython/libraries being used

---
 cmake/GtsamCythonWrap.cmake | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index f329d31ab..374e00c46 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -7,8 +7,10 @@ unset(PYTHON_INCLUDE_DIR CACHE)
 unset(PYTHON_MAJOR_VERSION CACHE)
 
 if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+  find_package(PythonInterp REQUIRED)
   find_package(PythonLibs REQUIRED)
 else()
+  find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
   find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
 endif()
 find_package(Cython 0.25.2 REQUIRED)

From 6bd8e44fc759e7291c41211ee998e12c2be11cdc Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Tue, 5 Mar 2019 17:35:50 -0500
Subject: [PATCH 160/281] init

---
 THANKS                               |  1 +
 cmake/GtsamCythonWrap.cmake          |  2 +-
 cython/CMakeLists.txt                | 10 ++++++----
 cython/gtsam_unstable/__init__.py.in |  1 +
 4 files changed, 9 insertions(+), 5 deletions(-)
 create mode 100644 cython/gtsam_unstable/__init__.py.in

diff --git a/THANKS b/THANKS
index f84cfa185..f2b51f61d 100644
--- a/THANKS
+++ b/THANKS
@@ -1,5 +1,6 @@
 GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
 
+* Jeremy Aguilon, Facebook
 * Sungtae An
 * Doru Balcan, Bank of America
 * Chris Beall
diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index 73e2b63e0..1c9868d0d 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -212,7 +212,7 @@ function(install_cython_scripts source_directory dest_directory patterns)
             FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
     endforeach()
   else()
-    install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
+    install(DIRECTORY "${source_directory}/" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
   endif()
 
 endfunction()
diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index bc21b91d1..7a4f518ef 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -22,19 +22,21 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
     set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *")
     wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
                                     "from gtsam.gtsam cimport *"                  # extra imports
-                                    "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path
+                                    "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path
                                     gtsam_unstable  # library to link with
                                     "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam"  # dependencies to be built before wrapping
                                     )
-    # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this
-    file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "")
+    configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py)
+    install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable")
   endif()
+    install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" "*.py")
 
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
   configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
   install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
   # install scripts and tests
-  install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
+  install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" "*.py")
 
 endif ()
+
diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in
new file mode 100644
index 000000000..85f7c3df2
--- /dev/null
+++ b/cython/gtsam_unstable/__init__.py.in
@@ -0,0 +1 @@
+${GTSAM_UNSTABLE_IMPORT}

From 73681b4d2db05127b691ad6199ca5b9852848c67 Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Tue, 5 Mar 2019 17:42:30 -0500
Subject: [PATCH 161/281] Revert unneeded change

---
 cmake/GtsamCythonWrap.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index 1c9868d0d..73e2b63e0 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -212,7 +212,7 @@ function(install_cython_scripts source_directory dest_directory patterns)
             FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
     endforeach()
   else()
-    install(DIRECTORY "${source_directory}/" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
+    install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
   endif()
 
 endfunction()

From 409806efc6d3ffeedb0a0a899b6f8c78f6cf52dc Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Tue, 5 Mar 2019 18:07:46 -0500
Subject: [PATCH 162/281] Clumsy mistake - line should be inside if statement

---
 cython/CMakeLists.txt | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index 7a4f518ef..b5f1365c7 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -28,15 +28,18 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
                                     )
     configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py)
     install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable")
-  endif()
     install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" "*.py")
+  endif()
 
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
   configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
   install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
   # install scripts and tests
-  install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" "*.py")
+
+  message("FOO")
+  message ("${GTSAM_CYTHON_INSTALL_PATH}")
+  install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
 
 endif ()
 

From f3baa4d5b008adf5c59eb0e92547e822014222bc Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Tue, 5 Mar 2019 18:08:22 -0500
Subject: [PATCH 163/281] Forgot to remove print statement

---
 cython/CMakeLists.txt | 2 --
 1 file changed, 2 deletions(-)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index b5f1365c7..8c2a478db 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -37,8 +37,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
   # install scripts and tests
 
-  message("FOO")
-  message ("${GTSAM_CYTHON_INSTALL_PATH}")
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
 
 endif ()

From ed2300dd397307f571a2b7a5494e0610129837ca Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Tue, 5 Mar 2019 18:08:48 -0500
Subject: [PATCH 164/281] Remove unwanted lines

---
 cython/CMakeLists.txt | 2 --
 1 file changed, 2 deletions(-)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index 8c2a478db..8e5d38eec 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -36,8 +36,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
   install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
   # install scripts and tests
-
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
 
 endif ()
-

From dc80bc07557dffa8ea95cbe87c432e759c9a680b Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 6 Mar 2019 10:00:13 +0000
Subject: [PATCH 165/281] find correct interpreter version before looking for
 cython

---
 cmake/FindCython.cmake | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake
index 292d9d51e..e5a32c30d 100644
--- a/cmake/FindCython.cmake
+++ b/cmake/FindCython.cmake
@@ -29,7 +29,12 @@
 
 # Use the Cython executable that lives next to the Python executable
 # if it is a local installation.
-find_package( PythonInterp )
+if(GTSAM_PYTHON_VERSION STREQUAL "Default")
+  find_package(PythonInterp)
+else()
+  find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT)
+endif()
+
 if ( PYTHONINTERP_FOUND )
   execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c"
       "import Cython; print(Cython.__path__[0])"

From 2a967c74ab091b55acda3497dfab4ada51e2cf82 Mon Sep 17 00:00:00 2001
From: cbeall <cbeall@nuro.ai>
Date: Thu, 25 Oct 2018 17:16:01 -0700
Subject: [PATCH 166/281] wip - plotting covariances in 2d

---
 cython/gtsam/examples/OdometryExample.py  | 17 +++++++++++++++++
 cython/gtsam/examples/Pose2SLAMExample.py | 10 ++++++++++
 cython/gtsam/utils/plot.py                | 20 +++++++++++++++++---
 3 files changed, 44 insertions(+), 3 deletions(-)

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

From 7d2e4d2e64ce16715a110e800c6ab81509ba480b Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Wed, 6 Mar 2019 17:47:34 -0500
Subject: [PATCH 167/281] Fix warning message in the unit tests/examples

---
 .../examples/FixedLagSmootherExample.py       | 39 ++++-----
 .../tests/test_FixedLagSmootherExample.py     | 86 +++++++++++--------
 .../examples/FixedLagSmootherExample.cpp      | 36 ++++----
 3 files changed, 91 insertions(+), 70 deletions(-)

diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
index 141162044..786701e0f 100644
--- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py
@@ -19,7 +19,7 @@ import gtsam_unstable
 
 def _timestamp_key_value(key, value):
     """
-    Creates a key value pair for a FixedLagSmootherKeyTimeStampMap
+
     """
     return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
         key, value
@@ -29,8 +29,7 @@ def _timestamp_key_value(key, value):
 def BatchFixedLagSmootherExample():
     """
     Runs a batch fixed smoother on an agent with two odometry
-    sensors that is simply moving along the x axis in constant
-    speed.
+    sensors that is simply moving to the
     """
 
     # Define a batch fixed lag smoother, which uses
@@ -38,14 +37,12 @@ def BatchFixedLagSmootherExample():
     lag = 2.0
     smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
 
-
     # Create containers to store the factors and linearization points
     # that will be sent to the smoothers
     new_factors = gtsam.NonlinearFactorGraph()
     new_values = gtsam.Values()
     new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
 
-
     # Create  a prior on the first pose, placing it at the origin
     prior_mean = gtsam.Pose2(0, 0, 0)
     prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@@ -57,9 +54,6 @@ def BatchFixedLagSmootherExample():
     delta_time = 0.25
     time = 0.25
 
-    # Iterates from 0.25s to 3.0s, adding 0.25s each loop
-    # In each iteration, the agent moves at a constant speed
-    # and its two odometers measure the change.
     while time <= 3.0:
         previous_key = 1000 * (time - delta_time)
         current_key = 1000 * time
@@ -72,32 +66,37 @@ def BatchFixedLagSmootherExample():
         current_pose = gtsam.Pose2(time * 2, 0, 0)
         new_values.insert(current_key, current_pose)
 
-        # Add odometry factors from two different sources with different error stats
+        # Add odometry factors from two different sources with different error
+        # stats
         odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
-        odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
+        odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.1, 0.1, 0.05]))
         new_factors.push_back(gtsam.BetweenFactorPose2(
             previous_key, current_key, odometry_measurement_1, odometry_noise_1
         ))
 
         odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
-        odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05]))
+        odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.05, 0.05, 0.05]))
         new_factors.push_back(gtsam.BetweenFactorPose2(
             previous_key, current_key, odometry_measurement_2, odometry_noise_2
         ))
 
-        # Update the smoothers with the new factors
-        smoother_batch.update(new_factors, new_values, new_timestamps)
-
-        print("Timestamp = " + str(time) + ", Key = " + str(current_key))
-        print(smoother_batch.calculateEstimatePose2(current_key))
-
-        new_timestamps.clear()
-        new_values.clear()
-        new_factors.resize(0)
+        # Update the smoothers with the new factors. In this case,
+        # one iteration must pass for Levenberg-Marquardt to accurately
+        # estimate
+        if time >= 0.50:
+            smoother_batch.update(new_factors, new_values, new_timestamps)
+            print("Timestamp = " + str(time) + ", Key = " + str(current_key))
+            print(smoother_batch.calculateEstimatePose2(current_key))
 
+            new_timestamps.clear()
+            new_values.clear()
+            new_factors.resize(0)
 
         time += delta_time
 
+
 if __name__ == '__main__':
     BatchFixedLagSmootherExample()
     print("Example complete")
diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
index e26c450a0..90c4e436b 100644
--- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
@@ -3,10 +3,13 @@ import gtsam
 import gtsam_unstable
 import numpy as np
 
+
 def _timestamp_key_value(key, value):
     return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
         key, value
     )
+
+
 class TestFixedLagSmootherExample(unittest.TestCase):
     '''
     Tests the fixed lag smoother wrapper
@@ -23,19 +26,20 @@ class TestFixedLagSmootherExample(unittest.TestCase):
         lag = 2.0
         smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
 
-
         # Create containers to store the factors and linearization points
         # that will be sent to the smoothers
         new_factors = gtsam.NonlinearFactorGraph()
         new_values = gtsam.Values()
         new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
 
-
         # Create  a prior on the first pose, placing it at the origin
         prior_mean = gtsam.Pose2(0, 0, 0)
-        prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+        prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.3, 0.3, 0.1]))
         X1 = 0
-        new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
+        new_factors.push_back(
+            gtsam.PriorFactorPose2(
+                X1, prior_mean, prior_noise))
         new_values.insert(X1, prior_mean)
         new_timestamps.insert(_timestamp_key_value(X1, 0.0))
 
@@ -45,19 +49,19 @@ class TestFixedLagSmootherExample(unittest.TestCase):
         i = 0
 
         ground_truth = [
-            gtsam.Pose2(0.49792, 0.007802, 0.015),
-            gtsam.Pose2(0.99547, 0.023019, 0.03),
-            gtsam.Pose2(1.4928, 0.045725, 0.045),
-            gtsam.Pose2(1.9898, 0.075888, 0.06),
-            gtsam.Pose2(2.4863, 0.1135, 0.075),
-            gtsam.Pose2(2.9821, 0.15856, 0.09),
-            gtsam.Pose2(3.4772, 0.21105, 0.105),
-            gtsam.Pose2(3.9715, 0.27096, 0.12),
-            gtsam.Pose2(4.4648, 0.33827, 0.135),
-            gtsam.Pose2(4.957, 0.41298, 0.15),
-            gtsam.Pose2(5.4481, 0.49506, 0.165),
-            gtsam.Pose2(5.9379, 0.5845, 0.18),
+            gtsam.Pose2(0.995821, 0.0231012, 0.0300001),
+            gtsam.Pose2(1.49284, 0.0457247, 0.045),
+            gtsam.Pose2(1.98981, 0.0758879, 0.06),
+            gtsam.Pose2(2.48627, 0.113502, 0.075),
+            gtsam.Pose2(2.98211, 0.158558, 0.09),
+            gtsam.Pose2(3.47722, 0.211047, 0.105),
+            gtsam.Pose2(3.97149, 0.270956, 0.12),
+            gtsam.Pose2(4.4648, 0.338272, 0.135),
+            gtsam.Pose2(4.95705, 0.41298, 0.15),
+            gtsam.Pose2(5.44812, 0.495063, 0.165),
+            gtsam.Pose2(5.9379, 0.584503, 0.18),
         ]
+
         # Iterates from 0.25s to 3.0s, adding 0.25s each loop
         # In each iteration, the agent moves at a constant speed
         # and its two odometers measure the change. The smoothed
@@ -70,35 +74,49 @@ class TestFixedLagSmootherExample(unittest.TestCase):
             new_timestamps.insert(_timestamp_key_value(current_key, time))
 
             # Add a guess for this pose to the new values
-            # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
+            # Assume that the robot moves at 2 m/s. Position is time[s] *
+            # 2[m/s]
             current_pose = gtsam.Pose2(time * 2, 0, 0)
             new_values.insert(current_key, current_pose)
 
-            # Add odometry factors from two different sources with different error stats
+            # Add odometry factors from two different sources with different
+            # error stats
             odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
-            odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
-            new_factors.push_back(gtsam.BetweenFactorPose2(
-                previous_key, current_key, odometry_measurement_1, odometry_noise_1
-            ))
+            odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+                np.array([0.1, 0.1, 0.05]))
+            new_factors.push_back(
+                gtsam.BetweenFactorPose2(
+                    previous_key,
+                    current_key,
+                    odometry_measurement_1,
+                    odometry_noise_1))
 
             odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
-            odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05]))
-            new_factors.push_back(gtsam.BetweenFactorPose2(
-                previous_key, current_key, odometry_measurement_2, odometry_noise_2
-            ))
+            odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+                np.array([0.05, 0.05, 0.05]))
+            new_factors.push_back(
+                gtsam.BetweenFactorPose2(
+                    previous_key,
+                    current_key,
+                    odometry_measurement_2,
+                    odometry_noise_2))
 
-            # Update the smoothers with the new factors
-            smoother_batch.update(new_factors, new_values, new_timestamps)
+            # Update the smoothers with the new factors. In this case,
+            # one iteration must pass for Levenberg-Marquardt to accurately
+            # estimate
+            if time >= 0.50:
+                smoother_batch.update(new_factors, new_values, new_timestamps)
 
-            estimate = smoother_batch.calculateEstimatePose2(current_key)
-            self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
+                estimate = smoother_batch.calculateEstimatePose2(current_key)
+                self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
+                i += 1
 
-            new_timestamps.clear()
-            new_values.clear()
-            new_factors.resize(0)
+                new_timestamps.clear()
+                new_values.clear()
+                new_factors.resize(0)
 
             time += delta_time
-            i += 1
+
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
index 931e85c1a..8ccc9cc2c 100644
--- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp
+++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
@@ -111,23 +111,27 @@ int main(int argc, char** argv) {
     noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
     newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
 
-    // Update the smoothers with the new factors
-    smootherBatch.update(newFactors, newValues, newTimestamps);
-    smootherISAM2.update(newFactors, newValues, newTimestamps);
-    for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
-      smootherISAM2.update();
+    // Update the smoothers with the new factors.
+    // In this example, Levenberg-Marquadt needs one iteration
+    // to pass to accurately estimate.
+    if (time >= 0.50) {
+      smootherBatch.update(newFactors, newValues, newTimestamps);
+      smootherISAM2.update(newFactors, newValues, newTimestamps);
+      for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
+          smootherISAM2.update();
+      }
+
+      // Print the optimized current pose
+      cout << setprecision(5) << "Timestamp = " << time << endl;
+      smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
+      smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
+      cout << endl;
+
+      // Clear contains for the next iteration
+      newTimestamps.clear();
+      newValues.clear();
+      newFactors.resize(0);
     }
-
-    // Print the optimized current pose
-    cout << setprecision(5) << "Timestamp = " << time << endl;
-    smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
-    smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
-    cout << endl;
-
-    // Clear contains for the next iteration
-    newTimestamps.clear();
-    newValues.clear();
-    newFactors.resize(0);
   }
 
   // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds

From 9a3d51792527ff284d8f6bbc4a5a9cf812d78f2b Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Wed, 6 Mar 2019 17:49:40 -0500
Subject: [PATCH 168/281] Make documentation on .cpp file more specific

---
 gtsam_unstable/examples/FixedLagSmootherExample.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
index 8ccc9cc2c..1376aca40 100644
--- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp
+++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
@@ -111,9 +111,9 @@ int main(int argc, char** argv) {
     noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
     newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
 
-    // Update the smoothers with the new factors.
-    // In this example, Levenberg-Marquadt needs one iteration
-    // to pass to accurately estimate.
+    // Update the smoothers with the new factors. In this example, batch smoother needs one iteration
+    // to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when
+    // both are ready for simplicity.
     if (time >= 0.50) {
       smootherBatch.update(newFactors, newValues, newTimestamps);
       smootherISAM2.update(newFactors, newValues, newTimestamps);

From 2f232fd4d467980a9dfeaddaf02fccbd398d3825 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Thu, 7 Mar 2019 15:58:05 +0000
Subject: [PATCH 169/281] removed redundant call to find_package

---
 cmake/GtsamCythonWrap.cmake | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake
index 374e00c46..6366c1508 100644
--- a/cmake/GtsamCythonWrap.cmake
+++ b/cmake/GtsamCythonWrap.cmake
@@ -44,11 +44,6 @@ endfunction()
 
 function(set_up_required_cython_packages)
   # Set up building of cython module
-  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
-    find_package(PythonLibs REQUIRED)
-  else()
-    find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
-  endif()
   include_directories(${PYTHON_INCLUDE_DIRS})
   find_package(NumPy REQUIRED)
   include_directories(${NUMPY_INCLUDE_DIRS})

From f4bf0d5b0b4319ed99edeec66a18a54b9fe2464a Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Thu, 7 Mar 2019 14:23:11 -0500
Subject: [PATCH 170/281] Update unstable.h file to match upstream

---
 gtsam_unstable/gtsam_unstable.h | 248 ++++++++++++++++----------------
 1 file changed, 125 insertions(+), 123 deletions(-)

diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index d77895d86..39c910826 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -505,130 +505,132 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
 //*************************************************************************
 // nonlinear
 //*************************************************************************
-#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-class FixedLagSmootherKeyTimestampMapValue {
-  FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp);
-  FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
-};
+// #include <gtsam_unstable/nonlinear/sequentialSummarization.h>
+// gtsam::GaussianFactorGraph* summarizeGraphSequential(
+//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
+// gtsam::GaussianFactorGraph* summarizeGraphSequential(
+//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);
 
-class FixedLagSmootherKeyTimestampMap {
-  FixedLagSmootherKeyTimestampMap();
-  FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
-
-  // Note: no print function
-
-  // common STL methods
-  size_t size() const;
-  bool empty() const;
-  void clear();
-
-  double at(const size_t key) const;
-  void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
-};
-
-class FixedLagSmootherResult {
-  size_t getIterations() const;
-  size_t getNonlinearVariables() const;
-  size_t getLinearVariables() const;
-  double getError() const;
-};
-
-#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-virtual class FixedLagSmoother {
-  void print(string s) const;
-  bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
-
-  gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
-  double smootherLag() const;
-
-  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
-  gtsam::Values calculateEstimate() const;
-};
-
-#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
-virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
-  BatchFixedLagSmoother();
-  BatchFixedLagSmoother(double smootherLag);
-  BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
-
-  gtsam::LevenbergMarquardtParams params() const;
-  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
-                     Vector, Matrix}>
-  VALUE calculateEstimate(size_t key) const;
-};
-
-#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
-  IncrementalFixedLagSmoother();
-  IncrementalFixedLagSmoother(double smootherLag);
-  IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
-
-  gtsam::ISAM2Params params() const;
-};
-
-#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
-virtual class ConcurrentFilter {
-  void print(string s) const;
-  bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
-};
-
-virtual class ConcurrentSmoother {
-  void print(string s) const;
-  bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
-};
-
-// Synchronize function
-void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
-
-#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
-class ConcurrentBatchFilterResult {
-  size_t getIterations() const;
-  size_t getLambdas() const;
-  size_t getNonlinearVariables() const;
-  size_t getLinearVariables() const;
-  double getError() const;
-};
-
-virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
-  ConcurrentBatchFilter();
-  ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
-
-  gtsam::NonlinearFactorGraph getFactors() const;
-  gtsam::Values getLinearizationPoint() const;
-  gtsam::Ordering getOrdering() const;
-  gtsam::VectorValues getDelta() const;
-
-  gtsam::ConcurrentBatchFilterResult update();
-  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
-  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
-  gtsam::Values calculateEstimate() const;
-};
-
-#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
-class ConcurrentBatchSmootherResult {
-  size_t getIterations() const;
-  size_t getLambdas() const;
-  size_t getNonlinearVariables() const;
-  size_t getLinearVariables() const;
-  double getError() const;
-};
-
-virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
-  ConcurrentBatchSmoother();
-  ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
-
-  gtsam::NonlinearFactorGraph getFactors() const;
-  gtsam::Values getLinearizationPoint() const;
-  gtsam::Ordering getOrdering() const;
-  gtsam::VectorValues getDelta() const;
-
-  gtsam::ConcurrentBatchSmootherResult update();
-  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
-  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-  gtsam::Values calculateEstimate() const;
-};
+// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+// class FixedLagSmootherKeyTimestampMapValue {
+//   FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp);
+//   FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
+// };
+// 
+// class FixedLagSmootherKeyTimestampMap {
+//   FixedLagSmootherKeyTimestampMap();
+//   FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
+// 
+//   // Note: no print function
+// 
+//   // common STL methods
+//   size_t size() const;
+//   bool empty() const;
+//   void clear();
+// 
+//   double at(const gtsam::Key& key) const;
+//   void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
+// };
+// 
+// class FixedLagSmootherResult {
+//   size_t getIterations() const;
+//   size_t getNonlinearVariables() const;
+//   size_t getLinearVariables() const;
+//   double getError() const;
+// };
+// 
+// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+// virtual class FixedLagSmoother {
+//   void print(string s) const;
+//   bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
+// 
+//   gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
+//   double smootherLag() const;
+// 
+//   gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
+//   gtsam::Values calculateEstimate() const;
+// };
+// 
+// #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
+//   BatchFixedLagSmoother();
+//   BatchFixedLagSmoother(double smootherLag);
+//   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
+// 
+//   gtsam::LevenbergMarquardtParams params() const;
+// };
+// 
+// #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
+//   IncrementalFixedLagSmoother();
+//   IncrementalFixedLagSmoother(double smootherLag);
+//   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
+// 
+//   gtsam::ISAM2Params params() const;
+// };
+// 
+// #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
+// virtual class ConcurrentFilter {
+//   void print(string s) const;
+//   bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
+// };
+// 
+// virtual class ConcurrentSmoother {
+//   void print(string s) const;
+//   bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
+// };
+// 
+// // Synchronize function
+// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
+// 
+// #include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
+// class ConcurrentBatchFilterResult {
+//   size_t getIterations() const;
+//   size_t getLambdas() const;
+//   size_t getNonlinearVariables() const;
+//   size_t getLinearVariables() const;
+//   double getError() const;
+// };
+// 
+// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
+//   ConcurrentBatchFilter();
+//   ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
+// 
+//   gtsam::NonlinearFactorGraph getFactors() const;
+//   gtsam::Values getLinearizationPoint() const;
+//   gtsam::Ordering getOrdering() const;
+//   gtsam::VectorValues getDelta() const;
+// 
+//   gtsam::ConcurrentBatchFilterResult update();
+//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
+//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
+//   gtsam::Values calculateEstimate() const;
+// };
+// 
+// #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
+// class ConcurrentBatchSmootherResult {
+//   size_t getIterations() const;
+//   size_t getLambdas() const;
+//   size_t getNonlinearVariables() const;
+//   size_t getLinearVariables() const;
+//   double getError() const;
+// };
+// 
+// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
+//   ConcurrentBatchSmoother();
+//   ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
+// 
+//   gtsam::NonlinearFactorGraph getFactors() const;
+//   gtsam::Values getLinearizationPoint() const;
+//   gtsam::Ordering getOrdering() const;
+//   gtsam::VectorValues getDelta() const;
+// 
+//   gtsam::ConcurrentBatchSmootherResult update();
+//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
+//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+//   gtsam::Values calculateEstimate() const;
+// };
 
 //*************************************************************************
 // slam

From fe3741e4662c8400e9b41196311d670d881573fa Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Thu, 7 Mar 2019 14:25:05 -0500
Subject: [PATCH 171/281] Fix broken file

---
 gtsam_unstable/gtsam_unstable.h | 248 ++++++++++++++++----------------
 1 file changed, 123 insertions(+), 125 deletions(-)

diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index 39c910826..d77895d86 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
 //*************************************************************************
 // nonlinear
 //*************************************************************************
-// #include <gtsam_unstable/nonlinear/sequentialSummarization.h>
-// gtsam::GaussianFactorGraph* summarizeGraphSequential(
-//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
-// gtsam::GaussianFactorGraph* summarizeGraphSequential(
-//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);
+#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+class FixedLagSmootherKeyTimestampMapValue {
+  FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp);
+  FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
+};
 
-// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-// class FixedLagSmootherKeyTimestampMapValue {
-//   FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp);
-//   FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
-// };
-// 
-// class FixedLagSmootherKeyTimestampMap {
-//   FixedLagSmootherKeyTimestampMap();
-//   FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
-// 
-//   // Note: no print function
-// 
-//   // common STL methods
-//   size_t size() const;
-//   bool empty() const;
-//   void clear();
-// 
-//   double at(const gtsam::Key& key) const;
-//   void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
-// };
-// 
-// class FixedLagSmootherResult {
-//   size_t getIterations() const;
-//   size_t getNonlinearVariables() const;
-//   size_t getLinearVariables() const;
-//   double getError() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-// virtual class FixedLagSmoother {
-//   void print(string s) const;
-//   bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
-// 
-//   gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
-//   double smootherLag() const;
-// 
-//   gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
-//   gtsam::Values calculateEstimate() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
-// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
-//   BatchFixedLagSmoother();
-//   BatchFixedLagSmoother(double smootherLag);
-//   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
-// 
-//   gtsam::LevenbergMarquardtParams params() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
-//   IncrementalFixedLagSmoother();
-//   IncrementalFixedLagSmoother(double smootherLag);
-//   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
-// 
-//   gtsam::ISAM2Params params() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
-// virtual class ConcurrentFilter {
-//   void print(string s) const;
-//   bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
-// };
-// 
-// virtual class ConcurrentSmoother {
-//   void print(string s) const;
-//   bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
-// };
-// 
-// // Synchronize function
-// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
-// 
-// #include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
-// class ConcurrentBatchFilterResult {
-//   size_t getIterations() const;
-//   size_t getLambdas() const;
-//   size_t getNonlinearVariables() const;
-//   size_t getLinearVariables() const;
-//   double getError() const;
-// };
-// 
-// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
-//   ConcurrentBatchFilter();
-//   ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
-// 
-//   gtsam::NonlinearFactorGraph getFactors() const;
-//   gtsam::Values getLinearizationPoint() const;
-//   gtsam::Ordering getOrdering() const;
-//   gtsam::VectorValues getDelta() const;
-// 
-//   gtsam::ConcurrentBatchFilterResult update();
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
-//   gtsam::Values calculateEstimate() const;
-// };
-// 
-// #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
-// class ConcurrentBatchSmootherResult {
-//   size_t getIterations() const;
-//   size_t getLambdas() const;
-//   size_t getNonlinearVariables() const;
-//   size_t getLinearVariables() const;
-//   double getError() const;
-// };
-// 
-// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
-//   ConcurrentBatchSmoother();
-//   ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
-// 
-//   gtsam::NonlinearFactorGraph getFactors() const;
-//   gtsam::Values getLinearizationPoint() const;
-//   gtsam::Ordering getOrdering() const;
-//   gtsam::VectorValues getDelta() const;
-// 
-//   gtsam::ConcurrentBatchSmootherResult update();
-//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
-//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-//   gtsam::Values calculateEstimate() const;
-// };
+class FixedLagSmootherKeyTimestampMap {
+  FixedLagSmootherKeyTimestampMap();
+  FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
+
+  // Note: no print function
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  double at(const size_t key) const;
+  void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
+};
+
+class FixedLagSmootherResult {
+  size_t getIterations() const;
+  size_t getNonlinearVariables() const;
+  size_t getLinearVariables() const;
+  double getError() const;
+};
+
+#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+virtual class FixedLagSmoother {
+  void print(string s) const;
+  bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
+
+  gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
+  double smootherLag() const;
+
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
+  gtsam::Values calculateEstimate() const;
+};
+
+#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
+  BatchFixedLagSmoother();
+  BatchFixedLagSmoother(double smootherLag);
+  BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
+
+  gtsam::LevenbergMarquardtParams params() const;
+  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+                     Vector, Matrix}>
+  VALUE calculateEstimate(size_t key) const;
+};
+
+#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
+  IncrementalFixedLagSmoother();
+  IncrementalFixedLagSmoother(double smootherLag);
+  IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
+
+  gtsam::ISAM2Params params() const;
+};
+
+#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
+virtual class ConcurrentFilter {
+  void print(string s) const;
+  bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
+};
+
+virtual class ConcurrentSmoother {
+  void print(string s) const;
+  bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
+};
+
+// Synchronize function
+void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
+
+#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
+class ConcurrentBatchFilterResult {
+  size_t getIterations() const;
+  size_t getLambdas() const;
+  size_t getNonlinearVariables() const;
+  size_t getLinearVariables() const;
+  double getError() const;
+};
+
+virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
+  ConcurrentBatchFilter();
+  ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::Ordering getOrdering() const;
+  gtsam::VectorValues getDelta() const;
+
+  gtsam::ConcurrentBatchFilterResult update();
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
+  gtsam::Values calculateEstimate() const;
+};
+
+#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
+class ConcurrentBatchSmootherResult {
+  size_t getIterations() const;
+  size_t getLambdas() const;
+  size_t getNonlinearVariables() const;
+  size_t getLinearVariables() const;
+  double getError() const;
+};
+
+virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
+  ConcurrentBatchSmoother();
+  ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::Values getLinearizationPoint() const;
+  gtsam::Ordering getOrdering() const;
+  gtsam::VectorValues getDelta() const;
+
+  gtsam::ConcurrentBatchSmootherResult update();
+  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
+  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+  gtsam::Values calculateEstimate() const;
+};
 
 //*************************************************************************
 // slam

From 549fcca2acd92556ac528ee9cdfad19426ee9192 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 9 Mar 2019 13:06:31 -0500
Subject: [PATCH 172/281] Fixed 2 unit tests

---
 cython/gtsam/tests/test_Scenario.py | 18 ++++++++++++++++-
 cython/gtsam/tests/test_Values.py   | 31 +++++++++++++++++++++--------
 2 files changed, 40 insertions(+), 9 deletions(-)

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()

From 8a81364dafcad5f2f22cc2948b98bce7bb7afa14 Mon Sep 17 00:00:00 2001
From: mbway <matthewbroadway@msn.com>
Date: Sun, 10 Mar 2019 16:25:42 +0000
Subject: [PATCH 173/281] started porting more examples to python

---
 cython/gtsam/examples/README.md            |  57 ++++++++++-
 cython/gtsam/examples/SFMExample.py        | 113 +++++++++++++++++++++
 cython/gtsam/examples/SimpleRotation.py    |  85 ++++++++++++++++
 cython/gtsam/examples/VisualISAMExample.py | 100 ++++++++++++++++++
 4 files changed, 353 insertions(+), 2 deletions(-)
 create mode 100644 cython/gtsam/examples/SFMExample.py
 create mode 100644 cython/gtsam/examples/SimpleRotation.py
 create mode 100644 cython/gtsam/examples/VisualISAMExample.py

diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md
index 067929a20..35ec4f66d 100644
--- a/cython/gtsam/examples/README.md
+++ b/cython/gtsam/examples/README.md
@@ -1,4 +1,57 @@
 These examples are almost identical to the old handwritten python wrapper
 examples. However, there are just some slight name changes, for example
-noiseModel.Diagonal becomes noiseModel_Diagonal etc...
-Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0))
+`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
+Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))`
+
+# Porting Progress
+
+| C++ Example Name                                      | Ported |
+|-------------------------------------------------------|--------|
+| CameraResectioning                                    |        |
+| CreateSFMExampleData                                  |        |
+| DiscreteBayesNet_FG                                   | none of the required discrete functionality is exposed through cython |
+| easyPoint2KalmanFilter                                | ExtendedKalmanFilter not exposed through cython |
+| elaboratePoint2KalmanFilter                           | GaussianSequentialSolver not exposed through cython |
+| ImuFactorExample2                                     | X      |
+| ImuFactorsExample                                     |        |
+| ISAM2Example_SmartFactor                              |        |
+| ISAM2_SmartFactorStereo_IMU                           |        |
+| LocalizationExample                                   | impossible? |
+| METISOrderingExample                                  |        |
+| OdometryExample                                       | X      |
+| PlanarSLAMExample                                     | X      |
+| Pose2SLAMExample                                      | X      |
+| Pose2SLAMExampleExpressions                           |        |
+| Pose2SLAMExample_g2o                                  | X      |
+| Pose2SLAMExample_graph                                |        |
+| Pose2SLAMExample_graphviz                             |        |
+| Pose2SLAMExample_lago                                 | lago not exposed through cython |
+| Pose2SLAMStressTest                                   |        |
+| Pose2SLAMwSPCG                                        |        |
+| Pose3SLAMExample_changeKeys                           |        |
+| Pose3SLAMExampleExpressions_BearingRangeWithTransform |        |
+| Pose3SLAMExample_g2o                                  | X      |
+| Pose3SLAMExample_initializePose3Chordal               |        |
+| Pose3SLAMExample_initializePose3Gradient              |        |
+| RangeISAMExample_plaza2                               |        |
+| SelfCalibrationExample                                |        |
+| SFMExample_bal_COLAMD_METIS                           |        |
+| SFMExample_bal                                        |        |
+| SFMExample                                            | X      |
+| SFMExampleExpressions_bal                             |        |
+| SFMExampleExpressions                                 |        |
+| SFMExample_SmartFactor                                |        |
+| SFMExample_SmartFactorPCG                             |        |
+| SimpleRotation                                        | X      |
+| SolverComparer                                        |        |
+| StereoVOExample                                       |        |
+| StereoVOExample_large                                 |        |
+| TimeTBB                                               |        |
+| UGM_chain                                             | discrete functionality not exposed |
+| UGM_small                                             | discrete functionality not exposed |
+| VisualISAM2Example                                    | X      |
+| VisualISAMExample                                     | X      |
+
+Extra Examples (with no C++ equivalent)
+- PlanarManipulatorExample
+- SFMData
diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py
new file mode 100644
index 000000000..3a64e0cdb
--- /dev/null
+++ b/cython/gtsam/examples/SFMExample.py
@@ -0,0 +1,113 @@
+"""
+GTSAM Copyright 2010, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+A structure-from-motion problem on a simulated dataset
+"""
+
+import gtsam
+import numpy as np
+from gtsam.examples import SFMdata
+from gtsam.gtsam import Values, NonlinearFactorGraph, PriorFactorPose3, SimpleCamera, \
+    GenericProjectionFactorCal3_S2, \
+    PriorFactorPoint3, Pose3, Rot3, Point3, DoglegOptimizer, Cal3_S2
+
+
+# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
+#
+# Each variable in the system (poses and landmarks) must be identified with a unique key.
+# We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+# Here we will use Symbols
+#
+# In GTSAM, measurement functions are represented as 'factors'. Several common factors
+# have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
+# Here we will use Projection factors to model the camera's landmark observations.
+# Also, we will initialize the robot at some location using a Prior factor.
+#
+# When the factors are created, we will add them to a Factor Graph. As the factors we are using
+# are nonlinear factors, we will need a Nonlinear Factor Graph.
+#
+# Finally, once all of the factors have been added to our factor graph, we will want to
+# solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
+# GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
+# trust-region method known as Powell's Degleg
+#
+# The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
+# nonlinear functions around an initial linearization point, then solve the linear system
+# to update the linearization point. This happens repeatedly until the solver converges
+# to a consistent set of variable values. This requires us to specify an initial guess
+# for each variable, held in a Values container.
+
+def symbol(name, index):
+    return gtsam.symbol(ord(name), index)
+
+
+def main():
+    # Define the camera calibration parameters
+    K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+
+    # Define the camera observation noise model
+    measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)  # one pixel in u and v
+
+    # Create the set of ground-truth landmarks
+    points = SFMdata.createPoints()
+
+    # Create the set of ground-truth poses
+    poses = SFMdata.createPoses(K)
+
+    # Create a factor graph
+    graph = NonlinearFactorGraph()
+
+    # Add a prior on pose x1. This indirectly specifies where the origin is.
+    # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+    pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+    factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
+    graph.push_back(factor)
+
+    # Simulated measurements from each camera pose, adding them to the factor graph
+    for i, pose in enumerate(poses):
+        camera = SimpleCamera(pose, K)
+        for j, point in enumerate(points):
+            measurement = camera.project(point)
+            factor = GenericProjectionFactorCal3_S2(
+                measurement, measurement_noise, symbol('x', i), symbol('l', j), K)
+            graph.push_back(factor)
+
+    # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
+    # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
+    # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
+    point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+    factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
+    graph.push_back(factor)
+    graph.print_('Factor Graph:\n')
+
+    # Create the data structure to hold the initial estimate to the solution
+    # Intentionally initialize the variables off from the ground truth
+    initial_estimate = Values()
+    for i, pose in enumerate(poses):
+        r = Rot3.Rodrigues(-0.1, 0.2, 0.25)
+        t = Point3(0.05, -0.10, 0.20)
+        transformed_pose = pose.compose(Pose3(r, t))
+        initial_estimate.insert(symbol('x', i), transformed_pose)
+    for j, point in enumerate(points):
+        transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15]))
+        initial_estimate.insert(symbol('l', j), transformed_point)
+    initial_estimate.print_('Initial Estimates:\n')
+
+    # Optimize the graph and print results
+    params = gtsam.DoglegParams()
+    params.setVerbosity('TERMINATION')
+    optimizer = DoglegOptimizer(graph, initial_estimate, params)
+    print('Optimizing:')
+    result = optimizer.optimize()
+    result.print_('Final results:\n')
+    print('initial error = {}'.format(graph.error(initial_estimate)))
+    print('final error = {}'.format(graph.error(result)))
+
+
+if __name__ == '__main__':
+    main()
diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py
new file mode 100644
index 000000000..0c3ac467f
--- /dev/null
+++ b/cython/gtsam/examples/SimpleRotation.py
@@ -0,0 +1,85 @@
+"""
+GTSAM Copyright 2010, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+This example will perform a relatively trivial optimization on
+a single variable with a single factor.
+"""
+
+import gtsam
+
+import numpy as np
+
+
+def main():
+    """
+    Step 1: Create a factor to express a unary constraint
+
+    The "prior" in this case is the measurement from a sensor,
+    with a model of the noise on the measurement.
+
+    The "Key" created here is a label used to associate parts of the
+    state (stored in "RotValues") with particular factors.  They require
+    an index to allow for lookup, and should be unique.
+
+    In general, creating a factor requires:
+    - A key or set of keys labeling the variables that are acted upon
+    - A measurement value
+    - A measurement model with the correct dimensionality for the factor
+    """
+    prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
+    prior.print_('goal angle')
+    model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
+    key = gtsam.symbol(ord('x'), 1)
+    factor = gtsam.PriorFactorRot2(key, prior, model)
+
+    """
+    Step 2: Create a graph container and add the factor to it
+
+    Before optimizing, all factors need to be added to a Graph container,
+    which provides the necessary top-level functionality for defining a
+    system of constraints.
+
+    In this case, there is only one factor, but in a practical scenario,
+    many more factors would be added.
+    """
+    graph = gtsam.NonlinearFactorGraph()
+    graph.push_back(factor)
+    graph.print_('full graph')
+
+    """
+    Step 3: Create an initial estimate
+
+    An initial estimate of the solution for the system is necessary to
+    start optimization.  This system state is the "Values" instance,
+    which is similar in structure to a dictionary, in that it maps
+    keys (the label created in step 1) to specific values.
+
+    The initial estimate provided to optimization will be used as
+    a linearization point for optimization, so it is important that
+    all of the variables in the graph have a corresponding value in
+    this structure.
+    """
+    initial = gtsam.Values()
+    initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
+    initial.print_('initial estimate')
+
+    """
+    Step 4: Optimize
+
+    After formulating the problem with a graph of constraints
+    and an initial estimate, executing optimization is as simple
+    as calling a general optimization function with the graph and
+    initial estimate.  This will yield a new RotValues structure
+    with the final state of the optimization.
+    """
+    result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
+    result.print_('final result')
+
+
+if __name__ == '__main__':
+    main()
diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py
new file mode 100644
index 000000000..23b880bec
--- /dev/null
+++ b/cython/gtsam/examples/VisualISAMExample.py
@@ -0,0 +1,100 @@
+"""
+GTSAM Copyright 2010, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+A visualSLAM example for the structure-from-motion problem on a simulated dataset
+This version uses iSAM to solve the problem incrementally
+"""
+
+# A structure-from-motion example with landmarks
+# - The landmarks form a 10 meter cube
+# - The robot rotates around the landmarks, always facing towards the cube
+
+import gtsam
+from gtsam.gtsam import Values, Cal3_S2, NonlinearISAM, NonlinearFactorGraph, SimpleCamera, Pose3, Rot3, Point3, \
+    PriorFactorPose3, PriorFactorPoint3, GenericProjectionFactorCal3_S2
+
+import numpy as np
+from gtsam.examples import SFMdata
+
+
+def symbol(name, index):
+    return gtsam.symbol(ord(name), index)
+
+
+def main():
+    # Define the camera calibration parameters
+    K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+
+    # Define the camera observation noise model
+    camera_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)  # one pixel in u and v
+
+    # Create the set of ground-truth landmarks
+    points = SFMdata.createPoints()
+    # Create the set of ground-truth poses
+    poses = SFMdata.createPoses(K)
+
+    # Create a NonlinearISAM object which will relinearize and reorder the variables
+    # every "reorderInterval" updates
+    isam = NonlinearISAM(reorderInterval=3)
+
+    # Create a Factor Graph and Values to hold the new data
+    graph = NonlinearFactorGraph()
+    initial_estimate = Values()
+
+    # Loop over the different poses, adding the observations to iSAM incrementally
+    for i, pose in enumerate(poses):
+        camera = SimpleCamera(pose, K)
+        # Add factors for each landmark observation
+        for j, point in enumerate(points):
+            measurement = camera.project(point)
+            factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K)
+            graph.push_back(factor)
+
+        # Intentionally initialize the variables off from the ground truth
+        noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20))
+        initial_xi = pose.compose(noise)
+
+        # Add an initial guess for the current pose
+        initial_estimate.insert(symbol('x', i), initial_xi)
+
+        # If this is the first iteration, add a prior on the first pose to set the coordinate frame
+        # and a prior on the first landmark to set the scale
+        # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
+        # adding it to iSAM.
+        if i == 0:
+            # Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+            pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+            factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
+            graph.push_back(factor)
+
+            # Add a prior on landmark l0
+            point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+            factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
+            graph.push_back(factor)
+
+            # Add initial guesses to all observed landmarks
+            noise = np.array([-0.25, 0.20, 0.15])
+            for j, point in enumerate(points):
+                # Intentionally initialize the variables off from the ground truth
+                initial_lj = points[j].vector() + noise
+                initial_estimate.insert(symbol('l', j), Point3(initial_lj))
+        else:
+            # Update iSAM with the new factors
+            isam.update(graph, initial_estimate)
+            current_estimate = isam.estimate()
+            print('*' * 50)
+            print('Frame {}:'.format(i))
+            current_estimate.print_('Current estimate: ')
+
+            # Clear the factor graph and values for the next iteration
+            graph.resize(0)
+            initial_estimate.clear()
+
+
+if __name__ == '__main__':
+    main()

From 54512731e31c90f10b136b4449ffc0abc0217555 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 11:57:05 +0000
Subject: [PATCH 174/281] added setup.py

---
 cython/CMakeLists.txt |  1 +
 cython/README.md      |  4 +++-
 cython/setup.py.in    | 28 ++++++++++++++++++++++++++++
 3 files changed, 32 insertions(+), 1 deletion(-)
 create mode 100644 cython/setup.py.in

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index e53c0b73e..317fb7e2d 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -33,6 +33,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
   configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
+  configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
   install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
   # install scripts and tests
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
diff --git a/cython/README.md b/cython/README.md
index 8ba824f8d..78fc7e594 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -17,10 +17,12 @@ named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy.
 The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is
 by default: `<your CMAKE_INSTALL_PREFIX>/cython`
 
-- Modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
+- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
 ```bash
 export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
 ```
+- To install system-wide: navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
+    - (the same command can be used to install into a virtual environment if it is active)
 
 UNIT TESTS
 ==========
diff --git a/cython/setup.py.in b/cython/setup.py.in
new file mode 100644
index 000000000..6a2009913
--- /dev/null
+++ b/cython/setup.py.in
@@ -0,0 +1,28 @@
+try:
+    from setuptools import setup
+except ImportError:
+    from distutils.core import setup
+
+setup(
+    name='gtsam',
+
+    description='Georgia Tech Smoothing And Mapping library',
+    url='https://bitbucket.org/gtborg/gtsam',
+    version='${GTSAM_VERSION_STRING}',  # https://www.python.org/dev/peps/pep-0440/
+    license='Simplified BSD license',
+    keywords='slam sam',
+    long_description=open('${PROJECT_SOURCE_DIR}/README.md', 'r').read(),
+    # https://pypi.org/pypi?%3Aaction=list_classifiers
+    classifiers=[
+        'Development Status :: 5 - Production/Stable',
+        'Intended Audience :: Education',
+        'Intended Audience :: Developers',
+        'Intended Audience :: Science/Research',
+        'License :: OSI Approved :: BSD License',
+        'Programming Language :: Python :: 2',
+        'Programming Language :: Python :: 3',
+    ],
+
+    packages=['gtsam', 'gtsam_eigency'],
+    install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines()
+)

From f6af989e6776aee45d968f9c7eada5f74e88f63f Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 15:15:20 +0000
Subject: [PATCH 175/281] setup.py install .so files

---
 cython/setup.py.in | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index 6a2009913..e99bd5de1 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -1,8 +1,15 @@
+import os
 try:
     from setuptools import setup
 except ImportError:
     from distutils.core import setup
 
+
+packages = ['gtsam', 'gtsam_eigency']
+if ${GTSAM_BUILD_UNSTABLE}:
+    packages += 'gtsam_unstable'
+
+
 setup(
     name='gtsam',
 
@@ -23,6 +30,10 @@ setup(
         'Programming Language :: Python :: 3',
     ],
 
-    packages=['gtsam', 'gtsam_eigency'],
+    packages=packages,
+    package_data={package:
+        [f for f in os.listdir(package) if os.path.splitext(f)[1] == '.so']
+        for package in packages
+    },
     install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines()
 )

From a62197ec6ecc98ff092bbf43b4b7bfa4dc490999 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 15:34:09 +0000
Subject: [PATCH 176/281] gtsam_unstable as a separate package

---
 cython/CMakeLists.txt             |  7 ++-----
 cython/gtsam/__init__.py          |  1 +
 cython/gtsam/__init__.py.in       |  2 --
 cython/gtsam_unstable/__init__.py |  2 ++
 cython/setup.py.in                | 10 +++-------
 5 files changed, 8 insertions(+), 14 deletions(-)
 create mode 100644 cython/gtsam/__init__.py
 delete mode 100644 cython/gtsam/__init__.py.in
 create mode 100644 cython/gtsam_unstable/__init__.py

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index 317fb7e2d..c2d38fd47 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -22,19 +22,16 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
     set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *")
     wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
                                     "from gtsam.gtsam cimport *"                  # extra imports
-                                    "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path
+                                    "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path
                                     gtsam_unstable  # library to link with
                                     "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam"  # dependencies to be built before wrapping
                                     )
-    # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this
-    file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "")
   endif()
 
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
-  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
   configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
-  install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
+  install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/")
   # install scripts and tests
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
 
diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py
new file mode 100644
index 000000000..9bda86efe
--- /dev/null
+++ b/cython/gtsam/__init__.py
@@ -0,0 +1 @@
+from .gtsam import *
diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in
deleted file mode 100644
index 85451c680..000000000
--- a/cython/gtsam/__init__.py.in
+++ /dev/null
@@ -1,2 +0,0 @@
-from .gtsam import *
-${GTSAM_UNSTABLE_IMPORT}
diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py
new file mode 100644
index 000000000..04ce7f1e0
--- /dev/null
+++ b/cython/gtsam_unstable/__init__.py
@@ -0,0 +1,2 @@
+from .gtsam_unstable import *
+
diff --git a/cython/setup.py.in b/cython/setup.py.in
index e99bd5de1..e72dbd7c1 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -1,18 +1,14 @@
 import os
 try:
-    from setuptools import setup
+    from setuptools import setup, find_packages
 except ImportError:
-    from distutils.core import setup
+    from distutils.core import setup, find_packages
 
 
-packages = ['gtsam', 'gtsam_eigency']
-if ${GTSAM_BUILD_UNSTABLE}:
-    packages += 'gtsam_unstable'
-
+packages = find_packages()
 
 setup(
     name='gtsam',
-
     description='Georgia Tech Smoothing And Mapping library',
     url='https://bitbucket.org/gtborg/gtsam',
     version='${GTSAM_VERSION_STRING}',  # https://www.python.org/dev/peps/pep-0440/

From fe3003a688a85cb7cdb80511bffec50e138ec2aa Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 15:38:25 +0000
Subject: [PATCH 177/281] install gtsam_unstable __init__.py

---
 cython/CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index c2d38fd47..3dedbeec2 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -34,5 +34,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/")
   # install scripts and tests
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
+  install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
 
 endif ()

From c1b048020ee99ee209abe1d35fb0a9aaba3d6ec7 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 15:55:49 +0000
Subject: [PATCH 178/281] fixed bug with detecting nested python packages in
 setup.py

---
 cython/setup.py.in | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index e72dbd7c1..0a994b18f 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -28,7 +28,7 @@ setup(
 
     packages=packages,
     package_data={package:
-        [f for f in os.listdir(package) if os.path.splitext(f)[1] == '.so']
+        [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] == '.so']
         for package in packages
     },
     install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines()

From 88dac759d7e675662919ad8fc8524d741ca81ca4 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 15:57:52 +0000
Subject: [PATCH 179/281] setup.py gets installed into correct directory

---
 cython/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index 3dedbeec2..d5e8b8fac 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -31,7 +31,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
   configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
-  install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/")
+  install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}")
   # install scripts and tests
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")

From 0d7b52d99fb47a2aa9131e99567ad0e1b8213a01 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Fri, 8 Feb 2019 16:03:51 +0000
Subject: [PATCH 180/281] copy __init__.py before compiling c++

---
 cython/CMakeLists.txt | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index d5e8b8fac..ca5c0ecfb 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -30,6 +30,8 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
 
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
+  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY)
+  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY)
   configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
   install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}")
   # install scripts and tests

From d56efcceadb6d9258a6441a0484d6ae85c1ca32f Mon Sep 17 00:00:00 2001
From: Matthew Broadway <mattdbway@gmail.com>
Date: Fri, 8 Feb 2019 20:32:05 +0000
Subject: [PATCH 181/281] setup.py only installs for the python version it is
 built for

---
 cython/setup.py.in | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index 0a994b18f..0f133f781 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -22,8 +22,7 @@ setup(
         'Intended Audience :: Developers',
         'Intended Audience :: Science/Research',
         'License :: OSI Approved :: BSD License',
-        'Programming Language :: Python :: 2',
-        'Programming Language :: Python :: 3',
+        'Programming Language :: Python :: ${PYTHON_VERSION_MAJOR}',
     ],
 
     packages=packages,

From fe9ede47d1956390894175ce6f26b4df66d16e3a Mon Sep 17 00:00:00 2001
From: Matthew Broadway <mattdbway@gmail.com>
Date: Fri, 8 Feb 2019 20:32:38 +0000
Subject: [PATCH 182/281] added dlls to package_data

---
 cython/setup.py.in | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index 0f133f781..3161c36e7 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -27,7 +27,7 @@ setup(
 
     packages=packages,
     package_data={package:
-        [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] == '.so']
+        [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')]
         for package in packages
     },
     install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines()

From 135ef5a0d0647e0d9ef14071c5c27a1b88d951bf Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Feb 2019 14:38:46 +0000
Subject: [PATCH 183/281] baking in requirements and README to setup.py rather
 than reading at install time

---
 cython/CMakeLists.txt | 3 +++
 cython/setup.py.in    | 6 ++++--
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index ca5c0ecfb..400e96e13 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -28,6 +28,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
                                     )
   endif()
 
+  file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS)
+  file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
+
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
   configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY)
diff --git a/cython/setup.py.in b/cython/setup.py.in
index 3161c36e7..0e22dbf11 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -14,7 +14,7 @@ setup(
     version='${GTSAM_VERSION_STRING}',  # https://www.python.org/dev/peps/pep-0440/
     license='Simplified BSD license',
     keywords='slam sam',
-    long_description=open('${PROJECT_SOURCE_DIR}/README.md', 'r').read(),
+    long_description='''${README_CONTENTS}''',
     # https://pypi.org/pypi?%3Aaction=list_classifiers
     classifiers=[
         'Development Status :: 5 - Production/Stable',
@@ -30,5 +30,7 @@ setup(
         [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')]
         for package in packages
     },
-    install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines()
+    install_requires=[line.strip() for line in '''
+${CYTHON_INSTALL_REQUIREMENTS}
+'''.splitlines() if len(line) > 0 and not line.strip().startswith('#')]
 )

From 290aad3372fb9372cef8f852d9c27cb57a4154b3 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 20 Feb 2019 09:03:34 +0000
Subject: [PATCH 184/281] small change to setup.py

---
 cython/gtsam_unstable/__init__.py | 1 -
 cython/setup.py.in                | 8 ++++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py
index 04ce7f1e0..3195e6da4 100644
--- a/cython/gtsam_unstable/__init__.py
+++ b/cython/gtsam_unstable/__init__.py
@@ -1,2 +1 @@
 from .gtsam_unstable import *
-
diff --git a/cython/setup.py.in b/cython/setup.py.in
index 0e22dbf11..e631d89c0 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -21,8 +21,12 @@ setup(
         'Intended Audience :: Education',
         'Intended Audience :: Developers',
         'Intended Audience :: Science/Research',
+        'Operating System :: MacOS',
+        'Operating System :: Microsoft :: Windows',
+        'Operating System :: POSIX',
         'License :: OSI Approved :: BSD License',
-        'Programming Language :: Python :: ${PYTHON_VERSION_MAJOR}',
+        'Programming Language :: Python :: 2',
+        'Programming Language :: Python :: 3',
     ],
 
     packages=packages,
@@ -32,5 +36,5 @@ setup(
     },
     install_requires=[line.strip() for line in '''
 ${CYTHON_INSTALL_REQUIREMENTS}
-'''.splitlines() if len(line) > 0 and not line.strip().startswith('#')]
+'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')]
 )

From 803c14deb3058e12aa6779c1f9cb4fe7e1764e7d Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 6 Mar 2019 10:15:07 +0000
Subject: [PATCH 185/281] removed unnecessary variable from cmake

---
 cython/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt
index 400e96e13..a351ec52b 100644
--- a/cython/CMakeLists.txt
+++ b/cython/CMakeLists.txt
@@ -19,7 +19,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   # wrap gtsam_unstable
   if(GTSAM_BUILD_UNSTABLE)
     add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h")
-    set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *")
     wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
                                     "from gtsam.gtsam cimport *"                  # extra imports
                                     "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path

From fcfcceef71bc833bfc9000b487333bf20ad6df26 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Thu, 7 Mar 2019 09:11:16 +0000
Subject: [PATCH 186/281] added gtsam_unstable import back to gtsam

---
 cython/gtsam/__init__.py | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py
index 9bda86efe..724572240 100644
--- a/cython/gtsam/__init__.py
+++ b/cython/gtsam/__init__.py
@@ -1 +1,19 @@
 from .gtsam import *
+
+import gtsam_unstable
+
+
+def deprecated_wrapper(item, name):
+    def wrapper(*args, **kwargs):
+        from warnings import warn
+        warn('importing the unstable item "{}" from gtsam is deprecated. Please import it from gtsam_unstable.'.format(name))
+        return item(*args, **kwargs)
+    return wrapper
+
+for name in dir(gtsam_unstable):
+    if not name.startswith('__'):
+        item = getattr(gtsam_unstable, name)
+        if callable(item):
+            item = deprecated_wrapper(item, name)
+        globals()[name] = item
+

From 7161c0461095b92d407a2a40ce76d295a51d9ec6 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Thu, 7 Mar 2019 10:05:02 +0000
Subject: [PATCH 187/281] fixed __init__.py to not crash if gtsam_unstable
 doesn't exist

---
 cython/gtsam/__init__.py | 33 ++++++++++++++++++++-------------
 1 file changed, 20 insertions(+), 13 deletions(-)

diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py
index 724572240..f27c6fa44 100644
--- a/cython/gtsam/__init__.py
+++ b/cython/gtsam/__init__.py
@@ -1,19 +1,26 @@
 from .gtsam import *
 
-import gtsam_unstable
+try:
+    import gtsam_unstable
 
 
-def deprecated_wrapper(item, name):
-    def wrapper(*args, **kwargs):
-        from warnings import warn
-        warn('importing the unstable item "{}" from gtsam is deprecated. Please import it from gtsam_unstable.'.format(name))
-        return item(*args, **kwargs)
-    return wrapper
+    def _deprecated_wrapper(item, name):
+        def wrapper(*args, **kwargs):
+            from warnings import warn
+            message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
+                       'Please import it from gtsam_unstable.')
+            warn(message, category=DeprecationWarning)
+            return item(*args, **kwargs)
+        return wrapper
 
-for name in dir(gtsam_unstable):
-    if not name.startswith('__'):
-        item = getattr(gtsam_unstable, name)
-        if callable(item):
-            item = deprecated_wrapper(item, name)
-        globals()[name] = item
+
+    for name in dir(gtsam_unstable):
+        if not name.startswith('__'):
+            item = getattr(gtsam_unstable, name)
+            if callable(item):
+                item = _deprecated_wrapper(item, name)
+            globals()[name] = item
+
+except ImportError:
+    pass
 

From 9b80f4b15865346d57a82bef44ccd59e191aaca9 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Thu, 7 Mar 2019 10:11:01 +0000
Subject: [PATCH 188/281] not using DeprecationWarning because it is ignored by
 default

---
 cython/gtsam/__init__.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py
index f27c6fa44..d40ee4502 100644
--- a/cython/gtsam/__init__.py
+++ b/cython/gtsam/__init__.py
@@ -9,7 +9,7 @@ try:
             from warnings import warn
             message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
                        'Please import it from gtsam_unstable.')
-            warn(message, category=DeprecationWarning)
+            warn(message)
             return item(*args, **kwargs)
         return wrapper
 

From 91fa7adf0753de4f8ff92a13c39e83bcfc2978eb Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Mon, 11 Mar 2019 14:54:12 +0000
Subject: [PATCH 189/281] added more keywords

---
 cython/setup.py.in | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index e631d89c0..a4c96f1b8 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -13,7 +13,7 @@ setup(
     url='https://bitbucket.org/gtborg/gtsam',
     version='${GTSAM_VERSION_STRING}',  # https://www.python.org/dev/peps/pep-0440/
     license='Simplified BSD license',
-    keywords='slam sam',
+    keywords='slam sam robotics localization mapping optimization',
     long_description='''${README_CONTENTS}''',
     # https://pypi.org/pypi?%3Aaction=list_classifiers
     classifiers=[

From 03500b004bc5e16e48725a3d3500b0f25bab43dd Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Mon, 11 Mar 2019 15:02:11 +0000
Subject: [PATCH 190/281] enforcing the setup script from being run from the
 installation directory

---
 cython/setup.py.in | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index a4c96f1b8..0c37cd660 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -1,10 +1,19 @@
 import os
+import sys
 try:
     from setuptools import setup, find_packages
 except ImportError:
     from distutils.core import setup, find_packages
 
 
+script_path = os.path.abspath(os.path.realpath(__file__))
+install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py')))
+if script_path != install_path:
+    print('setup.py is being run from an unexpected location: "{script_path}"')
+    print('please run `make install` and run the script from there')
+    sys.exit(1)
+
+
 packages = find_packages()
 
 setup(

From 5a0e7bb92a38cc3d2e6fe91584ac590dab5fb98c Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Mon, 11 Mar 2019 15:05:53 +0000
Subject: [PATCH 191/281] fixed string formatting to work with python 2 and 3

---
 cython/setup.py.in | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index 0c37cd660..b7b0b7bc5 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -9,7 +9,7 @@ except ImportError:
 script_path = os.path.abspath(os.path.realpath(__file__))
 install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py')))
 if script_path != install_path:
-    print('setup.py is being run from an unexpected location: "{script_path}"')
+    print('setup.py is being run from an unexpected location: "{}"'.format(script_path))
     print('please run `make install` and run the script from there')
     sys.exit(1)
 

From 0d924e23dbd2d519dfb3d9b50589caabae20ebe2 Mon Sep 17 00:00:00 2001
From: Fan Jiang <i@fanjiang.me>
Date: Tue, 12 Mar 2019 14:49:18 +0800
Subject: [PATCH 192/281] Fix compilation on MKL 2019

---
 cmake/FindMKL.cmake | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake
index cbe46a908..32e183baa 100644
--- a/cmake/FindMKL.cmake
+++ b/cmake/FindMKL.cmake
@@ -206,6 +206,15 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
                 )
         ENDIF()
 
+        IF(NOT MKL_LAPACK_LIBRARY)
+                FIND_LIBRARY(MKL_LAPACK_LIBRARY
+                  mkl_intel_lp64
+                  PATHS
+                        ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
+                        ${MKL_ROOT_DIR}/lib/
+                )
+        ENDIF()
+
         # iomp5
         IF("${MKL_ARCH_DIR}" STREQUAL "32")
                 IF(UNIX AND NOT APPLE)

From ecb8471e860850f8c8f8a665519d3ea6bd6fa16a Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Tue, 12 Mar 2019 09:47:25 +0000
Subject: [PATCH 193/281] updated cython README

---
 cython/README.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/cython/README.md b/cython/README.md
index 78fc7e594..4e76225c7 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -21,8 +21,9 @@ by default: `<your CMAKE_INSTALL_PREFIX>/cython`
 ```bash
 export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
 ```
-- To install system-wide: navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
+- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
     - (the same command can be used to install into a virtual environment if it is active)
+    - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to install gtsam to a subdirectory of the build directory.
 
 UNIT TESTS
 ==========

From a4b713454a9f955cafd44bace050957734c4f484 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Tue, 12 Mar 2019 09:49:03 +0000
Subject: [PATCH 194/281] updated cython README

---
 cython/README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cython/README.md b/cython/README.md
index 4e76225c7..a524ac04c 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -23,7 +23,7 @@ export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
 ```
 - To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
     - (the same command can be used to install into a virtual environment if it is active)
-    - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to install gtsam to a subdirectory of the build directory.
+    - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory.
 
 UNIT TESTS
 ==========

From e24b402db44850db06730008b73b96471069c3b5 Mon Sep 17 00:00:00 2001
From: chrisbeall <chrisbeall@gmail.com>
Date: Tue, 12 Mar 2019 09:22:09 -0700
Subject: [PATCH 195/281] Turn MKL off by default. Add section on performance
 to INSTALL.md

---
 CMakeLists.txt | 10 ++--------
 INSTALL.md     | 37 +++++++++++++++++++++++++++++++------
 2 files changed, 33 insertions(+), 14 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 834ce732e..d9633b3c3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -61,8 +61,8 @@ option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the defau
 option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
 option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
-option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" ON)
-option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
+option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" OFF)
+option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module" OFF)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions deprecated in GTSAM 4" ON)
@@ -563,12 +563,6 @@ message(STATUS "==============================================================="
 if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
 	message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled.  Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
 endif()
-if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND)
-	message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance.  Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.")
-endif()
-if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
-	message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.")
-endif()
 if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
 	message(WARNING "${GTSAM_PYTHON_WARNINGS}")
 endif()
diff --git a/INSTALL.md b/INSTALL.md
index e19363c7e..23953decf 100644
--- a/INSTALL.md
+++ b/INSTALL.md
@@ -23,18 +23,22 @@ $ make install
        disable the CMake flag GTSAM_WITH_TBB (enabled by default).  On Ubuntu, TBB
        may be installed from the Ubuntu repositories, and for other platforms it
        may be downloaded from https://www.threadingbuildingblocks.org/
+     - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
+       `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
+       achieved with MKL disabled. We therefore advise you to benchmark your problem 
+       before using MKL.
 
     Tested compilers:
 
-    - GCC 4.2-4.7
-    - OSX Clang 2.9-5.0
-    - OSX GCC 4.2
-    - MSVC 2010, 2012
+    - GCC 4.2-7.3
+    - OS X Clang 2.9-10.0
+    - OS X GCC 4.2
+    - MSVC 2010, 2012, 2017
 
     Tested systems:
 
-    - Ubuntu 11.04 - 18.04
-    - MacOS 10.6 - 10.9
+    - Ubuntu 16.04 - 18.04
+    - MacOS 10.6 - 10.14
     - Windows 7, 8, 8.1, 10
 
     Known issues:
@@ -134,6 +138,27 @@ MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included i
 
 $MATLABROOT can be found by executing the command `matlabroot` in MATLAB
 
+## Performance
+
+Here are some tips to get the best possible performance out of GTSAM.
+
+1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode.
+2. Enable TBB. On modern processors with multiple cores, this can easily speed up
+    optimization by 30-50%. Please note that this may not be true for very small 
+    problems where the overhead of dispatching work to multiple threads outweighs
+    the benefit. We recommend that you benchmark your problem with/without TBB.
+3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of
+    25-30% can be expected on modern processors. Note that this affects the portability
+    of your executable. It may not run when copied to another system with older/different
+    processor architecture.
+    Also note that all dependent projects *must* be compiled with the same flag, or
+    seg-faults and other undefined behavior may result.
+4. Enable MKL. Please note that our benchmarks have shown that this helps only
+    in very limited cases, and actually hurts performance in the usual case. We therefore
+    recommend that you do *not* enable MKL, unless you have benchmarked it on
+    your problem and have verified that it improves performance.
+
+
 ## Debugging tips
 
 Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find.

From 40134c3a9ecaf32a8312ef856d383ae92c04af04 Mon Sep 17 00:00:00 2001
From: Chris Beall <cbeall@nuro.ai>
Date: Tue, 12 Mar 2019 11:16:32 -0700
Subject: [PATCH 196/281] Restore warnings about MKL, change notice about
 performance.

---
 CMakeLists.txt | 6 ++++++
 INSTALL.md     | 2 +-
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d9633b3c3..186dabaf4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -563,6 +563,12 @@ message(STATUS "==============================================================="
 if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
 	message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled.  Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
 endif()
+if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND)
+	message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled.  Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.  See INSTALL.md for notes on performance.")
+endif()
+if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
+	message(WARNING "Your compiler does not support OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
+endif()
 if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
 	message(WARNING "${GTSAM_PYTHON_WARNINGS}")
 endif()
diff --git a/INSTALL.md b/INSTALL.md
index 23953decf..3437d074d 100644
--- a/INSTALL.md
+++ b/INSTALL.md
@@ -153,7 +153,7 @@ Here are some tips to get the best possible performance out of GTSAM.
     processor architecture.
     Also note that all dependent projects *must* be compiled with the same flag, or
     seg-faults and other undefined behavior may result.
-4. Enable MKL. Please note that our benchmarks have shown that this helps only
+4. Possibly enable MKL. Please note that our benchmarks have shown that this helps only
     in very limited cases, and actually hurts performance in the usual case. We therefore
     recommend that you do *not* enable MKL, unless you have benchmarked it on
     your problem and have verified that it improves performance.

From fe647a9e941bfad17a6a3d22983866fb91e78f39 Mon Sep 17 00:00:00 2001
From: Jeremy Aguilon <jeraguilon@gmail.com>
Date: Wed, 13 Mar 2019 01:32:49 -0400
Subject: [PATCH 197/281] Remove unneeeded file in build chain

---
 cython/gtsam_unstable/__init__.py.in | 1 -
 1 file changed, 1 deletion(-)
 delete mode 100644 cython/gtsam_unstable/__init__.py.in

diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in
deleted file mode 100644
index 85f7c3df2..000000000
--- a/cython/gtsam_unstable/__init__.py.in
+++ /dev/null
@@ -1 +0,0 @@
-${GTSAM_UNSTABLE_IMPORT}

From 9982d79d74c20704cc987cd4ca63cb0934dfb8b1 Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Mar 2019 09:02:04 +0000
Subject: [PATCH 198/281] added reasoning behind the setup.py unexpected
 location check

---
 cython/README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/cython/README.md b/cython/README.md
index a524ac04c..b9ce2f012 100644
--- a/cython/README.md
+++ b/cython/README.md
@@ -24,6 +24,8 @@ export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
 - To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
     - (the same command can be used to install into a virtual environment if it is active)
     - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory.
+    - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`.
+      Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package.
 
 UNIT TESTS
 ==========

From 173191621eee0c3ae0fde428e547335f540fe3ac Mon Sep 17 00:00:00 2001
From: Matthew Broadway <matthew.broadway@navenio.com>
Date: Wed, 13 Mar 2019 09:45:56 +0000
Subject: [PATCH 199/281] made it possible to disable the setup.py check

---
 cython/setup.py.in | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/cython/setup.py.in b/cython/setup.py.in
index b7b0b7bc5..d6af28762 100644
--- a/cython/setup.py.in
+++ b/cython/setup.py.in
@@ -5,13 +5,13 @@ try:
 except ImportError:
     from distutils.core import setup, find_packages
 
-
-script_path = os.path.abspath(os.path.realpath(__file__))
-install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py')))
-if script_path != install_path:
-    print('setup.py is being run from an unexpected location: "{}"'.format(script_path))
-    print('please run `make install` and run the script from there')
-    sys.exit(1)
+if 'SETUP_PY_NO_CHECK' not in os.environ:
+    script_path = os.path.abspath(os.path.realpath(__file__))
+    install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py')))
+    if script_path != install_path:
+        print('setup.py is being run from an unexpected location: "{}"'.format(script_path))
+        print('please run `make install` and run the script from there')
+        sys.exit(1)
 
 
 packages = find_packages()

From 724a906bee985b3c7b3141fce81dd72141163094 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 13 Mar 2019 22:47:23 -0400
Subject: [PATCH 200/281] Test existing readG2o

---
 cython/gtsam/tests/test_dataset.py | 33 ++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)
 create mode 100644 cython/gtsam/tests/test_dataset.py

diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py
new file mode 100644
index 000000000..c634fb3f7
--- /dev/null
+++ b/cython/gtsam/tests/test_dataset.py
@@ -0,0 +1,33 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for testing dataset access.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+
+
+class TestDataset(unittest.TestCase):
+    def setUp(self):
+        pass
+
+    def test_3d_graph(self):
+        is3D = True
+        g2o_file = gtsam.findExampleDataFile("pose3example.txt")
+        graph, initial = gtsam.readG2o(g2o_file, is3D)
+        self.assertEqual(graph.size(), 6)
+        self.assertEqual(initial.size(), 5)
+
+
+if __name__ == '__main__':
+    unittest.main()

From 0ca3f9d199e8900f119248a64a71694af27bcf17 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 13 Mar 2019 23:22:37 -0400
Subject: [PATCH 201/281] Use c+11 initializer lists

---
 gtsam/slam/tests/testDataset.cpp | 91 +++++++++++++++-----------------
 1 file changed, 44 insertions(+), 47 deletions(-)

diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index 39c2d3722..4a327b8e1 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -162,65 +162,62 @@ TEST( dataSet, readG2o)
 }
 
 /* ************************************************************************* */
-TEST( dataSet, readG2o3D)
-{
+TEST(dataSet, readG2o3D) {
   const string g2oFile = findExampleDataFile("pose3example");
   NonlinearFactorGraph::shared_ptr actualGraph;
   Values::shared_ptr actualValues;
   bool is3D = true;
   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
 
-  Values expectedValues;
-  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
-  Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
-  expectedValues.insert(0, Pose3(R0, p0));
+  // Initialize 5 poses with quaternion/point3 values:
+  const std::vector<Pose3> poses = {
+      {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.421446, -0.351729, -0.597838, 0.584174},
+       {1.993500, 0.023275, 0.003793}},
+      {{0.067024, 0.331798, -0.200659, 0.919323},
+       {2.004291, 1.024305, 0.018047}},
+      {{0.765488, -0.035697, -0.462490, 0.445933},
+       {0.999908, 1.055073, 0.020212}},
+  };
 
-  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
-  expectedValues.insert(1, Pose3(R1, p1));
+  // Check values
+  for (size_t i : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(poses[i], actualValues->at<Pose3>(i), 1e-5));
+  }
 
-  Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
-  Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
-  expectedValues.insert(2, Pose3(R2, p2));
+  // Initialize 6 relative measurements with quaternion/point3 values:
+  const std::vector<Pose3> measurements = {
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.105373, 0.311512, 0.656877, -0.678505},
+       {0.523923, 0.776654, 0.326659}},
+      {{0.568551, 0.595795, -0.561677, 0.079353},
+       {0.910927, 0.055169, -0.411761}},
+      {{0.542221, -0.592077, 0.303380, -0.513226},
+       {0.775288, 0.228798, -0.596923}},
+      {{0.327419, -0.125250, -0.534379, 0.769122},
+       {-0.577841, 0.628016, -0.543592}},
+      {{0.083672, 0.104639, 0.627755, 0.766795},
+       {-0.623267, 0.086928, 0.773222}},
+  };
 
-  Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323);
-  Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
-  expectedValues.insert(3, Pose3(R3, p3));
+  // Specify connectivity:
+  std::vector<pair<Key, Key>> pairs = {{0, 1}, {1, 2}, {2, 3},
+                                       {3, 4}, {1, 4}, {3, 0}};
 
-  Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933);
-  Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
-  expectedValues.insert(4, Pose3(R4, p4));
-
-  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
-
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished());
+  // Create expected factor graph
+  auto model = noiseModel::Isotropic::Precision(6, 10000);
   NonlinearFactorGraph expectedGraph;
+  size_t i = 0;
+  for (const auto& keys : pairs) {
+    expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
+        keys.first, keys.second, measurements[i++], model);
+  }
 
-  Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
-  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
-
-  Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
-  Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, Pose3(R12,p12), model);
-
-  Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
-  Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, Pose3(R23,p23), model);
-
-  Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
-  Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, Pose3(R34,p34), model);
-
-  Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
-  Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 4, Pose3(R14,p14), model);
-
-  Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
-  Rot3 R30 = Rot3::Quaternion(0.083672 ,  0.104639, 0.627755, 0.766795  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 0, Pose3(R30,p30), model);
-
-  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
+  // Check if actual graph is correct
+  EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
 }
 
 /* ************************************************************************* */

From d8ee79fb8fd747bf2d497bacb9c808cb8d1b5fb6 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 14 Mar 2019 00:27:02 -0400
Subject: [PATCH 202/281] Working parseG2o3D

---
 gtsam/slam/tests/testDataset.cpp | 167 +++++++++++++++++++++++++------
 1 file changed, 135 insertions(+), 32 deletions(-)

diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index 4a327b8e1..3e2ed51c7 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -162,33 +162,96 @@ TEST( dataSet, readG2o)
 }
 
 /* ************************************************************************* */
+using BetweenFactors = std::vector<BetweenFactor<Pose3>::shared_ptr>;
+using Poses = std::map<Key, Pose3>;
+std::pair<BetweenFactors, Poses> parseG2o3D(const std::string&);
+
+#include <fstream>
+#define LINESIZE 81920
+
+/* ************************************************************************* */
+std::pair<BetweenFactors, Poses> parseG2o3D(const string& filename) {
+  ifstream is(filename.c_str());
+  if (!is) throw invalid_argument("load3D: can not find file " + filename);
+
+  BetweenFactors factors;
+  Poses poses;
+
+  while (!is.eof()) {
+    char buf[LINESIZE];
+    is.getline(buf, LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+
+    if (tag == "VERTEX3") {
+      Key id;
+      double x, y, z, roll, pitch, yaw;
+      ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
+      poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
+    }
+    if (tag == "VERTEX_SE3:QUAT") {
+      Key id;
+      double x, y, z, qx, qy, qz, qw;
+      ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
+      poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
+    }
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  is.seekg(0, ios::beg);
+
+  while (!is.eof()) {
+    char buf[LINESIZE];
+    is.getline(buf, LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+
+    if (tag == "EDGE3") {
+      Key id1, id2;
+      double x, y, z, roll, pitch, yaw;
+      ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
+      Matrix m(6, 6);
+      for (size_t i = 0; i < 6; i++)
+        for (size_t j = i; j < 6; j++) ls >> m(i, j);
+      SharedNoiseModel model = noiseModel::Gaussian::Information(m);
+      factors.emplace_back(new BetweenFactor<Pose3>(
+          id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
+    }
+    if (tag == "EDGE_SE3:QUAT") {
+      Key id1, id2;
+      double x, y, z, qx, qy, qz, qw;
+      ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
+      Matrix m(6, 6);
+      for (size_t i = 0; i < 6; i++) {
+        for (size_t j = i; j < 6; j++) {
+          double mij;
+          ls >> mij;
+          m(i, j) = mij;
+          m(j, i) = mij;
+        }
+      }
+      Matrix mgtsam(6, 6);
+
+      mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
+      mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
+      mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
+      mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
+
+      SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
+      factors.emplace_back(new BetweenFactor<Pose3>(
+          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
+    }
+  }
+  return make_pair(factors, poses);
+}
+
 TEST(dataSet, readG2o3D) {
   const string g2oFile = findExampleDataFile("pose3example");
-  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values::shared_ptr actualValues;
-  bool is3D = true;
-  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
-
-  // Initialize 5 poses with quaternion/point3 values:
-  const std::vector<Pose3> poses = {
-      {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
-      {{0.854230, 0.190253, 0.283162, -0.392318},
-       {1.001367, 0.015390, 0.004948}},
-      {{0.421446, -0.351729, -0.597838, 0.584174},
-       {1.993500, 0.023275, 0.003793}},
-      {{0.067024, 0.331798, -0.200659, 0.919323},
-       {2.004291, 1.024305, 0.018047}},
-      {{0.765488, -0.035697, -0.462490, 0.445933},
-       {0.999908, 1.055073, 0.020212}},
-  };
-
-  // Check values
-  for (size_t i : {0, 1, 2, 3, 4}) {
-    EXPECT(assert_equal(poses[i], actualValues->at<Pose3>(i), 1e-5));
-  }
+  auto model = noiseModel::Isotropic::Precision(6, 10000);
 
   // Initialize 6 relative measurements with quaternion/point3 values:
-  const std::vector<Pose3> measurements = {
+  const std::vector<Pose3> relative_poses = {
       {{0.854230, 0.190253, 0.283162, -0.392318},
        {1.001367, 0.015390, 0.004948}},
       {{0.105373, 0.311512, 0.656877, -0.678505},
@@ -203,21 +266,61 @@ TEST(dataSet, readG2o3D) {
        {-0.623267, 0.086928, 0.773222}},
   };
 
-  // Specify connectivity:
-  std::vector<pair<Key, Key>> pairs = {{0, 1}, {1, 2}, {2, 3},
-                                       {3, 4}, {1, 4}, {3, 0}};
+  // Initialize 5 poses with quaternion/point3 values:
+  const std::vector<Pose3> poses = {
+      {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.421446, -0.351729, -0.597838, 0.584174},
+       {1.993500, 0.023275, 0.003793}},
+      {{0.067024, 0.331798, -0.200659, 0.919323},
+       {2.004291, 1.024305, 0.018047}},
+      {{0.765488, -0.035697, -0.462490, 0.445933},
+       {0.999908, 1.055073, 0.020212}},
+  };
 
-  // Create expected factor graph
-  auto model = noiseModel::Isotropic::Precision(6, 10000);
-  NonlinearFactorGraph expectedGraph;
+  // Specify connectivity:
+  using KeyPair = pair<Key, Key>;
+  std::vector<KeyPair> edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
+
+  // Created expected factor graph
   size_t i = 0;
-  for (const auto& keys : pairs) {
+  NonlinearFactorGraph expectedGraph;
+  for (const auto& keys : edges) {
     expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
-        keys.first, keys.second, measurements[i++], model);
+        keys.first, keys.second, relative_poses[i++], model);
   }
 
-  // Check if actual graph is correct
+  // Call parse version
+  BetweenFactors actualFactors;
+  Poses actualPoses;
+  std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile);
+
+  // Check factors
+  for (size_t i : {0, 1, 2, 3, 4, 5}) {
+    EXPECT(assert_equal(
+        *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
+        *actualFactors[i], 1e-5));
+  }
+
+  // Check poses
+  for (size_t j : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
+  }
+
+  // Call graph version
+  NonlinearFactorGraph::shared_ptr actualGraph;
+  Values::shared_ptr actualValues;
+  bool is3D = true;
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
+
+  // Check factor graph
   EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
+
+  // Check values
+  for (size_t j : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
+  }
 }
 
 /* ************************************************************************* */

From a47c52cb5ee858fec44b4fde82734aff95e47f64 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 14 Mar 2019 00:58:22 -0400
Subject: [PATCH 203/281] Split parsing and moved to dataset.*

---
 gtsam/slam/dataset.cpp           | 100 ++++++++++++++++--------------
 gtsam/slam/dataset.h             |  10 ++-
 gtsam/slam/tests/testDataset.cpp | 101 ++-----------------------------
 3 files changed, 67 insertions(+), 144 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 34370d4e2..b7d1b04d6 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
 
 /* ************************************************************************* */
 GraphAndValues readG2o(const string& g2oFile, const bool is3D,
-    KernelFunctionType kernelFunctionType) {
-  // just call load2D
-  int maxID = 0;
-  bool addNoise = false;
-  bool smart = true;
-
-  if(is3D)
+                       KernelFunctionType kernelFunctionType) {
+  if (is3D) {
     return load3D(g2oFile);
-
-  return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
-      NoiseFormatG2O, kernelFunctionType);
+  } else {
+    // just call load2D
+    int maxID = 0;
+    bool addNoise = false;
+    bool smart = true;
+    return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+                  NoiseFormatG2O, kernelFunctionType);
+  }
 }
 
 /* ************************************************************************* */
@@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
 }
 
 /* ************************************************************************* */
-GraphAndValues load3D(const string& filename) {
-
+std::map<Key, Pose3> parse3DPoses(const string& filename) {
   ifstream is(filename.c_str());
   if (!is)
-    throw invalid_argument("load3D: can not find file " + filename);
-
-  Values::shared_ptr initial(new Values);
-  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
+    throw invalid_argument("parse3DPoses: can not find file " + filename);
 
+  std::map<Key, Pose3> poses;
   while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
@@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) {
       Key id;
       double x, y, z, roll, pitch, yaw;
       ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
+      poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
     }
     if (tag == "VERTEX_SE3:QUAT") {
       Key id;
       double x, y, z, qx, qy, qz, qw;
       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
+      poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
     }
   }
-  is.clear(); /* clears the end-of-file and error flags */
-  is.seekg(0, ios::beg);
+  return poses;
+}
 
+/* ************************************************************************* */
+std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const string& filename) {
+  ifstream is(filename.c_str());
+  if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
+
+  std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
@@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) {
       Key id1, id2;
       double x, y, z, roll, pitch, yaw;
       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
-      Point3 t = Point3(x, y, z);
-      Matrix m = I_6x6;
-      for (int i = 0; i < 6; i++)
-        for (int j = i; j < 6; j++)
-          ls >> m(i, j);
+      Matrix m(6, 6);
+      for (size_t i = 0; i < 6; i++)
+        for (size_t j = i; j < 6; j++) ls >> m(i, j);
       SharedNoiseModel model = noiseModel::Gaussian::Information(m);
-      NonlinearFactor::shared_ptr factor(
-          new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
-      graph->push_back(factor);
+      factors.emplace_back(new BetweenFactor<Pose3>(
+          id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
     }
     if (tag == "EDGE_SE3:QUAT") {
-      Matrix m = I_6x6;
       Key id1, id2;
       double x, y, z, qx, qy, qz, qw;
       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
-      Point3 t = Point3(x, y, z);
-      for (int i = 0; i < 6; i++){
-        for (int j = i; j < 6; j++){
+      Matrix m(6, 6);
+      for (size_t i = 0; i < 6; i++) {
+        for (size_t j = i; j < 6; j++) {
           double mij;
           ls >> mij;
           m(i, j) = mij;
           m(j, i) = mij;
         }
       }
-      Matrix mgtsam = I_6x6;
+      Matrix mgtsam(6, 6);
 
-      mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation
-      mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation
-      mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
-      mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
+      mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
+      mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
+      mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
+      mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
-      NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
-      graph->push_back(factor);
+      factors.emplace_back(new BetweenFactor<Pose3>(
+          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
     }
   }
+  return factors;
+}
+
+/* ************************************************************************* */
+GraphAndValues load3D(const string& filename) {
+  const auto factors = parse3DFactors(filename);
+  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
+  for (const auto& factor : factors) {
+    graph->push_back(factor);
+  }
+
+  const auto poses = parse3DPoses(filename);
+  Values::shared_ptr initial(new Values);
+  for (const auto& key_pose : poses) {
+    initial->insert(key_pose.first, key_pose.second);
+  }
+
   return make_pair(graph, initial);
 }
 
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 7c9b54a6f..9706bace0 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -153,9 +153,13 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
 GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
     const Values& estimate, const std::string& filename);
 
-/**
- * Load TORO 3D Graph
- */
+/// Parse edges in 3D TORO graph file into a set of BetweenFactors.
+GTSAM_EXPORT std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const std::string& filename);
+
+/// Parse vertices in 3D TORO graph file into a map of Pose3s.
+GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
+
+/// Load TORO 3D Graph
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
 /// A measurement with its camera index
diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index 3e2ed51c7..0e131af32 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -162,90 +162,6 @@ TEST( dataSet, readG2o)
 }
 
 /* ************************************************************************* */
-using BetweenFactors = std::vector<BetweenFactor<Pose3>::shared_ptr>;
-using Poses = std::map<Key, Pose3>;
-std::pair<BetweenFactors, Poses> parseG2o3D(const std::string&);
-
-#include <fstream>
-#define LINESIZE 81920
-
-/* ************************************************************************* */
-std::pair<BetweenFactors, Poses> parseG2o3D(const string& filename) {
-  ifstream is(filename.c_str());
-  if (!is) throw invalid_argument("load3D: can not find file " + filename);
-
-  BetweenFactors factors;
-  Poses poses;
-
-  while (!is.eof()) {
-    char buf[LINESIZE];
-    is.getline(buf, LINESIZE);
-    istringstream ls(buf);
-    string tag;
-    ls >> tag;
-
-    if (tag == "VERTEX3") {
-      Key id;
-      double x, y, z, roll, pitch, yaw;
-      ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
-      poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
-    }
-    if (tag == "VERTEX_SE3:QUAT") {
-      Key id;
-      double x, y, z, qx, qy, qz, qw;
-      ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
-      poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
-    }
-  }
-  is.clear(); /* clears the end-of-file and error flags */
-  is.seekg(0, ios::beg);
-
-  while (!is.eof()) {
-    char buf[LINESIZE];
-    is.getline(buf, LINESIZE);
-    istringstream ls(buf);
-    string tag;
-    ls >> tag;
-
-    if (tag == "EDGE3") {
-      Key id1, id2;
-      double x, y, z, roll, pitch, yaw;
-      ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
-      Matrix m(6, 6);
-      for (size_t i = 0; i < 6; i++)
-        for (size_t j = i; j < 6; j++) ls >> m(i, j);
-      SharedNoiseModel model = noiseModel::Gaussian::Information(m);
-      factors.emplace_back(new BetweenFactor<Pose3>(
-          id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
-    }
-    if (tag == "EDGE_SE3:QUAT") {
-      Key id1, id2;
-      double x, y, z, qx, qy, qz, qw;
-      ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Matrix m(6, 6);
-      for (size_t i = 0; i < 6; i++) {
-        for (size_t j = i; j < 6; j++) {
-          double mij;
-          ls >> mij;
-          m(i, j) = mij;
-          m(j, i) = mij;
-        }
-      }
-      Matrix mgtsam(6, 6);
-
-      mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
-      mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
-      mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
-      mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
-
-      SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
-      factors.emplace_back(new BetweenFactor<Pose3>(
-          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
-    }
-  }
-  return make_pair(factors, poses);
-}
-
 TEST(dataSet, readG2o3D) {
   const string g2oFile = findExampleDataFile("pose3example");
   auto model = noiseModel::Isotropic::Precision(6, 10000);
@@ -291,33 +207,26 @@ TEST(dataSet, readG2o3D) {
         keys.first, keys.second, relative_poses[i++], model);
   }
 
-  // Call parse version
-  BetweenFactors actualFactors;
-  Poses actualPoses;
-  std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile);
-
-  // Check factors
+  // Check factor parsing
+  const auto actualFactors = parse3DFactors(g2oFile);
   for (size_t i : {0, 1, 2, 3, 4, 5}) {
     EXPECT(assert_equal(
         *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
         *actualFactors[i], 1e-5));
   }
 
-  // Check poses
+  // Check pose parsing
+  const auto actualPoses = parse3DPoses(g2oFile);
   for (size_t j : {0, 1, 2, 3, 4}) {
     EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
   }
 
-  // Call graph version
+  // Check graph version
   NonlinearFactorGraph::shared_ptr actualGraph;
   Values::shared_ptr actualValues;
   bool is3D = true;
   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
-
-  // Check factor graph
   EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
-
-  // Check values
   for (size_t j : {0, 1, 2, 3, 4}) {
     EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
   }

From 88ac6de4afb1cef6ac6837f1bd33280c9f0cfb5d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 14 Mar 2019 01:25:06 -0400
Subject: [PATCH 204/281] Wrapped parse3DFactors

---
 cython/gtsam/tests/test_dataset.py | 23 +++++++++++++++++------
 gtsam.h                            | 10 ++++++++++
 gtsam/slam/dataset.cpp             |  2 +-
 gtsam/slam/dataset.h               |  5 ++++-
 gtsam/slam/tests/testDataset.cpp   |  2 +-
 5 files changed, 33 insertions(+), 9 deletions(-)

diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py
index c634fb3f7..e561be1a8 100644
--- a/cython/gtsam/tests/test_dataset.py
+++ b/cython/gtsam/tests/test_dataset.py
@@ -8,26 +8,37 @@ See LICENSE for the license information
 Unit tests for testing dataset access.
 Author: Frank Dellaert
 """
-# pylint: disable=invalid-name, E1101
+# pylint: disable=invalid-name, no-name-in-module, no-member
 
 from __future__ import print_function
 
 import unittest
 
 import gtsam
+from gtsam import BetweenFactorPose3, BetweenFactorPose3s
 
 
 class TestDataset(unittest.TestCase):
-    def setUp(self):
-        pass
+    """Tests for datasets.h wrapper."""
 
-    def test_3d_graph(self):
+    def setUp(self):
+        """Get some common paths."""
+        self.pose3_example_g2o_file = gtsam.findExampleDataFile(
+            "pose3example.txt")
+
+    def test_readG2o3D(self):
+        """Test reading directly into factor graph."""
         is3D = True
-        g2o_file = gtsam.findExampleDataFile("pose3example.txt")
-        graph, initial = gtsam.readG2o(g2o_file, is3D)
+        graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
         self.assertEqual(graph.size(), 6)
         self.assertEqual(initial.size(), 5)
 
+    def test_parse3Dfactors(self):
+        """Test parsing into data structure."""
+        factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
+        self.assertEqual(factors.size(), 6)
+        self.assertIsInstance(factors.at(0), BetweenFactorPose3)
+
 
 if __name__ == '__main__':
     unittest.main()
diff --git a/gtsam.h b/gtsam.h
index 48768db40..255a7a449 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2480,6 +2480,16 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
     string filename);
 
+// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+class BetweenFactorPose3s
+{
+  size_t size() const;
+  gtsam::BetweenFactorPose3* at(size_t i) const;
+};
+
+gtsam::BetweenFactorPose3s parse3DFactors(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
+
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index b7d1b04d6..c32a049e2 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -540,7 +540,7 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
 }
 
 /* ************************************************************************* */
-std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const string& filename) {
+BetweenFactorPose3s parse3DFactors(const string& filename) {
   ifstream is(filename.c_str());
   if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
 
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 9706bace0..2106df48d 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
@@ -34,6 +35,7 @@
 #include <utility> // for pair
 #include <vector>
 #include <iosfwd>
+#include <map>
 
 namespace gtsam {
 
@@ -154,7 +156,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
     const Values& estimate, const std::string& filename);
 
 /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
-GTSAM_EXPORT std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const std::string& filename);
+using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
+GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename);
 
 /// Parse vertices in 3D TORO graph file into a map of Pose3s.
 GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index 0e131af32..9434280d4 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -16,8 +16,8 @@
  */
 
 
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/TestableAssertions.h>

From cbb84a64360cce71ced04ba0ad5f8224ef746c53 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 14 Mar 2019 14:14:20 +0000
Subject: [PATCH 205/281] Added information about LieGroup helper class

---
 GTSAM-Concepts.md | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md
index 366a58a09..a6cfee984 100644
--- a/GTSAM-Concepts.md
+++ b/GTSAM-Concepts.md
@@ -97,12 +97,24 @@ Note that in the Lie group case, the usual valid expressions for Retract and Loc
 
 For Lie groups, the `exponential map` above is the most obvious mapping: it 
 associates straight lines in the tangent space with geodesics on the manifold 
-(and it's inverse, the log map). However, there are two cases in which we deviate from this:
+(and it's inverse, the log map). However, there are several cases in which we deviate from this:
 
 However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`)
 
 Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method.
 
+A CRTP helper class `LieGroup` is available that can take a class and create some of the Lie group methods automatically. The class needs:
+
+* operator* : implements group operator
+* inverse: implements group inverse
+* AdjointMap: maps tangent vectors according to group element
+* Expmap/Logmap: exponential map and its inverse
+* ChartAtOrigin: struct where you define Retract/Local at origin
+
+To use, simply derive, but also say `using LieGroup<Class,N>::inverse` so you get an inverse with a derivative.
+
+Finally, to create the traits automatically you can use `internal::LieGroupTraits<Class>`
+
 Vector Space
 ------------
 

From dd7fa966e4093d166c4301b64c1bb9cf6a0cb9c7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Fri, 15 Mar 2019 00:25:52 -0400
Subject: [PATCH 206/281] Added print in base class so all derived have it.
 Added comment how wrap currently does not handle Base class correctly in case
 of name clash, apparently.

---
 gtsam.h | 17 ++++++++---------
 1 file changed, 8 insertions(+), 9 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 48768db40..ed4bf8425 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1218,6 +1218,13 @@ class VariableIndex {
 namespace noiseModel {
 #include <gtsam/linear/NoiseModel.h>
 virtual class Base {
+  void print(string s) const;
+  // Methods below are available for all noise models. However, can't add them 
+  // because wrap (incorrectly) thinks robust classes derive from this Base as well.
+  // bool isConstrained() const;
+  // bool isUnit() const;
+  // size_t dim() const;
+  // Vector sigmas() const;
 };
 
 virtual class Gaussian : gtsam::noiseModel::Base {
@@ -1225,7 +1232,6 @@ virtual class Gaussian : gtsam::noiseModel::Base {
   static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
   Matrix R() const;
   bool equals(gtsam::noiseModel::Base& expected, double tol);
-  void print(string s) const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -1236,7 +1242,6 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
   static gtsam::noiseModel::Diagonal* Variances(Vector variances);
   static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
   Matrix R() const;
-  void print(string s) const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -1263,7 +1268,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
   static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
   static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
   static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
-  void print(string s) const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -1271,7 +1275,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
 
 virtual class Unit : gtsam::noiseModel::Isotropic {
   static gtsam::noiseModel::Unit* Create(size_t dim);
-  void print(string s) const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -1279,11 +1282,11 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
 
 namespace mEstimator {
 virtual class Base {
+  void print(string s) const;
 };
 
 virtual class Null: gtsam::noiseModel::mEstimator::Base {
   Null();
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Null* Create();
 
   // enabling serialization functionality
@@ -1292,7 +1295,6 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
   Fair(double c);
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Fair* Create(double c);
 
   // enabling serialization functionality
@@ -1301,7 +1303,6 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {
   Huber(double k);
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Huber* Create(double k);
 
   // enabling serialization functionality
@@ -1310,7 +1311,6 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   Tukey(double k);
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
 
   // enabling serialization functionality
@@ -1322,7 +1322,6 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
 virtual class Robust : gtsam::noiseModel::Base {
   Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
   static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
-  void print(string s) const;
 
   // enabling serialization functionality
   void serializable() const;

From a7e60a08fe5a5c6345eb8db9206fe0a24b40b231 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Mon, 18 Mar 2019 23:15:53 -0400
Subject: [PATCH 207/281] Wrapped more useful noiseModel methods

---
 gtsam.h | 20 +++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

diff --git a/gtsam.h b/gtsam.h
index ed4bf8425..fb72660d5 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1230,9 +1230,19 @@ virtual class Base {
 virtual class Gaussian : gtsam::noiseModel::Base {
   static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
   static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
-  Matrix R() const;
+
   bool equals(gtsam::noiseModel::Base& expected, double tol);
 
+  // access to noise model
+  Matrix R() const;
+  Matrix information() const;
+  Matrix covariance() const;
+
+  // Whitening operations
+  Vector whiten(Vector v) const;
+  Vector unwhiten(Vector v) const;
+  Matrix Whiten(Matrix H) const;
+
   // enabling serialization functionality
   void serializable() const;
 };
@@ -1243,6 +1253,11 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
   static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
   Matrix R() const;
 
+  // access to noise model
+  Vector sigmas() const;
+  Vector invsigmas() const;
+  Vector precisions() const;
+    
   // enabling serialization functionality
   void serializable() const;
 };
@@ -1269,6 +1284,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
   static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
   static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
 
+  // access to noise model
+  double sigma() const;
+
   // enabling serialization functionality
   void serializable() const;
 };

From 205803a0eabd2c0b1fd4098bb37ee0e6b1b71a30 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 19 Mar 2019 00:11:45 -0400
Subject: [PATCH 208/281] Better optimization parameter wrapping, plus python
 test

---
 cython/gtsam/tests/test_NonlinearOptimizer.py | 71 +++++++++++++++++++
 gtsam.h                                       | 29 ++++++--
 gtsam/nonlinear/LevenbergMarquardtParams.h    |  6 +-
 gtsam/nonlinear/NonlinearOptimizerParams.h    | 47 +++---------
 4 files changed, 109 insertions(+), 44 deletions(-)
 create mode 100644 cython/gtsam/tests/test_NonlinearOptimizer.py

diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py
new file mode 100644
index 000000000..0d0318f40
--- /dev/null
+++ b/cython/gtsam/tests/test_NonlinearOptimizer.py
@@ -0,0 +1,71 @@
+"""
+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, no-name-in-module
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
+                   GaussNewtonParams, LevenbergMarquardtOptimizer,
+                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   Point2, PriorFactorPoint2, Values)
+
+KEY1 = 1
+KEY2 = 2
+
+
+class TestScenario(unittest.TestCase):
+    def test_optimize(self):
+        """Do trivial test with three optimizer variants."""
+        fg = NonlinearFactorGraph()
+        model = gtsam.noiseModel_Unit.Create(2)
+        fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
+
+        # test error at minimum
+        xstar = Point2(0, 0)
+        optimal_values = Values()
+        optimal_values.insert(KEY1, xstar)
+        self.assertEqual(0.0, fg.error(optimal_values), 0.0)
+
+        # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
+        x0 = Point2(3, 3)
+        initial_values = Values()
+        initial_values.insert(KEY1, x0)
+        self.assertEqual(9.0, fg.error(initial_values), 1e-3)
+
+        # optimize parameters
+        ordering = Ordering()
+        ordering.push_back(KEY1)
+
+        # Gauss-Newton
+        gnParams = GaussNewtonParams()
+        gnParams.setOrdering(ordering)
+        actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual1))
+
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setOrdering(ordering)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
+        # Dogleg
+        dlParams = DoglegParams()
+        dlParams.setOrdering(ordering)
+        actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual3))
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/gtsam.h b/gtsam.h
index ed4bf8425..50d59f973 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2001,10 +2001,12 @@ virtual class NonlinearOptimizerParams {
   void setVerbosity(string s);
 
   string getLinearSolverType() const;
-
   void setLinearSolverType(string solver);
-  void setOrdering(const gtsam::Ordering& ordering);
+
   void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
+  void setOrdering(const gtsam::Ordering& ordering);
+  string getOrderingType() const;
+  void setOrderingType(string ordering);
 
   bool isMultifrontal() const;
   bool isSequential() const;
@@ -2025,15 +2027,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
 virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
   LevenbergMarquardtParams();
 
-  double getlambdaInitial() const;
+  bool getDiagonalDamping() const;
   double getlambdaFactor() const;
+  double getlambdaInitial() const;
+  double getlambdaLowerBound() const;
   double getlambdaUpperBound() const;
+  bool getUseFixedLambdaFactor();
+  string getLogFile() const;
   string getVerbosityLM() const;
-
-  void setlambdaInitial(double value);
+  
+  void setDiagonalDamping(bool flag);
   void setlambdaFactor(double value);
+  void setlambdaInitial(double value);
+  void setlambdaLowerBound(double value);
   void setlambdaUpperBound(double value);
+  void setUseFixedLambdaFactor(bool flag);
+  void setLogFile(string s);
   void setVerbosityLM(string s);
+
+  static gtsam::LevenbergMarquardtParams LegacyDefaults();
+  static gtsam::LevenbergMarquardtParams CeresDefaults();
+
+  static gtsam::LevenbergMarquardtParams EnsureHasOrdering(
+      gtsam::LevenbergMarquardtParams params,
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::LevenbergMarquardtParams ReplaceOrdering(
+      gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering);
 };
 
 #include <gtsam/nonlinear/DoglegOptimizer.h>
diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h
index abb8c3c22..491fbd233 100644
--- a/gtsam/nonlinear/LevenbergMarquardtParams.h
+++ b/gtsam/nonlinear/LevenbergMarquardtParams.h
@@ -122,22 +122,24 @@ public:
   virtual ~LevenbergMarquardtParams() {}
   void print(const std::string& str = "") const override;
 
-  /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++.
+  /// @name Getters/Setters, mainly for wrappers. Use fields above in C++.
   /// @{
   bool getDiagonalDamping() const { return diagonalDamping; }
   double getlambdaFactor() const { return lambdaFactor; }
   double getlambdaInitial() const { return lambdaInitial; }
   double getlambdaLowerBound() const { return lambdaLowerBound; }
   double getlambdaUpperBound() const { return lambdaUpperBound; }
+  bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; }
   std::string getLogFile() const { return logFile; }
   std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);}
+  
   void setDiagonalDamping(bool flag) { diagonalDamping = flag; }
   void setlambdaFactor(double value) { lambdaFactor = value; }
   void setlambdaInitial(double value) { lambdaInitial = value; }
   void setlambdaLowerBound(double value) { lambdaLowerBound = value; }
   void setlambdaUpperBound(double value) { lambdaUpperBound = value; }
-  void setLogFile(const std::string& s) { logFile = s; }
   void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;}
+  void setLogFile(const std::string& s) { logFile = s; }
   void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);}
   // @}
 };
diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h
index 88fc0f850..8e4678590 100644
--- a/gtsam/nonlinear/NonlinearOptimizerParams.h
+++ b/gtsam/nonlinear/NonlinearOptimizerParams.h
@@ -54,47 +54,24 @@ public:
   }
   virtual void print(const std::string& str = "") const;
 
-  size_t getMaxIterations() const {
-    return maxIterations;
-  }
-  double getRelativeErrorTol() const {
-    return relativeErrorTol;
-  }
-  double getAbsoluteErrorTol() const {
-    return absoluteErrorTol;
-  }
-  double getErrorTol() const {
-    return errorTol;
-  }
-  std::string getVerbosity() const {
-    return verbosityTranslator(verbosity);
-  }
+  size_t getMaxIterations() const { return maxIterations; }
+  double getRelativeErrorTol() const { return relativeErrorTol; }
+  double getAbsoluteErrorTol() const { return absoluteErrorTol; }
+  double getErrorTol() const { return errorTol; }
+  std::string getVerbosity() const { return verbosityTranslator(verbosity); }
 
-  void setMaxIterations(int value) {
-    maxIterations = value;
-  }
-  void setRelativeErrorTol(double value) {
-    relativeErrorTol = value;
-  }
-  void setAbsoluteErrorTol(double value) {
-    absoluteErrorTol = value;
-  }
-  void setErrorTol(double value) {
-    errorTol = value;
-  }
-  void setVerbosity(const std::string &src) {
+  void setMaxIterations(int value) { maxIterations = value; }
+  void setRelativeErrorTol(double value) { relativeErrorTol = value; }
+  void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
+  void setErrorTol(double value) { errorTol = value; }
+  void setVerbosity(const std::string& src) {
     verbosity = verbosityTranslator(src);
   }
 
   static Verbosity verbosityTranslator(const std::string &s) ;
   static std::string verbosityTranslator(Verbosity value) ;
 
-  // Successive Linearization Parameters
-
-public:
-
   /** See NonlinearOptimizerParams::linearSolverType */
-
   enum LinearSolverType {
     MULTIFRONTAL_CHOLESKY,
     MULTIFRONTAL_QR,
@@ -168,13 +145,9 @@ public:
 
 private:
   std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
-
   LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
-
   std::string orderingTypeTranslator(Ordering::OrderingType type) const;
-
   Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
-
 };
 
 // For backward compatibility:

From 5af2256277372cfd5c17016155604e3c45f998e8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 10:30:30 -0400
Subject: [PATCH 209/281] Added missing clone for MATLAB wrapper

---
 gtsam/nonlinear/LevenbergMarquardtParams.h | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h
index 491fbd233..4962ad366 100644
--- a/gtsam/nonlinear/LevenbergMarquardtParams.h
+++ b/gtsam/nonlinear/LevenbergMarquardtParams.h
@@ -142,6 +142,15 @@ public:
   void setLogFile(const std::string& s) { logFile = s; }
   void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);}
   // @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  boost::shared_ptr<NonlinearOptimizerParams> clone() const {
+    return boost::shared_ptr<NonlinearOptimizerParams>(new LevenbergMarquardtParams(*this));
+  }
+
+  /// @}
 };
 
 }

From 98ed4d78502fdc746a68024a0e0af25b0202e326 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 12:09:00 -0400
Subject: [PATCH 210/281] Only create typedef to SharedXXX where really needed.

---
 wrap/GlobalFunction.cpp |  2 --
 wrap/Method.cpp         |  4 ++--
 wrap/MethodBase.cpp     |  5 -----
 wrap/ReturnType.cpp     | 21 ++++++++++-----------
 wrap/ReturnType.h       |  3 ---
 wrap/ReturnValue.cpp    |  8 +-------
 wrap/ReturnValue.h      |  2 --
 7 files changed, 13 insertions(+), 32 deletions(-)

diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp
index c8482f2c4..123511dfa 100644
--- a/wrap/GlobalFunction.cpp
+++ b/wrap/GlobalFunction.cpp
@@ -94,8 +94,6 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath,
     // start
     file.oss << "{\n";
 
-    returnVal.wrapTypeUnwrap(file);
-
     // check arguments
     // NOTE: for static functions, there is no object passed
     file.oss << "  checkArguments(\"" << matlabUniqueName
diff --git a/wrap/Method.cpp b/wrap/Method.cpp
index 8f0ef17b9..f7247341c 100644
--- a/wrap/Method.cpp
+++ b/wrap/Method.cpp
@@ -64,8 +64,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName,
                   << "\",nargout,nargin-1," << args.size() << ");\n";
 
   // get class pointer
-  // example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
-  wrapperFile.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName
+  // example: auto obj = unwrap_shared_ptr< Test >(in[0], "Test");
+  wrapperFile.oss << "  auto obj = unwrap_shared_ptr<" << cppClassName
                   << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
 
   // unwrap arguments, see Argument.cpp, we start at 1 as first is obj
diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp
index ef169d989..a2ed68780 100644
--- a/wrap/MethodBase.cpp
+++ b/wrap/MethodBase.cpp
@@ -108,11 +108,6 @@ string MethodBase::wrapper_fragment(
   // start
   wrapperFile.oss << "{\n";
 
-  returnVal.wrapTypeUnwrap(wrapperFile);
-
-  wrapperFile.oss << "  typedef boost::shared_ptr<" << cppClassName
-                  << "> Shared;" << endl;
-
   // get call
   // for static methods: cppClassName::staticMethod<TemplateVal>
   // for instance methods: obj->instanceMethod<TemplateVal>
diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp
index 506e7d471..a97b1cf31 100644
--- a/wrap/ReturnType.cpp
+++ b/wrap/ReturnType.cpp
@@ -23,6 +23,7 @@ void ReturnType::wrap_result(const string& out, const string& result,
                              const TypeAttributesTable& typeAttributes) const {
   string cppType = qualifiedName("::"), matlabType = qualifiedName(".");
 
+  const string sharedType = "Shared" + name();
   if (category == CLASS) {
     // Handle Classes
     string objCopy, ptrType;
@@ -35,9 +36,12 @@ void ReturnType::wrap_result(const string& out, const string& result,
         // A virtual class needs to be cloned, so the whole hierarchy is
         // returned
         objCopy = result + ".clone()";
-      else
+      else {
         // ...but a non-virtual class can just be copied
-        objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))";
+        wrapperFile.oss << "  typedef boost::shared_ptr<" << qualifiedName("::")
+                        << "> " << sharedType << ";" << endl;
+        objCopy = sharedType + "(new " + cppType + "(" + result + "))";
+      }
     }
     // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false);
     wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\""
@@ -46,8 +50,10 @@ void ReturnType::wrap_result(const string& out, const string& result,
 
   } else if (isPtr) {
     // Handle shared pointer case for BASIS/EIGEN/VOID
-    wrapperFile.oss << "  {\n  Shared" << name() << "* ret = new Shared"
-                    << name() << "(" << result << ");" << endl;
+    wrapperFile.oss << "  typedef boost::shared_ptr<" << qualifiedName("::")
+                    << "> " << sharedType << ";" << endl;
+    wrapperFile.oss << "  {\n  auto ret = new " << sharedType << "(" << result
+                    << ");" << endl;
     wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType
                     << "\");\n  }\n";
 
@@ -58,13 +64,6 @@ void ReturnType::wrap_result(const string& out, const string& result,
                     << ");\n";
 }
 
-/* ************************************************************************* */
-void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const {
-  if (category == CLASS)
-    wrapperFile.oss << "  typedef boost::shared_ptr<" << qualifiedName("::")
-                    << "> Shared" << name() << ";" << endl;
-}
-
 /* ************************************************************************* */
 void ReturnType::emit_cython_pxd(
     FileWriter& file, const std::string& className,
diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h
index de1835f28..e4b5b193f 100644
--- a/wrap/ReturnType.h
+++ b/wrap/ReturnType.h
@@ -60,9 +60,6 @@ private:
   void wrap_result(const std::string& out, const std::string& result,
                    FileWriter& wrapperFile,
                    const TypeAttributesTable& typeAttributes) const;
-
-  /// Creates typedef
-  void wrapTypeUnwrap(FileWriter& wrapperFile) const;
 };
 
 //******************************************************************************
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 3f318eddc..9bdd9f5fb 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -40,7 +40,7 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile,
   if (isPair) {
     // For a pair, store the returned pair so we do not evaluate the function
     // twice
-    wrapperFile.oss << "  " << return_type(true) << " pairResult = " << result
+    wrapperFile.oss << "  auto pairResult = " << result
                     << ";\n";
     type1.wrap_result("  out[0]", "pairResult.first", wrapperFile,
                       typeAttributes);
@@ -51,12 +51,6 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile,
   }
 }
 
-/* ************************************************************************* */
-void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const {
-  type1.wrapTypeUnwrap(wrapperFile);
-  if (isPair) type2.wrapTypeUnwrap(wrapperFile);
-}
-
 /* ************************************************************************* */
 void ReturnValue::emit_matlab(FileWriter& proxyFile) const {
   string output;
diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h
index 589db5bd6..3e3ca1cc7 100644
--- a/wrap/ReturnValue.h
+++ b/wrap/ReturnValue.h
@@ -70,8 +70,6 @@ struct ReturnValue {
   void wrap_result(const std::string& result, FileWriter& wrapperFile,
       const TypeAttributesTable& typeAttributes) const;
 
-  void wrapTypeUnwrap(FileWriter& wrapperFile) const;
-
   void emit_matlab(FileWriter& proxyFile) const;
 
   /// @param className the actual class name to use when "This" is specified

From 40051a62262d2a8e0e480bdf2ab11696acdf9d21 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 12:42:09 -0400
Subject: [PATCH 211/281] New expected files after no more Shared

---
 wrap/tests/expected/geometry_wrapper.cpp      | 239 ++++++------------
 .../testNamespaces_wrapper.cpp                |  19 +-
 2 files changed, 88 insertions(+), 170 deletions(-)

diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp
index dec78b80c..6946a969b 100644
--- a/wrap/tests/expected/geometry_wrapper.cpp
+++ b/wrap/tests/expected/geometry_wrapper.cpp
@@ -183,35 +183,31 @@ void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const
 
 void gtsamPoint2_argChar_4(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("argChar",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
   char a = unwrap< char >(in[1]);
   obj->argChar(a);
 }
 
 void gtsamPoint2_argUChar_5(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("argUChar",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
   unsigned char a = unwrap< unsigned char >(in[1]);
   obj->argUChar(a);
 }
 
 void gtsamPoint2_dim_6(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("dim",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
   out[0] = wrap< int >(obj->dim());
 }
 
 void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("eigenArguments",nargout,nargin-1,2);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
   Vector v = unwrap< Vector >(in[1]);
   Matrix m = unwrap< Matrix >(in[2]);
   obj->eigenArguments(v,m);
@@ -219,34 +215,29 @@ void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const
 
 void gtsamPoint2_returnChar_8(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("returnChar",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
   out[0] = wrap< char >(obj->returnChar());
 }
 
 void gtsamPoint2_vectorConfusion_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<VectorNotEigen> SharedVectorNotEigen;
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("vectorConfusion",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(SharedVectorNotEigen(new VectorNotEigen(obj->vectorConfusion())),"VectorNotEigen", false);
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  out[0] = wrap_shared_ptr(boost::make_shared<VectorNotEigen>(obj->vectorConfusion()),"VectorNotEigen", false);
 }
 
 void gtsamPoint2_x_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("x",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
   out[0] = wrap< double >(obj->x());
 }
 
 void gtsamPoint2_y_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> Shared;
   checkArguments("y",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
+  auto obj = unwrap_shared_ptr<gtsam::Point2>(in[0], "ptr_gtsamPoint2");
   out[0] = wrap< double >(obj->y());
 }
 
@@ -288,9 +279,8 @@ void gtsamPoint3_deconstructor_14(int nargout, mxArray *out[], int nargin, const
 
 void gtsamPoint3_norm_15(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point3> Shared;
   checkArguments("norm",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<gtsam::Point3>(in[0], "ptr_gtsamPoint3");
+  auto obj = unwrap_shared_ptr<gtsam::Point3>(in[0], "ptr_gtsamPoint3");
   out[0] = wrap< double >(obj->norm());
 }
 
@@ -306,16 +296,13 @@ void gtsamPoint3_string_serialize_16(int nargout, mxArray *out[], int nargin, co
 }
 void gtsamPoint3_StaticFunctionRet_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
-  typedef boost::shared_ptr<gtsam::Point3> Shared;
   checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1);
   double z = unwrap< double >(in[0]);
-  out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(gtsam::Point3::StaticFunctionRet(z))),"gtsam.Point3", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(gtsam::Point3::StaticFunctionRet(z)),"gtsam.Point3", false);
 }
 
 void gtsamPoint3_staticFunction_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point3> Shared;
   checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0);
   out[0] = wrap< double >(gtsam::Point3::staticFunction());
 }
@@ -379,187 +366,159 @@ void Test_deconstructor_23(int nargout, mxArray *out[], int nargin, const mxArra
 
 void Test_arg_EigenConstRef_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   Matrix value = unwrap< Matrix >(in[1]);
   obj->arg_EigenConstRef(value);
 }
 
 void Test_create_MixedPtrs_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  pair< Test, SharedTest > pairResult = obj->create_MixedPtrs();
-  out[0] = wrap_shared_ptr(SharedTest(new Test(pairResult.first)),"Test", false);
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto pairResult = obj->create_MixedPtrs();
+  out[0] = wrap_shared_ptr(boost::make_shared<Test>(pairResult.first),"Test", false);
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
 void Test_create_ptrs_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("create_ptrs",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
-  pair< SharedTest, SharedTest > pairResult = obj->create_ptrs();
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto pairResult = obj->create_ptrs();
   out[0] = wrap_shared_ptr(pairResult.first,"Test", false);
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
 void Test_print_27(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("print",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   obj->print();
 }
 
 void Test_return_Point2Ptr_28(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_Point2Ptr",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   bool value = unwrap< bool >(in[1]);
   out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false);
 }
 
 void Test_return_Test_29(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_Test",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "ptr_Test");
-  out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<Test>(obj->return_Test(value)),"Test", false);
 }
 
 void Test_return_TestPtr_30(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_TestPtr",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "ptr_Test");
   out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false);
 }
 
 void Test_return_bool_31(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_bool",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   bool value = unwrap< bool >(in[1]);
   out[0] = wrap< bool >(obj->return_bool(value));
 }
 
 void Test_return_double_32(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_double",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   double value = unwrap< double >(in[1]);
   out[0] = wrap< double >(obj->return_double(value));
 }
 
 void Test_return_field_33(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_field",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test");
   out[0] = wrap< bool >(obj->return_field(t));
 }
 
 void Test_return_int_34(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_int",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   int value = unwrap< int >(in[1]);
   out[0] = wrap< int >(obj->return_int(value));
 }
 
 void Test_return_matrix1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_matrix1",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   Matrix value = unwrap< Matrix >(in[1]);
   out[0] = wrap< Matrix >(obj->return_matrix1(value));
 }
 
 void Test_return_matrix2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_matrix2",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   Matrix value = unwrap< Matrix >(in[1]);
   out[0] = wrap< Matrix >(obj->return_matrix2(value));
 }
 
 void Test_return_pair_37(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_pair",nargout,nargin-1,2);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   Vector v = unwrap< Vector >(in[1]);
   Matrix A = unwrap< Matrix >(in[2]);
-  pair< Vector, Matrix > pairResult = obj->return_pair(v,A);
+  auto pairResult = obj->return_pair(v,A);
   out[0] = wrap< Vector >(pairResult.first);
   out[1] = wrap< Matrix >(pairResult.second);
 }
 
 void Test_return_ptrs_38(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> SharedTest;
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_ptrs",nargout,nargin-1,2);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   boost::shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test");
   boost::shared_ptr<Test> p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test");
-  pair< SharedTest, SharedTest > pairResult = obj->return_ptrs(p1,p2);
+  auto pairResult = obj->return_ptrs(p1,p2);
   out[0] = wrap_shared_ptr(pairResult.first,"Test", false);
   out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
 }
 
 void Test_return_size_t_39(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_size_t",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   size_t value = unwrap< size_t >(in[1]);
   out[0] = wrap< size_t >(obj->return_size_t(value));
 }
 
 void Test_return_string_40(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_string",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   string value = unwrap< string >(in[1]);
   out[0] = wrap< string >(obj->return_string(value));
 }
 
 void Test_return_vector1_41(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_vector1",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   Vector value = unwrap< Vector >(in[1]);
   out[0] = wrap< Vector >(obj->return_vector1(value));
 }
 
 void Test_return_vector2_42(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<Test> Shared;
   checkArguments("return_vector2",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
+  auto obj = unwrap_shared_ptr<Test>(in[0], "ptr_Test");
   Vector value = unwrap< Vector >(in[1]);
   out[0] = wrap< Vector >(obj->return_vector2(value));
 }
@@ -647,114 +606,93 @@ void MyTemplatePoint2_deconstructor_49(int nargout, mxArray *out[], int nargin,
 
 void MyTemplatePoint2_accept_T_50(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("accept_T",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
   obj->accept_T(value);
 }
 
 void MyTemplatePoint2_accept_Tptr_51(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("accept_Tptr",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
   obj->accept_Tptr(value);
 }
 
 void MyTemplatePoint2_create_MixedPtrs_52(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  pair< gtsam::Point2, SharedPoint2 > pairResult = obj->create_MixedPtrs();
-  out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(pairResult.first)),"gtsam.Point2", false);
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto pairResult = obj->create_MixedPtrs();
+  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(pairResult.first),"gtsam.Point2", false);
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
 }
 
 void MyTemplatePoint2_create_ptrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("create_ptrs",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
-  pair< SharedPoint2, SharedPoint2 > pairResult = obj->create_ptrs();
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto pairResult = obj->create_ptrs();
   out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
 }
 
 void MyTemplatePoint2_return_T_54(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("return_T",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->return_T(value))),"gtsam.Point2", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->return_T(value)),"gtsam.Point2", false);
 }
 
 void MyTemplatePoint2_return_Tptr_55(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("return_Tptr",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   boost::shared_ptr<gtsam::Point2> value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
   out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false);
 }
 
 void MyTemplatePoint2_return_ptrs_56(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("return_ptrs",nargout,nargin-1,2);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   boost::shared_ptr<gtsam::Point2> p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
   boost::shared_ptr<gtsam::Point2> p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2");
-  pair< SharedPoint2, SharedPoint2 > pairResult = obj->return_ptrs(p1,p2);
+  auto pairResult = obj->return_ptrs(p1,p2);
   out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false);
   out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false);
 }
 
 void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   Matrix t = unwrap< Matrix >(in[1]);
   out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
 }
 
 void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod<gtsam::Point2>(t))),"gtsam.Point2", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
 }
 
 void MyTemplatePoint2_templatedMethod_59(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod<gtsam::Point3>(t))),"gtsam.Point3", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
 }
 
 void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplatePoint2> Shared;
   checkArguments("templatedMethodVector",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
+  auto obj = unwrap_shared_ptr<MyTemplatePoint2>(in[0], "ptr_MyTemplatePoint2");
   Vector t = unwrap< Vector >(in[1]);
   out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
 }
@@ -811,124 +749,111 @@ void MyTemplateMatrix_deconstructor_64(int nargout, mxArray *out[], int nargin,
 
 void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("accept_T",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   obj->accept_T(value);
 }
 
 void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("accept_Tptr",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   obj->accept_Tptr(value);
 }
 
 void MyTemplateMatrix_create_MixedPtrs_67(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("create_MixedPtrs",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  pair< Matrix, SharedMatrix > pairResult = obj->create_MixedPtrs();
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto pairResult = obj->create_MixedPtrs();
   out[0] = wrap< Matrix >(pairResult.first);
   {
-  SharedMatrix* ret = new SharedMatrix(pairResult.second);
-  out[1] = wrap_shared_ptr(ret,"Matrix");
+  boost::shared_ptr<Matrix> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Matrix");
   }
 }
 
 void MyTemplateMatrix_create_ptrs_68(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("create_ptrs",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
-  pair< SharedMatrix, SharedMatrix > pairResult = obj->create_ptrs();
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto pairResult = obj->create_ptrs();
   {
-  SharedMatrix* ret = new SharedMatrix(pairResult.first);
-  out[0] = wrap_shared_ptr(ret,"Matrix");
+  boost::shared_ptr<Matrix> shared(pairResult.first);
+  out[0] = wrap_shared_ptr(shared,"Matrix");
   }
   {
-  SharedMatrix* ret = new SharedMatrix(pairResult.second);
-  out[1] = wrap_shared_ptr(ret,"Matrix");
+  boost::shared_ptr<Matrix> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Matrix");
   }
 }
 
 void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("return_T",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   out[0] = wrap< Matrix >(obj->return_T(value));
 }
 
 void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("return_Tptr",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   Matrix value = unwrap< Matrix >(in[1]);
   {
-  SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value));
-  out[0] = wrap_shared_ptr(ret,"Matrix");
+  boost::shared_ptr<Matrix> shared(obj->return_Tptr(value));
+  out[0] = wrap_shared_ptr(shared,"Matrix");
   }
 }
 
 void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("return_ptrs",nargout,nargin-1,2);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   Matrix p1 = unwrap< Matrix >(in[1]);
   Matrix p2 = unwrap< Matrix >(in[2]);
-  pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2);
+  auto pairResult = obj->return_ptrs(p1,p2);
   {
-  SharedMatrix* ret = new SharedMatrix(pairResult.first);
-  out[0] = wrap_shared_ptr(ret,"Matrix");
+  boost::shared_ptr<Matrix> shared(pairResult.first);
+  out[0] = wrap_shared_ptr(shared,"Matrix");
   }
   {
-  SharedMatrix* ret = new SharedMatrix(pairResult.second);
-  out[1] = wrap_shared_ptr(ret,"Matrix");
+  boost::shared_ptr<Matrix> shared(pairResult.second);
+  out[1] = wrap_shared_ptr(shared,"Matrix");
   }
 }
 
 void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("templatedMethodMatrix",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   Matrix t = unwrap< Matrix >(in[1]);
   out[0] = wrap< Matrix >(obj->templatedMethod<Matrix>(t));
 }
 
 void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point2> SharedPoint2;
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("templatedMethodPoint2",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2");
-  out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod<gtsam::Point2>(t))),"gtsam.Point2", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point2>(obj->templatedMethod<gtsam::Point2>(t)),"gtsam.Point2", false);
 }
 
 void MyTemplateMatrix_templatedMethod_74(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<gtsam::Point3> SharedPoint3;
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("templatedMethodPoint3",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3");
-  out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod<gtsam::Point3>(t))),"gtsam.Point3", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<gtsam::Point3>(obj->templatedMethod<gtsam::Point3>(t)),"gtsam.Point3", false);
 }
 
 void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<MyTemplateMatrix> Shared;
   checkArguments("templatedMethodVector",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
+  auto obj = unwrap_shared_ptr<MyTemplateMatrix>(in[0], "ptr_MyTemplateMatrix");
   Vector t = unwrap< Vector >(in[1]);
   out[0] = wrap< Vector >(obj->templatedMethod<Vector>(t));
 }
diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
index 0f5c70348..6e0c5670d 100644
--- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
+++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
@@ -199,34 +199,29 @@ void ns2ClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mx
 
 void ns2ClassA_memberFunction_9(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<ns2::ClassA> Shared;
   checkArguments("memberFunction",nargout,nargin-1,0);
-  Shared obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
+  auto obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
   out[0] = wrap< double >(obj->memberFunction());
 }
 
 void ns2ClassA_nsArg_10(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<ns2::ClassA> Shared;
   checkArguments("nsArg",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
+  auto obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
   ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ptr_ns1ClassB");
   out[0] = wrap< int >(obj->nsArg(arg));
 }
 
 void ns2ClassA_nsReturn_11(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<ns2::ns3::ClassB> SharedClassB;
-  typedef boost::shared_ptr<ns2::ClassA> Shared;
   checkArguments("nsReturn",nargout,nargin-1,1);
-  Shared obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
+  auto obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
   double q = unwrap< double >(in[1]);
-  out[0] = wrap_shared_ptr(SharedClassB(new ns2::ns3::ClassB(obj->nsReturn(q))),"ns2.ns3.ClassB", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<ns2::ns3::ClassB>(obj->nsReturn(q)),"ns2.ns3.ClassB", false);
 }
 
 void ns2ClassA_afunction_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<ns2::ClassA> Shared;
   checkArguments("ns2ClassA.afunction",nargout,nargin,0);
   out[0] = wrap< double >(ns2::ClassA::afunction());
 }
@@ -343,18 +338,16 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra
 }
 void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<ns1::ClassA> SharedClassA;
   checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1);
   ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA");
-  out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<ns1::ClassA>(ns2::overloadedGlobalFunction(a)),"ns1.ClassA", false);
 }
 void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
-  typedef boost::shared_ptr<ns1::ClassA> SharedClassA;
   checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2);
   ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA");
   double b = unwrap< double >(in[1]);
-  out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false);
+  out[0] = wrap_shared_ptr(boost::make_shared<ns1::ClassA>(ns2::overloadedGlobalFunction(a,b)),"ns1.ClassA", false);
 }
 
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])

From aa8b40b59469998e134f9a368638d7a2a1fc1f06 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 12:42:40 -0400
Subject: [PATCH 212/281] Got rid of some obsolete methods/arguments

---
 wrap/FullyOverloadedFunction.h |  8 ++++----
 wrap/ReturnType.cpp            | 22 ++++++----------------
 wrap/ReturnType.h              |  2 --
 wrap/ReturnValue.cpp           |  7 ++++---
 wrap/ReturnValue.h             |  4 ++--
 5 files changed, 16 insertions(+), 27 deletions(-)

diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h
index 87c5169dd..bf7051662 100644
--- a/wrap/FullyOverloadedFunction.h
+++ b/wrap/FullyOverloadedFunction.h
@@ -79,8 +79,8 @@ public:
       if (argLCount != nrOverloads() - 1)
         proxyFile.oss << ", ";
       else
-        proxyFile.oss << " : returns " << returnValue(0).return_type(false)
-            << std::endl;
+        proxyFile.oss << " : returns " << returnValue(0).returnType()
+                      << std::endl;
       argLCount++;
     }
   }
@@ -91,8 +91,8 @@ public:
     for(ArgumentList argList: argLists_) {
       proxyFile.oss << "%";
       argList.emit_prototype(proxyFile, name);
-      proxyFile.oss << " : returns " << returnVals_[i++].return_type(false)
-          << std::endl;
+      proxyFile.oss << " : returns " << returnVals_[i++].returnType()
+                    << std::endl;
     }
   }
 
diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp
index a97b1cf31..d308bc28a 100644
--- a/wrap/ReturnType.cpp
+++ b/wrap/ReturnType.cpp
@@ -12,18 +12,12 @@
 using namespace std;
 using namespace wrap;
 
-/* ************************************************************************* */
-string ReturnType::str(bool add_ptr) const {
-  return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name());
-}
-
 /* ************************************************************************* */
 void ReturnType::wrap_result(const string& out, const string& result,
                              FileWriter& wrapperFile,
                              const TypeAttributesTable& typeAttributes) const {
   string cppType = qualifiedName("::"), matlabType = qualifiedName(".");
 
-  const string sharedType = "Shared" + name();
   if (category == CLASS) {
     // Handle Classes
     string objCopy, ptrType;
@@ -38,9 +32,7 @@ void ReturnType::wrap_result(const string& out, const string& result,
         objCopy = result + ".clone()";
       else {
         // ...but a non-virtual class can just be copied
-        wrapperFile.oss << "  typedef boost::shared_ptr<" << qualifiedName("::")
-                        << "> " << sharedType << ";" << endl;
-        objCopy = sharedType + "(new " + cppType + "(" + result + "))";
+        objCopy = "boost::make_shared<" + cppType + ">(" + result + ")";
       }
     }
     // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false);
@@ -50,17 +42,15 @@ void ReturnType::wrap_result(const string& out, const string& result,
 
   } else if (isPtr) {
     // Handle shared pointer case for BASIS/EIGEN/VOID
-    wrapperFile.oss << "  typedef boost::shared_ptr<" << qualifiedName("::")
-                    << "> " << sharedType << ";" << endl;
-    wrapperFile.oss << "  {\n  auto ret = new " << sharedType << "(" << result
-                    << ");" << endl;
-    wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType
+    // This case does not actually occur in GTSAM wrappers, so untested!
+    wrapperFile.oss << "  {\n  boost::shared_ptr<" << qualifiedName("::")
+                    << "> shared(" << result << ");" << endl;
+    wrapperFile.oss << out << " = wrap_shared_ptr(shared,\"" << matlabType
                     << "\");\n  }\n";
 
   } else if (matlabType != "void")
-
     // Handle normal case case for BASIS/EIGEN
-    wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result
+    wrapperFile.oss << out << " = wrap< " << qualifiedName("::") << " >(" << result
                     << ");\n";
 }
 
diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h
index e4b5b193f..ba5086507 100644
--- a/wrap/ReturnType.h
+++ b/wrap/ReturnType.h
@@ -54,8 +54,6 @@ struct ReturnType : public Qualified {
 private:
   friend struct ReturnValue;
 
-  std::string str(bool add_ptr) const;
-
   /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false);
   void wrap_result(const std::string& out, const std::string& result,
                    FileWriter& wrapperFile,
diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp
index 9bdd9f5fb..e58e85602 100644
--- a/wrap/ReturnValue.cpp
+++ b/wrap/ReturnValue.cpp
@@ -22,11 +22,12 @@ ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const {
 }
 
 /* ************************************************************************* */
-string ReturnValue::return_type(bool add_ptr) const {
+string ReturnValue::returnType() const {
   if (isPair)
-    return "pair< " + type1.str(add_ptr) + ", " + type2.str(add_ptr) + " >";
+    return "pair< " + type1.qualifiedName("::") + ", " +
+           type2.qualifiedName("::") + " >";
   else
-    return type1.str(add_ptr);
+    return type1.qualifiedName("::");
 }
 
 /* ************************************************************************* */
diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h
index 3e3ca1cc7..721132797 100644
--- a/wrap/ReturnValue.h
+++ b/wrap/ReturnValue.h
@@ -63,7 +63,7 @@ struct ReturnValue {
   /// Substitute template argument
   ReturnValue expandTemplate(const TemplateSubstitution& ts) const;
 
-  std::string return_type(bool add_ptr) const;
+  std::string returnType() const;
 
   std::string matlab_returnType() const;
 
@@ -82,7 +82,7 @@ struct ReturnValue {
     if (!r.isPair && r.type1.category == ReturnType::VOID)
       os << "void";
     else
-      os << r.return_type(true);
+      os << r.returnType();
     return os;
   }
 

From 37eba50932df03dae90cc3c5c94f19275c16dbb8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 15:47:01 -0400
Subject: [PATCH 213/281] Modernized, documented

---
 gtsam/slam/InitializePose3.cpp           | 186 +++++++++--------------
 gtsam/slam/InitializePose3.h             |  61 +++++---
 gtsam/slam/tests/testInitializePose3.cpp |  24 +--
 3 files changed, 129 insertions(+), 142 deletions(-)

diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 3979996da..b781f79f0 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -16,7 +16,7 @@
  *  @date   August, 2014
  */
 
-#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/InitializePose3.h> 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -29,13 +29,9 @@
 using namespace std;
 
 namespace gtsam {
-namespace InitializePose3 {
+namespace initialize_pose3 {
 
-static const Matrix I9 = I_9x9;
-static const Vector zero9 = Vector::Zero(9);
-static const Matrix zero33 = Z_3x3;
-
-static const Key keyAnchor = symbol('Z', 9999999);
+static const Key kAnchorKey = symbol('Z', 9999999);
 
 /* ************************************************************************* */
 GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
@@ -43,26 +39,29 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
   GaussianFactorGraph linearGraph;
   noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
 
-  for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
+  for(const auto& factor: g) {
     Matrix3 Rij;
 
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between)
       Rij = pose3Between->measured().rotation().matrix();
     else
-      std::cout << "Error in buildLinearOrientationGraph" << std::endl;
+      cout << "Error in buildLinearOrientationGraph" << endl;
 
-    const KeyVector& keys = factor->keys();
+    const auto& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
     Matrix M9 = Z_9x9;
     M9.block(0,0,3,3) = Rij;
     M9.block(3,3,3,3) = Rij;
     M9.block(6,6,3,3) = Rij;
-    linearGraph.add(key1, -I9, key2, M9, zero9, model);
+    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model);
   }
   // prior on the anchor orientation
-  linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0).finished(), model);
+  linearGraph.add(
+      kAnchorKey, I_9x9,
+      (Vector(9) << 1.0, 0.0, 0.0, /*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0)
+          .finished(),
+      model);
   return linearGraph;
 }
 
@@ -75,23 +74,15 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
   ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
 
   Values validRot3;
-  for(const VectorValues::value_type& it: relaxedRot3) {
+  for(const auto& it: relaxedRot3) {
     Key key = it.first;
-    if (key != keyAnchor) {
-      const Vector& rotVector = it.second;
-      Matrix3 rotMat;
-      rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
-      rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
-      rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
+    if (key != kAnchorKey) {
+      Matrix3 R; R << it.second;
 
       Matrix U, V; Vector s;
-      svd(rotMat, U, s, V);
+      svd(R.transpose(), U, s, V);
       Matrix3 normalizedRotMat = U * V.transpose();
 
-      //      std::cout << "rotMat \n" << rotMat << std::endl;
-      //      std::cout << "U V' \n" << U * V.transpose() << std::endl;
-      //      std::cout << "V \n" << V << std::endl;
-
       if(normalizedRotMat.determinant() < 0)
         normalizedRotMat = U * ppm * V.transpose();
 
@@ -103,31 +94,27 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
 }
 
 /* ************************************************************************* */
-// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
 NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
   gttic(InitializePose3_buildPose3graph);
   NonlinearFactorGraph pose3Graph;
 
-  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
+  for(const auto& factor: graph) {
 
     // recast to a between on Pose3
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between)
       pose3Graph.add(pose3Between);
 
     // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
-    boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
-        boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
+    const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
     if (pose3Prior)
-      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
+      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
               pose3Prior->prior(), pose3Prior->noiseModel());
   }
   return pose3Graph;
 }
 
 /* ************************************************************************* */
-// Return the orientations of a graph including only BetweenFactors<Pose3>
 Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
   gttic(InitializePose3_computeOrientationsChordal);
 
@@ -142,14 +129,16 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
 }
 
 /* ************************************************************************* */
-// Return the orientations of a graph including only BetweenFactors<Pose3>
-Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
+Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
+                                   const Values& givenGuess,
+                                   const size_t maxIter,
+                                   const bool setRefFrame) {
   gttic(InitializePose3_computeOrientationsGradient);
 
   // this works on the inverse rotations, according to Tron&Vidal,2011
   Values inverseRot;
-  inverseRot.insert(keyAnchor, Rot3());
-  for(const Values::ConstKeyValuePair& key_value: givenGuess) {
+  inverseRot.insert(kAnchorKey, Rot3());
+  for(const auto& key_value: givenGuess) {
     Key key = key_value.key;
     const Pose3& pose = givenGuess.at<Pose3>(key);
     inverseRot.insert(key, pose.rotation().inverse());
@@ -164,9 +153,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   // calculate max node degree & allocate gradient
   size_t maxNodeDeg = 0;
   VectorValues grad;
-  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+  for(const auto& key_value: inverseRot) {
     Key key = key_value.key;
-    grad.insert(key,Vector3::Zero());
+    grad.insert(key,Z_3x1);
     size_t currNodeDeg = (adjEdgesMap.at(key)).size();
     if(currNodeDeg > maxNodeDeg)
       maxNodeDeg = currNodeDeg;
@@ -180,42 +169,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   double mu_max = maxNodeDeg * rho;
   double stepsize = 2/mu_max; // = 1/(a b dG)
 
-  std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
   double maxGrad;
   // gradient iterations
   size_t it;
-  for(it=0; it < maxIter; it++){
+  for (it = 0; it < maxIter; it++) {
     //////////////////////////////////////////////////////////////////////////
     // compute the gradient at each node
-    //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
-    //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
     maxGrad = 0;
-    for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+    for (const auto& key_value : inverseRot) {
       Key key = key_value.key;
-      //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
-      Vector gradKey = Vector3::Zero();
+      Vector gradKey = Z_3x1;
       // collect the gradient for each edge incident on key
-      for(const size_t& factorId: adjEdgesMap.at(key)){
+      for (const size_t& factorId : adjEdgesMap.at(key)) {
         Rot3 Rij = factorId2RotMap.at(factorId);
         Rot3 Ri = inverseRot.at<Rot3>(key);
-        if( key == (pose3Graph.at(factorId))->keys()[0] ){
-          Key key1 = (pose3Graph.at(factorId))->keys()[1];
+        auto factor = pose3Graph.at(factorId);
+        const auto& keys = factor->keys();
+        if (key == keys[0]) {
+          Key key1 = keys[1];
           Rot3 Rj = inverseRot.at<Rot3>(key1);
-          gradKey = gradKey + gradientTron(Ri, Rij  * Rj, a, b);
-          //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij  * Rj, a, b) \n " << gradientTron(Ri, Rij  * Rj, a, b) << std::endl;
-        }else if( key == (pose3Graph.at(factorId))->keys()[1] ){
-          Key key0 = (pose3Graph.at(factorId))->keys()[0];
+          gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
+        } else if (key == keys[1]) {
+          Key key0 = keys[0];
           Rot3 Rj = inverseRot.at<Rot3>(key0);
           gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
-          //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse()  * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
-        }else{
-          std::cout << "Error in gradient computation" << std::endl;
+        } else {
+          cout << "Error in gradient computation" << endl;
         }
-      } // end of i-th gradient computation
-      grad.at(key) = stepsize *  gradKey;
+      }  // end of i-th gradient computation
+      grad.at(key) = stepsize * gradKey;
 
       double normGradKey = (gradKey).norm();
-      //std::cout << "key  " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
       if(normGradKey>maxGrad)
         maxGrad = normGradKey;
     } // end of loop over nodes
@@ -230,14 +214,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
       break;
   } // enf of gradient iterations
 
-  std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad <<  std::endl;
-
   // Return correct rotations
-  const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
+  const Rot3& Rref = inverseRot.at<Rot3>(kAnchorKey); // This will be set to the identity as so far we included no prior
   Values estimateRot;
-  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+  for(const auto& key_value: inverseRot) {
     Key key = key_value.key;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
       const Rot3& R = inverseRot.at<Rot3>(key);
       if(setRefFrame)
         estimateRot.insert(key, Rref.compose(R.inverse()));
@@ -249,11 +231,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
 }
 
 /* ************************************************************************* */
-void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
+void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+                         const NonlinearFactorGraph& pose3Graph) {
   size_t factorId = 0;
-  for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+  for(const auto& factor: pose3Graph) {
+    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between){
       Rot3 Rij = pose3Between->measured().rotation();
       factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
@@ -275,7 +257,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
         adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
       }
     }else{
-      std::cout << "Error in computeOrientationsGradient" << std::endl;
+      cout << "Error in createSymbolicGraph" << endl;
     }
     factorId++;
   }
@@ -292,10 +274,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
     th = logRot.norm();
   }
   // exclude small or invalid rotations
-  if (th > 1e-5 && th == th){ // nonzero valid rotations
+  if (th > 1e-5 && th == th) {  // nonzero valid rotations
     logRot = logRot / th;
-  }else{
-    logRot = Vector3::Zero();
+  } else {
+    logRot = Z_3x1;
     th = 0.0;
   }
 
@@ -320,24 +302,25 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 
   // put into Values structure
   Values initialPose;
-  for(const Values::ConstKeyValuePair& key_value: initialRot){
+  for (const auto& key_value : initialRot) {
     Key key = key_value.key;
     const Rot3& rot = initialRot.at<Rot3>(key);
     Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
     initialPose.insert(key, initializedPose);
   }
+
   // add prior
   noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
-  initialPose.insert(keyAnchor, Pose3());
-  pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);
+  initialPose.insert(kAnchorKey, Pose3());
+  pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);
 
   // Create optimizer
   GaussNewtonParams params;
   bool singleIter = true;
-  if(singleIter){
+  if (singleIter) {
     params.maxIterations = 1;
-  }else{
-    std::cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" <<std::endl;
+  } else {
+    cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" << endl;
     params.setVerbosity("TERMINATION");
   }
   GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
@@ -345,9 +328,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 
   // put into Values structure
   Values estimate;
-  for(const Values::ConstKeyValuePair& key_value: GNresult) {
+  for (const auto& key_value : GNresult) {
     Key key = key_value.key;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
       const Pose3& pose = GNresult.at<Pose3>(key);
       estimate.insert(key, pose);
     }
@@ -356,22 +339,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 }
 
 /* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph) {
+Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
+                  bool useGradient) {
   gttic(InitializePose3_initialize);
-
-  // We "extract" the Pose3 subgraph of the original graph: this
-  // is done to properly model priors and avoiding operating on a larger graph
-  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
-
-  // Get orientations from relative orientation measurements
-  Values valueRot3 = computeOrientationsChordal(pose3Graph);
-
-  // Compute the full poses (1 GN iteration on full poses)
-  return computePoses(pose3Graph, valueRot3);
-}
-
-/* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
   Values initialValues;
 
   // We "extract" the Pose3 subgraph of the original graph: this
@@ -380,27 +350,19 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
 
   // Get orientations from relative orientation measurements
   Values orientations;
-  if(useGradient)
+  if (useGradient)
     orientations = computeOrientationsGradient(pose3Graph, givenGuess);
   else
     orientations = computeOrientationsChordal(pose3Graph);
 
-//  orientations.print("orientations\n");
-
   // Compute the full poses (1 GN iteration on full poses)
   return computePoses(pose3Graph, orientations);
-
-  //  for(const Values::ConstKeyValuePair& key_value: orientations) {
-  //    Key key = key_value.key;
-  //    if (key != keyAnchor) {
-  //      const Point3& pos = givenGuess.at<Pose3>(key).translation();
-  //      const Rot3& rot = orientations.at<Rot3>(key);
-  //      Pose3 initializedPoses = Pose3(rot, pos);
-  //      initialValues.insert(key, initializedPoses);
-  //    }
-  //  }
-  //  return initialValues;
 }
 
-} // end of namespace lago
-} // end of namespace gtsam
+/* ************************************************************************* */
+Values initialize(const NonlinearFactorGraph& graph) {
+  return initialize(graph, Values(), false);
+}
+
+} // namespace initialize_pose3
+} // namespace gtsam
diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h
index dba5ceac3..d9bea7932 100644
--- a/gtsam/slam/InitializePose3.h
+++ b/gtsam/slam/InitializePose3.h
@@ -20,40 +20,65 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/graph.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/graph.h>
-#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 namespace gtsam {
 
 typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
-typedef std::map<Key, Rot3 > KeyRotMap;
+typedef std::map<Key, Rot3> KeyRotMap;
 
-namespace InitializePose3 {
+namespace initialize_pose3 {
 
-GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g);
+GTSAM_EXPORT GaussianFactorGraph
+buildLinearOrientationGraph(const NonlinearFactorGraph& g);
 
 GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
 
-GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
+/**
+ * Return the orientations of a graph including only BetweenFactors<Pose3>
+ */
+GTSAM_EXPORT Values
+computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
 
-GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
-    const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true);
+/**
+ * Return the orientations of a graph including only BetweenFactors<Pose3>
+ */
+GTSAM_EXPORT Values computeOrientationsGradient(
+    const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
+    size_t maxIter = 10000, const bool setRefFrame = true);
 
-GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
-    const NonlinearFactorGraph& pose3Graph);
+GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
+                                      KeyRotMap& factorId2RotMap,
+                                      const NonlinearFactorGraph& pose3Graph);
 
-GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b);
+GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2,
+                                  const double a, const double b);
 
-GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
+/**
+ * Select the subgraph of betweenFactors and transforms priors into between wrt
+ * a fictitious node
+ */
+GTSAM_EXPORT NonlinearFactorGraph
+buildPose3graph(const NonlinearFactorGraph& graph);
 
-GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot);
+GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,
+                                 Values& initialRot);
 
+/**
+ * "extract" the Pose3 subgraph of the original graph, get orientations from
+ * relative orientation measurements (using either gradient or chordal method),
+ * and finish up with 1 GN iteration on full poses.
+ */
+GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
+                               const Values& givenGuess,
+                               bool useGradient = false);
+
+/// Calls initialize above using Chordal method.
 GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
 
-GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false);
-
-} // end of namespace lago
-} // end of namespace gtsam
+}  // namespace initialize_pose3
+}  // end of namespace gtsam
diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp
index e6f08286a..ea474c480 100644
--- a/gtsam/slam/tests/testInitializePose3.cpp
+++ b/gtsam/slam/tests/testInitializePose3.cpp
@@ -74,15 +74,15 @@ NonlinearFactorGraph graph() {
 
 /* *************************************************************************** */
 TEST( InitializePose3, buildPose3graph ) {
-  NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph());
   // pose3graph.print("");
 }
 
 /* *************************************************************************** */
 TEST( InitializePose3, orientations ) {
-  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
 
-  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
+  Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
@@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) {
 
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
-  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
 
   KeyVectorMap adjEdgesMap;
   KeyRotMap factorId2RotMap;
 
-  InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
+  initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
 
   EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
   EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
@@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) {
   double a = 6.010534238540223;
   double b = 1.0;
 
-  Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
+  Vector actual = initialize_pose3::gradientTron(R1, R2, a, b);
   Vector expected = Vector3::Zero();
   expected(2) = 1.962658662803917;
 
@@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) {
 
 /* *************************************************************************** */
 TEST( InitializePose3, iterationGradient ) {
-  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
 
   // Wrong initial guess - initialization should fix the rotations
   Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
@@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) {
 
   size_t maxIter = 1; // test gradient at the first iteration
   bool setRefFrame = false;
-  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
+  Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
 
   Matrix M0 = (Matrix(3,3) <<  0.999435813876064,  -0.033571481675497,   0.001004768630281,
       0.033572116359134,   0.999436104312325,  -0.000621610948719,
@@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) {
 
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradient ) {
-  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
 
   // Wrong initial guess - initialization should fix the rotations
   Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
@@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) {
   givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
   // do 10 gradient iterations
   bool setRefFrame = false;
-  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
+  Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
 
   //  const Key keyAnchor = symbol('Z', 9999999);
   //  givenPoses.insert(keyAnchor,simple::pose0);
@@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) {
   givenPoses.insert(x2,simple::pose2);
   givenPoses.insert(x3,simple::pose3);
 
-  Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
+  Values initial = initialize_pose3::initialize(simple::graph(), givenPoses);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal(givenPoses, initial, 1e-6));
@@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses )
   noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
   inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
 
-  Values initial = InitializePose3::initialize(*inputGraph);
+  Values initial = initialize_pose3::initialize(*inputGraph);
   EXPECT(assert_equal(*expectedValues,initial,0.1));  // TODO(frank): very loose !!
 }
 

From 3a371a1cf284139717204b5c0fad71a87247486d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 15:47:51 -0400
Subject: [PATCH 214/281] Wrapped

---
 gtsam.h | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 9a06b7bd5..0aca81d02 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2523,6 +2523,23 @@ class BetweenFactorPose3s
   gtsam::BetweenFactorPose3* at(size_t i) const;
 };
 
+#include <gtsam/slam/InitializePose3.h>
+namespace initialize_pose3 {
+gtsam::Values computeOrientationsChordal(
+    const gtsam::NonlinearFactorGraph& pose3Graph);
+gtsam::Values computeOrientationsGradient(
+    const gtsam::NonlinearFactorGraph& pose3Graph,
+    const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
+gtsam::Values computeOrientationsGradient(
+    const gtsam::NonlinearFactorGraph& pose3Graph,
+    const gtsam::Values& givenGuess);
+gtsam::NonlinearFactorGraph buildPose3graph(
+    const gtsam::NonlinearFactorGraph& graph);
+gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
+                         const gtsam::Values& givenGuess, bool useGradient);
+gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
+}  // namespace initialize_pose3
+
 gtsam::BetweenFactorPose3s parse3DFactors(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
 

From e2cf42773a67abdb386c12de5ef710325a35ff66 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 17:04:31 -0400
Subject: [PATCH 215/281] Switched to struct with static methods as apparently
 global methods in namespaces are not wrapped.

---
 gtsam.h                                  | 33 +++++-----
 gtsam/slam/InitializePose3.cpp           | 32 +++++----
 gtsam/slam/InitializePose3.h             | 83 ++++++++++++------------
 gtsam/slam/tests/testInitializePose3.cpp | 24 +++----
 4 files changed, 88 insertions(+), 84 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 0aca81d02..014907037 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2524,21 +2524,24 @@ class BetweenFactorPose3s
 };
 
 #include <gtsam/slam/InitializePose3.h>
-namespace initialize_pose3 {
-gtsam::Values computeOrientationsChordal(
-    const gtsam::NonlinearFactorGraph& pose3Graph);
-gtsam::Values computeOrientationsGradient(
-    const gtsam::NonlinearFactorGraph& pose3Graph,
-    const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
-gtsam::Values computeOrientationsGradient(
-    const gtsam::NonlinearFactorGraph& pose3Graph,
-    const gtsam::Values& givenGuess);
-gtsam::NonlinearFactorGraph buildPose3graph(
-    const gtsam::NonlinearFactorGraph& graph);
-gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
-                         const gtsam::Values& givenGuess, bool useGradient);
-gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
-}  // namespace initialize_pose3
+class InitializePose3 {
+  static gtsam::Values computeOrientationsChordal(
+      const gtsam::NonlinearFactorGraph& pose3Graph);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess);
+  static gtsam::NonlinearFactorGraph buildPose3graph(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initializeOrientations(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
+                                  const gtsam::Values& givenGuess,
+                                  bool useGradient);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
+};
 
 gtsam::BetweenFactorPose3s parse3DFactors(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index b781f79f0..667ccef0d 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -29,12 +29,11 @@
 using namespace std;
 
 namespace gtsam {
-namespace initialize_pose3 {
 
 static const Key kAnchorKey = symbol('Z', 9999999);
 
 /* ************************************************************************* */
-GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
+GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 
   GaussianFactorGraph linearGraph;
   noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
@@ -67,7 +66,8 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 
 /* ************************************************************************* */
 // Transform VectorValues into valid Rot3
-Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
+Values InitializePose3::normalizeRelaxedRotations(
+    const VectorValues& relaxedRot3) {
   gttic(InitializePose3_computeOrientationsChordal);
 
   Matrix ppm = Z_3x3; // plus plus minus
@@ -94,7 +94,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
 }
 
 /* ************************************************************************* */
-NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
+NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
   gttic(InitializePose3_buildPose3graph);
   NonlinearFactorGraph pose3Graph;
 
@@ -115,7 +115,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
 }
 
 /* ************************************************************************* */
-Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
+Values InitializePose3::computeOrientationsChordal(
+    const NonlinearFactorGraph& pose3Graph) {
   gttic(InitializePose3_computeOrientationsChordal);
 
   // regularize measurements and plug everything in a factor graph
@@ -129,10 +130,9 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
 }
 
 /* ************************************************************************* */
-Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
-                                   const Values& givenGuess,
-                                   const size_t maxIter,
-                                   const bool setRefFrame) {
+Values InitializePose3::computeOrientationsGradient(
+    const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
+    const size_t maxIter, const bool setRefFrame) {
   gttic(InitializePose3_computeOrientationsGradient);
 
   // this works on the inverse rotations, according to Tron&Vidal,2011
@@ -231,7 +231,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
 }
 
 /* ************************************************************************* */
-void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
                          const NonlinearFactorGraph& pose3Graph) {
   size_t factorId = 0;
   for(const auto& factor: pose3Graph) {
@@ -264,7 +264,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
 }
 
 /* ************************************************************************* */
-Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
+Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
   Vector3 logRot = Rot3::Logmap(R1.between(R2));
 
   double th = logRot.norm();
@@ -286,8 +286,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
 }
 
 /* ************************************************************************* */
-Values initializeOrientations(const NonlinearFactorGraph& graph) {
-
+Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) {
   // We "extract" the Pose3 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
   NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
@@ -297,7 +296,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
 }
 
 ///* ************************************************************************* */
-Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
+Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
   gttic(InitializePose3_computePoses);
 
   // put into Values structure
@@ -339,7 +338,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 }
 
 /* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
+Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
                   bool useGradient) {
   gttic(InitializePose3_initialize);
   Values initialValues;
@@ -360,9 +359,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
 }
 
 /* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph) {
+Values InitializePose3::initialize(const NonlinearFactorGraph& graph) {
   return initialize(graph, Values(), false);
 }
 
-} // namespace initialize_pose3
 } // namespace gtsam
diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h
index d9bea7932..ce1b28854 100644
--- a/gtsam/slam/InitializePose3.h
+++ b/gtsam/slam/InitializePose3.h
@@ -31,54 +31,57 @@ namespace gtsam {
 typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
 typedef std::map<Key, Rot3> KeyRotMap;
 
-namespace initialize_pose3 {
+struct GTSAM_EXPORT InitializePose3 {
+  static GaussianFactorGraph buildLinearOrientationGraph(
+      const NonlinearFactorGraph& g);
 
-GTSAM_EXPORT GaussianFactorGraph
-buildLinearOrientationGraph(const NonlinearFactorGraph& g);
+  static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
 
-GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
+  /**
+   * Return the orientations of a graph including only BetweenFactors<Pose3>
+   */
+  static Values computeOrientationsChordal(
+      const NonlinearFactorGraph& pose3Graph);
 
-/**
- * Return the orientations of a graph including only BetweenFactors<Pose3>
- */
-GTSAM_EXPORT Values
-computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
+  /**
+   * Return the orientations of a graph including only BetweenFactors<Pose3>
+   */
+  static Values computeOrientationsGradient(
+      const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
+      size_t maxIter = 10000, const bool setRefFrame = true);
 
-/**
- * Return the orientations of a graph including only BetweenFactors<Pose3>
- */
-GTSAM_EXPORT Values computeOrientationsGradient(
-    const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
-    size_t maxIter = 10000, const bool setRefFrame = true);
+  static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
+                                  KeyRotMap& factorId2RotMap,
+                                  const NonlinearFactorGraph& pose3Graph);
 
-GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
-                                      KeyRotMap& factorId2RotMap,
-                                      const NonlinearFactorGraph& pose3Graph);
+  static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
+                              const double b);
 
-GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2,
-                                  const double a, const double b);
+  /**
+   * Select the subgraph of betweenFactors and transforms priors into between
+   * wrt a fictitious node
+   */
+  static NonlinearFactorGraph buildPose3graph(
+      const NonlinearFactorGraph& graph);
 
-/**
- * Select the subgraph of betweenFactors and transforms priors into between wrt
- * a fictitious node
- */
-GTSAM_EXPORT NonlinearFactorGraph
-buildPose3graph(const NonlinearFactorGraph& graph);
+  static Values computePoses(NonlinearFactorGraph& pose3graph,
+                             Values& initialRot);
 
-GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,
-                                 Values& initialRot);
+  /**
+   * "extract" the Pose3 subgraph of the original graph, get orientations from
+   * relative orientation measurements using chordal method.
+   */
+  static Values initializeOrientations(const NonlinearFactorGraph& graph);
 
-/**
- * "extract" the Pose3 subgraph of the original graph, get orientations from
- * relative orientation measurements (using either gradient or chordal method),
- * and finish up with 1 GN iteration on full poses.
- */
-GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
-                               const Values& givenGuess,
-                               bool useGradient = false);
+  /**
+   * "extract" the Pose3 subgraph of the original graph, get orientations from
+   * relative orientation measurements (using either gradient or chordal
+   * method), and finish up with 1 GN iteration on full poses.
+   */
+  static Values initialize(const NonlinearFactorGraph& graph,
+                           const Values& givenGuess, bool useGradient = false);
 
-/// Calls initialize above using Chordal method.
-GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
-
-}  // namespace initialize_pose3
+  /// Calls initialize above using Chordal method.
+  static Values initialize(const NonlinearFactorGraph& graph);
+};
 }  // end of namespace gtsam
diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp
index ea474c480..e6f08286a 100644
--- a/gtsam/slam/tests/testInitializePose3.cpp
+++ b/gtsam/slam/tests/testInitializePose3.cpp
@@ -74,15 +74,15 @@ NonlinearFactorGraph graph() {
 
 /* *************************************************************************** */
 TEST( InitializePose3, buildPose3graph ) {
-  NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
   // pose3graph.print("");
 }
 
 /* *************************************************************************** */
 TEST( InitializePose3, orientations ) {
-  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
 
-  Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph);
+  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
@@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) {
 
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
-  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
 
   KeyVectorMap adjEdgesMap;
   KeyRotMap factorId2RotMap;
 
-  initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
+  InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
 
   EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
   EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
@@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) {
   double a = 6.010534238540223;
   double b = 1.0;
 
-  Vector actual = initialize_pose3::gradientTron(R1, R2, a, b);
+  Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
   Vector expected = Vector3::Zero();
   expected(2) = 1.962658662803917;
 
@@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) {
 
 /* *************************************************************************** */
 TEST( InitializePose3, iterationGradient ) {
-  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
 
   // Wrong initial guess - initialization should fix the rotations
   Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
@@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) {
 
   size_t maxIter = 1; // test gradient at the first iteration
   bool setRefFrame = false;
-  Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
+  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
 
   Matrix M0 = (Matrix(3,3) <<  0.999435813876064,  -0.033571481675497,   0.001004768630281,
       0.033572116359134,   0.999436104312325,  -0.000621610948719,
@@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) {
 
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradient ) {
-  NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
 
   // Wrong initial guess - initialization should fix the rotations
   Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
@@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) {
   givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
   // do 10 gradient iterations
   bool setRefFrame = false;
-  Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
+  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
 
   //  const Key keyAnchor = symbol('Z', 9999999);
   //  givenPoses.insert(keyAnchor,simple::pose0);
@@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) {
   givenPoses.insert(x2,simple::pose2);
   givenPoses.insert(x3,simple::pose3);
 
-  Values initial = initialize_pose3::initialize(simple::graph(), givenPoses);
+  Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal(givenPoses, initial, 1e-6));
@@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses )
   noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
   inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
 
-  Values initial = initialize_pose3::initialize(*inputGraph);
+  Values initial = InitializePose3::initialize(*inputGraph);
   EXPECT(assert_equal(*expectedValues,initial,0.1));  // TODO(frank): very loose !!
 }
 

From a89e422a8ac4c49a5b476f10c65da7e7b8ab7b29 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Mar 2019 17:04:48 -0400
Subject: [PATCH 216/281] Added python example and test

---
 ...Pose3SLAMExample_initializePose3Chordal.py | 35 ++++++++
 cython/gtsam/tests/test_initialize_pose3.py   | 88 +++++++++++++++++++
 2 files changed, 123 insertions(+)
 create mode 100644 cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
 create mode 100644 cython/gtsam/tests/test_initialize_pose3.py

diff --git a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
new file mode 100644
index 000000000..02c696905
--- /dev/null
+++ b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py
@@ -0,0 +1,35 @@
+"""
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Initialize PoseSLAM with Chordal init
+Author: Luca Carlone, Frank Dellaert (python port)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+# Read graph from file
+g2oFile = gtsam.findExampleDataFile("pose3example.txt")
+
+is3D = True
+graph, initial = gtsam.readG2o(g2oFile, is3D)
+
+# Add prior on the first key. TODO: assumes first key ios z
+priorModel = gtsam.noiseModel_Diagonal.Variances(
+    np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
+firstKey = initial.keys().at(0)
+graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
+
+# Initializing Pose3 - chordal relaxation"
+initialization = gtsam.InitializePose3.initialize(graph)
+
+print(initialization)
diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py
new file mode 100644
index 000000000..ee9195c48
--- /dev/null
+++ b/cython/gtsam/tests/test_initialize_pose3.py
@@ -0,0 +1,88 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for 3D SLAM initialization, using rotation relaxation.
+Author: Luca Carlone and Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
+
+x0, x1, x2, x3 = 0, 1, 2, 3
+
+
+class TestValues(unittest.TestCase):
+
+    def setUp(self):
+
+        model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+
+        # We consider a small graph:
+        #                            symbolic FG
+        #               x2               0  1
+        #             / | \              1  2
+        #            /  |  \             2  3
+        #          x3   |   x1           2  0
+        #           \   |   /            0  3
+        #            \  |  /
+        #               x0
+        #
+        p0 = Point3(0, 0, 0)
+        self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
+        p1 = Point3(1, 2, 0)
+        self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
+        p2 = Point3(0, 2, 0)
+        self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
+        p3 = Point3(-1, 1, 0)
+        self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
+
+        pose0 = Pose3(self.R0, p0)
+        pose1 = Pose3(self.R1, p1)
+        pose2 = Pose3(self.R2, p2)
+        pose3 = Pose3(self.R3, p3)
+
+        g = NonlinearFactorGraph()
+        g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
+        g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
+        g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
+        g.add(gtsam.PriorFactorPose3(x0, pose0, model))
+        self.graph = g
+
+    def test_buildPose3graph(self):
+        pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+    def test_orientations(self):
+        pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+        initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
+
+        # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+        self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6))
+        self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6))
+        self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6))
+        self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6))
+
+    def test_initializePoses(self):
+        g2oFile = gtsam.findExampleDataFile("pose3example-grid")
+        is3D = True
+        inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
+        priorModel = gtsam.noiseModel_Unit.Create(6)
+        inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
+
+        initial = gtsam.InitializePose3.initialize(inputGraph)
+        # TODO(frank): very loose !!
+        self.assertTrue(initial.equals(expectedValues, 0.1))
+
+
+if __name__ == "__main__":
+    unittest.main()

From 1e944fb86a5420d7e9d60185a0d28a7725654998 Mon Sep 17 00:00:00 2001
From: lucacarlone <lcarlone@mit.edu>
Date: Tue, 19 Mar 2019 18:44:09 -0400
Subject: [PATCH 217/281] now initialization is aware of rotation noise model

---
 gtsam/slam/InitializePose3.cpp           | 15 ++++++++++-----
 gtsam/slam/tests/testInitializePose3.cpp | 24 ++++++++++++++++++++++++
 2 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 667ccef0d..2f247811d 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -36,16 +36,21 @@ static const Key kAnchorKey = symbol('Z', 9999999);
 GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 
   GaussianFactorGraph linearGraph;
-  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
 
   for(const auto& factor: g) {
     Matrix3 Rij;
+    double rotationPrecision = 1.0;
 
     auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-    if (pose3Between)
+    if (pose3Between){
       Rij = pose3Between->measured().rotation().matrix();
-    else
+      Vector precisions = Vector::Zero(6);
+      precisions[0] = 1.0; // vector of all zeros except first entry equal to 1
+      pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable
+      rotationPrecision = precisions[0]; // rotations first
+    }else{
       cout << "Error in buildLinearOrientationGraph" << endl;
+    }
 
     const auto& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
@@ -53,14 +58,14 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear
     M9.block(0,0,3,3) = Rij;
     M9.block(3,3,3,3) = Rij;
     M9.block(6,6,3,3) = Rij;
-    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model);
+    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
   }
   // prior on the anchor orientation
   linearGraph.add(
       kAnchorKey, I_9x9,
       (Vector(9) << 1.0, 0.0, 0.0, /*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0)
           .finished(),
-      model);
+          noiseModel::Isotropic::Precision(9, 1));
   return linearGraph;
 }
 
diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp
index e6f08286a..6fe8b3d7c 100644
--- a/gtsam/slam/tests/testInitializePose3.cpp
+++ b/gtsam/slam/tests/testInitializePose3.cpp
@@ -70,6 +70,17 @@ NonlinearFactorGraph graph() {
   g.add(PriorFactor<Pose3>(x0, pose0, model));
   return g;
 }
+
+NonlinearFactorGraph graph2() {
+  NonlinearFactorGraph g;
+  g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
+  g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
+  g.add(PriorFactor<Pose3>(x0, pose0, model));
+  return g;
+}
 }
 
 /* *************************************************************************** */
@@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) {
   EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
 }
 
+/* *************************************************************************** */
+TEST( InitializePose3, orientationsPrecisions ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2());
+
+  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
+  EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
+  EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
+  EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
+}
+
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
   NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());

From 214b1208b1150ad43084cebec3234e6d08207bc7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 20 Mar 2019 15:30:49 -0400
Subject: [PATCH 218/281] changed MATLAB README to markdown file for better
 rendering

---
 matlab/README-gtsam-toolbox.md  | 57 +++++++++++++++++++++++
 matlab/README-gtsam-toolbox.txt | 82 ---------------------------------
 2 files changed, 57 insertions(+), 82 deletions(-)
 create mode 100644 matlab/README-gtsam-toolbox.md
 delete mode 100644 matlab/README-gtsam-toolbox.txt

diff --git a/matlab/README-gtsam-toolbox.md b/matlab/README-gtsam-toolbox.md
new file mode 100644
index 000000000..f23cf1da1
--- /dev/null
+++ b/matlab/README-gtsam-toolbox.md
@@ -0,0 +1,57 @@
+# GTSAM - Georgia Tech Smoothing and Mapping Library
+
+## MATLAB wrapper
+
+http://borg.cc.gatech.edu/projects/gtsam
+
+This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake.
+
+The interface to the toolbox is generated automatically by the wrap
+tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console.
+
+## Newer Ubuntu versions unsupported by MATLAB (later than 10.04)
+
+If you have a newer Ubuntu system, you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library.  Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:
+
+	/usr/local/MATLAB/[version]/sys/os/[system]/ 
+	/usr/local/MATLAB/[version]/bin/[system]/
+
+
+## Adding the toolbox to your MATLAB path
+
+To get started, first add the `toolbox` (or `gtsam_toolbox`) folder to your MATLAB path - in the MATLAB file browser, right-click on the folder and click 'Add to path -> This folder' (**do not add the subfolders to your path**).
+
+## Trying out the examples
+
+The examples are located in the 'gtsam_examples' subfolder.  You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'.  Example:
+
+```matlab
+>> cd /Users/yourname/toolbox  % Change to wherever you installed the toolbox
+>> cd gtsam_examples           % Change to the examples directory
+>> gtsamExamples               % Run the GTSAM examples GUI
+```
+
+## Running the unit tests
+
+The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory.  Example:
+
+```matlab
+>> cd /Users/yourname/toolbox  % Change to wherever you installed the toolbox
+>> cd gtsam_tests              % Change to the examples directory
+>> test_gtsam                  % Run the unit tests
+Starting: testJacobianFactor
+Starting: testKalmanFilter
+Starting: testLocalizationExample
+Starting: testOdometryExample
+Starting: testPlanarSLAMExample
+Starting: testPose2SLAMExample
+Starting: testPose3SLAMExample
+Starting: testSFMExample
+Starting: testStereoVOExample
+Starting: testVisualISAMExample
+Tests complete!
+```
+
+## Writing your own code
+
+Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts!  Please see the manual to get started.
diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt
deleted file mode 100644
index a1691be32..000000000
--- a/matlab/README-gtsam-toolbox.txt
+++ /dev/null
@@ -1,82 +0,0 @@
-================================================================================
-GTSAM - Georgia Tech Smoothing and Mapping Library
-
-MATLAB wrapper
-
-http://borg.cc.gatech.edu/projects/gtsam
-================================================================================
-
-This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++
-library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake.
-
-The interface to the toolbox is generated automatically by the wrap
-tool which directly parses C++ header files. The tool generates matlab
-proxy objects together with all the native functions for wrapping and
-unwrapping scalar and non scalar types and objects. The interface
-generated by the wrap tool also redirects the standard output stream
-(cout) to matlab's console.
-
-----------------------------------------
-Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04)
-----------------------------------------
-
-If you have a newer Ubuntu system, you must make a small modification to your
-MATLAB installation, due to MATLAB being distributed with an old version of
-the C++ standard library.  Delete or rename all files starting with
-'libstdc++' in your MATLAB installation directory, in paths:
-
-	/usr/local/MATLAB/[version]/sys/os/[system]/ 
-	/usr/local/MATLAB/[version]/bin/[system]/
-
-
-----------------------------------------
-Adding the toolbox to your MATLAB path
-----------------------------------------
-
-To get started, first add the 'toolbox' folder to your MATLAB path - in the
-MATLAB file browser, right-click on the folder and click 'Add to path -> This
-folder' (do not add the subfolders to your path).
-
-
-----------------------------------------
-Trying out the examples
-----------------------------------------
-
-The examples are located in the 'gtsam_examples' subfolder.  You may either
-run them individually at the MATLAB command line, or open the GTSAM example
-GUI by running 'gtsamExamples'.  Example:
-
->> cd /Users/yourname/toolbox  % Change to wherever you installed the toolbox
->> cd gtsam_examples           % Change to the examples directory
->> gtsamExamples               % Run the GTSAM examples GUI
-
-
-----------------------------------------
-Running the unit tests
-----------------------------------------
-
-The GTSAM MATLAB toolbox also has a small set of unit tests located in the
-gtsam_tests directory.  Example:
-
->> cd /Users/yourname/toolbox  % Change to wherever you installed the toolbox
->> cd gtsam_tests              % Change to the examples directory
->> test_gtsam                  % Run the unit tests
-Starting: testJacobianFactor
-Starting: testKalmanFilter
-Starting: testLocalizationExample
-Starting: testOdometryExample
-Starting: testPlanarSLAMExample
-Starting: testPose2SLAMExample
-Starting: testPose3SLAMExample
-Starting: testSFMExample
-Starting: testStereoVOExample
-Starting: testVisualISAMExample
-Tests complete!
-
-
-----------------------------------------
-Writing your own code
-----------------------------------------
-
-Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you
-understand a few basic concepts!  Please see the manual to get started.

From 366bf54f45dc10b2e9bc287b71f23b2b8aa459b5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 20 Mar 2019 15:35:49 -0400
Subject: [PATCH 219/281] added instructions for performing linker setup to
 find libgtsam.so correctly

---
 matlab/README-gtsam-toolbox.md | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/matlab/README-gtsam-toolbox.md b/matlab/README-gtsam-toolbox.md
index f23cf1da1..7e16782d4 100644
--- a/matlab/README-gtsam-toolbox.md
+++ b/matlab/README-gtsam-toolbox.md
@@ -21,6 +21,38 @@ If you have a newer Ubuntu system, you must make a small modification to your MA
 
 To get started, first add the `toolbox` (or `gtsam_toolbox`) folder to your MATLAB path - in the MATLAB file browser, right-click on the folder and click 'Add to path -> This folder' (**do not add the subfolders to your path**).
 
+## Final setup on Linux
+
+MATLAB needs to know where the GTSAM shared object file (`libgtsam.so.4`) is so that it can link to it correctly.
+
+### System-wide
+
+If you installed GTSAM system-wide (e.g. with `sudo make install`), then simply run `sudo ldconfig`.
+
+### Custom Install
+
+If you have a custom install location, denoted by `<install-path>`, you need to update your `LD_LIBRARY_PATH` environment variable.
+
+```sh
+export LD_LIBRARY_PATH=<install-path>/gtsam:$LD_LIBRARY_PATH
+```
+
+### Linker issues
+
+If you compile the Matlab toolbox and everything compiles smoothly, but when you run any Matlab script, you get following error messages in Matlab
+```
+Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64':
+Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64'
+Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64'
+...
+```
+run following shell line
+```sh
+export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libprotobuf.so.9
+```
+before you run Matlab, or write this line in your `$HOME/.bashrc` so you don't have to type everytime before start Matlab. This mainly happens if you have GCC >= 5 and newer version Matlab like R2017a.
+
+
 ## Trying out the examples
 
 The examples are located in the 'gtsam_examples' subfolder.  You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'.  Example:

From 90e6eb95cf0372d95d166d0384bd77650cc36e43 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 20 Mar 2019 17:35:31 -0400
Subject: [PATCH 220/281] Added GtsamTestCase

---
 cython/gtsam/utils/test_case.py | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)
 create mode 100644 cython/gtsam/utils/test_case.py

diff --git a/cython/gtsam/utils/test_case.py b/cython/gtsam/utils/test_case.py
new file mode 100644
index 000000000..7df1e6ee9
--- /dev/null
+++ b/cython/gtsam/utils/test_case.py
@@ -0,0 +1,27 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+TestCase class with GTSAM assert utils.
+Author: Frank Dellaert
+"""
+import unittest
+
+
+class GtsamTestCase(unittest.TestCase):
+    """TestCase class with GTSAM assert utils."""
+
+    def gtsamAssertEquals(self, actual, expected, tol=1e-9):
+        """ AssertEqual function that prints out actual and expected if not equal.
+            Usage:
+                self.gtsamAssertEqual(actual,expected)
+            Keyword Arguments:
+                tol {float} -- tolerance passed to 'equals', default 1e-9
+        """
+        equal = actual.equals(expected, tol)
+        if not equal:
+            raise self.failureException(
+                "Values are not equal:\n{}!={}".format(actual, expected))

From c442df38664a719ed7c5a482a82e8bdb3ee1acf8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 20 Mar 2019 17:35:53 -0400
Subject: [PATCH 221/281] Modernized all tests

---
 cython/gtsam/tests/test_Cal3Unified.py        | 26 +++++++++++---
 cython/gtsam/tests/test_JacobianFactor.py     | 22 +++++++++---
 cython/gtsam/tests/test_KalmanFilter.py       | 18 ++++++++--
 .../gtsam/tests/test_LocalizationExample.py   | 20 +++++++++--
 cython/gtsam/tests/test_NonlinearOptimizer.py |  3 +-
 cython/gtsam/tests/test_OdometryExample.py    | 20 +++++++++--
 cython/gtsam/tests/test_PlanarSLAMExample.py  | 22 +++++++++---
 cython/gtsam/tests/test_Pose2.py              | 15 ++++++--
 cython/gtsam/tests/test_Pose2SLAMExample.py   | 20 +++++++++--
 cython/gtsam/tests/test_Pose3.py              | 19 +++++++---
 cython/gtsam/tests/test_Pose3SLAMExample.py   | 28 +++++++++++----
 cython/gtsam/tests/test_PriorFactor.py        | 18 ++++++++--
 cython/gtsam/tests/test_SFMExample.py         | 23 +++++++++---
 cython/gtsam/tests/test_Scenario.py           | 13 +++----
 cython/gtsam/tests/test_SimpleCamera.py       | 25 +++++++++----
 cython/gtsam/tests/test_StereoVOExample.py    | 23 +++++++++---
 cython/gtsam/tests/test_Values.py             | 36 ++++++++++---------
 cython/gtsam/tests/test_VisualISAMExample.py  | 23 +++++++++---
 cython/gtsam/tests/test_dataset.py            |  5 +--
 cython/gtsam/tests/test_initialize_pose3.py   | 13 +++----
 .../tests/test_FixedLagSmootherExample.py     | 17 +++++++--
 21 files changed, 316 insertions(+), 93 deletions(-)

diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py
index 3225d2ff9..fbf5f3565 100644
--- a/cython/gtsam/tests/test_Cal3Unified.py
+++ b/cython/gtsam/tests/test_Cal3Unified.py
@@ -1,9 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Cal3Unified unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
 
-class TestCal3Unified(unittest.TestCase):
+
+class TestCal3Unified(GtsamTestCase):
 
     def test_Cal3Unified(self):
         K = gtsam.Cal3Unified()
@@ -11,12 +24,15 @@ class TestCal3Unified(unittest.TestCase):
         self.assertEqual(K.fx(), 1.)
 
     def test_retract(self):
-        expected  = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
-        K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
+        expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
+                                     1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
+        K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240,
+                              1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
         d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F')
         actual = K.retract(d)
-        self.assertTrue(actual.equals(expected, 1e-9))
+        self.gtsamAssertEquals(actual, expected)
         np.testing.assert_allclose(d, K.localCoordinates(actual))
 
+
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py
index bf63c839b..04433492b 100644
--- a/cython/gtsam/tests/test_JacobianFactor.py
+++ b/cython/gtsam/tests/test_JacobianFactor.py
@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+JacobianFactor unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestJacobianFactor(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestJacobianFactor(GtsamTestCase):
 
     def test_eliminate(self):
         # Recommended way to specify a matrix (see cython/README)
@@ -54,7 +68,7 @@ class TestJacobianFactor(unittest.TestCase):
         expectedCG = gtsam.GaussianConditional(
             x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
         # check if the result matches
-        self.assertTrue(actualCG.equals(expectedCG, 1e-4))
+        self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
 
         # the expected linear factor
         Bl1 = np.array([[4.47214, 0.00],
@@ -72,7 +86,7 @@ class TestJacobianFactor(unittest.TestCase):
         expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
 
         # check if the result matches the combined (reduced) factor
-        self.assertTrue(lf.equals(expectedLF, 1e-4))
+        self.gtsamAssertEquals(lf, expectedLF, 1e-4)
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/cython/gtsam/tests/test_KalmanFilter.py
index 56f9e2573..94c41df72 100644
--- a/cython/gtsam/tests/test_KalmanFilter.py
+++ b/cython/gtsam/tests/test_KalmanFilter.py
@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+KalmanFilter unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestKalmanFilter(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestKalmanFilter(GtsamTestCase):
 
     def test_KalmanFilter(self):
         F = np.eye(2)
diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/cython/gtsam/tests/test_LocalizationExample.py
index c373f162c..6ce65f087 100644
--- a/cython/gtsam/tests/test_LocalizationExample.py
+++ b/cython/gtsam/tests/test_LocalizationExample.py
@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Localization unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestLocalizationExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestLocalizationExample(GtsamTestCase):
 
     def test_LocalizationExample(self):
         # Create the graph (defined in pose2SLAM.h, derived from
@@ -43,7 +57,7 @@ class TestLocalizationExample(unittest.TestCase):
         P = [None] * result.size()
         for i in range(0, result.size()):
             pose_i = result.atPose2(i)
-            self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4))
+            self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4)
             P[i] = marginals.marginalCovariance(i)
 
 if __name__ == "__main__":
diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py
index 0d0318f40..efefb218a 100644
--- a/cython/gtsam/tests/test_NonlinearOptimizer.py
+++ b/cython/gtsam/tests/test_NonlinearOptimizer.py
@@ -19,12 +19,13 @@ from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
                    GaussNewtonParams, LevenbergMarquardtOptimizer,
                    LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
                    Point2, PriorFactorPoint2, Values)
+from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
 KEY2 = 2
 
 
-class TestScenario(unittest.TestCase):
+class TestScenario(GtsamTestCase):
     def test_optimize(self):
         """Do trivial test with three optimizer variants."""
         fg = NonlinearFactorGraph()
diff --git a/cython/gtsam/tests/test_OdometryExample.py b/cython/gtsam/tests/test_OdometryExample.py
index 1100e8334..c8ea95588 100644
--- a/cython/gtsam/tests/test_OdometryExample.py
+++ b/cython/gtsam/tests/test_OdometryExample.py
@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Odometry unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestOdometryExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestOdometryExample(GtsamTestCase):
 
     def test_OdometryExample(self):
         # Create the graph (defined in pose2SLAM.h, derived from
@@ -39,7 +53,7 @@ class TestOdometryExample(unittest.TestCase):
 
         # Check first pose equality
         pose_1 = result.atPose2(1)
-        self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/cython/gtsam/tests/test_PlanarSLAMExample.py
index 046a93f35..ae813d35c 100644
--- a/cython/gtsam/tests/test_PlanarSLAMExample.py
+++ b/cython/gtsam/tests/test_PlanarSLAMExample.py
@@ -1,11 +1,25 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PlanarSLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
 from math import pi
+
 import numpy as np
 
-class TestPose2SLAMExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
 
-    def test_Pose2SLAMExample(self):
+
+class TestPlanarSLAM(GtsamTestCase):
+
+    def test_PlanarSLAM(self):
         # Assumptions
         #  - All values are axis aligned
         #  - Robot poses are facing along the X axis (horizontal, to the right in images)
@@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase):
         P = marginals.marginalCovariance(1)
 
         pose_1 = result.atPose2(1)
-        self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
 
 
 
diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py
index afd0c5773..9652b594a 100644
--- a/cython/gtsam/tests/test_Pose2.py
+++ b/cython/gtsam/tests/test_Pose2.py
@@ -1,12 +1,23 @@
-"""Pose2 unit tests."""
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose2 unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
 
 import numpy as np
 
+import gtsam
 from gtsam import Pose2
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestPose2(unittest.TestCase):
+class TestPose2(GtsamTestCase):
     """Test selected Pose2 methods."""
 
     def test_adjoint(self):
diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/cython/gtsam/tests/test_Pose2SLAMExample.py
index bcaa7be4f..a79b6b18c 100644
--- a/cython/gtsam/tests/test_Pose2SLAMExample.py
+++ b/cython/gtsam/tests/test_Pose2SLAMExample.py
@@ -1,9 +1,23 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose2SLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
 from math import pi
+
 import numpy as np
 
-class TestPose2SLAMExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPose2SLAMExample(GtsamTestCase):
 
     def test_Pose2SLAMExample(self):
         # Assumptions
@@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase):
         P = marginals.marginalCovariance(1)
 
         pose_1 = result.atPose2(1)
-        self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py
index b878a9551..3eb4067c9 100644
--- a/cython/gtsam/tests/test_Pose3.py
+++ b/cython/gtsam/tests/test_Pose3.py
@@ -1,13 +1,24 @@
-"""Pose3 unit tests."""
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose3 unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import math
 import unittest
 
 import numpy as np
 
+import gtsam
 from gtsam import Point3, Pose3, Rot3
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestPose3(unittest.TestCase):
+class TestPose3(GtsamTestCase):
     """Test selected Pose3 methods."""
 
     def test_between(self):
@@ -16,14 +27,14 @@ class TestPose3(unittest.TestCase):
         T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
         expected = T2.inverse().compose(T3)
         actual = T2.between(T3)
-        self.assertTrue(actual.equals(expected, 1e-6))
+        self.gtsamAssertEquals(actual, expected, 1e-6)
 
     def test_transform_to(self):
         """Test transform_to method."""
         transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
         actual = transform.transform_to(Point3(3, 2, 10))
         expected = Point3(2, 1, 10)
-        self.assertTrue(actual.equals(expected, 1e-6))
+        self.gtsamAssertEquals(actual, expected, 1e-6)
 
     def test_range(self):
         """Test range method."""
diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/cython/gtsam/tests/test_Pose3SLAMExample.py
index e33db2145..1e9eaac67 100644
--- a/cython/gtsam/tests/test_Pose3SLAMExample.py
+++ b/cython/gtsam/tests/test_Pose3SLAMExample.py
@@ -1,10 +1,24 @@
-import unittest
-import numpy as np
-import gtsam
-from math import pi
-from gtsam.utils.circlePose3 import * 
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
 
-class TestPose3SLAMExample(unittest.TestCase):
+See LICENSE for the license information
+
+PoseSLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+from gtsam.utils.circlePose3 import *
+
+
+class TestPose3SLAMExample(GtsamTestCase):
 
     def test_Pose3SLAMExample(self):
         # Create a hexagon of poses
@@ -40,7 +54,7 @@ class TestPose3SLAMExample(unittest.TestCase):
         result = optimizer.optimizeSafely()
 
         pose_1 = result.atPose3(1)
-        self.assertTrue(pose_1.equals(p1, 1e-4))
+        self.gtsamAssertEquals(pose_1, p1, 1e-4)
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py
index 95ec2ae94..0acf50c2c 100644
--- a/cython/gtsam/tests/test_PriorFactor.py
+++ b/cython/gtsam/tests/test_PriorFactor.py
@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PriorFactor unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestPriorFactor(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPriorFactor(GtsamTestCase):
 
     def test_PriorFactor(self):
         values = gtsam.Values()
diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py
index 606b26a43..e8fa46186 100644
--- a/cython/gtsam/tests/test_SFMExample.py
+++ b/cython/gtsam/tests/test_SFMExample.py
@@ -1,11 +1,24 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+SFM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
-from gtsam import symbol
+
 import numpy as np
+
+import gtsam
 import gtsam.utils.visual_data_generator as generator
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestSFMExample(unittest.TestCase):
+class TestSFMExample(GtsamTestCase):
 
     def test_SFMExample(self):
         options = generator.Options()
@@ -59,11 +72,11 @@ class TestSFMExample(unittest.TestCase):
         # Check optimized results, should be equal to ground truth
         for i in range(len(truth.cameras)):
             pose_i = result.atPose3(symbol(ord('x'), i))
-            self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
+            self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
         for j in range(len(truth.points)):
             point_j = result.atPoint3(symbol(ord('p'), j))
-            self.assertTrue(point_j.equals(truth.points[j], 1e-5))
+            self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py
index d68566b25..09601fba5 100644
--- a/cython/gtsam/tests/test_Scenario.py
+++ b/cython/gtsam/tests/test_Scenario.py
@@ -5,11 +5,9 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-Unit tests for IMU testing scenarios.
+Scenario unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
-# pylint: disable=invalid-name, E1101
-
 from __future__ import print_function
 
 import math
@@ -18,9 +16,12 @@ import unittest
 import numpy as np
 
 import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+# pylint: disable=invalid-name, E1101
 
 
-class TestScenario(unittest.TestCase):
+class TestScenario(GtsamTestCase):
     def setUp(self):
         pass
 
@@ -44,8 +45,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.assertTrue(gtsam.Point3(
-            0, 0, 2.0 * R).equals(T30.translation(), 1e-9))
+        self.gtsamAssertEquals(gtsam.Point3(
+            0, 0, 2.0 * R), T30.translation(), 1e-9)
 
 
 if __name__ == '__main__':
diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py
index 7924a9b1c..efdfec561 100644
--- a/cython/gtsam/tests/test_SimpleCamera.py
+++ b/cython/gtsam/tests/test_SimpleCamera.py
@@ -1,18 +1,31 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+SimpleCamera unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import math
-import numpy as np
 import unittest
 
-from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera
+import numpy as np
+
+import gtsam
+from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
+from gtsam.utils.test_case import GtsamTestCase
 
 K = Cal3_S2(625, 625, 0, 0, 0)
 
-class TestSimpleCamera(unittest.TestCase):
+class TestSimpleCamera(GtsamTestCase):
 
     def test_constructor(self):
         pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
         camera = SimpleCamera(pose1, K)
-        self.assertTrue(camera.calibration().equals(K, 1e-9))
-        self.assertTrue(camera.pose().equals(pose1, 1e-9))
+        self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
+        self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
 
     def test_level2(self):
         # Create a level camera, looking in Y-direction
@@ -25,7 +38,7 @@ class TestSimpleCamera(unittest.TestCase):
         z = Point3(0,1,0)
         wRc = Rot3(x,y,z)
         expected = Pose3(wRc,Point3(0.4,0.3,0.1))
-        self.assertTrue(camera.pose().equals(expected, 1e-9))
+        self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
 
 
 if __name__ == "__main__":
diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py
index dacd4a116..3f5f57522 100644
--- a/cython/gtsam/tests/test_StereoVOExample.py
+++ b/cython/gtsam/tests/test_StereoVOExample.py
@@ -1,10 +1,23 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Stereo VO unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
-from gtsam import symbol
+
 import numpy as np
 
+import gtsam
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
 
-class TestStereoVOExample(unittest.TestCase):
+
+class TestStereoVOExample(GtsamTestCase):
 
     def test_StereoVOExample(self):
         ## Assumptions
@@ -60,10 +73,10 @@ class TestStereoVOExample(unittest.TestCase):
 
         ## check equality for the first pose and point
         pose_x1 = result.atPose3(x1)
-        self.assertTrue(pose_x1.equals(first_pose,1e-4))
+        self.gtsamAssertEquals(pose_x1, first_pose,1e-4)
 
         point_l1 = result.atPoint3(l1)
-        self.assertTrue(point_l1.equals(expected_l1,1e-4))
+        self.gtsamAssertEquals(point_l1, expected_l1,1e-4)
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py
index 0bb1e0806..20634a21c 100644
--- a/cython/gtsam/tests/test_Values.py
+++ b/cython/gtsam/tests/test_Values.py
@@ -5,7 +5,7 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-Unit tests for IMU testing scenarios.
+Values unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 # pylint: disable=invalid-name, E1101, E0611
@@ -13,12 +13,14 @@ import unittest
 
 import numpy as np
 
+import gtsam
 from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
                    Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
                    imuBias_ConstantBias)
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestValues(unittest.TestCase):
+class TestValues(GtsamTestCase):
 
     def test_values(self):
         values = Values()
@@ -58,26 +60,26 @@ class TestValues(unittest.TestCase):
         mat2 = np.array([[1, 2, ], [3, 5]])
         values.insert(13, mat2)
 
-        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))
-        self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
-        self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
-        self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
-        self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
-        self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
-        self.assertTrue(values.atimuBias_ConstantBias(
-            10).equals(imuBias_ConstantBias(), tol))
+        self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol)
+        self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol)
+        self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol)
+        self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol)
+        self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol)
+        self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol)
+        self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol)
+        self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
+        self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
+        self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
+        self.gtsamAssertEquals(values.atimuBias_ConstantBias(
+            10), imuBias_ConstantBias(), tol)
 
         # special cases for Vector and Matrix:
         actualVector = values.atVector(11)
-        self.assertTrue(np.allclose(vec, actualVector, tol))
+        np.testing.assert_allclose(vec, actualVector, tol)
         actualMatrix = values.atMatrix(12)
-        self.assertTrue(np.allclose(mat, actualMatrix, tol))
+        np.testing.assert_allclose(mat, actualMatrix, tol)
         actualMatrix2 = values.atMatrix(13)
-        self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
+        np.testing.assert_allclose(mat2, actualMatrix2, tol)
 
 
 if __name__ == "__main__":
diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py
index 39bfa6eb4..99d7e6160 100644
--- a/cython/gtsam/tests/test_VisualISAMExample.py
+++ b/cython/gtsam/tests/test_VisualISAMExample.py
@@ -1,10 +1,25 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+visual_isam unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
+
 import numpy as np
-from gtsam import symbol
+
+import gtsam
 import gtsam.utils.visual_data_generator as generator
 import gtsam.utils.visual_isam as visual_isam
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
 
-class TestVisualISAMExample(unittest.TestCase):
+
+class TestVisualISAMExample(GtsamTestCase):
 
     def test_VisualISAMExample(self):
         # Data Options
@@ -32,11 +47,11 @@ class TestVisualISAMExample(unittest.TestCase):
 
         for i in range(len(truth.cameras)):
             pose_i = result.atPose3(symbol(ord('x'), i))
-            self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
+            self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
         for j in range(len(truth.points)):
             point_j = result.atPoint3(symbol(ord('l'), j))
-            self.assertTrue(point_j.equals(truth.points[j], 1e-5))
+            self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py
index e561be1a8..60fb9450d 100644
--- a/cython/gtsam/tests/test_dataset.py
+++ b/cython/gtsam/tests/test_dataset.py
@@ -6,7 +6,7 @@ All Rights Reserved
 See LICENSE for the license information
 
 Unit tests for testing dataset access.
-Author: Frank Dellaert
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 # pylint: disable=invalid-name, no-name-in-module, no-member
 
@@ -16,9 +16,10 @@ import unittest
 
 import gtsam
 from gtsam import BetweenFactorPose3, BetweenFactorPose3s
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestDataset(unittest.TestCase):
+class TestDataset(GtsamTestCase):
     """Tests for datasets.h wrapper."""
 
     def setUp(self):
diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py
index ee9195c48..3aa7e3470 100644
--- a/cython/gtsam/tests/test_initialize_pose3.py
+++ b/cython/gtsam/tests/test_initialize_pose3.py
@@ -15,11 +15,12 @@ import numpy as np
 
 import gtsam
 from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
+from gtsam.utils.test_case import GtsamTestCase
 
 x0, x1, x2, x3 = 0, 1, 2, 3
 
 
-class TestValues(unittest.TestCase):
+class TestValues(GtsamTestCase):
 
     def setUp(self):
 
@@ -67,10 +68,10 @@ class TestValues(unittest.TestCase):
         initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
 
         # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
-        self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6))
-        self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6))
-        self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6))
-        self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6))
+        self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6)
 
     def test_initializePoses(self):
         g2oFile = gtsam.findExampleDataFile("pose3example-grid")
@@ -81,7 +82,7 @@ class TestValues(unittest.TestCase):
 
         initial = gtsam.InitializePose3.initialize(inputGraph)
         # TODO(frank): very loose !!
-        self.assertTrue(initial.equals(expectedValues, 0.1))
+        self.gtsamAssertEquals(initial, expectedValues, 0.1)
 
 
 if __name__ == "__main__":
diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
index 90c4e436b..8d3af311f 100644
--- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
+++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py
@@ -1,7 +1,20 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Cal3Unified unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
+
+import numpy as np
+
 import gtsam
 import gtsam_unstable
-import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
 
 
 def _timestamp_key_value(key, value):
@@ -10,7 +23,7 @@ def _timestamp_key_value(key, value):
     )
 
 
-class TestFixedLagSmootherExample(unittest.TestCase):
+class TestFixedLagSmootherExample(GtsamTestCase):
     '''
     Tests the fixed lag smoother wrapper
     '''

From 79880d6a7ced9a838d6a9387a2f027c51c66af61 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 20 Mar 2019 17:36:11 -0400
Subject: [PATCH 222/281] Use GtsamTestCase in example

---
 cython/gtsam/examples/PlanarManipulatorExample.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py
index af21e6ca5..73959b6c5 100644
--- a/cython/gtsam/examples/PlanarManipulatorExample.py
+++ b/cython/gtsam/examples/PlanarManipulatorExample.py
@@ -24,6 +24,7 @@ from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 import gtsam
 import gtsam.utils.plot as gtsam_plot
 from gtsam import Pose2
+from gtsam.utils.test_case import GtsamTestCase
 
 
 def vector3(x, y, z):
@@ -202,7 +203,7 @@ Q1 = np.radians(vector3(-30, -45, -90))
 Q2 = np.radians(vector3(-90, 90, 0))
 
 
-class TestPose2SLAMExample(unittest.TestCase):
+class TestPose2SLAMExample(GtsamTestCase):
     """Unit tests for functions used below."""
 
     def setUp(self):

From 927e8a6c2796017d24fec62002c7a4905fe3850b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 20 Mar 2019 17:38:58 -0400
Subject: [PATCH 223/281] removed instruction to add LD_PRELOAD to .bashrc

---
 matlab/README-gtsam-toolbox.md | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/matlab/README-gtsam-toolbox.md b/matlab/README-gtsam-toolbox.md
index 7e16782d4..e232c78c5 100644
--- a/matlab/README-gtsam-toolbox.md
+++ b/matlab/README-gtsam-toolbox.md
@@ -39,7 +39,7 @@ export LD_LIBRARY_PATH=<install-path>/gtsam:$LD_LIBRARY_PATH
 
 ### Linker issues
 
-If you compile the Matlab toolbox and everything compiles smoothly, but when you run any Matlab script, you get following error messages in Matlab
+If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get following error messages in MATLAB
 ```
 Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64':
 Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64'
@@ -50,7 +50,9 @@ run following shell line
 ```sh
 export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libprotobuf.so.9
 ```
-before you run Matlab, or write this line in your `$HOME/.bashrc` so you don't have to type everytime before start Matlab. This mainly happens if you have GCC >= 5 and newer version Matlab like R2017a.
+before you run MATLAB. 
+
+This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a.
 
 
 ## Trying out the examples

From 86973559a664b4a9731721754f2916f2db388b48 Mon Sep 17 00:00:00 2001
From: mbway <matthewbroadway@msn.com>
Date: Sat, 23 Mar 2019 11:32:58 +0000
Subject: [PATCH 224/281] addressed comments

---
 THANKS                                     |  1 +
 cython/gtsam/examples/SFMExample.py        | 65 ++++++++++++----------
 cython/gtsam/examples/SimpleRotation.py    |  3 +-
 cython/gtsam/examples/VisualISAMExample.py | 24 +++++---
 4 files changed, 52 insertions(+), 41 deletions(-)

diff --git a/THANKS b/THANKS
index f2b51f61d..7db7daaf3 100644
--- a/THANKS
+++ b/THANKS
@@ -27,6 +27,7 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, li
 * Natesh Srinivasan
 * Alex Trevor
 * Stephen Williams, BossaNova
+* Matthew Broadway
 
 at ETH, Zurich
 
diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py
index 3a64e0cdb..bf46f09d5 100644
--- a/cython/gtsam/examples/SFMExample.py
+++ b/cython/gtsam/examples/SFMExample.py
@@ -8,45 +8,50 @@ See LICENSE for the license information
 
 A structure-from-motion problem on a simulated dataset
 """
+from __future__ import print_function
 
 import gtsam
 import numpy as np
 from gtsam.examples import SFMdata
-from gtsam.gtsam import Values, NonlinearFactorGraph, PriorFactorPose3, SimpleCamera, \
-    GenericProjectionFactorCal3_S2, \
-    PriorFactorPoint3, Pose3, Rot3, Point3, DoglegOptimizer, Cal3_S2
+from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
+                         GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
+                         Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
+                         Rot3, SimpleCamera, Values)
 
 
-# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
-#
-# Each variable in the system (poses and landmarks) must be identified with a unique key.
-# We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
-# Here we will use Symbols
-#
-# In GTSAM, measurement functions are represented as 'factors'. Several common factors
-# have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
-# Here we will use Projection factors to model the camera's landmark observations.
-# Also, we will initialize the robot at some location using a Prior factor.
-#
-# When the factors are created, we will add them to a Factor Graph. As the factors we are using
-# are nonlinear factors, we will need a Nonlinear Factor Graph.
-#
-# Finally, once all of the factors have been added to our factor graph, we will want to
-# solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
-# GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
-# trust-region method known as Powell's Degleg
-#
-# The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
-# nonlinear functions around an initial linearization point, then solve the linear system
-# to update the linearization point. This happens repeatedly until the solver converges
-# to a consistent set of variable values. This requires us to specify an initial guess
-# for each variable, held in a Values container.
-
-def symbol(name, index):
+def symbol(name: str, index: int) -> int:
+    """ helper for creating a symbol without explicitly casting 'name' from str to int """
     return gtsam.symbol(ord(name), index)
 
 
 def main():
+    """
+    Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
+
+    Each variable in the system (poses and landmarks) must be identified with a unique key.
+    We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+    Here we will use Symbols
+
+    In GTSAM, measurement functions are represented as 'factors'. Several common factors
+    have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
+    Here we will use Projection factors to model the camera's landmark observations.
+    Also, we will initialize the robot at some location using a Prior factor.
+
+    When the factors are created, we will add them to a Factor Graph. As the factors we are using
+    are nonlinear factors, we will need a Nonlinear Factor Graph.
+
+    Finally, once all of the factors have been added to our factor graph, we will want to
+    solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
+    GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
+    trust-region method known as Powell's Degleg
+
+    The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
+    nonlinear functions around an initial linearization point, then solve the linear system
+    to update the linearization point. This happens repeatedly until the solver converges
+    to a consistent set of variable values. This requires us to specify an initial guess
+    for each variable, held in a Values container.
+    """
+
     # Define the camera calibration parameters
     K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
 
@@ -63,7 +68,7 @@ def main():
     graph = NonlinearFactorGraph()
 
     # Add a prior on pose x1. This indirectly specifies where the origin is.
-    # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+    # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
     pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
     factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
     graph.push_back(factor)
diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py
index 0c3ac467f..6e9b1320b 100644
--- a/cython/gtsam/examples/SimpleRotation.py
+++ b/cython/gtsam/examples/SimpleRotation.py
@@ -10,9 +10,8 @@ This example will perform a relatively trivial optimization on
 a single variable with a single factor.
 """
 
-import gtsam
-
 import numpy as np
+import gtsam
 
 
 def main():
diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py
index 23b880bec..f4d81eaf7 100644
--- a/cython/gtsam/examples/VisualISAMExample.py
+++ b/cython/gtsam/examples/VisualISAMExample.py
@@ -10,23 +10,29 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase
 This version uses iSAM to solve the problem incrementally
 """
 
-# A structure-from-motion example with landmarks
-# - The landmarks form a 10 meter cube
-# - The robot rotates around the landmarks, always facing towards the cube
-
-import gtsam
-from gtsam.gtsam import Values, Cal3_S2, NonlinearISAM, NonlinearFactorGraph, SimpleCamera, Pose3, Rot3, Point3, \
-    PriorFactorPose3, PriorFactorPoint3, GenericProjectionFactorCal3_S2
+from __future__ import print_function
 
 import numpy as np
+import gtsam
 from gtsam.examples import SFMdata
+from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
+                         NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
+                         PriorFactorPoint3, PriorFactorPose3, Rot3,
+                         SimpleCamera, Values)
 
 
-def symbol(name, index):
+def symbol(name: str, index: int) -> int:
+    """ helper for creating a symbol without explicitly casting 'name' from str to int """
     return gtsam.symbol(ord(name), index)
 
 
 def main():
+    """
+    A structure-from-motion example with landmarks
+    - The landmarks form a 10 meter cube
+    - The robot rotates around the landmarks, always facing towards the cube
+    """
+
     # Define the camera calibration parameters
     K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
 
@@ -67,7 +73,7 @@ def main():
         # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
         # adding it to iSAM.
         if i == 0:
-            # Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+            # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
             pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
             factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
             graph.push_back(factor)

From 53e74a8070dc4aa42460302e9100d1fb0f20cde1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 24 Mar 2019 22:18:42 -0400
Subject: [PATCH 225/281] further updated MATLAB instructions to remove
 inconsistencies and oddities

---
 matlab/README-gtsam-toolbox.md | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/matlab/README-gtsam-toolbox.md b/matlab/README-gtsam-toolbox.md
index e232c78c5..66a02e969 100644
--- a/matlab/README-gtsam-toolbox.md
+++ b/matlab/README-gtsam-toolbox.md
@@ -4,14 +4,14 @@
 
 http://borg.cc.gatech.edu/projects/gtsam
 
-This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake.
+This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake.
 
 The interface to the toolbox is generated automatically by the wrap
 tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console.
 
-## Newer Ubuntu versions unsupported by MATLAB (later than 10.04)
+## Ubuntu
 
-If you have a newer Ubuntu system, you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library.  Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:
+If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library.  Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:
 
 	/usr/local/MATLAB/[version]/sys/os/[system]/ 
 	/usr/local/MATLAB/[version]/bin/[system]/
@@ -48,9 +48,9 @@ Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/g
 ```
 run following shell line
 ```sh
-export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libprotobuf.so.9
+export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6
 ```
-before you run MATLAB. 
+before you run MATLAB from the same shell. 
 
 This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a.
 

From 1365a04b09e21db2a7c00c2db9991e7371351315 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 25 Mar 2019 23:01:05 -0400
Subject: [PATCH 226/281] Added default noise model argument in two crucial
 factors.

---
 gtsam/slam/BetweenFactor.h | 2 +-
 gtsam/slam/PriorFactor.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 89291fac5..c183a97ca 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -56,7 +56,7 @@ namespace gtsam {
 
     /** Constructor */
     BetweenFactor(Key key1, Key key2, const VALUE& measured,
-        const SharedNoiseModel& model) :
+        const SharedNoiseModel& model = nullptr) :
       Base(model, key1, key2), measured_(measured) {
     }
 
diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h
index 3c5c42ccc..ed9340ff2 100644
--- a/gtsam/slam/PriorFactor.h
+++ b/gtsam/slam/PriorFactor.h
@@ -53,7 +53,7 @@ namespace gtsam {
     virtual ~PriorFactor() {}
 
     /** Constructor */
-    PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model) :
+    PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
       Base(model, key), prior_(prior) {
     }
 

From 59df91d295bd7888a37b15168baf799f735b89e5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 3 Apr 2019 18:44:18 -0400
Subject: [PATCH 227/281] Added optional ordering argument when converting to
 Matrix/Vector

---
 gtsam/linear/GaussianBayesNet.cpp | 26 ++++++++++++++++++--------
 gtsam/linear/GaussianBayesNet.h   | 12 +++++++++++-
 gtsam/linear/VectorValues.cpp     | 20 +++++++++++++-------
 gtsam/linear/VectorValues.h       |  2 +-
 4 files changed, 43 insertions(+), 17 deletions(-)

diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index c9ef922be..22f1918e4 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -138,23 +138,33 @@ namespace gtsam {
   //}
 
   /* ************************************************************************* */
-  pair<Matrix, Vector> GaussianBayesNet::matrix() const {
+  Ordering GaussianBayesNet::ordering() const {
     GaussianFactorGraph factorGraph(*this);
-    KeySet keys = factorGraph.keys();
+    auto keys = factorGraph.keys();
     // add frontal keys in order
     Ordering ordering;
-    for (const sharedConditional& cg: *this)
+    for (const sharedConditional& cg : *this)
       if (cg) {
-        for (Key key: cg->frontals()) {
+        for (Key key : cg->frontals()) {
           ordering.push_back(key);
           keys.erase(key);
         }
       }
     // add remaining keys in case Bayes net is incomplete
-    for (Key key: keys)
-      ordering.push_back(key);
-    // return matrix and RHS
-    return factorGraph.jacobian(ordering);
+    for (Key key : keys) ordering.push_back(key);
+    return ordering;
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianBayesNet::matrix(boost::optional<const Ordering&> ordering) const {
+    if (ordering) {
+      // Convert to a GaussianFactorGraph and use its machinery
+      GaussianFactorGraph factorGraph(*this);
+      return factorGraph.jacobian(ordering);
+    } else {
+      // recursively call with default ordering
+      return matrix(this->ordering());
+    }
   }
 
   ///* ************************************************************************* */
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index 401583bbf..64627a276 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -74,6 +74,14 @@ namespace gtsam {
     /// Version of optimize for incomplete BayesNet, needs solution for missing variables
     VectorValues optimize(const VectorValues& solutionForMissing) const;
 
+    /**
+     * Return ordering corresponding to a topological sort.
+     * There are many topological sorts of a Bayes net. This one
+     * corresponds to the one that makes 'matrix' below upper-triangular.
+     * In case Bayes net is incomplete any non-frontal are added to the end.
+     */
+    Ordering ordering() const;
+
     ///@}
 
     ///@name Linear Algebra
@@ -81,8 +89,10 @@ namespace gtsam {
     
     /**
      * Return (dense) upper-triangular matrix representation
+     * Will return upper-triangular matrix only when using 'ordering' above.
+     * In case Bayes net is incomplete zero columns are added to the end.
      */
-    std::pair<Matrix, Vector> matrix() const;
+    std::pair<Matrix, Vector> matrix(boost::optional<const Ordering&> ordering = boost::none) const;
 
     /**
      * Optimize along the gradient direction, with a closed-form computation to perform the line
diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index b037aad92..7ff773bd5 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -142,19 +142,25 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Vector VectorValues::vector() const
-  {
+  Vector VectorValues::vector(boost::optional<const Ordering&> ordering) const {
     // Count dimensions
     DenseIndex totalDim = 0;
-    for(const Vector& v: *this | map_values)
-      totalDim += v.size();
+    for (const Vector& v : *this | map_values) totalDim += v.size();
 
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
-    for(const Vector& v: *this | map_values) {
-      result.segment(pos, v.size()) = v;
-      pos += v.size();
+    if (ordering) {
+      for (const auto& key : *ordering) {
+        const auto& v = (*this)[key];
+        result.segment(pos, v.size()) = v;
+        pos += v.size();
+      }
+    } else {
+      for (const Vector& v : *this | map_values) {
+        result.segment(pos, v.size()) = v;
+        pos += v.size();
+      }
     }
 
     return result;
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 39abe1b56..5360edeff 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -244,7 +244,7 @@ namespace gtsam {
     /// @{
 
     /** Retrieve the entire solution as a single vector */
-    Vector vector() const;
+    Vector vector(boost::optional<const Ordering&> ordering = boost::none) const;
 
     /** Access a vector that is a subset of relevant keys. */
     template <typename CONTAINER>

From ecaf415d1eaf2e647ba417acee262a67fdc34180 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 3 Apr 2019 18:45:16 -0400
Subject: [PATCH 228/281] Better tests on backSubstituteTranspose

---
 gtsam/linear/tests/testGaussianBayesNet.cpp | 28 +++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 13601844c..3bc5f3371 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -152,6 +152,34 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
 
   VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
   EXPECT(assert_equal(expected, actual));
+  
+  const auto ordering = noisyBayesNet.ordering();
+  const Matrix R = smallBayesNet.matrix(ordering).first;
+  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
+  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
+}
+
+/* ************************************************************************* */
+TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
+{
+  // x=R'*y, expected=inv(R')*x
+  // 2 = 1    2
+  // 5   1 1  3
+  VectorValues
+    x = map_list_of<Key, Vector>
+      (_x_, Vector1::Constant(2))
+      (_y_, Vector1::Constant(5)),
+    expected = map_list_of<Key, Vector>
+      (_x_, Vector1::Constant(4))
+      (_y_, Vector1::Constant(9));
+
+  VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
+  EXPECT(assert_equal(expected, actual));
+  
+  const auto ordering = noisyBayesNet.ordering();
+  const Matrix R = noisyBayesNet.matrix(ordering).first;
+  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
+  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
 }
 
 /* ************************************************************************* */

From c450222ff13404ae29427b9ab122e378e6037447 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 3 Apr 2019 20:16:37 -0400
Subject: [PATCH 229/281] test on ordering

---
 gtsam/linear/tests/testGaussianBayesNet.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 3bc5f3371..74f07b92e 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -136,6 +136,15 @@ TEST( GaussianBayesNet, optimize3 )
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(GaussianBayesNet, ordering) 
+{
+  Ordering expected;
+  expected += 0, 1;
+  const auto actual = noisyBayesNet.ordering();
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST( GaussianBayesNet, backSubstituteTranspose )
 {

From 5a8363a775ef0d3feb668cf16353594f76bbf327 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 3 Apr 2019 20:17:18 -0400
Subject: [PATCH 230/281] Removed Ordering again -> templated vector method
 simply works

---
 gtsam/linear/VectorValues.cpp | 16 ++++------------
 gtsam/linear/VectorValues.h   |  2 +-
 2 files changed, 5 insertions(+), 13 deletions(-)

diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index 7ff773bd5..ca95313cf 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -142,7 +142,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Vector VectorValues::vector(boost::optional<const Ordering&> ordering) const {
+  Vector VectorValues::vector() const {
     // Count dimensions
     DenseIndex totalDim = 0;
     for (const Vector& v : *this | map_values) totalDim += v.size();
@@ -150,17 +150,9 @@ namespace gtsam {
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
-    if (ordering) {
-      for (const auto& key : *ordering) {
-        const auto& v = (*this)[key];
-        result.segment(pos, v.size()) = v;
-        pos += v.size();
-      }
-    } else {
-      for (const Vector& v : *this | map_values) {
-        result.segment(pos, v.size()) = v;
-        pos += v.size();
-      }
+    for (const Vector& v : *this | map_values) {
+      result.segment(pos, v.size()) = v;
+      pos += v.size();
     }
 
     return result;
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 5360edeff..39abe1b56 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -244,7 +244,7 @@ namespace gtsam {
     /// @{
 
     /** Retrieve the entire solution as a single vector */
-    Vector vector(boost::optional<const Ordering&> ordering = boost::none) const;
+    Vector vector() const;
 
     /** Access a vector that is a subset of relevant keys. */
     template <typename CONTAINER>

From 3126979ad5d214d7eb31c4242c52bd2edafff50a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 3 Apr 2019 22:45:49 -0400
Subject: [PATCH 231/281] Fixed memory issue (passing temporary to optional
 reference)

---
 gtsam/linear/GaussianBayesNet.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index 22f1918e4..87e0ded15 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -163,7 +163,8 @@ namespace gtsam {
       return factorGraph.jacobian(ordering);
     } else {
       // recursively call with default ordering
-      return matrix(this->ordering());
+      const auto defaultOrdering = this->ordering();
+      return matrix(defaultOrdering);
     }
   }
 

From 3e10adb178be8bf49510eb95297834057a0461d1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 3 Apr 2019 23:42:36 -0400
Subject: [PATCH 232/281] Cleaned up tests, testing size() as well.

---
 gtsam/symbolic/tests/testVariableIndex.cpp | 75 ++++++++++------------
 1 file changed, 33 insertions(+), 42 deletions(-)

diff --git a/gtsam/symbolic/tests/testVariableIndex.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp
index 56ce1f9a5..580f5a1a4 100644
--- a/gtsam/symbolic/tests/testVariableIndex.cpp
+++ b/gtsam/symbolic/tests/testVariableIndex.cpp
@@ -11,60 +11,66 @@
 
 /**
  * @file    testVariableIndex.cpp
- * @brief   
+ * @brief   Unit tests for VariableIndex class
  * @author  Richard Roberts
- * @date Sep 26, 2010
+ * @date    Sep 26, 2010
  */
 
+#include <gtsam/inference/VariableIndex.h>
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
+#include <gtsam/base/TestableAssertions.h>
+
+#include <CppUnitLite/TestHarness.h>
+
 #include <boost/assign/std/list.hpp>
 #include <boost/assign/list_of.hpp>
 using namespace boost::assign;
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/TestableAssertions.h>
-
-#include <gtsam/inference/VariableIndex.h>
-#include <gtsam/symbolic/SymbolicFactorGraph.h>
-
 using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST(VariableIndex, augment) {
+// 2 small symbolic graphs shared by all tests
 
-  SymbolicFactorGraph fg1, fg2;
+SymbolicFactorGraph testGraph1() {
+  SymbolicFactorGraph fg1;
   fg1.push_factor(0, 1);
   fg1.push_factor(0, 2);
   fg1.push_factor(5, 9);
   fg1.push_factor(2, 3);
+  return fg1;
+}
+
+SymbolicFactorGraph testGraph2() {
+  SymbolicFactorGraph fg2;
   fg2.push_factor(1, 3);
   fg2.push_factor(2, 4);
   fg2.push_factor(3, 5);
   fg2.push_factor(5, 6);
+  return fg2;
+}
 
-  SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2);
+/* ************************************************************************* */
+TEST(VariableIndex, augment) {
+  auto fg1 = testGraph1(), fg2 = testGraph2();
+  SymbolicFactorGraph fgCombined;
+  fgCombined.push_back(fg1);
+  fgCombined.push_back(fg2);
 
   VariableIndex expected(fgCombined);
   VariableIndex actual(fg1);
   actual.augment(fg2);
 
-  LONGS_EQUAL(16, (long)actual.nEntries());
-  LONGS_EQUAL(8, (long)actual.nFactors());
+  LONGS_EQUAL(8, actual.size());
+  LONGS_EQUAL(16, actual.nEntries());
+  LONGS_EQUAL(8, actual.nFactors());
   EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */
 TEST(VariableIndex, augment2) {
 
-  SymbolicFactorGraph fg1, fg2;
-  fg1.push_factor(0, 1);
-  fg1.push_factor(0, 2);
-  fg1.push_factor(5, 9);
-  fg1.push_factor(2, 3);
-  fg2.push_factor(1, 3);
-  fg2.push_factor(2, 4);
-  fg2.push_factor(3, 5);
-  fg2.push_factor(5, 6);
+  auto fg1 = testGraph1(), fg2 = testGraph2();
 
   SymbolicFactorGraph fgCombined;
   fgCombined.push_back(fg1);
@@ -77,23 +83,16 @@ TEST(VariableIndex, augment2) {
   VariableIndex actual(fg1);
   actual.augment(fg2, newIndices);
 
-  LONGS_EQUAL(16, (long) actual.nEntries());
-  LONGS_EQUAL(9, (long) actual.nFactors());
+  LONGS_EQUAL(8, actual.size());
+  LONGS_EQUAL(16, actual.nEntries());
+  LONGS_EQUAL(9, actual.nFactors());
   EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */
 TEST(VariableIndex, remove) {
 
-  SymbolicFactorGraph fg1, fg2;
-  fg1.push_factor(0, 1);
-  fg1.push_factor(0, 2);
-  fg1.push_factor(5, 9);
-  fg1.push_factor(2, 3);
-  fg2.push_factor(1, 3);
-  fg2.push_factor(2, 4);
-  fg2.push_factor(3, 5);
-  fg2.push_factor(5, 6);
+  auto fg1 = testGraph1(), fg2 = testGraph2();
 
   SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2);
 
@@ -118,15 +117,7 @@ TEST(VariableIndex, remove) {
 /* ************************************************************************* */
 TEST(VariableIndex, deep_copy) {
 
-  SymbolicFactorGraph fg1, fg2;
-  fg1.push_factor(0, 1);
-  fg1.push_factor(0, 2);
-  fg1.push_factor(5, 9);
-  fg1.push_factor(2, 3);
-  fg2.push_factor(1, 3);
-  fg2.push_factor(2, 4);
-  fg2.push_factor(3, 5);
-  fg2.push_factor(5, 6);
+  auto fg1 = testGraph1(), fg2 = testGraph2();
 
   // Create original graph and VariableIndex
   SymbolicFactorGraph fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2);

From 89ebed53cc74b7e3a291fc2dddd842a629858998 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 3 Apr 2019 23:42:47 -0400
Subject: [PATCH 233/281] fixed comments

---
 gtsam/inference/VariableIndex.h | 25 +++++++++++--------------
 1 file changed, 11 insertions(+), 14 deletions(-)

diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
index ad8d6720b..c16946f80 100644
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@@ -63,7 +63,7 @@ public:
   /// @name Standard Constructors
   /// @{
 
-  /** Default constructor, creates an empty VariableIndex */
+  /// Default constructor, creates an empty VariableIndex
   VariableIndex() : nFactors_(0), nEntries_(0) {}
 
   /**
@@ -77,19 +77,16 @@ public:
   /// @name Standard Interface
   /// @{
 
-  /**
-   * The number of variable entries.  This is one greater than the variable
-   * with the highest index.
-   */
+  /// The number of variable entries.  This is equal to the number of unique variable Keys.
   size_t size() const { return index_.size(); }
 
-  /** The number of factors in the original factor graph */
+  /// The number of factors in the original factor graph
   size_t nFactors() const { return nFactors_; }
 
-  /** The number of nonzero blocks, i.e. the number of variable-factor entries */
+  /// The number of nonzero blocks, i.e. the number of variable-factor entries
   size_t nEntries() const { return nEntries_; }
 
-  /** Access a list of factors by variable */
+  /// Access a list of factors by variable
   const Factors& operator[](Key variable) const {
     KeyMap::const_iterator item = index_.find(variable);
     if(item == index_.end())
@@ -102,10 +99,10 @@ public:
   /// @name Testable
   /// @{
 
-  /** Test for equality (for unit tests and debug assertions). */
+  /// Test for equality (for unit tests and debug assertions).
   bool equals(const VariableIndex& other, double tol=0.0) const;
 
-  /** Print the variable index (for unit tests and debugging). */
+  /// Print the variable index (for unit tests and debugging).
   void print(const std::string& str = "VariableIndex: ",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
@@ -140,17 +137,17 @@ public:
   template<typename ITERATOR, class FG>
   void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
 
-  /** Remove unused empty variables (in debug mode verifies they are empty). */
+  /// Remove unused empty variables (in debug mode verifies they are empty).
   template<typename ITERATOR>
   void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
 
-  /** Iterator to the first variable entry */
+  /// Iterator to the first variable entry
   const_iterator begin() const { return index_.begin(); }
 
-  /** Iterator to the first variable entry */
+  /// Iterator to the first variable entry
   const_iterator end() const { return index_.end(); }
 
-  /** Find the iterator for the requested variable entry */
+  /// Find the iterator for the requested variable entry
   const_iterator find(Key key) const { return index_.find(key); }
 
 protected:

From 485175e2f89e71c8dd858e7a7683aae75cb85906 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 01:04:18 -0400
Subject: [PATCH 234/281] Fixed major bug: splitting off a subgraph
 preconditioner only worked if keys were numbered 0...n-1, because we used
 DSFVector to implement Kruskal. Now it'll be a bit slower but will work for
 any keys. Note this only affected two constructors.

---
 gtsam/linear/SubgraphSolver.cpp | 57 +++++++++++++++++----------------
 gtsam/linear/SubgraphSolver.h   |  6 +++-
 2 files changed, 34 insertions(+), 29 deletions(-)

diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp
index 22d39a7f2..3853a72fa 100644
--- a/gtsam/linear/SubgraphSolver.cpp
+++ b/gtsam/linear/SubgraphSolver.cpp
@@ -22,7 +22,9 @@
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/base/DSFVector.h>
+#include <gtsam_unstable/base/DSFMap.h>
+
+#include <iostream>
 
 using namespace std;
 
@@ -120,45 +122,44 @@ void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1,
 }
 
 /**************************************************************************************************/
+// Run Kruskal algorithm to create a spanning tree of factor "edges".
+// Edges are not weighted, and will only work if factors are binary.
+// Unary factors are ignored for this purpose and added to tree graph.
 boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
-SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
+SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
 
-  const VariableIndex index(jfg);
-  const size_t n = index.size();
-  DSFVector D(n);
+  // Create disjoint set forest data structure for Kruskal algorithm
+  DSFMap<Key> dsf;
 
-  GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
-  GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
+  // Allocate two output graphs
+  auto tree = boost::make_shared<GaussianFactorGraph>();
+  auto constraints = boost::make_shared<GaussianFactorGraph>();
 
-  size_t t = 0;
-  for ( const GaussianFactor::shared_ptr &gf: jfg ) {
+  // Loop over all "edges"
+  for ( const auto &factor: factorGraph ) {
 
-    if (gf->keys().size() > 2) {
+    // Fail on > binary factors
+    const auto& keys = factor->keys();
+    if (keys.size() > 2) {
       throw runtime_error(
           "SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
     }
 
-    bool augment = false;
-
-    /* check whether this factor should be augmented to the "tree" graph */
-    if (gf->keys().size() == 1)
-      augment = true;
-    else {
-      const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.find(u),
-          v_root = D.find(v);
-      if (u_root != v_root) {
-        t++;
-        augment = true;
-        D.merge(u_root, v_root);
-      }
+    // check whether this factor should be augmented to the "tree" graph
+    if (keys.size() == 1)
+      tree->push_back(factor);
+    else if (dsf.find(keys[0]) != dsf.find(keys[1])) {
+      // We merge two trees joined by this edge if they are still disjoint
+      tree->push_back(factor);
+      // Record this fact in DSF
+      dsf.merge(keys[0], keys[1]);
+    } else {
+      // This factor would create a loop, so we add it to non-tree edges...
+      constraints->push_back(factor);
     }
-    if (augment)
-      At->push_back(gf);
-    else
-      Ac->push_back(gf);
   }
 
-  return boost::tie(At, Ac);
+  return boost::tie(tree, constraints);
 }
 
 /****************************************************************************/
diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h
index 318c029f3..44308ca1c 100644
--- a/gtsam/linear/SubgraphSolver.h
+++ b/gtsam/linear/SubgraphSolver.h
@@ -75,7 +75,11 @@ protected:
 
 public:
 
-  /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
+  /**
+   * Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
+   * Will throw exception if there are ternary factors or higher arity, as we use Kruskal's
+   * algorithm to split the graph, treating binary factors as edges.
+   */
   SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
       const Ordering& ordering);
 

From 6d938ce5cc94dbb24643f80eeb9c4a7a199ec491 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 01:17:28 -0400
Subject: [PATCH 235/281] Replaced initialize calls with C++11 delegating
 constructors

---
 gtsam/linear/SubgraphSolver.cpp | 118 +++++++++++++-------------------
 gtsam/linear/SubgraphSolver.h   |  21 ++----
 2 files changed, 54 insertions(+), 85 deletions(-)

diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp
index 3853a72fa..406109758 100644
--- a/gtsam/linear/SubgraphSolver.cpp
+++ b/gtsam/linear/SubgraphSolver.cpp
@@ -33,54 +33,56 @@ namespace gtsam {
 /**************************************************************************************************/
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
     const Parameters &parameters, const Ordering& ordering) :
-    parameters_(parameters), ordering_(ordering) {
-  initialize(gfg);
+    parameters_(parameters) {
+  
+  GaussianFactorGraph::shared_ptr Ab1,Ab2;
+  boost::tie(Ab1, Ab2) = splitGraph(gfg);
+  if (parameters_.verbosity())
+    cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
+         << " factors" << endl;
+
+  auto Rc1 = Ab1->eliminateSequential(ordering, EliminateQR);
+  auto xbar = boost::make_shared<VectorValues>(Rc1->optimize());
+  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
 }
 
-/**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg,
+// delegate up
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &factorGraph,
     const Parameters &parameters, const Ordering& ordering) :
-    parameters_(parameters), ordering_(ordering) {
-  initialize(*jfg);
-}
+    SubgraphSolver(*factorGraph, parameters, ordering) {}
 
 /**************************************************************************************************/
+SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
+                               const GaussianFactorGraph::shared_ptr &Ab2,
+                               const Parameters &parameters)
+    : parameters_(parameters) {
+  auto xbar = boost::make_shared<VectorValues>(Rc1->optimize());
+  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
+}
+
+// delegate up
+SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
+                               const GaussianFactorGraph &Ab2,
+                               const Parameters &parameters)
+    : SubgraphSolver(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2),
+                     parameters_) {}
+
+// delegate up
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
-    const GaussianFactorGraph &Ab2, const Parameters &parameters,
-    const Ordering& ordering) :
-    parameters_(parameters), ordering_(ordering) {
+                               const GaussianFactorGraph &Ab2,
+                               const Parameters &parameters,
+                               const Ordering &ordering)
+    : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR),
+                     boost::make_shared<GaussianFactorGraph>(Ab2),
+                     parameters_) {}
 
-  GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_,
-      EliminateQR);
-  initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
-}
-
-/**************************************************************************************************/
+// delegate up
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
-    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
-    const Ordering& ordering) :
-    parameters_(parameters), ordering_(ordering) {
-
-  GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
-      EliminateQR);
-  initialize(Rc1, Ab2);
-}
-
-/**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
-    const GaussianFactorGraph &Ab2, const Parameters &parameters,
-    const Ordering& ordering) :
-    parameters_(parameters), ordering_(ordering) {
-  initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
-}
-
-/**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
-    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
-    const Ordering& ordering) :
-    parameters_(parameters), ordering_(ordering) {
-  initialize(Rc1, Ab2);
-}
+                               const GaussianFactorGraph::shared_ptr &Ab2,
+                               const Parameters &parameters,
+                               const Ordering &ordering)
+    : SubgraphSolver(Ab1->eliminateSequential(ordering, EliminateQR), Ab2,
+                     parameters) {}
 
 /**************************************************************************************************/
 VectorValues SubgraphSolver::optimize() {
@@ -89,38 +91,16 @@ VectorValues SubgraphSolver::optimize() {
   return pc_->x(ybar);
 }
 
-/**************************************************************************************************/
 VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
   // the initial is ignored in this case ...
   return optimize();
 }
 
-/**************************************************************************************************/
-void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) {
-  GaussianFactorGraph::shared_ptr Ab1 =
-      boost::make_shared<GaussianFactorGraph>(), Ab2 = boost::make_shared<
-      GaussianFactorGraph>();
-
-  boost::tie(Ab1, Ab2) = splitGraph(jfg);
-  if (parameters_.verbosity())
-    cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
-        << " factors" << endl;
-
-  GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
-      EliminateQR);
-  VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
-      Rc1->optimize());
-  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
+VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
+    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
+    const VectorValues &initial) {
+  return VectorValues();
 }
-
-/**************************************************************************************************/
-void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1,
-    const GaussianFactorGraph::shared_ptr &Ab2) {
-  VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
-      Rc1->optimize());
-  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
-}
-
 /**************************************************************************************************/
 // Run Kruskal algorithm to create a spanning tree of factor "edges".
 // Edges are not weighted, and will only work if factors are binary.
@@ -163,9 +143,5 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
 }
 
 /****************************************************************************/
-VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
-    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
-    const VectorValues &initial) {
-  return VectorValues();
-}
+
 } // \namespace gtsam
diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h
index 44308ca1c..099982b53 100644
--- a/gtsam/linear/SubgraphSolver.h
+++ b/gtsam/linear/SubgraphSolver.h
@@ -70,7 +70,6 @@ public:
 
 protected:
   Parameters parameters_;
-  Ordering ordering_;
   boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
 
 public:
@@ -88,8 +87,8 @@ public:
       const Parameters &parameters, const Ordering& ordering);
 
   /**
-   * The user specify the subgraph part and the constraint part
-   * may throw exception if A1 is underdetermined
+   * The user specifies the subgraph part and the constraints part.
+   * May throw exception if A1 is underdetermined. An ordering is required to eliminate Ab1.
    */
   SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
       const Parameters &parameters, const Ordering& ordering);
@@ -99,15 +98,14 @@ public:
       const boost::shared_ptr<GaussianFactorGraph> &Ab2,
       const Parameters &parameters, const Ordering& ordering);
 
-  /* The same as above, but the A1 is solved before */
+  /// The same as above, but we assume A1 was solved by caller
   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
-      const GaussianFactorGraph &Ab2, const Parameters &parameters,
-      const Ordering& ordering);
+      const GaussianFactorGraph &Ab2, const Parameters &parameters);
 
   /// Shared pointer version
   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
       const boost::shared_ptr<GaussianFactorGraph> &Ab2,
-      const Parameters &parameters, const Ordering& ordering);
+      const Parameters &parameters);
 
   /// Destructor
   virtual ~SubgraphSolver() {
@@ -125,13 +123,8 @@ public:
       const VectorValues &initial);
 
 protected:
-
-  void initialize(const GaussianFactorGraph &jfg);
-  void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1,
-      const boost::shared_ptr<GaussianFactorGraph> &Ab2);
-
-  boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
-      boost::shared_ptr<GaussianFactorGraph> >
+  /// Split graph using Kruskal algorithm, treating binary factors as edges.
+  static boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph>>
   splitGraph(const GaussianFactorGraph &gfg);
 };
 

From aaf2ff568903e655b60637dd56aaa066ce8cdd8e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 01:17:47 -0400
Subject: [PATCH 236/281] Resurrected tests

---
 tests/testSubgraphSolver.cpp | 51 ++++++++++++++++--------------------
 1 file changed, 22 insertions(+), 29 deletions(-)

diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp
index aeeed1b9f..0a2a1788d 100644
--- a/tests/testSubgraphSolver.cpp
+++ b/tests/testSubgraphSolver.cpp
@@ -15,26 +15,27 @@
  *  @author Yong-Dian Jian
  **/
 
-#include <CppUnitLite/TestHarness.h>
-
-#if 0
-
 #include <tests/smallExample.h>
-#include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/iterative.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphSolver.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <CppUnitLite/TestHarness.h>
+
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/list.hpp>
 using namespace boost::assign;
 
 using namespace std;
 using namespace gtsam;
-using namespace example;
+
+static size_t N = 3;
+static SubgraphSolverParameters kParameters;
+static auto kOrdering = example::planarOrdering(N);
 
 /* ************************************************************************* */
 /** unnormalized error */
@@ -45,20 +46,17 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
   return total_error;
 }
 
-
 /* ************************************************************************* */
 TEST( SubgraphSolver, constructor1 )
 {
   // Build a planar graph
   GaussianFactorGraph Ab;
   VectorValues xtrue;
-  size_t N = 3;
-  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+  boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
-  // The first constructor just takes a factor graph (and parameters)
+  // The first constructor just takes a factor graph (and kParameters)
   // and it will split the graph into A1 and A2, where A1 is a spanning tree
-  SubgraphSolverParameters parameters;
-  SubgraphSolver solver(Ab, parameters);
+  SubgraphSolver solver(Ab, kParameters, kOrdering);
   VectorValues optimized = solver.optimize(); // does PCG optimization
   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
 }
@@ -70,16 +68,15 @@ TEST( SubgraphSolver, constructor2 )
   GaussianFactorGraph Ab;
   VectorValues xtrue;
   size_t N = 3;
-  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+  boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
-  // Get the spanning tree and corresponding ordering
+  // Get the spanning tree
   GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
+  boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
 
-  // The second constructor takes two factor graphs,
-  // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2)
-  SubgraphSolverParameters parameters;
-  SubgraphSolver solver(Ab1_, Ab2_, parameters);
+  // The second constructor takes two factor graphs, so the caller can specify
+  // the preconditioner (Ab1) and the constraints that are left out (Ab2)
+  SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering);
   VectorValues optimized = solver.optimize();
   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
 }
@@ -91,26 +88,22 @@ TEST( SubgraphSolver, constructor3 )
   GaussianFactorGraph Ab;
   VectorValues xtrue;
   size_t N = 3;
-  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+  boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
-  // Get the spanning tree and corresponding ordering
+  // Get the spanning tree and corresponding kOrdering
   GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
+  boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
 
-  // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT
-  GaussianBayesNet::shared_ptr Rc1 = //
-      EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);
+  // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
+  auto Rc1 = Ab1_.eliminateSequential();
 
   // The third constructor allows the caller to pass an already solved preconditioner Rc1_
   // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
-  SubgraphSolverParameters parameters;
-  SubgraphSolver solver(Rc1, Ab2_, parameters);
+  SubgraphSolver solver(Rc1, Ab2_, kParameters);
   VectorValues optimized = solver.optimize();
   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
 }
 
-#endif
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From 140c666c417e7b7a47ebe3a0dc59302f264b00f4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 08:27:28 -0400
Subject: [PATCH 237/281] Moved DSFMap to gtsam

---
 {gtsam_unstable => gtsam}/base/DSFMap.h             | 0
 {gtsam_unstable => gtsam}/base/tests/testDSFMap.cpp | 3 +--
 gtsam/linear/SubgraphSolver.cpp                     | 2 +-
 gtsam_unstable/timing/timeDSFvariants.cpp           | 2 +-
 4 files changed, 3 insertions(+), 4 deletions(-)
 rename {gtsam_unstable => gtsam}/base/DSFMap.h (100%)
 rename {gtsam_unstable => gtsam}/base/tests/testDSFMap.cpp (97%)

diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam/base/DSFMap.h
similarity index 100%
rename from gtsam_unstable/base/DSFMap.h
rename to gtsam/base/DSFMap.h
diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam/base/tests/testDSFMap.cpp
similarity index 97%
rename from gtsam_unstable/base/tests/testDSFMap.cpp
rename to gtsam/base/tests/testDSFMap.cpp
index 9c9143a15..5ffa0d12a 100644
--- a/gtsam_unstable/base/tests/testDSFMap.cpp
+++ b/gtsam/base/tests/testDSFMap.cpp
@@ -16,7 +16,7 @@
  * @brief unit tests for DSFMap
  */
 
-#include <gtsam_unstable/base/DSFMap.h>
+#include <gtsam/base/DSFMap.h>
 
 #include <boost/assign/std/list.hpp>
 #include <boost/assign/std/set.hpp>
@@ -115,7 +115,6 @@ TEST(DSFMap, mergePairwiseMatches2) {
 TEST(DSFMap, sets){
   // Create some "matches"
   typedef pair<size_t,size_t> Match;
-  typedef pair<size_t, set<size_t> > key_pair;
   list<Match> matches;
   matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6);
 
diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp
index 406109758..e7ff38ca5 100644
--- a/gtsam/linear/SubgraphSolver.cpp
+++ b/gtsam/linear/SubgraphSolver.cpp
@@ -22,7 +22,7 @@
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam_unstable/base/DSFMap.h>
+#include <gtsam/base/DSFMap.h>
 
 #include <iostream>
 
diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp
index f39bdda59..378d2b572 100644
--- a/gtsam_unstable/timing/timeDSFvariants.cpp
+++ b/gtsam_unstable/timing/timeDSFvariants.cpp
@@ -18,7 +18,7 @@
 
 #include <gtsam/base/DSFVector.h>
 #include <gtsam_unstable/base/DSF.h>
-#include <gtsam_unstable/base/DSFMap.h>
+#include <gtsam/base/DSFMap.h>
 
 #include <boost/random.hpp>
 #include <boost/timer.hpp>

From 3737474d1e760ed16c5b9a40d33a5d0689f5b628 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 09:23:02 -0400
Subject: [PATCH 238/281] Deprecated all but three constructors.

---
 gtsam/linear/SubgraphSolver.cpp |  48 +++++------
 gtsam/linear/SubgraphSolver.h   | 140 ++++++++++++++++++--------------
 tests/smallExample.h            |  12 +--
 tests/testSubgraphSolver.cpp    |  21 +++--
 4 files changed, 120 insertions(+), 101 deletions(-)

diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp
index e7ff38ca5..edf39e462 100644
--- a/gtsam/linear/SubgraphSolver.cpp
+++ b/gtsam/linear/SubgraphSolver.cpp
@@ -31,12 +31,12 @@ using namespace std;
 namespace gtsam {
 
 /**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
+// Just taking system [A|b]
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab,
     const Parameters &parameters, const Ordering& ordering) :
     parameters_(parameters) {
-  
   GaussianFactorGraph::shared_ptr Ab1,Ab2;
-  boost::tie(Ab1, Ab2) = splitGraph(gfg);
+  boost::tie(Ab1, Ab2) = splitGraph(Ab);
   if (parameters_.verbosity())
     cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
          << " factors" << endl;
@@ -46,12 +46,8 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
 }
 
-// delegate up
-SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &factorGraph,
-    const Parameters &parameters, const Ordering& ordering) :
-    SubgraphSolver(*factorGraph, parameters, ordering) {}
-
 /**************************************************************************************************/
+// Taking eliminated tree [R1|c] and constraint graph [A2|b2]
 SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
                                const GaussianFactorGraph::shared_ptr &Ab2,
                                const Parameters &parameters)
@@ -60,42 +56,40 @@ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
 }
 
+/**************************************************************************************************/
+// Taking subgraphs [A1|b1] and [A2|b2]
 // delegate up
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
+                               const GaussianFactorGraph::shared_ptr &Ab2,
+                               const Parameters &parameters,
+                               const Ordering &ordering)
+    : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2,
+                     parameters) {}
+
+/**************************************************************************************************/
+// deprecated variants
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
                                const GaussianFactorGraph &Ab2,
                                const Parameters &parameters)
     : SubgraphSolver(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2),
-                     parameters_) {}
+                     parameters) {}
 
-// delegate up
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
                                const GaussianFactorGraph &Ab2,
                                const Parameters &parameters,
                                const Ordering &ordering)
-    : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR),
-                     boost::make_shared<GaussianFactorGraph>(Ab2),
-                     parameters_) {}
-
-// delegate up
-SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
-                               const GaussianFactorGraph::shared_ptr &Ab2,
-                               const Parameters &parameters,
-                               const Ordering &ordering)
-    : SubgraphSolver(Ab1->eliminateSequential(ordering, EliminateQR), Ab2,
-                     parameters) {}
+    : SubgraphSolver(Ab1, boost::make_shared<GaussianFactorGraph>(Ab2),
+                     parameters, ordering) {}
+#endif
 
 /**************************************************************************************************/
-VectorValues SubgraphSolver::optimize() {
+VectorValues SubgraphSolver::optimize() const {
   VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
       Errors>(*pc_, pc_->zero(), parameters_);
   return pc_->x(ybar);
 }
 
-VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
-  // the initial is ignored in this case ...
-  return optimize();
-}
-
 VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
     const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
     const VectorValues &initial) {
diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h
index 099982b53..76fe31d32 100644
--- a/gtsam/linear/SubgraphSolver.h
+++ b/gtsam/linear/SubgraphSolver.h
@@ -28,30 +28,34 @@ class GaussianFactorGraph;
 class GaussianBayesNet;
 class SubgraphPreconditioner;
 
-class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters {
-public:
+class GTSAM_EXPORT SubgraphSolverParameters
+    : public ConjugateGradientParameters {
+ public:
   typedef ConjugateGradientParameters Base;
-  SubgraphSolverParameters() :
-      Base() {
-  }
-  void print() const {
-    Base::print();
-  }
-  virtual void print(std::ostream &os) const {
-    Base::print(os);
-  }
+  SubgraphSolverParameters() : Base() {}
+  void print() const { Base::print(); }
+  virtual void print(std::ostream &os) const { Base::print(os); }
 };
 
 /**
- * This class implements the SPCG solver presented in Dellaert et al in IROS'10.
+ * This class implements the linear SPCG solver presented in Dellaert et al in
+ * IROS'10.
  *
- * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into
- * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
- * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
- * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
+ * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the
+ * problem into \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$
+ * denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
  *
- * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it
- * with the least-squares variation of the conjugate gradient method.
+ * \f$A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute
+ * \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
+ *
+ * Then we solve a reparametrized problem
+ * \f$ f(y) = |y|^2 + |A_c R_t^{-1} y -\bar{b_y}|^2 \f$,
+ * where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
+ *
+ * In the matrix form, it is equivalent to solving
+ * \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$.
+ * We can solve it with the least-squares variation of the conjugate gradient
+ * method.
  *
  * To use it in nonlinear optimization, please see the following example
  *
@@ -63,69 +67,83 @@ public:
  *
  * \nosubgrouping
  */
-class GTSAM_EXPORT SubgraphSolver: public IterativeSolver {
-
-public:
+class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
+ public:
   typedef SubgraphSolverParameters Parameters;
 
-protected:
+ protected:
   Parameters parameters_;
-  boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
-
-public:
+  boost::shared_ptr<SubgraphPreconditioner> pc_;  ///< preconditioner object
 
+ public:
+  /// @name Constructors
+  /// @{
   /**
-   * Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
-   * Will throw exception if there are ternary factors or higher arity, as we use Kruskal's
-   * algorithm to split the graph, treating binary factors as edges.
+   * Given a gaussian factor graph, split it into a spanning tree (A1) + others
+   * (A2) for SPCG Will throw exception if there are ternary factors or higher
+   * arity, as we use Kruskal's algorithm to split the graph, treating binary
+   * factors as edges.
    */
   SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
-      const Ordering& ordering);
-
-  /// Shared pointer version
-  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
-      const Parameters &parameters, const Ordering& ordering);
+                 const Ordering &ordering);
 
   /**
    * The user specifies the subgraph part and the constraints part.
-   * May throw exception if A1 is underdetermined. An ordering is required to eliminate Ab1.
+   * May throw exception if A1 is underdetermined. An ordering is required to
+   * eliminate Ab1. We take Ab1 as a const reference, as it will be transformed
+   * into Rc1, but take Ab2 as a shared pointer as we need to keep it around.
+   */
+  SubgraphSolver(const GaussianFactorGraph &Ab1,
+                 const boost::shared_ptr<GaussianFactorGraph> &Ab2,
+                 const Parameters &parameters, const Ordering &ordering);
+  /**
+   * The same as above, but we assume A1 was solved by caller.
+   * We take two shared pointers as we keep both around.
    */
-  SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
-      const Parameters &parameters, const Ordering& ordering);
-
-  /// Shared pointer version
-  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
-      const boost::shared_ptr<GaussianFactorGraph> &Ab2,
-      const Parameters &parameters, const Ordering& ordering);
-
-  /// The same as above, but we assume A1 was solved by caller
   SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
-      const GaussianFactorGraph &Ab2, const Parameters &parameters);
-
-  /// Shared pointer version
-  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
-      const boost::shared_ptr<GaussianFactorGraph> &Ab2,
-      const Parameters &parameters);
+                 const boost::shared_ptr<GaussianFactorGraph> &Ab2,
+                 const Parameters &parameters);
 
   /// Destructor
-  virtual ~SubgraphSolver() {
-  }
+  virtual ~SubgraphSolver() {}
+
+  /// @}
+  /// @name Implement interface
+  /// @{
 
   /// Optimize from zero
-  VectorValues optimize();
+  VectorValues optimize() const;
 
-  /// Optimize from given initial values
-  VectorValues optimize(const VectorValues &initial);
-
-  /** Interface that IterativeSolver subclasses have to implement */
+  /// Interface that IterativeSolver subclasses have to implement
   virtual VectorValues optimize(const GaussianFactorGraph &gfg,
-      const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
-      const VectorValues &initial);
+                                const KeyInfo &keyInfo,
+                                const std::map<Key, Vector> &lambda,
+                                const VectorValues &initial) override;
 
-protected:
+  /// @}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+  /// @name Deprecated
+  /// @{
+  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
+                 const Parameters &parameters, const Ordering &ordering)
+      : SubgraphSolver(*A, parameters, ordering){};
+  SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &,
+                 const Parameters &, const Ordering &);
+  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
+                 const boost::shared_ptr<GaussianFactorGraph> &Ab2,
+                 const Parameters &parameters, const Ordering &ordering)
+      : SubgraphSolver(*Ab1, Ab2, parameters, ordering) {}
+  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &,
+                 const GaussianFactorGraph &, const Parameters &);
+  /// @}
+#endif
+
+ protected:
   /// Split graph using Kruskal algorithm, treating binary factors as edges.
-  static boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph>>
+  static boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
+                      boost::shared_ptr<GaussianFactorGraph>>
   splitGraph(const GaussianFactorGraph &gfg);
 };
 
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/tests/smallExample.h b/tests/smallExample.h
index d3a69b0bd..8cd219bff 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -614,26 +614,26 @@ inline Ordering planarOrdering(size_t N) {
 }
 
 /* ************************************************************************* */
-inline std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
+inline std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr > splitOffPlanarTree(size_t N,
     const GaussianFactorGraph& original) {
-  GaussianFactorGraph T, C;
+  auto T = boost::make_shared<GaussianFactorGraph>(), C= boost::make_shared<GaussianFactorGraph>();
 
   // Add the x11 constraint to the tree
-  T.push_back(original[0]);
+  T->push_back(original[0]);
 
   // Add all horizontal constraints to the tree
   size_t i = 1;
   for (size_t x = 1; x < N; x++)
     for (size_t y = 1; y <= N; y++, i++)
-      T.push_back(original[i]);
+      T->push_back(original[i]);
 
   // Add first vertical column of constraints to T, others to C
   for (size_t x = 1; x <= N; x++)
     for (size_t y = 1; y < N; y++, i++)
       if (x == 1)
-        T.push_back(original[i]);
+        T->push_back(original[i]);
       else
-        C.push_back(original[i]);
+        C->push_back(original[i]);
 
   return std::make_pair(T, C);
 }
diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp
index 0a2a1788d..93101131d 100644
--- a/tests/testSubgraphSolver.cpp
+++ b/tests/testSubgraphSolver.cpp
@@ -46,6 +46,13 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
   return total_error;
 }
 
+/* ************************************************************************* */
+TEST( SubgraphSolver, Parameters )
+{
+  LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity());
+  LONGS_EQUAL(500, kParameters.maxIterations());
+}
+
 /* ************************************************************************* */
 TEST( SubgraphSolver, constructor1 )
 {
@@ -71,12 +78,12 @@ TEST( SubgraphSolver, constructor2 )
   boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
   // Get the spanning tree
-  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
+  GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
+  boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
 
   // The second constructor takes two factor graphs, so the caller can specify
   // the preconditioner (Ab1) and the constraints that are left out (Ab2)
-  SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering);
+  SubgraphSolver solver(*Ab1, Ab2, kParameters, kOrdering);
   VectorValues optimized = solver.optimize();
   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
 }
@@ -91,15 +98,15 @@ TEST( SubgraphSolver, constructor3 )
   boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
   // Get the spanning tree and corresponding kOrdering
-  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab);
+  GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
+  boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
 
   // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
-  auto Rc1 = Ab1_.eliminateSequential();
+  auto Rc1 = Ab1->eliminateSequential();
 
   // The third constructor allows the caller to pass an already solved preconditioner Rc1_
   // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
-  SubgraphSolver solver(Rc1, Ab2_, kParameters);
+  SubgraphSolver solver(Rc1, Ab2, kParameters);
   VectorValues optimized = solver.optimize();
   DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
 }

From 703b56f9ff4d85a566e25f8edaf9d16d47720d25 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Fri, 5 Apr 2019 08:10:27 -0400
Subject: [PATCH 239/281] Fix SubgraphSolver wrapper

---
 gtsam.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam.h b/gtsam.h
index 014907037..ba3b7f7c7 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1730,7 +1730,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
 
 virtual class SubgraphSolver  {
   SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
-  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
+  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
   gtsam::VectorValues optimize() const;
 };
 

From 9581e4939bf8c946874d94d91126ea35a438dea2 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 31 Mar 2019 22:03:10 -0400
Subject: [PATCH 240/281] Made it so X can be variable dimension as long as you
 know dimension of tested value at compile time.

---
 gtsam/base/numericalDerivative.h | 42 +++++++++++++++-----------------
 1 file changed, 20 insertions(+), 22 deletions(-)

diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h
index cc1cbdb51..a9a088108 100644
--- a/gtsam/base/numericalDerivative.h
+++ b/gtsam/base/numericalDerivative.h
@@ -67,28 +67,29 @@ struct FixedSizeMatrix {
 }
 
 /**
- * Numerically compute gradient of scalar function
+ * @brief Numerically compute gradient of scalar function
+ * @return n-dimensional gradient computed via central differencing
  * Class X is the input argument
  * The class X needs to have dim, expmap, logmap
+  * int N is the dimension of the X input value if variable dimension type but known at test time
  */
-template<class X>
-typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<double(const X&)> h, const X& x,
-    double delta = 1e-5) {
+
+template <class X, int N = traits<X>::dimension>
+typename Eigen::Matrix<double, N, 1> numericalGradient(
+    boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
   double factor = 1.0 / (2.0 * delta);
 
   BOOST_STATIC_ASSERT_MSG(
       (boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
-  static const int N = traits<X>::dimension;
-  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
-
-  typedef typename traits<X>::TangentVector TangentX;
+  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
 
   // Prepare a tangent vector to perturb x with, only works for fixed size
-  TangentX d;
+  typename traits<X>::TangentVector d;
   d.setZero();
 
-  Eigen::Matrix<double,N,1> g; g.setZero(); // Can be fixed size
+  Eigen::Matrix<double,N,1> g; 
+  g.setZero();
   for (int j = 0; j < N; j++) {
     d(j) = delta;
     double hxplus = h(traits<X>::Retract(x, d));
@@ -108,37 +109,34 @@ typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<do
  * @param delta increment for numerical derivative
  * Class Y is the output argument
  * Class X is the input argument
+ * int N is the dimension of the X input value if variable dimension type but known at test time
  * @return m*n Jacobian computed via central differencing
  */
 
-template<class Y, class X>
+template <class Y, class X, int N = traits<X>::dimension>
 // TODO Should compute fixed-size matrix
-typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
-    double delta = 1e-5) {
-
+typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
+    boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
   typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
 
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   typedef traits<Y> TraitsY;
-  typedef typename TraitsY::TangentVector TangentY;
 
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
-  static const int N = traits<X>::dimension;
-  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
+  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
   typedef traits<X> TraitsX;
-  typedef typename TraitsX::TangentVector TangentX;
 
   // get value at x, and corresponding chart
   const Y hx = h(x);
 
   // Bit of a hack for now to find number of rows
-  const TangentY zeroY = TraitsY::Local(hx, hx);
+  const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
   const size_t m = zeroY.size();
 
   // Prepare a tangent vector to perturb x with, only works for fixed size
-  TangentX dx;
+  Eigen::Matrix<double, N, 1> dx;
   dx.setZero();
 
   // Fill in Jacobian H
@@ -146,9 +144,9 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
   const double factor = 1.0 / (2.0 * delta);
   for (int j = 0; j < N; j++) {
     dx(j) = delta;
-    const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
+    const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = -delta;
-    const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
+    const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = 0;
     H.col(j) << (dy1 - dy2) * factor;
   }

From a7826ab417d91a6efdf1b04e35c75dfd9802ade4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 22:57:27 -0400
Subject: [PATCH 241/281] Added comments, removed cruft

---
 gtsam/linear/Preconditioner.h         | 28 +++++++++------------------
 gtsam/linear/SubgraphPreconditioner.h | 21 +++++++++++---------
 2 files changed, 21 insertions(+), 28 deletions(-)

diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h
index 623b29863..31901db3f 100644
--- a/gtsam/linear/Preconditioner.h
+++ b/gtsam/linear/Preconditioner.h
@@ -70,21 +70,20 @@ public:
   Preconditioner() {}
   virtual ~Preconditioner() {}
 
-  /* Computation Interfaces */
+  /* 
+  * Abstract interface for raw vectors. VectorValues is a speed bottleneck
+  * and Yong-Dian has profiled preconditioners (outside GTSAM) with the the
+  * three methods below. In GTSAM, unfortunately, we are still using the
+  * VectorValues methods called in iterative-inl.h
+  */
 
-  /* implement x = L^{-1} y */
+  /// implement x = L^{-1} y
   virtual void solve(const Vector& y, Vector &x) const = 0;
-//  virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
 
-  /* implement x = L^{-T} y */
+  /// implement x = L^{-T} y
   virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
-//  virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
 
-//  /* implement x = L^{-1} L^{-T} y */
-//  virtual void fullSolve(const Vector& y, Vector &x) const = 0;
-//  virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
-
-  /* build/factorize the preconditioner */
+  /// build/factorize the preconditioner
   virtual void build(
     const GaussianFactorGraph &gfg,
     const KeyInfo &info,
@@ -113,14 +112,7 @@ public:
 
   /* Computation Interfaces for raw vector */
   virtual void solve(const Vector& y, Vector &x) const { x = y; }
-//  virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
-
   virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
-//  virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
-
-//  virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
-//  virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
-
   virtual void build(
     const GaussianFactorGraph &gfg,
     const KeyInfo &info,
@@ -145,8 +137,6 @@ public:
   /* Computation Interfaces for raw vector */
   virtual void solve(const Vector& y, Vector &x) const;
   virtual void transposeSolve(const Vector& y, Vector& x) const ;
-//  virtual void fullSolve(const Vector& y, Vector &x) const ;
-
   virtual void build(
     const GaussianFactorGraph &gfg,
     const KeyInfo &info,
diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h
index e440f32e4..7995afedc 100644
--- a/gtsam/linear/SubgraphPreconditioner.h
+++ b/gtsam/linear/SubgraphPreconditioner.h
@@ -285,15 +285,18 @@ namespace gtsam {
     /*****************************************************************************/
     /* implement virtual functions of Preconditioner */
 
-    /* Computation Interfaces for Vector */
-    virtual void solve(const Vector& y, Vector &x) const;
-    virtual void transposeSolve(const Vector& y, Vector& x) const ;
+    /// implement x = R^{-1} y
+    void solve(const Vector& y, Vector &x) const override;
 
-    virtual void build(
+    /// implement x = R^{-T} y
+    void transposeSolve(const Vector& y, Vector& x) const override;
+
+    /// build/factorize the preconditioner
+    void build(
       const GaussianFactorGraph &gfg,
       const KeyInfo &info,
       const std::map<Key,Vector> &lambda
-      ) ;
+      ) override;
     /*****************************************************************************/
   };
 
@@ -310,9 +313,9 @@ namespace gtsam {
 
 
   /* sort the container and return permutation index with default comparator */
-   template <typename Container>
-   std::vector<size_t> sort_idx(const Container &src)
-   {
+  template <typename Container>
+  std::vector<size_t> sort_idx(const Container &src)
+  {
      typedef typename Container::value_type T;
      const size_t n = src.size() ;
      std::vector<std::pair<size_t,T> > tmp;
@@ -329,6 +332,6 @@ namespace gtsam {
        idx.push_back(tmp[i].first) ;
      }
      return idx;
-   }
+  }
 
 } // namespace gtsam

From 334c85a298279ab449d1dc5b0e74d2df69fe090d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 23:01:18 -0400
Subject: [PATCH 242/281] Using keys not indices

---
 gtsam/linear/SubgraphPreconditioner.cpp | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index d796e28b7..de0df8192 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -508,8 +508,10 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
 void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
 
   Errors::iterator ei = e.begin();
-  for (size_t i = 0; i < y.size(); ++i, ++ei)
-    *ei = y[i];
+  for(const auto& key_value: y) {
+    *ei = key_value.second;
+    ++ei;
+  }
 
   // Add A2 contribution
   VectorValues x = Rc1()->backSubstitute(y);      // x=inv(R1)*y
@@ -522,8 +524,10 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
 
   Errors::const_iterator it = e.begin();
   VectorValues y = zero();
-  for (size_t i = 0; i < y.size(); ++i, ++it)
-    y[i] = *it;
+  for(auto& key_value: y) {
+    key_value.second = *it;
+    ++it;
+  }
   transposeMultiplyAdd2(1.0, it, e.end(), y);
   return y;
 }
@@ -534,9 +538,10 @@ void SubgraphPreconditioner::transposeMultiplyAdd
 (double alpha, const Errors& e, VectorValues& y) const {
 
   Errors::const_iterator it = e.begin();
-  for (size_t i = 0; i < y.size(); ++i, ++it) {
+  for(auto& key_value: y) {
     const Vector& ei = *it;
-    axpy(alpha, ei, y[i]);
+    axpy(alpha, ei, key_value.second);
+    ++it;
   }
   transposeMultiplyAdd2(alpha, it, e.end(), y);
 }

From 6b637bda9e1c758d7ac3497707dbd408c198481d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 23:01:32 -0400
Subject: [PATCH 243/281] Cleanup

---
 gtsam/linear/SubgraphPreconditioner.cpp | 158 +++++++++++++-----------
 1 file changed, 87 insertions(+), 71 deletions(-)

diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index de0df8192..c75bcd4e2 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -52,17 +52,21 @@
 #include <utility>
 #include <vector>
 
-using namespace std;
+using std::cout;
+using std::endl;
+using std::vector;
+using std::ostream;
 
 namespace gtsam {
 
 /* ************************************************************************* */
+// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
 static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
-  GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
-  for(const GaussianFactor::shared_ptr &gf: gfg) {
-    JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
+  auto result = boost::make_shared<GaussianFactorGraph>();
+  for (const auto &factor : gfg) {
+    auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
     if( !jf ) {
-      jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+      jf = boost::make_shared<JacobianFactor>(*factor); 
     }
     result->push_back(jf);
   }
@@ -70,7 +74,7 @@ static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFa
 }
 
 /*****************************************************************************/
-static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
+static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
 
   /* compute the sum of the weights */
   const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
@@ -107,10 +111,10 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
   vector<size_t> result;
 
   size_t count = 0;
-  std::vector<bool> touched(m, false);
+  vector<bool> touched(m, false);
   while ( count < n ) {
-    std::vector<size_t> localIndices; localIndices.reserve(n-count);
-    std::vector<double> localWeights; localWeights.reserve(n-count);
+    vector<size_t> localIndices; localIndices.reserve(n-count);
+    vector<double> localWeights; localWeights.reserve(n-count);
 
     /* collect data */
     for ( size_t i = 0 ; i < m ; ++i ) {
@@ -134,16 +138,16 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
 }
 
 /****************************************************************************/
-Subgraph::Subgraph(const std::vector<size_t> &indices) {
+Subgraph::Subgraph(const vector<size_t> &indices) {
   edges_.reserve(indices.size());
   for ( const size_t &idx: indices ) {
-    edges_.push_back(SubgraphEdge(idx, 1.0));
+    edges_.emplace_back(idx, 1.0);
   }
 }
 
 /****************************************************************************/
-std::vector<size_t> Subgraph::edgeIndices() const {
-  std::vector<size_t> eid; eid.reserve(size());
+vector<size_t> Subgraph::edgeIndices() const {
+  vector<size_t> eid; eid.reserve(size());
   for ( const SubgraphEdge &edge: edges_ ) {
     eid.push_back(edge.index_);
   }
@@ -169,7 +173,7 @@ Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
 }
 
 /****************************************************************************/
-std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
+ostream &operator<<(ostream &os, const SubgraphEdge &edge) {
   if ( edge.weight() != 1.0 )
     os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
   else
@@ -178,7 +182,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
 }
 
 /****************************************************************************/
-std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
+ostream &operator<<(ostream &os, const Subgraph &subgraph) {
   os << "Subgraph" << endl;
   for ( const SubgraphEdge &e: subgraph.edges() ) {
     os << e << ", " ;
@@ -212,7 +216,7 @@ SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslato
   if (s == "NATURALCHAIN")    return NATURALCHAIN;
   else if (s == "BFS")        return BFS;
   else if (s == "KRUSKAL")    return KRUSKAL;
-  throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
+  throw std::invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
   return KRUSKAL;
 }
 
@@ -231,7 +235,7 @@ SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWei
   else if (s == "RHS")    return RHS_2NORM;
   else if (s == "LHS")    return LHS_FNORM;
   else if (s == "RANDOM") return RANDOM;
-  throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
+  throw std::invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
   return EQUAL;
 }
 
@@ -245,12 +249,14 @@ std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w
 }
 
 /****************************************************************/
-SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
+SubgraphBuilderParameters::AugmentationWeight
+SubgraphBuilderParameters::augmentationWeightTranslator(
+    const std::string &src) {
   std::string s = src;  boost::algorithm::to_upper(s);
   if (s == "SKELETON")      return SKELETON;
 //  else if (s == "STRETCH")  return STRETCH;
 //  else if (s == "GENERALIZED_STRETCH")  return GENERALIZED_STRETCH;
-  throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
+  throw std::invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
   return SKELETON;
 }
 
@@ -263,7 +269,9 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(Augmentation
 }
 
 /****************************************************************/
-std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
+vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
+                                          const FastMap<Key, size_t> &ordering,
+                                          const vector<double> &w) const {
   const SubgraphBuilderParameters &p = parameters_;
   switch (p.skeleton_) {
   case SubgraphBuilderParameters::NATURALCHAIN:
@@ -276,18 +284,18 @@ std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c
     return kruskal(gfg, ordering, w);
     break;
   default:
-    cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
+    std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
     break;
   }
   return vector<size_t>();
 }
 
 /****************************************************************/
-std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
-  std::vector<size_t> result ;
+vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
+  vector<size_t> result ;
   size_t idx = 0;
-  for ( const GaussianFactor::shared_ptr &gf: gfg ) {
-    if ( gf->size() == 1 ) {
+  for (const auto &factor : gfg) {
+    if ( factor->size() == 1 ) {
       result.push_back(idx);
     }
     idx++;
@@ -296,8 +304,8 @@ std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const
 }
 
 /****************************************************************/
-std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
-  std::vector<size_t> result ;
+vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
+  vector<size_t> result ;
   size_t idx = 0;
   for ( const GaussianFactor::shared_ptr &gf: gfg ) {
     if ( gf->size() == 2 ) {
@@ -311,7 +319,7 @@ std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gf
 }
 
 /****************************************************************/
-std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
+vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
   const VariableIndex variableIndex(gfg);
   /* start from the first key of the first factor */
   Key seed = gfg[0]->keys()[0];
@@ -319,7 +327,7 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
   const size_t n = variableIndex.size();
 
   /* each vertex has self as the predecessor */
-  std::vector<size_t> result;
+  vector<size_t> result;
   result.reserve(n-1);
 
   /* Initialize */
@@ -347,7 +355,9 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
 }
 
 /****************************************************************/
-std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
+vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
+                                        const FastMap<Key, size_t> &ordering,
+                                        const vector<double> &w) const {
   const VariableIndex variableIndex(gfg);
   const size_t n = variableIndex.size();
   const vector<size_t> idx = sort_idx(w) ;
@@ -357,18 +367,17 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
   result.reserve(n-1);
 
   // container for acsendingly sorted edges
-  DSFVector D(n) ;
+  DSFVector dsf(n);
 
   size_t count = 0 ; double sum = 0.0 ;
   for (const size_t id: idx) {
     const GaussianFactor &gf = *gfg[id];
-    if ( gf.keys().size() != 2 ) continue;
-    const size_t u = ordering.find(gf.keys()[0])->second,
-                 u_root = D.find(u),
-                 v = ordering.find(gf.keys()[1])->second,
-                 v_root = D.find(v) ;
-    if ( u_root != v_root ) {
-      D.merge(u_root, v_root) ;
+    const auto keys = gf.keys();
+    if ( keys.size() != 2 ) continue;
+    const size_t u = ordering.find(keys[0])->second,
+                 v = ordering.find(keys[1])->second;
+    if ( dsf.find(u) != dsf.find(v) ) {
+      dsf.merge(u, v) ;
       result.push_back(id) ;
       sum += w[id] ;
       if ( ++count == n-1 ) break ;
@@ -378,7 +387,7 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
 }
 
 /****************************************************************/
-std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
+vector<size_t> SubgraphBuilder::sample(const vector<double> &weights, const size_t t) const {
   return uniqueSampler(weights, t);
 }
 
@@ -395,7 +404,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
 
   /* sanity check */
   if ( tree.size() != n-1 ) {
-    throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
+    throw std::runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
   }
 
   /* down weight the tree edges to zero */
@@ -404,7 +413,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
   }
 
   /* decide how many edges to augment */
-  std::vector<size_t> offTree = sample(w, t);
+  vector<size_t> offTree = sample(w, t);
 
   vector<size_t> subgraph = unary(gfg);
   subgraph.insert(subgraph.end(), tree.begin(), tree.end());
@@ -450,7 +459,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg
       break;
 
     default:
-      throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
+      throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
       break;
     }
   }
@@ -484,21 +493,20 @@ double SubgraphPreconditioner::error(const VectorValues& y) const {
 
 /* ************************************************************************* */
 // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
-VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
+VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const {
   VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
-  Errors e = (*Ab2()*x - *b2bar());               /* (A2*inv(R1)*y-b2bar) */
+  Errors e = (*Ab2() * x - *b2bar());        /* (A2*inv(R1)*y-b2bar) */
   VectorValues v = VectorValues::Zero(x);
-  Ab2()->transposeMultiplyAdd(1.0, e, v);           /* A2'*(A2*inv(R1)*y-b2bar) */
+  Ab2()->transposeMultiplyAdd(1.0, e, v);    /* A2'*(A2*inv(R1)*y-b2bar) */
   return y + Rc1()->backSubstituteTranspose(v);
 }
 
 /* ************************************************************************* */
 // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
 Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
-
   Errors e(y);
   VectorValues x = Rc1()->backSubstitute(y);   /* x=inv(R1)*y */
-  Errors e2 = *Ab2() * x;                              /* A2*x */
+  Errors e2 = *Ab2() * x;                      /* A2*x */
   e.splice(e.end(), e2);
   return e;
 }
@@ -568,47 +576,55 @@ void SubgraphPreconditioner::print(const std::string& s) const {
 }
 
 /*****************************************************************************/
-void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
-{
+void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
   /* copy first */
+  assert(x.size() == y.size());
   std::copy(y.data(), y.data() + y.rows(), x.data());
 
   /* in place back substitute */
-  for (auto cg: boost::adaptors::reverse(*Rc1_)) {
+  for (const auto &cg : boost::adaptors::reverse(*Rc1_)) {
     /* collect a subvector of x that consists of the parents of cg (S) */
-    const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
-    const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
+    const Vector xParent = getSubvector(
+        x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
+    const Vector rhsFrontal = getSubvector(
+        x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
 
     /* compute the solution for the current pivot */
-    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
+    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(
+        rhsFrontal - cg->get_S() * xParent);
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_,
+                 KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
   }
 }
 
 /*****************************************************************************/
-void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
-{
+void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
   /* copy first */
+  assert(x.size() == y.size());
   std::copy(y.data(), y.data() + y.rows(), x.data());
 
   /* in place back substitute */
-  for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
-    const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
-//    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
-    const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
+  for (const auto &cg : *Rc1_) {
+    const Vector rhsFrontal = getSubvector(
+        x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
+    const Vector solFrontal =
+        cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
+            rhsFrontal);
 
     // Check for indeterminant solution
-    if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
+    if (solFrontal.hasNaN())
+      throw IndeterminantLinearSystemException(cg->keys().front());
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_,
+                 KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
 
     /* substract from parent variables */
-    for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
-      KeyInfo::const_iterator it2 = keyInfo_.find(*it);
-      Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
+    for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
+      const KeyInfoEntry &info = keyInfo_.find(*it)->second;
+      Eigen::Map<Vector> rhsParent(x.data() + info.colstart(), info.dim(), 1);
       rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
     }
   }
@@ -634,14 +650,14 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
 Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
 
   /* a cache of starting index and dim */
-  typedef vector<pair<size_t, size_t> > Cache;
+  typedef vector<std::pair<size_t, size_t> > Cache;
   Cache cache;
 
   /* figure out dimension by traversing the keys */
   size_t d = 0;
   for ( const Key &key: keys ) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
-    cache.push_back(make_pair(entry.colstart(), entry.dim()));
+    cache.emplace_back(entry.colstart(), entry.dim());
     d += entry.dim();
   }
 
@@ -668,10 +684,10 @@ void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &ke
 }
 
 /*****************************************************************************/
-boost::shared_ptr<GaussianFactorGraph>
-buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {
-
-  GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
+GaussianFactorGraph::shared_ptr buildFactorSubgraph(
+    const GaussianFactorGraph &gfg, const Subgraph &subgraph,
+    const bool clone) {
+  auto result = boost::make_shared<GaussianFactorGraph>();
   result->reserve(subgraph.size());
   for ( const SubgraphEdge &e: subgraph ) {
     const size_t idx = e.index();

From 3889b29305c5fec24b40919a024b967d5dabeebf Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Thu, 4 Apr 2019 23:01:47 -0400
Subject: [PATCH 244/281] Resurrected tests

---
 tests/testSubgraphPreconditioner.cpp | 194 +++++++++++++++++----------
 1 file changed, 122 insertions(+), 72 deletions(-)

diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index accf9a65e..f51263bfb 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -17,12 +17,11 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#if 0
-
 #include <tests/smallExample.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/iterative.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/numericalDerivative.h>
@@ -49,7 +48,7 @@ TEST( SubgraphPreconditioner, planarOrdering ) {
       key(3, 3), key(2, 3), key(1, 3),
       key(3, 2), key(2, 2), key(1, 2),
       key(3, 1), key(2, 1), key(1, 1);
-  CHECK(assert_equal(expected,ordering));
+  EXPECT(assert_equal(expected,ordering));
 }
 
 /* ************************************************************************* */
@@ -73,9 +72,9 @@ TEST( SubgraphPreconditioner, planarGraph )
   DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue
 
   // Check that xtrue is optimal
-  GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
-  VectorValues actual = optimize(*R1);
-  CHECK(assert_equal(xtrue,actual));
+  GaussianBayesNet::shared_ptr R1 = A.eliminateSequential();
+  VectorValues actual = R1->optimize();
+  EXPECT(assert_equal(xtrue,actual));
 }
 
 /* ************************************************************************* */
@@ -87,19 +86,18 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree )
   boost::tie(A, xtrue) = planarGraph(3);
 
   // Get the spanning tree and constraints, and check their sizes
-  GaussianFactorGraph T, C;
+  GaussianFactorGraph::shared_ptr T, C;
   boost::tie(T, C) = splitOffPlanarTree(3, A);
-  LONGS_EQUAL(9,T.size());
-  LONGS_EQUAL(4,C.size());
+  LONGS_EQUAL(9,T->size());
+  LONGS_EQUAL(4,C->size());
 
   // Check that the tree can be solved to give the ground xtrue
-  GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
-  VectorValues xbar = optimize(*R1);
-  CHECK(assert_equal(xtrue,xbar));
+  GaussianBayesNet::shared_ptr R1 = T->eliminateSequential();
+  VectorValues xbar = R1->optimize();
+  EXPECT(assert_equal(xtrue,xbar));
 }
 
 /* ************************************************************************* */
-
 TEST( SubgraphPreconditioner, system )
 {
   // Build a planar graph
@@ -108,71 +106,128 @@ TEST( SubgraphPreconditioner, system )
   size_t N = 3;
   boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
 
-  // Get the spanning tree and corresponding ordering
-  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
-  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
-  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
+  // Get the spanning tree and remaining graph
+  GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
+  boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
 
   // Eliminate the spanning tree to build a prior
-  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
-  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
+  const Ordering ord = planarOrdering(N);
+  auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1
+  VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1
 
   // Create Subgraph-preconditioned system
   VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
-  SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
+  const SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
+
+  // Get corresponding matrices for tests. Add dummy factors to Ab2 to make
+  // sure it works with the ordering.
+  Ordering ordering = Rc1->ordering(); // not ord in general!
+  Ab2->add(key(1,1),Z_2x2, Z_2x1);
+  Ab2->add(key(1,2),Z_2x2, Z_2x1);
+  Ab2->add(key(1,3),Z_2x2, Z_2x1);
+  Matrix A, A1, A2;
+  Vector b, b1, b2;
+  std::tie(A,b) = Ab.jacobian(ordering);
+  std::tie(A1,b1) = Ab1->jacobian(ordering);
+  std::tie(A2,b2) = Ab2->jacobian(ordering);
+  Matrix R1 = Rc1->matrix(ordering).first;
+  Matrix Abar(13 * 2, 9 * 2);
+  Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2);
+  Abar.bottomRows(4 * 2) = A2 * R1.inverse();
+
+  // Helper function to vectorize in correct order, which is the order in which
+  // we eliminated the spanning tree.
+  auto vec = [ordering](const VectorValues& x) { return x.vector(ordering);};
 
   // Create zero config
-  VectorValues zeros = VectorValues::Zero(xbar);
+  const VectorValues zeros = VectorValues::Zero(xbar);
 
   // Set up y0 as all zeros
-  VectorValues y0 = zeros;
+  const VectorValues y0 = zeros;
 
   // y1 = perturbed y0
   VectorValues y1 = zeros;
-  y1[1] = Vector2(1.0, -1.0);
+  y1[key(3,3)] = Vector2(1.0, -1.0);
 
-  // Check corresponding x  values
-  VectorValues expected_x1 = xtrue, x1 = system.x(y1);
-  expected_x1[1] = Vector2(2.01, 2.99);
-  expected_x1[0] = Vector2(3.01, 2.99);
-  CHECK(assert_equal(xtrue, system.x(y0)));
-  CHECK(assert_equal(expected_x1,system.x(y1)));
+  // Check backSubstituteTranspose works with R1
+  VectorValues actual = Rc1->backSubstituteTranspose(y1);
+  Vector expected = R1.transpose().inverse() * vec(y1);
+  EXPECT(assert_equal(expected, vec(actual)));
+
+  // Check corresponding x values
+  // for y = 0, we get xbar:
+  EXPECT(assert_equal(xbar, system.x(y0)));
+  // for non-zero y, answer is x = xbar + inv(R1)*y
+  const Vector expected_x1 = vec(xbar) + R1.inverse() * vec(y1);
+  const VectorValues x1 = system.x(y1);
+  EXPECT(assert_equal(expected_x1, vec(x1)));
 
   // Check errors
-  DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9);
-  DOUBLES_EQUAL(3,error(Ab,x1),1e-9);
-  DOUBLES_EQUAL(0,error(system,y0),1e-9);
-  DOUBLES_EQUAL(3,error(system,y1),1e-9);
+  DOUBLES_EQUAL(0,error(Ab,xbar),1e-9);
+  DOUBLES_EQUAL(0,system.error(y0),1e-9);
+  DOUBLES_EQUAL(2,error(Ab,x1),1e-9);
+  DOUBLES_EQUAL(2,system.error(y1),1e-9);
 
-  // Test gradient in x
-  VectorValues expected_gx0 = zeros;
-  VectorValues expected_gx1 = zeros;
-  CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
-  expected_gx1[2] = Vector2(-100., 100.);
-  expected_gx1[4] = Vector2(-100., 100.);
-  expected_gx1[1] = Vector2(200., -200.);
-  expected_gx1[3] = Vector2(-100., 100.);
-  expected_gx1[0] = Vector2(100., -100.);
-  CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
+  // Check that transposeMultiplyAdd <=> y += alpha * Abar' * e
+  // We check for e1 =[1;0] and e2=[0;1] corresponding to T and C
+  const double alpha = 0.5;
+  Errors e1,e2;
+  for (size_t i=0;i<13;i++) {
+    e1 += i<9 ? Vector2(1, 1) : Vector2(0, 0);
+    e2 += i>=9 ? Vector2(1, 1) : Vector2(0, 0);
+  }
+  Vector ee1(13*2), ee2(13*2);
+  ee1 << Vector::Ones(9*2), Vector::Zero(4*2);
+  ee2 << Vector::Zero(9*2), Vector::Ones(4*2);
+
+  // Check transposeMultiplyAdd for e1
+  VectorValues y = zeros;
+  system.transposeMultiplyAdd(alpha, e1, y);
+  Vector expected_y = alpha * Abar.transpose() * ee1;
+  EXPECT(assert_equal(expected_y, vec(y)));
+
+  // Check transposeMultiplyAdd for e2
+  y = zeros;
+  system.transposeMultiplyAdd(alpha, e2, y);
+  expected_y = alpha * Abar.transpose() * ee2;
+  EXPECT(assert_equal(expected_y, vec(y)));
 
   // Test gradient in y
-  VectorValues expected_gy0 = zeros;
-  VectorValues expected_gy1 = zeros;
-  expected_gy1[2] = Vector2(2., -2.);
-  expected_gy1[4] = Vector2(-2., 2.);
-  expected_gy1[1] = Vector2(3., -3.);
-  expected_gy1[3] = Vector2(-1., 1.);
-  expected_gy1[0] = Vector2(1., -1.);
-  CHECK(assert_equal(expected_gy0,gradient(system,y0)));
-  CHECK(assert_equal(expected_gy1,gradient(system,y1)));
+  auto g = system.gradient(y0);
+  Vector expected_g = Vector::Zero(18);
+  EXPECT(assert_equal(expected_g, vec(g)));
+}
 
-  // Check it numerically for good measure
-  // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
-  //  Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
-  //  Vector expected_g1 = (Vector(18) << 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
-  //      3., -3., 0., 0., -1., 1., 1., -1.);
-  //  CHECK(assert_equal(expected_g1,numerical_g1));
+/* ************************************************************************* */
+  // Test raw vector interface
+TEST( SubgraphPreconditioner, RawVectorAPI )
+{
+  // Build a planar graph
+  GaussianFactorGraph Ab;
+  VectorValues xtrue;
+  size_t N = 3;
+  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+
+  SubgraphPreconditioner system;
+  
+  // Call build, a non-const method needed to make solve work :-(
+  KeyInfo keyInfo(Ab);
+  std::map<Key,Vector> lambda;
+  system.build(Ab, keyInfo, lambda);
+  const auto ordering1 = system.Rc1()->ordering(); // build changed R1 !
+  const auto ordering2 = keyInfo.ordering();
+  const Matrix R1 = system.Rc1()->matrix(ordering1).first;
+
+  // Test that 'solve' does implement x = R^{-1} y
+  Vector y2 = Vector::Zero(18), x2(18), x3(18);
+  y2.head(2) << 100, -100;
+  system.solve(y2, x2);
+  EXPECT(assert_equal(R1.inverse() * y2, x2));
+
+  // I can't get test below to pass!
+  // Test that transposeSolve does implement x = R^{-T} y
+  // system.transposeSolve(y2, x3);
+  // EXPECT(assert_equal(R1.transpose().inverse() * y2, x3));
 }
 
 /* ************************************************************************* */
@@ -184,16 +239,13 @@ TEST( SubgraphPreconditioner, conjugateGradients )
   size_t N = 3;
   boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
 
-  // Get the spanning tree and corresponding ordering
-  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
-  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
-  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
+  // Get the spanning tree
+  GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
+  boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
 
   // Eliminate the spanning tree to build a prior
-  Ordering ordering = planarOrdering(N);
-  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
-  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
+  SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1->eliminateSequential(); // R1*x-c1
+  VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1
 
   // Create Subgraph-preconditioned system
   VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
@@ -203,22 +255,20 @@ TEST( SubgraphPreconditioner, conjugateGradients )
   VectorValues y0 = VectorValues::Zero(xbar);
 
   VectorValues y1 = y0;
-  y1[1] = Vector2(1.0, -1.0);
+  y1[key(2, 2)] = Vector2(1.0, -1.0);
   VectorValues x1 = system.x(y1);
 
   // Solve for the remaining constraints using PCG
   ConjugateGradientParameters parameters;
   VectorValues actual = conjugateGradients<SubgraphPreconditioner,
       VectorValues, Errors>(system, y1, parameters);
-  CHECK(assert_equal(y0,actual));
+  EXPECT(assert_equal(y0,actual));
 
   // Compare with non preconditioned version:
   VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
-  CHECK(assert_equal(xtrue,actual2,1e-4));
+  EXPECT(assert_equal(xtrue,actual2,1e-4));
 }
 
-#endif
-
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From 18d26d12af0e0b8f4ab15c62de869097d65abed3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 6 Apr 2019 22:53:08 -0400
Subject: [PATCH 245/281] Added some xml test files

---
 examples/Data/randomGrid3D.xml | 3414 ++++++++++++++++++++++++++++++++
 examples/Data/toy3D.xml        |  169 ++
 gtsam/slam/dataset.cpp         |    1 +
 3 files changed, 3584 insertions(+)
 create mode 100644 examples/Data/randomGrid3D.xml
 create mode 100644 examples/Data/toy3D.xml

diff --git a/examples/Data/randomGrid3D.xml b/examples/Data/randomGrid3D.xml
new file mode 100644
index 000000000..6a82ce31c
--- /dev/null
+++ b/examples/Data/randomGrid3D.xml
@@ -0,0 +1,3414 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<!DOCTYPE boost_serialization>
+<boost_serialization signature="serialization::archive" version="17">
+<graph class_id="0" tracking_level="0" version="0">
+	<Base class_id="1" tracking_level="0" version="0">
+		<factors_ class_id="2" tracking_level="0" version="0">
+			<count>32</count>
+			<item_version>1</item_version>
+			<item class_id="3" tracking_level="0" version="1">
+				<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
+					<Base class_id="5" tracking_level="0" version="0">
+						<Base class_id="6" tracking_level="0" version="0">
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>0</item>
+								<item>1</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_ class_id="8" tracking_level="0" version="0">
+						<matrix_ class_id="9" tracking_level="0" version="0">
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>3.20776022311033415e+01</item>
+								<item>-3.21030367555546334e+01</item>
+								<item>-4.10921918858809718e+01</item>
+								<item>-2.97297166857905353e+01</item>
+								<item>5.12273135695288744e+01</item>
+								<item>5.32024088136580744e+01</item>
+								<item>7.91786461660617107e+01</item>
+								<item>6.02804880595302208e+01</item>
+								<item>-3.20503372222358784e+00</item>
+								<item>-4.49785699465720157e+00</item>
+								<item>9.06604158034029126e+01</item>
+								<item>5.65784169718906416e+00</item>
+								<item>3.12765298180226239e+01</item>
+								<item>2.69796747523965372e+01</item>
+								<item>6.45574939874757661e+01</item>
+								<item>-3.41086208590283135e+01</item>
+								<item>3.16243899688174857e+01</item>
+								<item>-6.07548743284260979e+01</item>
+								<item>5.22759578856884062e+01</item>
+								<item>2.73875651690404389e+01</item>
+								<item>-6.70630095670253041e+01</item>
+								<item>8.09592862312814248e+01</item>
+								<item>-2.90562646005293850e+01</item>
+								<item>1.97217242898365228e+01</item>
+								<item>1.92063557124931243e+01</item>
+								<item>-3.42017680808024593e+01</item>
+								<item>5.18983240742203904e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>-8.81008786560076906e+00</item>
+								<item>9.93170954106413291e+01</item>
+								<item>7.64832733308121160e+00</item>
+								<item>9.74088247837876793e+01</item>
+								<item>6.98423466470551624e+00</item>
+								<item>2.15114230210294117e+01</item>
+								<item>8.81008786560076906e+00</item>
+								<item>-9.93170954106413291e+01</item>
+								<item>-7.64832733308121160e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>2.08303433971152465e+01</item>
+								<item>9.34532104006540187e+00</item>
+								<item>-9.73589326595991906e+01</item>
+								<item>-9.74088247837876793e+01</item>
+								<item>-6.98423466470551624e+00</item>
+								<item>-2.15114230210294117e+01</item>
+								<item>-2.08303433971152465e+01</item>
+								<item>-9.34532104006540187e+00</item>
+								<item>9.73589326595991906e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-9.96847734946016431e+01</item>
+								<item>-9.41956956915240795e+00</item>
+								<item>3.58611803229205250e+01</item>
+								<item>1.37202268725982691e+02</item>
+								<item>8.31655615217660085e+01</item>
+								<item>-2.96050013164699770e+01</item>
+								<item>5.56978428429618404e+01</item>
+								<item>-1.64096733189184050e+02</item>
+								<item>-6.76911341847846018e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_ class_id="11" tracking_level="0" version="1">
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_1">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>1</item>
+								<item>2</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-5.23677913323444741e+01</item>
+								<item>2.23054746283748031e+01</item>
+								<item>-9.06323879014932388e+00</item>
+								<item>5.52805622243718773e+01</item>
+								<item>7.94539068269289146e+01</item>
+								<item>1.94541508392102571e+01</item>
+								<item>-6.13821741431047201e+01</item>
+								<item>5.56974973159738553e+01</item>
+								<item>-7.78665862090978944e+00</item>
+								<item>-3.50785425178010168e+00</item>
+								<item>-8.59643527732278869e+01</item>
+								<item>4.37949256925254957e+01</item>
+								<item>1.46448346762102268e+01</item>
+								<item>-9.84525875940883211e+00</item>
+								<item>-6.31187688571179351e+01</item>
+								<item>-1.68743787040301001e+01</item>
+								<item>4.96429783126596220e+01</item>
+								<item>6.02933235638437850e+01</item>
+								<item>-8.50324227920073668e+01</item>
+								<item>-8.14408668091479448e+00</item>
+								<item>7.80614049299559465e+00</item>
+								<item>-3.12168328896096590e+01</item>
+								<item>-8.16862254520412101e+00</item>
+								<item>7.01147846710003364e+01</item>
+								<item>4.14413157080187986e+01</item>
+								<item>-1.74987783157454646e+00</item>
+								<item>7.04591203724149580e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>-2.60374020501451531e+00</item>
+								<item>-5.42127786775135831e+01</item>
+								<item>8.39892562474970532e+01</item>
+								<item>-9.97226081614345645e+01</item>
+								<item>7.26909685044996312e+00</item>
+								<item>1.60051631037066500e+00</item>
+								<item>2.60374020501451531e+00</item>
+								<item>5.42127786775135831e+01</item>
+								<item>-8.39892562474970532e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-6.97294474564191535e+00</item>
+								<item>-8.37146036187337330e+01</item>
+								<item>-5.42516652512392241e+01</item>
+								<item>9.97226081614345645e+01</item>
+								<item>-7.26909685044996312e+00</item>
+								<item>-1.60051631037066500e+00</item>
+								<item>6.97294474564191535e+00</item>
+								<item>8.37146036187337330e+01</item>
+								<item>5.42516652512392241e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.08200476037924709e+01</item>
+								<item>1.28950357649989712e+02</item>
+								<item>1.43352339174516089e+02</item>
+								<item>-2.38602247867744630e+01</item>
+								<item>-5.20893463997723600e+01</item>
+								<item>2.84608516229487805e+01</item>
+								<item>6.76429765595175212e+01</item>
+								<item>1.20773294728176111e+02</item>
+								<item>-1.20589752280510609e+02</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_2">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>2</item>
+								<item>3</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-1.96993691143317093e+01</item>
+								<item>-2.72709220997507202e+01</item>
+								<item>4.46131383257576886e+01</item>
+								<item>9.30649268771392428e+01</item>
+								<item>3.22308546546604946e+00</item>
+								<item>-1.69350730671129561e+01</item>
+								<item>-3.00372292195983448e+01</item>
+								<item>4.73051200126789553e+01</item>
+								<item>-6.91348627543176519e+01</item>
+								<item>8.77985890195871921e-01</item>
+								<item>2.95766095687473936e+01</item>
+								<item>-7.96244285285575017e+01</item>
+								<item>7.31430977105361357e+00</item>
+								<item>9.52212779955403192e+01</item>
+								<item>2.32608409272044945e+01</item>
+								<item>-1.06369613385613393e+00</item>
+								<item>-2.30114188850419632e+00</item>
+								<item>-5.58240668503478901e+01</item>
+								<item>8.63825257313862096e+01</item>
+								<item>3.81456041339674954e+01</item>
+								<item>2.72998827723357493e+01</item>
+								<item>3.09037953339841458e+01</item>
+								<item>-1.77447464898715950e+01</item>
+								<item>-1.04957062263738177e+01</item>
+								<item>3.97018822008379715e+01</item>
+								<item>-7.27393475106917862e+01</item>
+								<item>-4.57204758371371796e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.59114589009974594e+01</item>
+								<item>8.09444187944725400e+01</item>
+								<item>3.66079090854293696e+01</item>
+								<item>-7.62990082722604797e+01</item>
+								<item>-1.48208463505055157e+01</item>
+								<item>-6.29190261377608593e+01</item>
+								<item>4.59114589009974594e+01</item>
+								<item>-8.09444187944725400e+01</item>
+								<item>-3.66079090854293696e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.55038380606685564e+01</item>
+								<item>-5.68185144075392898e+01</item>
+								<item>6.85642555729200751e+01</item>
+								<item>7.62990082722604797e+01</item>
+								<item>1.48208463505055157e+01</item>
+								<item>6.29190261377608593e+01</item>
+								<item>4.55038380606685564e+01</item>
+								<item>5.68185144075392898e+01</item>
+								<item>-6.85642555729200751e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-8.55922050845758786e-01</item>
+								<item>1.40043738778271234e+02</item>
+								<item>-3.81613152068798769e+01</item>
+								<item>-5.81227180496655009e+01</item>
+								<item>-3.94713178138164338e+01</item>
+								<item>3.22753989340650662e+01</item>
+								<item>1.32631681079235051e+02</item>
+								<item>-3.12863815406522541e+01</item>
+								<item>-4.03072840384836510e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_3">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>3</item>
+								<item>4</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>8.66048172555662319e+01</item>
+								<item>-1.52328612569721304e+01</item>
+								<item>4.48534445123967629e+01</item>
+								<item>2.02981055828854124e+01</item>
+								<item>-4.93746572823205057e+01</item>
+								<item>-2.74451016941264996e+01</item>
+								<item>4.12224960002507945e+00</item>
+								<item>-6.40449795031895519e+01</item>
+								<item>-5.03376341440287618e+01</item>
+								<item>4.92248012617162161e+01</item>
+								<item>3.93527186553859920e+01</item>
+								<item>-6.89623533878909143e+01</item>
+								<item>-4.08605085624496382e+01</item>
+								<item>5.95700970246627293e+01</item>
+								<item>3.55176671176527492e+01</item>
+								<item>9.26276585311693879e+00</item>
+								<item>-6.84332455006064038e+01</item>
+								<item>4.88721436512163976e+00</item>
+								<item>-4.03054194818879719e+00</item>
+								<item>-1.78980733327891492e+01</item>
+								<item>3.45198923956961394e+01</item>
+								<item>-8.80228615455582002e+01</item>
+								<item>-4.55655125601066899e+01</item>
+								<item>-9.64228954249413128e+00</item>
+								<item>1.13078887631363152e+01</item>
+								<item>-3.24259783798813004e+01</item>
+								<item>8.58793556515557270e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>-8.08548316492842929e+01</item>
+								<item>5.81668844249688917e+01</item>
+								<item>8.89436648998715995e+00</item>
+								<item>4.55868302729299728e+01</item>
+								<item>5.23625800953892124e+01</item>
+								<item>7.19722245829668879e+01</item>
+								<item>8.08548316492842929e+01</item>
+								<item>-5.81668844249688917e+01</item>
+								<item>-8.89436648998715995e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>3.72066809139564114e+01</item>
+								<item>6.22476807764455131e+01</item>
+								<item>-6.88541148612105047e+01</item>
+								<item>-4.55868302729299728e+01</item>
+								<item>-5.23625800953892124e+01</item>
+								<item>-7.19722245829668879e+01</item>
+								<item>-3.72066809139564114e+01</item>
+								<item>-6.22476807764455131e+01</item>
+								<item>6.88541148612105047e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.49706199136485765e+01</item>
+								<item>2.66285359051983370e+01</item>
+								<item>1.14028652024075640e+02</item>
+								<item>5.86398529753103119e+01</item>
+								<item>8.34746189227890056e+00</item>
+								<item>1.60810773504427289e+02</item>
+								<item>1.79694799386287997e+02</item>
+								<item>-4.53728709677613509e+01</item>
+								<item>-1.70780906392258842e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_4">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>4</item>
+								<item>5</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>6.43003424386522084e+01</item>
+								<item>1.19796180071396279e+01</item>
+								<item>4.55761542738693137e+01</item>
+								<item>-6.52039759607597773e+01</item>
+								<item>5.89636417155482349e+01</item>
+								<item>1.80719252805888360e+01</item>
+								<item>-1.51516354244859457e+01</item>
+								<item>-5.00500232613797280e+01</item>
+								<item>-5.34352949392747476e+01</item>
+								<item>7.42583468150113362e+01</item>
+								<item>7.45315542640838657e+00</item>
+								<item>-5.24573072801267415e+01</item>
+								<item>4.09274245687232323e+01</item>
+								<item>-1.68412174720425938e+01</item>
+								<item>-1.36705786744805522e+01</item>
+								<item>2.70644906205976383e+01</item>
+								<item>-8.31804320954470597e+01</item>
+								<item>4.33744783589924054e+01</item>
+								<item>-1.22788002717957223e+01</item>
+								<item>-6.11064170046755617e+00</item>
+								<item>-7.16592171005377452e+01</item>
+								<item>-5.30800076837930419e+01</item>
+								<item>-7.82610172559931669e+01</item>
+								<item>2.92797087670289500e+01</item>
+								<item>-2.22315273959740871e+01</item>
+								<item>-2.13187310855278938e+01</item>
+								<item>-6.26781275987031279e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>6.86610695050494400e+01</item>
+								<item>1.26119199040840346e+01</item>
+								<item>7.16002584545316836e+01</item>
+								<item>-7.26277336162735736e+01</item>
+								<item>1.63646703530006263e+01</item>
+								<item>6.67638365734117087e+01</item>
+								<item>-6.86610695050494400e+01</item>
+								<item>-1.26119199040840346e+01</item>
+								<item>-7.16002584545316836e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-3.29694467444833483e+00</item>
+								<item>-9.78424092128285139e+01</item>
+								<item>2.03959092771819215e+01</item>
+								<item>7.26277336162735736e+01</item>
+								<item>-1.63646703530006263e+01</item>
+								<item>-6.67638365734117087e+01</item>
+								<item>3.29694467444833483e+00</item>
+								<item>9.78424092128285139e+01</item>
+								<item>-2.03959092771819215e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.74531092467603024e+01</item>
+								<item>-9.68528064078233442e-01</item>
+								<item>-1.43955992851650620e+01</item>
+								<item>-3.71890171910416072e+01</item>
+								<item>2.70827825121624883e+01</item>
+								<item>1.59657441575989878e+02</item>
+								<item>-1.61093211332558155e+02</item>
+								<item>-2.36356649401689936e+01</item>
+								<item>-3.50656643781457547e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_5">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>5</item>
+								<item>6</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-6.40637543398504619e+01</item>
+								<item>-1.35919926214779476e+01</item>
+								<item>-7.55586787939104454e+01</item>
+								<item>7.49591051894634859e+01</item>
+								<item>-1.51203828101076443e+01</item>
+								<item>-6.04179319959996945e+01</item>
+								<item>1.63126149967003826e+01</item>
+								<item>-3.67352169146467711e+00</item>
+								<item>-1.49855747686980401e+01</item>
+								<item>-7.64033153697704415e-01</item>
+								<item>5.83204655642280656e+00</item>
+								<item>-2.26064773215979020e+00</item>
+								<item>-1.21801830224353154e+01</item>
+								<item>9.23282518419687364e+01</item>
+								<item>-3.58808093806811215e+01</item>
+								<item>6.76481543691339766e+01</item>
+								<item>3.42590164955414807e+01</item>
+								<item>6.51923982648987277e+01</item>
+								<item>-2.26801968652302843e+00</item>
+								<item>-9.78783400225560598e+01</item>
+								<item>1.94155506146273105e+01</item>
+								<item>1.64832141235940597e+01</item>
+								<item>2.49817026599325320e+00</item>
+								<item>-1.62466150125265507e+01</item>
+								<item>-7.07730069025061255e+01</item>
+								<item>1.59540577889256383e+01</item>
+								<item>6.50575730811547004e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>-2.60428350122129437e+01</item>
+								<item>-6.97694761102178944e+01</item>
+								<item>6.67382270354284231e+01</item>
+								<item>-9.26426416957655618e+01</item>
+								<item>3.75218214997053465e+01</item>
+								<item>3.07471152699694139e+00</item>
+								<item>2.60428350122129437e+01</item>
+								<item>6.97694761102178944e+01</item>
+								<item>-6.67382270354284231e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-2.71866085445878198e+01</item>
+								<item>-6.10273144964611944e+01</item>
+								<item>-7.44080318325472234e+01</item>
+								<item>9.26426416957655618e+01</item>
+								<item>-3.75218214997053465e+01</item>
+								<item>-3.07471152699694139e+00</item>
+								<item>2.71866085445878198e+01</item>
+								<item>6.10273144964611944e+01</item>
+								<item>7.44080318325472234e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.03933661487596595e+02</item>
+								<item>4.68471191704998802e+01</item>
+								<item>1.18875204735324314e+01</item>
+								<item>-2.97055153316788463e+01</item>
+								<item>7.27443939906840598e+01</item>
+								<item>7.23444094058463492e+01</item>
+								<item>3.82415703375670404e+01</item>
+								<item>-2.27415768204949451e+01</item>
+								<item>-3.07813391103898937e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_6">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>6</item>
+								<item>7</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.85630374271583953e+01</item>
+								<item>-5.29313735130414926e+01</item>
+								<item>3.66303674355102942e+01</item>
+								<item>1.04645716047590440e+01</item>
+								<item>5.73485858047936077e+01</item>
+								<item>-5.08591115829850366e+01</item>
+								<item>9.38445241664948213e+01</item>
+								<item>-1.36042785531324792e+01</item>
+								<item>-2.31303222840427054e+01</item>
+								<item>-9.56612084104992455e+00</item>
+								<item>-2.98701442261074703e+01</item>
+								<item>7.62840716168778670e+01</item>
+								<item>-1.92910486636706899e+01</item>
+								<item>-5.04699434611332478e+01</item>
+								<item>3.46538201809487560e+01</item>
+								<item>3.08818026153629788e+01</item>
+								<item>7.17821170583860777e+01</item>
+								<item>5.45013702935509130e+01</item>
+								<item>-5.34080385938858839e+01</item>
+								<item>6.23631140795577465e+01</item>
+								<item>4.43545654716690905e+01</item>
+								<item>-7.95485560661077074e+01</item>
+								<item>-2.30201810552558968e+01</item>
+								<item>-5.51074857191607634e+01</item>
+								<item>1.18903119380668851e+01</item>
+								<item>-2.67045605220385767e+01</item>
+								<item>-2.32776001751549053e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.79872266803328138e+01</item>
+								<item>8.64209977399809475e+01</item>
+								<item>-4.69879859748907549e+01</item>
+								<item>9.82455450157955568e+01</item>
+								<item>-1.33902787243874393e+01</item>
+								<item>1.29812680517931049e+01</item>
+								<item>-1.79872266803328138e+01</item>
+								<item>-8.64209977399809475e+01</item>
+								<item>4.69879859748907549e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>4.92671908064704667e+00</item>
+								<item>-4.84985730234346519e+01</item>
+								<item>-8.73133200250165942e+01</item>
+								<item>-9.82455450157955568e+01</item>
+								<item>1.33902787243874393e+01</item>
+								<item>-1.29812680517931049e+01</item>
+								<item>-4.92671908064704667e+00</item>
+								<item>4.84985730234346519e+01</item>
+								<item>8.73133200250165942e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-8.68503697620646733e+01</item>
+								<item>-6.63265206223623283e-01</item>
+								<item>5.77899517416623070e+01</item>
+								<item>1.54728954524297080e+02</item>
+								<item>-7.36729123490597857e+01</item>
+								<item>-4.33714485898333280e+01</item>
+								<item>-8.08065933920789270e+00</item>
+								<item>-1.49263004123200204e+02</item>
+								<item>1.24142002075972883e+02</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_7">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>7</item>
+								<item>8</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>4.74062871911341759e+01</item>
+								<item>5.71222009314007693e+01</item>
+								<item>-2.90538462259461063e+01</item>
+								<item>4.09276699085499303e+01</item>
+								<item>3.99974137866047528e+01</item>
+								<item>-1.99073676332006535e+01</item>
+								<item>-7.78029399881401531e+01</item>
+								<item>5.27745236895511312e+01</item>
+								<item>-3.37039333639074385e+01</item>
+								<item>-9.12848490867447815e+00</item>
+								<item>-6.92290536396862421e+01</item>
+								<item>-2.32360574632871630e+00</item>
+								<item>1.58993825041621264e+01</item>
+								<item>5.31010116114126660e+01</item>
+								<item>-6.55564733827918502e+01</item>
+								<item>-3.41955054720470475e+00</item>
+								<item>4.69927897084473187e+01</item>
+								<item>7.43534572740371971e+01</item>
+								<item>-6.28440645552295862e+01</item>
+								<item>4.27829075558255525e+01</item>
+								<item>5.46299457034644860e+01</item>
+								<item>7.56316908352858093e+01</item>
+								<item>1.42185369226067397e+01</item>
+								<item>5.51031175481571864e+01</item>
+								<item>2.63284733063378651e+00</item>
+								<item>2.23353419791525312e+01</item>
+								<item>4.20873574259948739e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.80585028832817862e+01</item>
+								<item>-9.52829540657210430e+01</item>
+								<item>-2.43936290478516966e+01</item>
+								<item>-9.74947022342857252e+01</item>
+								<item>-2.06160114466111395e+01</item>
+								<item>8.35243127964282905e+00</item>
+								<item>-1.80585028832817862e+01</item>
+								<item>9.52829540657210430e+01</item>
+								<item>2.43936290478516966e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-1.29874366163019737e+01</item>
+								<item>2.22741719608807998e+01</item>
+								<item>-9.66187753679099757e+01</item>
+								<item>9.74947022342857252e+01</item>
+								<item>2.06160114466111395e+01</item>
+								<item>-8.35243127964282905e+00</item>
+								<item>1.29874366163019737e+01</item>
+								<item>-2.22741719608807998e+01</item>
+								<item>9.66187753679099757e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>7.39787443001634273e+01</item>
+								<item>-3.29520218737291373e+01</item>
+								<item>1.75142945360573606e+02</item>
+								<item>-4.89974435425370416e+01</item>
+								<item>-9.39530685963477481e+01</item>
+								<item>-3.92888663032043226e+01</item>
+								<item>4.46158732311312889e+01</item>
+								<item>1.62422063134678211e+02</item>
+								<item>-1.51571321993876627e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_8">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>8</item>
+								<item>9</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>9.41130157063151671e+01</item>
+								<item>1.30096002577236085e+01</item>
+								<item>-9.65142385029295902e+00</item>
+								<item>-2.97023439769390656e+01</item>
+								<item>3.10157410489098062e-01</item>
+								<item>4.06407354840771795e+00</item>
+								<item>-9.58311576289570155e+00</item>
+								<item>9.66152903465384156e+01</item>
+								<item>2.35615250878309794e+01</item>
+								<item>1.77277266667175297e+01</item>
+								<item>-4.94856229862016690e+01</item>
+								<item>8.47852845508831336e+01</item>
+								<item>1.34310978254269742e+01</item>
+								<item>-8.43883523083151488e+01</item>
+								<item>-5.15167787166412623e+01</item>
+								<item>3.41073399110957187e-01</item>
+								<item>2.18139939555895035e+00</item>
+								<item>9.36548913434189423e+00</item>
+								<item>-2.80240621096084510e+01</item>
+								<item>-7.66577929709818839e+00</item>
+								<item>9.19688865464188510e+00</item>
+								<item>-9.38736839860108603e+01</item>
+								<item>-1.81862826844923511e+01</item>
+								<item>1.54239226370280336e+00</item>
+								<item>-8.73344590429244683e+00</item>
+								<item>2.30576115914954443e+01</item>
+								<item>-9.65297250180462356e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-9.99929178544290664e+01</item>
+								<item>6.62091181731783163e-01</item>
+								<item>-9.88946016968861308e-01</item>
+								<item>-6.23564336177379053e-01</item>
+								<item>4.16306145486879302e+01</item>
+								<item>9.09203118110426516e+01</item>
+								<item>9.99929178544290664e+01</item>
+								<item>-6.62091181731783163e-01</item>
+								<item>9.88946016968861308e-01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.01367967132287018e+00</item>
+								<item>9.09200394168724984e+01</item>
+								<item>-4.16235376434812636e+01</item>
+								<item>6.23564336177379053e-01</item>
+								<item>-4.16306145486879302e+01</item>
+								<item>-9.09203118110426516e+01</item>
+								<item>-1.01367967132287018e+00</item>
+								<item>-9.09200394168724984e+01</item>
+								<item>4.16235376434812636e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>5.55282235936336566e+00</item>
+								<item>-1.76495493086246057e+02</item>
+								<item>-9.69626169859593645e+00</item>
+								<item>-1.18086675146736617e+01</item>
+								<item>9.21060305839002780e+01</item>
+								<item>5.32168712759193419e+00</item>
+								<item>1.99148216329777483e+02</item>
+								<item>1.06989143861811957e+01</item>
+								<item>-5.26829527913709494e+00</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_9">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>9</item>
+								<item>10</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-8.63087793249897430e+01</item>
+								<item>-1.92823330270961684e+01</item>
+								<item>-4.42211315553459627e+01</item>
+								<item>2.21505354932965801e+01</item>
+								<item>-2.63320996942306031e+01</item>
+								<item>-5.69788664986630025e+01</item>
+								<item>-4.53775288609686314e+01</item>
+								<item>2.58526272351369890e+01</item>
+								<item>5.53658756361871127e+01</item>
+								<item>-1.54315640909267664e+01</item>
+								<item>-4.32654755327350813e+01</item>
+								<item>1.97045712499729326e+01</item>
+								<item>7.39853470327932428e+01</item>
+								<item>-5.76879953686396973e+01</item>
+								<item>2.69216116215563304e+01</item>
+								<item>6.54801163688819514e+01</item>
+								<item>5.53812651216026666e+01</item>
+								<item>-2.49089008945160231e+01</item>
+								<item>-9.71189061792836705e-01</item>
+								<item>-7.25245368327360609e+01</item>
+								<item>4.96429238183381116e+01</item>
+								<item>6.85930292060070612e-01</item>
+								<item>-1.12841816293779988e+01</item>
+								<item>-7.69094846357958062e+01</item>
+								<item>-5.16757922033419734e-02</item>
+								<item>-6.79147932987382035e+01</item>
+								<item>-4.02434226181160639e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>7.83449748282976799e+01</item>
+								<item>6.13201754047114846e+01</item>
+								<item>1.00946028891096997e+01</item>
+								<item>-1.98027611608243852e+01</item>
+								<item>9.23644092983748521e+00</item>
+								<item>9.75834966034573341e+01</item>
+								<item>-7.83449748282976799e+01</item>
+								<item>-6.13201754047114846e+01</item>
+								<item>-1.00946028891096997e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>5.89059892503364324e+01</item>
+								<item>-7.84507759508154265e+01</item>
+								<item>1.93793752003053505e+01</item>
+								<item>1.98027611608243852e+01</item>
+								<item>-9.23644092983748521e+00</item>
+								<item>-9.75834966034573341e+01</item>
+								<item>-5.89059892503364324e+01</item>
+								<item>7.84507759508154265e+01</item>
+								<item>-1.93793752003053505e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-1.06986312175636783e+02</item>
+								<item>1.28415573927117009e+02</item>
+								<item>5.26747761738054123e+01</item>
+								<item>-8.33248123208498015e+01</item>
+								<item>-6.72578872464476092e+01</item>
+								<item>1.08240234163658471e+02</item>
+								<item>-1.79030185090507175e+01</item>
+								<item>-1.01966657528716965e+02</item>
+								<item>5.84227812301104876e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_10">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>10</item>
+								<item>11</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.17302291731448682e+00</item>
+								<item>2.23736436785092039e+01</item>
+								<item>8.70065012231049764e+01</item>
+								<item>7.67707833816357947e+01</item>
+								<item>5.73162887867323292e+01</item>
+								<item>-1.32881532104113664e+00</item>
+								<item>-2.51982778355239070e+01</item>
+								<item>-7.72901736529863381e+00</item>
+								<item>4.53049731782644329e+01</item>
+								<item>8.54937155952362531e+01</item>
+								<item>-4.11223504877864485e+01</item>
+								<item>2.12416634370425328e+01</item>
+								<item>-2.90259802009985357e+01</item>
+								<item>-8.76138896821732693e+00</item>
+								<item>-5.06100435935214055e+00</item>
+								<item>-3.81597643193563130e+01</item>
+								<item>-9.02600433698408864e+01</item>
+								<item>7.99447271884073940e-01</item>
+								<item>-2.24916749275050378e+01</item>
+								<item>2.24299023473424732e+01</item>
+								<item>3.83080111263447165e+01</item>
+								<item>-5.03856743318980023e+01</item>
+								<item>7.20086537445330919e+01</item>
+								<item>-4.63731853719149143e+01</item>
+								<item>2.85914723565250490e+01</item>
+								<item>-2.34837711903294171e+01</item>
+								<item>-7.92472197577816644e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-2.63903093070078647e+01</item>
+								<item>-2.67183596992454575e+01</item>
+								<item>9.26805310173727577e+01</item>
+								<item>-8.93547271763020774e+01</item>
+								<item>4.29554589851731663e+01</item>
+								<item>-1.30599109729635039e+01</item>
+								<item>2.63903093070078647e+01</item>
+								<item>2.67183596992454575e+01</item>
+								<item>-9.26805310173727577e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-3.63219534982506076e+01</item>
+								<item>-8.62609865371063478e+01</item>
+								<item>-3.52101959056759028e+01</item>
+								<item>8.93547271763020774e+01</item>
+								<item>-4.29554589851731663e+01</item>
+								<item>1.30599109729635039e+01</item>
+								<item>3.63219534982506076e+01</item>
+								<item>8.62609865371063478e+01</item>
+								<item>3.52101959056759028e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-1.04067559180773088e+01</item>
+								<item>7.90108728350369383e-01</item>
+								<item>5.78189525430708571e+01</item>
+								<item>-1.16280923178694167e+02</item>
+								<item>8.10714898076704600e+01</item>
+								<item>7.53829806570444134e+01</item>
+								<item>-5.78201683295110769e+01</item>
+								<item>6.19589188064910701e+01</item>
+								<item>-1.33505720595716895e+02</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_11">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>11</item>
+								<item>12</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>3.68344254692879192e+00</item>
+								<item>4.86691203808377892e+00</item>
+								<item>-1.57231292941544805e+01</item>
+								<item>3.72290369400325147e+01</item>
+								<item>2.06331817836064921e+01</item>
+								<item>-8.89534944072540839e+01</item>
+								<item>-8.53296212948073389e+01</item>
+								<item>4.59306709423131920e+01</item>
+								<item>-2.45010409318042690e+01</item>
+								<item>2.57025147685637876e+01</item>
+								<item>2.56000347344376031e+01</item>
+								<item>-9.16529948262070491e+01</item>
+								<item>2.63288801817711970e-01</item>
+								<item>6.59382843634221683e+00</item>
+								<item>1.98864164108950128e+01</item>
+								<item>-3.68109153857383404e+01</item>
+								<item>-8.63480489179755466e+01</item>
+								<item>-3.21511205713796500e+01</item>
+								<item>8.19562719339757138e+01</item>
+								<item>-5.68704775649636858e+01</item>
+								<item>6.94175446640136773e+00</item>
+								<item>-5.03482257069523769e+01</item>
+								<item>-7.72158009035328661e+01</item>
+								<item>-3.65965187643956114e+01</item>
+								<item>-7.19547325119217618e+00</item>
+								<item>-9.46558534641098959e+00</item>
+								<item>-4.77765582369805042e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-7.29991262018860425e+01</item>
+								<item>6.43134564832916169e+01</item>
+								<item>-2.31280541536218962e+01</item>
+								<item>-5.24720785637083011e+01</item>
+								<item>-7.44213220616246360e+01</item>
+								<item>-4.13297446617319864e+01</item>
+								<item>7.29991262018860425e+01</item>
+								<item>-6.43134564832916169e+01</item>
+								<item>2.31280541536218962e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.37927910159324512e+01</item>
+								<item>-1.80345817187895037e+01</item>
+								<item>8.80735222258265793e+01</item>
+								<item>5.24720785637083011e+01</item>
+								<item>7.44213220616246360e+01</item>
+								<item>4.13297446617319864e+01</item>
+								<item>4.37927910159324512e+01</item>
+								<item>1.80345817187895037e+01</item>
+								<item>-8.80735222258265793e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>9.48709591330575535e+01</item>
+								<item>9.60518679589807789e+01</item>
+								<item>-5.19581436253384297e+01</item>
+								<item>2.54957713470891250e+01</item>
+								<item>-1.34158356561916207e+02</item>
+								<item>-2.25547332522903439e+01</item>
+								<item>1.09221138500000791e+02</item>
+								<item>-4.57450052559468858e+01</item>
+								<item>-6.82129290695598485e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_12">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>12</item>
+								<item>13</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.93941690967042213e+01</item>
+								<item>-3.90998618366849584e+01</item>
+								<item>1.63698620253740224e+00</item>
+								<item>1.66123034210985914e+01</item>
+								<item>8.85555525420248557e+01</item>
+								<item>2.63934202043697148e+01</item>
+								<item>-8.61979615512473032e+01</item>
+								<item>1.74318161103396001e+01</item>
+								<item>-3.92907393347052789e+01</item>
+								<item>-7.36585675537442341e+01</item>
+								<item>5.45643959900256945e+01</item>
+								<item>-4.73596788713465688e+00</item>
+								<item>1.23230795012408407e+01</item>
+								<item>3.75671217770436385e+01</item>
+								<item>-8.32914041112130690e+01</item>
+								<item>-4.08563087068878872e+01</item>
+								<item>8.53843323058891990e+00</item>
+								<item>3.64960868565984242e+01</item>
+								<item>-6.45808132649689668e+01</item>
+								<item>-7.41043072657268738e+01</item>
+								<item>-2.22937919750649804e+00</item>
+								<item>-1.10797951271104100e+00</item>
+								<item>-1.86389067743133623e+01</item>
+								<item>-4.83331185591061043e+01</item>
+								<item>2.23431972515867372e+01</item>
+								<item>-4.99220528272985842e+00</item>
+								<item>-8.44039814228390526e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>5.32207681440972067e+01</item>
+								<item>-3.06680603945050478e+01</item>
+								<item>7.89114688102511224e+01</item>
+								<item>-5.56684880542022569e+01</item>
+								<item>-8.29014582098623265e+01</item>
+								<item>5.32613034365356164e+00</item>
+								<item>-5.32207681440972067e+01</item>
+								<item>3.06680603945050478e+01</item>
+								<item>-7.89114688102511224e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>6.37853374680371630e+01</item>
+								<item>-4.67634290692784660e+01</item>
+								<item>-6.11932383991170568e+01</item>
+								<item>5.56684880542022569e+01</item>
+								<item>8.29014582098623265e+01</item>
+								<item>-5.32613034365356164e+00</item>
+								<item>-6.37853374680371630e+01</item>
+								<item>4.67634290692784660e+01</item>
+								<item>6.11932383991170568e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-6.90389218165145877e+01</item>
+								<item>4.83379444724982577e+01</item>
+								<item>1.61042728694415416e+02</item>
+								<item>-1.53499694264126390e+02</item>
+								<item>-6.29210660700233788e+01</item>
+								<item>-1.36328982272427429e-01</item>
+								<item>-7.32570951739669738e+01</item>
+								<item>-6.73028752573054874e+01</item>
+								<item>-7.84207879822722163e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_13">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>13</item>
+								<item>14</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>6.06804300720567937e+01</item>
+								<item>6.78223389316968621e+01</item>
+								<item>1.14214673416742691e+01</item>
+								<item>2.85045083366314387e+01</item>
+								<item>2.67345752695483867e+01</item>
+								<item>9.28156907783291274e+00</item>
+								<item>-3.79052729172203868e+01</item>
+								<item>4.99857449838935750e+01</item>
+								<item>-7.77096553357260120e+01</item>
+								<item>-7.82277784247720263e+01</item>
+								<item>5.40246939742984793e+01</item>
+								<item>2.32633737948870305e+01</item>
+								<item>2.47956201341556586e+01</item>
+								<item>-1.05317240685613065e+01</item>
+								<item>9.52360095902563302e+01</item>
+								<item>-1.29078256939381077e+01</item>
+								<item>9.88755369559818398e+00</item>
+								<item>1.89881719971782701e+01</item>
+								<item>9.09983932556493968e+00</item>
+								<item>4.26215664457260388e+01</item>
+								<item>1.04271324229186870e+01</item>
+								<item>-6.00988012459201499e+01</item>
+								<item>-6.92347566083080466e+01</item>
+								<item>1.36255057217382767e+01</item>
+								<item>5.89311254734215737e+01</item>
+								<item>-4.94916274785966195e+01</item>
+								<item>-5.89796648826666967e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-2.89030917873941853e+01</item>
+								<item>8.70749520313458305e+01</item>
+								<item>-3.97814531392241904e+01</item>
+								<item>-9.56630827209552450e+01</item>
+								<item>-2.78467078053438328e+01</item>
+								<item>8.55192777842975183e+00</item>
+								<item>2.89030917873941853e+01</item>
+								<item>-8.70749520313458305e+01</item>
+								<item>3.97814531392241904e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-3.63123800557649812e+00</item>
+								<item>4.05279359595652906e+01</item>
+								<item>9.13471429077244466e+01</item>
+								<item>9.56630827209552450e+01</item>
+								<item>2.78467078053438328e+01</item>
+								<item>-8.55192777842975183e+00</item>
+								<item>3.63123800557649812e+00</item>
+								<item>-4.05279359595652906e+01</item>
+								<item>-9.13471429077244466e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.43780450624784670e+01</item>
+								<item>-6.63129258801445189e+01</item>
+								<item>4.67182022743824987e+00</item>
+								<item>-2.52301003257763057e+01</item>
+								<item>-9.40350333946463337e+01</item>
+								<item>-1.71054540098886392e+01</item>
+								<item>9.90721820600476235e+01</item>
+								<item>-1.66885836907954435e+01</item>
+								<item>5.08294565998758543e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_14">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>14</item>
+								<item>15</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-2.76355843137293995e+01</item>
+								<item>-7.18186699571359952e+01</item>
+								<item>3.07651775764515065e+01</item>
+								<item>-6.40640399324160086e+01</item>
+								<item>-2.68919032866484322e+01</item>
+								<item>9.38443754617159698e+00</item>
+								<item>-7.15464009647350991e+01</item>
+								<item>4.97633461308791567e+01</item>
+								<item>-2.49225935590679519e+01</item>
+								<item>1.43932895637872207e+01</item>
+								<item>-2.88438285303894943e+01</item>
+								<item>6.70843814351506182e+01</item>
+								<item>-2.31361564910724091e+01</item>
+								<item>9.02877315480068603e+01</item>
+								<item>3.51897296575981642e+01</item>
+								<item>1.03017517985169054e+01</item>
+								<item>-1.55151545070516619e+01</item>
+								<item>6.47165615401468699e+01</item>
+								<item>5.11107200952757168e+01</item>
+								<item>4.26513944039867638e+01</item>
+								<item>5.62224381230135037e+01</item>
+								<item>-6.85270728206629229e+01</item>
+								<item>-1.63576632701424352e+01</item>
+								<item>1.39839419419296167e+01</item>
+								<item>4.30853333607984084e+01</item>
+								<item>-1.81999495152501289e+01</item>
+								<item>-7.11401633803200042e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-8.15059339771809164e+01</item>
+								<item>-5.54598536753216109e+01</item>
+								<item>-1.67626774955357369e+01</item>
+								<item>-1.04808841952049807e+01</item>
+								<item>4.25686446848321935e+01</item>
+								<item>-8.98780371179923492e+01</item>
+								<item>8.15059339771809164e+01</item>
+								<item>5.54598536753216109e+01</item>
+								<item>1.67626774955357369e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>5.69818724946287603e+01</item>
+								<item>-7.14990567770542356e+01</item>
+								<item>-4.05086544703406517e+01</item>
+								<item>1.04808841952049807e+01</item>
+								<item>-4.25686446848321935e+01</item>
+								<item>8.98780371179923492e+01</item>
+								<item>-5.69818724946287603e+01</item>
+								<item>7.14990567770542356e+01</item>
+								<item>4.05086544703406517e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>2.31230716791378725e+01</item>
+								<item>2.46912491883496621e+01</item>
+								<item>3.19609408881894597e+00</item>
+								<item>1.52981265769411010e+01</item>
+								<item>1.32876273309615431e+01</item>
+								<item>2.19836964146123970e+00</item>
+								<item>2.74814898754146313e+01</item>
+								<item>-2.79167095337952915e+01</item>
+								<item>5.37372498127196874e+00</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_15">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>15</item>
+								<item>16</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-5.10421334268497020e+01</item>
+								<item>-3.97471595810313332e+01</item>
+								<item>-1.64389363316277826e+00</item>
+								<item>2.54089618298921245e+01</item>
+								<item>-3.15327493370988705e+01</item>
+								<item>9.13980507822707153e+01</item>
+								<item>-5.91789668556331776e+01</item>
+								<item>-4.81012757100621684e+01</item>
+								<item>1.65563384387473489e+00</item>
+								<item>-6.78890977332195860e+01</item>
+								<item>-3.50650650222022904e+01</item>
+								<item>-8.69105008709039062e+00</item>
+								<item>-5.80472933502369273e+01</item>
+								<item>6.88255140605843536e+01</item>
+								<item>3.93666342323419940e+01</item>
+								<item>4.37233513120340049e+01</item>
+								<item>4.71317258377345070e+01</item>
+								<item>1.72241668652535651e+01</item>
+								<item>-4.44141172746378885e+01</item>
+								<item>7.81566561420669927e+01</item>
+								<item>-4.26269969902096406e+01</item>
+								<item>1.02994128500864690e+01</item>
+								<item>-1.21754201166700025e+01</item>
+								<item>-9.79654030213998794e+00</item>
+								<item>-3.57503904294827279e+01</item>
+								<item>2.58360247551576450e+01</item>
+								<item>8.83514468333149239e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-8.11187605223852870e+01</item>
+								<item>-5.83599337897738124e+01</item>
+								<item>3.72354929672284474e+00</item>
+								<item>4.13366904132922599e+01</item>
+								<item>-5.27200270517053724e+01</item>
+								<item>7.42420148793328991e+01</item>
+								<item>8.11187605223852870e+01</item>
+								<item>5.83599337897738124e+01</item>
+								<item>-3.72354929672284474e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.13645345312568224e+01</item>
+								<item>6.17633943021323262e+01</item>
+								<item>6.68898976474907556e+01</item>
+								<item>-4.13366904132922599e+01</item>
+								<item>5.27200270517053724e+01</item>
+								<item>-7.42420148793328991e+01</item>
+								<item>4.13645345312568224e+01</item>
+								<item>-6.17633943021323262e+01</item>
+								<item>-6.68898976474907556e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>6.98804353612945732e+01</item>
+								<item>-9.46593060564561881e+01</item>
+								<item>-1.56916014852066439e+02</item>
+								<item>-3.53367495425767473e+01</item>
+								<item>-1.16911080827186709e+02</item>
+								<item>7.34112761651981316e+01</item>
+								<item>2.36033204137547230e+01</item>
+								<item>1.27622949564988858e+02</item>
+								<item>-4.72506285651327431e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_16">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>16</item>
+								<item>17</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-5.85435867217505290e+01</item>
+								<item>2.33746012476837990e+01</item>
+								<item>-5.77863483723881544e+01</item>
+								<item>-5.63329646716383934e+01</item>
+								<item>-7.47638541726440593e+01</item>
+								<item>3.41977282497739097e+01</item>
+								<item>-4.10879444630993547e+01</item>
+								<item>7.01966859196727366e+00</item>
+								<item>-3.18903501380708931e+01</item>
+								<item>7.49027535040175820e+01</item>
+								<item>-1.88037812404022127e+01</item>
+								<item>-5.50573390958028739e+01</item>
+								<item>-1.05105126900153234e+01</item>
+								<item>3.04370847103721403e+01</item>
+								<item>2.74657650627104459e+01</item>
+								<item>-5.06989372188159564e+01</item>
+								<item>-7.70752893724180126e+01</item>
+								<item>-2.65036593071036783e+01</item>
+								<item>-8.31624785461406368e+00</item>
+								<item>7.75118867469858053e+00</item>
+								<item>-5.96862946091213260e+01</item>
+								<item>3.40815941189988152e+01</item>
+								<item>-5.16283223754141716e+01</item>
+								<item>-6.67021661737265248e+01</item>
+								<item>4.67870803895238083e+01</item>
+								<item>-6.21976850537821235e+01</item>
+								<item>4.44353424269327562e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-9.98522291990096846e+01</item>
+								<item>-2.17344032867269910e+00</item>
+								<item>4.98081129196222250e+00</item>
+								<item>-1.69510675143138556e+00</item>
+								<item>-7.46243396331473150e+01</item>
+								<item>-6.65457327513788073e+01</item>
+								<item>9.98522291990096846e+01</item>
+								<item>2.17344032867269910e+00</item>
+								<item>-4.98081129196222250e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>5.16322932762926534e+00</item>
+								<item>-6.65318276575533361e+01</item>
+								<item>7.44772245149880234e+01</item>
+								<item>1.69510675143138556e+00</item>
+								<item>7.46243396331473150e+01</item>
+								<item>6.65457327513788073e+01</item>
+								<item>-5.16322932762926534e+00</item>
+								<item>6.65318276575533361e+01</item>
+								<item>-7.44772245149880234e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-3.50474015754750923e+01</item>
+								<item>-2.85468905697825797e+01</item>
+								<item>-8.26608268609589487e+01</item>
+								<item>7.28338697761986396e+01</item>
+								<item>-1.03233140690458100e+02</item>
+								<item>-6.32144076335772187e+00</item>
+								<item>4.02504937228882511e+01</item>
+								<item>1.40718088483059116e+01</item>
+								<item>7.44299004116042227e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_17">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>17</item>
+								<item>18</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-8.82393493416751795e+01</item>
+								<item>-6.12481728289156635e+00</item>
+								<item>6.45898243198292366e-01</item>
+								<item>3.86841248560220095e+01</item>
+								<item>-4.52281770219861770e+01</item>
+								<item>-4.30849606407744687e+01</item>
+								<item>-2.62811256127213326e+01</item>
+								<item>-5.90797411700203270e+01</item>
+								<item>-5.09550094510600857e+01</item>
+								<item>4.68685179819303457e+01</item>
+								<item>-2.73625956906887646e+00</item>
+								<item>1.87456681275038339e+00</item>
+								<item>7.09242852066529537e+01</item>
+								<item>-3.96201491012311280e+01</item>
+								<item>4.26233675831482230e+01</item>
+								<item>-5.26328438238840803e+01</item>
+								<item>-5.34225198350244597e+01</item>
+								<item>6.12486839444391720e+01</item>
+								<item>-3.76709521788862034e+00</item>
+								<item>9.35400534400023815e+01</item>
+								<item>-3.47030072986988287e+01</item>
+								<item>3.20432748703922687e+00</item>
+								<item>2.41487264929615755e+01</item>
+								<item>7.47759826419143678e+01</item>
+								<item>-2.25246616168526836e+00</item>
+								<item>-2.57366654666450962e+01</item>
+								<item>-5.63865719112063317e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>9.03046090553630876e+01</item>
+								<item>2.33581936525576914e+01</item>
+								<item>-3.60481951371722431e+01</item>
+								<item>1.31196185024049097e+01</item>
+								<item>-9.49105504326207523e+01</item>
+								<item>-2.86332503730943557e+01</item>
+								<item>-9.03046090553630876e+01</item>
+								<item>-2.33581936525576914e+01</item>
+								<item>3.60481951371722431e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.09017504968844747e+01</item>
+								<item>2.11277591302666750e+01</item>
+								<item>-8.87731074167399186e+01</item>
+								<item>-1.31196185024049097e+01</item>
+								<item>9.49105504326207523e+01</item>
+								<item>2.86332503730943557e+01</item>
+								<item>4.09017504968844747e+01</item>
+								<item>-2.11277591302666750e+01</item>
+								<item>8.87731074167399186e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>4.26251548517658989e+01</item>
+								<item>-5.58446106332349572e+01</item>
+								<item>-4.99134201995332649e+00</item>
+								<item>7.19670254876823492e+01</item>
+								<item>-1.87430011171921045e+01</item>
+								<item>-5.57531304221817550e+01</item>
+								<item>-9.47135556308831283e+00</item>
+								<item>-7.80687492596315167e+01</item>
+								<item>5.77908380638228465e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_18">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>18</item>
+								<item>19</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>6.21423423624779545e+01</item>
+								<item>1.02374321697767794e+01</item>
+								<item>-2.61952819865811541e+01</item>
+								<item>1.64810134236028816e+01</item>
+								<item>-9.33026901138422886e+01</item>
+								<item>-2.97993293115291316e+01</item>
+								<item>6.47591857034336442e+01</item>
+								<item>2.72657426895055011e+01</item>
+								<item>-2.33482872492159856e+01</item>
+								<item>-6.84639855508772825e+01</item>
+								<item>-1.58264297171629540e+01</item>
+								<item>2.10899487969025330e+01</item>
+								<item>-3.01026938649666285e+01</item>
+								<item>2.36828763275138563e+01</item>
+								<item>-9.22944706954510821e+01</item>
+								<item>6.50721593326321113e+01</item>
+								<item>1.34399733570446998e+01</item>
+								<item>-1.47336875153178202e+01</item>
+								<item>1.81996160352434408e+01</item>
+								<item>5.50471142576061965e+01</item>
+								<item>8.12609439867173649e+01</item>
+								<item>3.09138639218085887e+00</item>
+								<item>-1.18364488818499112e+01</item>
+								<item>7.45772273447166401e-02</item>
+								<item>3.87852402909780736e+01</item>
+								<item>-7.92723675773540748e+01</item>
+								<item>4.57949875755143552e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-7.14353163221378225e+01</item>
+								<item>-6.97621833385175307e+01</item>
+								<item>5.49848686450887225e+00</item>
+								<item>4.54649938592219129e+01</item>
+								<item>-4.02948505639650421e+01</item>
+								<item>7.94308463470498509e+01</item>
+								<item>7.14353163221378225e+01</item>
+								<item>6.97621833385175307e+01</item>
+								<item>-5.49848686450887225e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-5.31970855906319784e+01</item>
+								<item>5.92415630606653920e+01</item>
+								<item>6.05021263328172836e+01</item>
+								<item>-4.54649938592219129e+01</item>
+								<item>4.02948505639650421e+01</item>
+								<item>-7.94308463470498509e+01</item>
+								<item>5.31970855906319784e+01</item>
+								<item>-5.92415630606653920e+01</item>
+								<item>-6.05021263328172836e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>8.66602846584249278e+01</item>
+								<item>-1.40571348898168708e+02</item>
+								<item>-1.29029667681538402e+01</item>
+								<item>-4.84107069228954998e+01</item>
+								<item>-6.46593270163667029e+01</item>
+								<item>1.03797317950936275e+02</item>
+								<item>6.32154043923236131e+01</item>
+								<item>1.69243370521808174e+01</item>
+								<item>-9.00005277714220426e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_19">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>19</item>
+								<item>20</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>5.40325063921370585e+01</item>
+								<item>5.12983919091620777e+00</item>
+								<item>4.24856501928409855e+01</item>
+								<item>-6.01366381168498805e+01</item>
+								<item>-6.66348312341534665e+01</item>
+								<item>1.23707663998909716e+01</item>
+								<item>2.51821654645419386e+01</item>
+								<item>-4.49879933093015580e+01</item>
+								<item>6.61923323512717872e+01</item>
+								<item>-8.16765559697753787e+01</item>
+								<item>-1.72226454079742588e+01</item>
+								<item>3.80134460532522880e+01</item>
+								<item>-3.51922609512735107e+01</item>
+								<item>-2.39876166901732191e+01</item>
+								<item>7.97471671594631992e+00</item>
+								<item>-4.83518374187914013e+00</item>
+								<item>8.66273522162050824e+01</item>
+								<item>4.67131848907273408e+01</item>
+								<item>1.17456223558336159e+01</item>
+								<item>-5.59162876565777989e+00</item>
+								<item>8.16530126090797523e+01</item>
+								<item>6.71811935630704937e+01</item>
+								<item>-7.04411581080473894e+01</item>
+								<item>-2.09183319463410733e+01</item>
+								<item>-1.56755267575532145e+01</item>
+								<item>1.18327312907135145e+01</item>
+								<item>-5.35254607305620027e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>1.72947781738215802e+01</item>
+								<item>-7.37458142645349284e+01</item>
+								<item>6.52874071041194242e+01</item>
+								<item>-9.78660995310737718e+01</item>
+								<item>-5.39934004805816503e+00</item>
+								<item>1.98260860892763304e+01</item>
+								<item>-1.72947781738215802e+01</item>
+								<item>7.37458142645349284e+01</item>
+								<item>-6.52874071041194242e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-1.10958195052130399e+01</item>
+								<item>-6.73231164274660614e+01</item>
+								<item>-7.31059558722925402e+01</item>
+								<item>9.78660995310737718e+01</item>
+								<item>5.39934004805816503e+00</item>
+								<item>-1.98260860892763304e+01</item>
+								<item>1.10958195052130399e+01</item>
+								<item>6.73231164274660614e+01</item>
+								<item>7.31059558722925402e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-5.38048032045895752e+00</item>
+								<item>1.65536142881929180e+02</item>
+								<item>8.22017033006773943e+01</item>
+								<item>-7.27309788858385105e+01</item>
+								<item>-1.01358249880792499e+01</item>
+								<item>1.16499706864703938e+02</item>
+								<item>7.80821322304104655e+01</item>
+								<item>9.19602704161340796e+01</item>
+								<item>-8.91929751904364565e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_20">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>20</item>
+								<item>21</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>6.79396504738739537e+01</item>
+								<item>4.82037868237773779e+01</item>
+								<item>-5.47024548652807283e+01</item>
+								<item>-7.00832895591236564e+01</item>
+								<item>5.14309272143008727e+01</item>
+								<item>-3.67255818969308834e+01</item>
+								<item>1.86928047352814559e+01</item>
+								<item>-2.23350198922307115e+01</item>
+								<item>1.77311293144151279e+01</item>
+								<item>7.44105813075135192e+00</item>
+								<item>5.99087969415584212e+01</item>
+								<item>5.30460372996668283e+01</item>
+								<item>-1.22158131494639797e+01</item>
+								<item>-1.50072994727732638e+01</item>
+								<item>-6.43870652455934760e+01</item>
+								<item>-1.47523300546991756e+01</item>
+								<item>7.84637907473050689e+01</item>
+								<item>-5.14524303561770466e+01</item>
+								<item>1.48130048314395335e+00</item>
+								<item>-3.96193494527523526e+01</item>
+								<item>-4.51446929817367320e+01</item>
+								<item>2.41623483145540554e+01</item>
+								<item>-4.80504534286710054e+01</item>
+								<item>-6.06763607845337773e+01</item>
+								<item>9.54718801387472098e+01</item>
+								<item>2.61345793187026096e+01</item>
+								<item>4.22977195343479639e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.31638784413171734e+01</item>
+								<item>8.58566719453584426e+01</item>
+								<item>2.76678781327787284e+01</item>
+								<item>-6.66070179567136051e+01</item>
+								<item>-9.65155343163612223e+00</item>
+								<item>-7.39618325575448523e+01</item>
+								<item>4.31638784413171734e+01</item>
+								<item>-8.58566719453584426e+01</item>
+								<item>-2.76678781327787284e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-6.08307879023215037e+01</item>
+								<item>-5.03535440542508113e+01</item>
+								<item>6.13525536906117850e+01</item>
+								<item>6.66070179567136051e+01</item>
+								<item>9.65155343163612223e+00</item>
+								<item>7.39618325575448523e+01</item>
+								<item>6.08307879023215037e+01</item>
+								<item>5.03535440542508113e+01</item>
+								<item>-6.13525536906117850e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.33814642652391399e+02</item>
+								<item>1.77445244940366287e-01</item>
+								<item>-1.49228346303260491e+01</item>
+								<item>-6.12411970511728132e-01</item>
+								<item>5.97803936488639280e+01</item>
+								<item>-1.02665793382055853e+02</item>
+								<item>2.53293581625552058e+01</item>
+								<item>-3.42663008134674527e+01</item>
+								<item>5.61197444293587395e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_21">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>21</item>
+								<item>22</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-6.74405298173737435e+01</item>
+								<item>7.32759993494661614e+01</item>
+								<item>-6.72758148030300518e+00</item>
+								<item>2.84952832715093294e+01</item>
+								<item>3.71106482095568992e+01</item>
+								<item>5.87105666468560798e+01</item>
+								<item>3.06497832963929113e+01</item>
+								<item>2.67945705145493136e+01</item>
+								<item>5.23800862467740629e+01</item>
+								<item>-2.34193589858697777e+01</item>
+								<item>-2.67958038635302600e+01</item>
+								<item>2.45872057837964846e+01</item>
+								<item>1.16252649553510334e+01</item>
+								<item>8.99424898505579051e+01</item>
+								<item>-2.22061693363508326e+01</item>
+								<item>6.98562239082597500e+01</item>
+								<item>-3.31539117547968303e+01</item>
+								<item>-5.85832970823880999e+01</item>
+								<item>5.89049685210982901e+01</item>
+								<item>4.59874265404548481e+01</item>
+								<item>-5.08083954437895216e+01</item>
+								<item>-2.22191873368612889e+01</item>
+								<item>1.16539331220696187e+01</item>
+								<item>-7.08270509506904347e+01</item>
+								<item>6.46018011815524602e+01</item>
+								<item>1.94415681363770645e+01</item>
+								<item>4.04539860386468320e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>9.78145711723120286e+01</item>
+								<item>1.76232674770143696e+01</item>
+								<item>-1.10331368980137903e+01</item>
+								<item>-1.40616464377447681e+01</item>
+								<item>1.69815554662718142e+01</item>
+								<item>-9.75392068524539155e+01</item>
+								<item>-9.78145711723120286e+01</item>
+								<item>-1.76232674770143696e+01</item>
+								<item>1.10331368980137903e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-1.53159970565603700e+01</item>
+								<item>9.69589976091932186e+01</item>
+								<item>1.90885572211179095e+01</item>
+								<item>1.40616464377447681e+01</item>
+								<item>-1.69815554662718142e+01</item>
+								<item>9.75392068524539155e+01</item>
+								<item>1.53159970565603700e+01</item>
+								<item>-9.69589976091932186e+01</item>
+								<item>-1.90885572211179095e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-2.25457773322750015e+01</item>
+								<item>-1.39359117049240723e+02</item>
+								<item>-1.01360767749937452e+02</item>
+								<item>-1.06577486560693373e+02</item>
+								<item>3.69148207609075527e+01</item>
+								<item>-6.52361857149522422e+01</item>
+								<item>-9.51115531141346224e+01</item>
+								<item>7.07220482565039248e+01</item>
+								<item>-3.57407782543718326e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_22">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>22</item>
+								<item>23</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>9.41929589378244003e+01</item>
+								<item>8.94097901372627391e+00</item>
+								<item>3.01620867599012286e+01</item>
+								<item>2.46922319059140740e+01</item>
+								<item>1.35465056881596215e+01</item>
+								<item>-4.89963411381141611e+01</item>
+								<item>-1.68344538935716344e+01</item>
+								<item>-1.83167626535859007e+01</item>
+								<item>7.95313381974681306e+01</item>
+								<item>-5.18828216803281350e+00</item>
+								<item>-4.19655627938833504e+01</item>
+								<item>-6.55823195034947481e+00</item>
+								<item>7.35562337314085681e+01</item>
+								<item>5.97403143502788865e+01</item>
+								<item>-2.03387857576245445e-01</item>
+								<item>6.60668686562018337e+01</item>
+								<item>-6.61937508072900442e+01</item>
+								<item>-2.10487850516630495e+01</item>
+								<item>1.54947760148805180e+01</item>
+								<item>-8.96286058833042887e+01</item>
+								<item>-5.79472767879112993e+00</item>
+								<item>-6.70795355915261116e+00</item>
+								<item>-1.69646265262306706e+01</item>
+								<item>-8.65671984840189168e+01</item>
+								<item>-1.21336256996353509e+01</item>
+								<item>3.69909891189522924e+01</item>
+								<item>-4.84851501469951671e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>8.51395966446430350e+01</item>
+								<item>-5.19584580090174413e+01</item>
+								<item>7.18106708732509347e+00</item>
+								<item>-5.24346283807042397e+01</item>
+								<item>-8.39535649250525182e+01</item>
+								<item>1.42270405549584336e+01</item>
+								<item>-8.51395966446430350e+01</item>
+								<item>5.19584580090174413e+01</item>
+								<item>-7.18106708732509347e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-1.36338907320492919e+00</item>
+								<item>-1.58782107839693651e+01</item>
+								<item>-9.87219509153607788e+01</item>
+								<item>5.24346283807042397e+01</item>
+								<item>8.39535649250525182e+01</item>
+								<item>-1.42270405549584336e+01</item>
+								<item>1.36338907320492919e+00</item>
+								<item>1.58782107839693651e+01</item>
+								<item>9.87219509153607788e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>3.07007154914777551e+01</item>
+								<item>2.70881185233511914e+01</item>
+								<item>3.78166527462940882e+00</item>
+								<item>-1.15162420127751133e+02</item>
+								<item>-6.75390943713141123e+00</item>
+								<item>3.95885323960980529e+00</item>
+								<item>-1.29970649317129876e+01</item>
+								<item>1.14524822858325365e+02</item>
+								<item>2.24989739182521227e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_23">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>23</item>
+								<item>24</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-1.83421298069983294e+01</item>
+								<item>-8.40178887379047552e+01</item>
+								<item>1.37665553988889080e+01</item>
+								<item>3.46271545199608042e+01</item>
+								<item>4.05922653756383482e+01</item>
+								<item>-7.00698095538007770e+00</item>
+								<item>9.19927590625682683e+01</item>
+								<item>-3.22667773518094236e+01</item>
+								<item>3.91925773350330608e+00</item>
+								<item>4.07207141973007651e+01</item>
+								<item>-3.85719388369921674e+01</item>
+								<item>-7.75337815930943464e+01</item>
+								<item>7.24865255322073381e+01</item>
+								<item>-3.53413936296266158e+01</item>
+								<item>5.86044610642822335e+01</item>
+								<item>-1.83884977004002117e+01</item>
+								<item>1.46625501311897892e+01</item>
+								<item>1.87515674799128185e+01</item>
+								<item>-2.53722481762123628e+01</item>
+								<item>-3.66484706705800178e+01</item>
+								<item>3.56472023092901580e+01</item>
+								<item>-4.40875208799477321e+01</item>
+								<item>-6.95087704456724254e+01</item>
+								<item>1.97777311636326232e+01</item>
+								<item>1.27979930474377834e+01</item>
+								<item>3.35520358632383733e+01</item>
+								<item>9.10304934695741821e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-2.81762066375282245e+01</item>
+								<item>9.59328037613137070e+01</item>
+								<item>-1.73162987170684679e+00</item>
+								<item>-8.31278175295505264e+01</item>
+								<item>-2.53085961788951153e+01</item>
+								<item>-4.94898061445724977e+01</item>
+								<item>2.81762066375282245e+01</item>
+								<item>-9.59328037613137070e+01</item>
+								<item>1.73162987170684679e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-4.79152098220707003e+01</item>
+								<item>-1.25048839237672293e+01</item>
+								<item>8.68778484181097070e+01</item>
+								<item>8.31278175295505264e+01</item>
+								<item>2.53085961788951153e+01</item>
+								<item>4.94898061445724977e+01</item>
+								<item>4.79152098220707003e+01</item>
+								<item>1.25048839237672293e+01</item>
+								<item>-8.68778484181097070e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-3.78847831737257366e+01</item>
+								<item>2.29972416224487830e+01</item>
+								<item>-1.37159770597291157e+02</item>
+								<item>-4.30909041970757869e+01</item>
+								<item>-7.29718405033518991e+01</item>
+								<item>-1.27753851859623239e+02</item>
+								<item>-4.00153271264082644e+00</item>
+								<item>-1.83214678716897197e+02</item>
+								<item>3.84258732608477231e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_24">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>24</item>
+								<item>25</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-1.42022591781322873e+01</item>
+								<item>-3.56662961537267762e+01</item>
+								<item>-1.29665633112741379e+01</item>
+								<item>-4.42547548664715933e+01</item>
+								<item>8.53484085500931968e+01</item>
+								<item>-1.21228131477824768e+01</item>
+								<item>7.44576769541145751e+01</item>
+								<item>3.58775048496875613e+01</item>
+								<item>4.62292669635554603e+01</item>
+								<item>1.27026107710752729e+01</item>
+								<item>-9.11117710856913021e+01</item>
+								<item>2.52571349253896464e+01</item>
+								<item>-3.19164772916096373e+00</item>
+								<item>-2.63470878340458832e+01</item>
+								<item>1.88346366483448016e+01</item>
+								<item>5.40205809637968812e+01</item>
+								<item>-1.90775864349462978e+01</item>
+								<item>-8.09820522299282857e+01</item>
+								<item>6.16280452912953720e+01</item>
+								<item>1.93876135715390099e+01</item>
+								<item>7.12990641830701719e+01</item>
+								<item>6.58414093447228908e+01</item>
+								<item>1.80759831933341069e+01</item>
+								<item>-6.99233759491107918e+01</item>
+								<item>3.27566373625739402e+01</item>
+								<item>9.70961890249048842e+00</item>
+								<item>4.91628253265810322e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>9.34197183252336458e+01</item>
+								<item>2.76233972145661149e+01</item>
+								<item>2.25766284985226449e+01</item>
+								<item>-2.39170403241009595e+01</item>
+								<item>1.53702168542746431e+00</item>
+								<item>9.70855949483436831e+01</item>
+								<item>-9.34197183252336458e+01</item>
+								<item>-2.76233972145661149e+01</item>
+								<item>-2.25766284985226449e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>2.64713318548450189e+01</item>
+								<item>-9.60967506769340645e+01</item>
+								<item>8.04258037981846208e+00</item>
+								<item>2.39170403241009595e+01</item>
+								<item>-1.53702168542746431e+00</item>
+								<item>-9.70855949483436831e+01</item>
+								<item>-2.64713318548450189e+01</item>
+								<item>9.60967506769340645e+01</item>
+								<item>-8.04258037981846208e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>4.99415394471549448e+01</item>
+								<item>8.89774612805008616e+01</item>
+								<item>-7.21549206200566857e+01</item>
+								<item>-8.47137695131298614e+01</item>
+								<item>-3.96302269514171712e+01</item>
+								<item>2.91958404364136328e+01</item>
+								<item>-1.14979727399682560e+02</item>
+								<item>6.32312875848932592e+01</item>
+								<item>-5.83619937525406982e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_25">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>25</item>
+								<item>26</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>4.75279810851286726e+01</item>
+								<item>1.27698906927141866e+01</item>
+								<item>-3.85248964418578721e+00</item>
+								<item>8.38265009034030300e+01</item>
+								<item>2.34263445942633055e+01</item>
+								<item>4.00325151654156208e+00</item>
+								<item>-3.67133537961906153e+00</item>
+								<item>7.31784749045807636e+00</item>
+								<item>9.95211070535517166e+01</item>
+								<item>7.72966987607180016e+01</item>
+								<item>3.83523138848579492e+01</item>
+								<item>1.84348658933893006e+01</item>
+								<item>-5.00282378767150675e+01</item>
+								<item>1.62934700342947103e+00</item>
+								<item>-1.23502536597363957e+01</item>
+								<item>3.08280910111897235e+01</item>
+								<item>-9.23259671614170685e+01</item>
+								<item>9.05617058795025009e+00</item>
+								<item>-3.32567607414421573e+01</item>
+								<item>4.65581163028938505e+01</item>
+								<item>8.06472888898161528e+01</item>
+								<item>-1.09449242551094059e+01</item>
+								<item>8.15866317932763252e+01</item>
+								<item>-5.45355209481987444e+01</item>
+								<item>-6.85221032691171050e+00</item>
+								<item>2.03202505385987919e+01</item>
+								<item>3.49109728776780415e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>5.75486223700384372e+01</item>
+								<item>-3.56976774925786202e+01</item>
+								<item>7.35787461495951334e+01</item>
+								<item>-7.77278254550152923e+01</item>
+								<item>4.09823526552186745e+00</item>
+								<item>6.27820803871861983e+01</item>
+								<item>-5.75486223700384372e+01</item>
+								<item>3.56976774925786202e+01</item>
+								<item>-7.35787461495951334e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-2.54271747023806824e+01</item>
+								<item>-9.33213817372218273e+01</item>
+								<item>-2.53885505161348775e+01</item>
+								<item>7.77278254550152923e+01</item>
+								<item>-4.09823526552186745e+00</item>
+								<item>-6.27820803871861983e+01</item>
+								<item>2.54271747023806824e+01</item>
+								<item>9.33213817372218273e+01</item>
+								<item>2.53885505161348775e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>5.11234651369603128e+01</item>
+								<item>1.45916426283882128e+01</item>
+								<item>8.14361101436695947e+01</item>
+								<item>-9.64495575814352151e+01</item>
+								<item>5.69392664765279974e+01</item>
+								<item>1.45591038998347216e+02</item>
+								<item>-1.52360008121073719e+02</item>
+								<item>3.92577375478538881e+00</item>
+								<item>-7.47401324352360774e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_26">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>7</item>
+								<item>10</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>-5.40766118135344485e+01</item>
+								<item>-6.55945677789347137e+00</item>
+								<item>5.92166444746720977e-01</item>
+								<item>8.33981325801565987e+01</item>
+								<item>-1.15536936327200639e+01</item>
+								<item>1.11233523720273890e+01</item>
+								<item>-9.80808079024424018e+00</item>
+								<item>-8.64371084278765380e+01</item>
+								<item>4.74584491239462309e+01</item>
+								<item>1.25135196935038806e+01</item>
+								<item>9.77748374151099995e+01</item>
+								<item>6.14999725352584115e+00</item>
+								<item>1.36807989121553284e+01</item>
+								<item>4.43989849701706429e+00</item>
+								<item>-9.89599830977766572e+01</item>
+								<item>-2.01505456436379626e+00</item>
+								<item>-1.55263904030224698e+01</item>
+								<item>-7.65145679760131925e-01</item>
+								<item>8.31600037168701789e+01</item>
+								<item>-1.88305696684158264e+01</item>
+								<item>-2.79941305069657931e+00</item>
+								<item>5.18811705043763141e+01</item>
+								<item>-7.17174238817171439e+00</item>
+								<item>6.66998670151083850e+00</item>
+								<item>-8.31970807941664781e+00</item>
+								<item>-4.61019176373218968e+01</item>
+								<item>-8.79761519740697651e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>7.83449748282976799e+01</item>
+								<item>6.13201754047114846e+01</item>
+								<item>1.00946028891096997e+01</item>
+								<item>-1.98027611608243852e+01</item>
+								<item>9.23644092983748521e+00</item>
+								<item>9.75834966034573341e+01</item>
+								<item>-7.83449748282976799e+01</item>
+								<item>-6.13201754047114846e+01</item>
+								<item>-1.00946028891096997e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>5.89059892503364324e+01</item>
+								<item>-7.84507759508154265e+01</item>
+								<item>1.93793752003053505e+01</item>
+								<item>1.98027611608243852e+01</item>
+								<item>-9.23644092983748521e+00</item>
+								<item>-9.75834966034573341e+01</item>
+								<item>-5.89059892503364324e+01</item>
+								<item>7.84507759508154265e+01</item>
+								<item>-1.93793752003053505e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-6.07889604414481113e+01</item>
+								<item>8.49672366649234476e+01</item>
+								<item>-1.19149060367207383e+02</item>
+								<item>-3.26841768394553398e+01</item>
+								<item>-8.97350957633814517e+01</item>
+								<item>9.13622785379021281e+01</item>
+								<item>-1.77493968880864855e+02</item>
+								<item>-4.85855715361152676e+01</item>
+								<item>-7.39158595946127228e+00</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_27">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>9</item>
+								<item>14</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>4.43615564152903019e+01</item>
+								<item>-3.09502672537490433e+01</item>
+								<item>-6.65256070847414378e+01</item>
+								<item>4.96695435786911474e+01</item>
+								<item>2.78172662190901541e+01</item>
+								<item>6.19720498005987466e+01</item>
+								<item>-7.45919697090238145e+01</item>
+								<item>1.35179597700267684e+00</item>
+								<item>1.13620760877569249e+00</item>
+								<item>5.07131897166802617e+01</item>
+								<item>-6.73003419642306540e+01</item>
+								<item>3.11633063652204427e+01</item>
+								<item>5.47056695452382584e+01</item>
+								<item>6.11187671649742512e+01</item>
+								<item>-2.76098847773204135e+01</item>
+								<item>6.65963623398869657e+01</item>
+								<item>1.43297991149407444e+00</item>
+								<item>-1.99391285797603790e-01</item>
+								<item>7.65967691022320984e-01</item>
+								<item>1.84943157433388130e+01</item>
+								<item>-6.50656556956834748e+01</item>
+								<item>8.44674932355512387e-01</item>
+								<item>2.30555803308049860e+01</item>
+								<item>-6.99700625824593629e+01</item>
+								<item>-3.40876610014427706e-01</item>
+								<item>-9.55299072188111609e+01</item>
+								<item>-2.94902469873441930e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-2.89030917873941853e+01</item>
+								<item>8.70749520313458305e+01</item>
+								<item>-3.97814531392241904e+01</item>
+								<item>-9.56630827209552450e+01</item>
+								<item>-2.78467078053438328e+01</item>
+								<item>8.55192777842975183e+00</item>
+								<item>2.89030917873941853e+01</item>
+								<item>-8.70749520313458305e+01</item>
+								<item>3.97814531392241904e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-3.63123800557649812e+00</item>
+								<item>4.05279359595652906e+01</item>
+								<item>9.13471429077244466e+01</item>
+								<item>9.56630827209552450e+01</item>
+								<item>2.78467078053438328e+01</item>
+								<item>-8.55192777842975183e+00</item>
+								<item>3.63123800557649812e+00</item>
+								<item>-4.05279359595652906e+01</item>
+								<item>-9.13471429077244466e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-7.02581401004128878e+01</item>
+								<item>-1.05108950320020767e+02</item>
+								<item>-1.10573546354685661e+02</item>
+								<item>-2.82871715200345584e+01</item>
+								<item>-9.82678476821147342e+01</item>
+								<item>-1.38389269150812240e+01</item>
+								<item>2.98238876860104902e+01</item>
+								<item>-1.16573384649075678e+02</item>
+								<item>1.35327233223631623e+02</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_28">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>10</item>
+								<item>25</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>3.95766223652825175e+01</item>
+								<item>2.36448299245490254e+01</item>
+								<item>-2.45799893899839930e+01</item>
+								<item>-7.00228044208398188e+01</item>
+								<item>-4.93985238797199173e+01</item>
+								<item>1.28698697829822883e+01</item>
+								<item>7.78345939301136092e+00</item>
+								<item>2.90892349948341078e+01</item>
+								<item>9.40990441114148695e+01</item>
+								<item>-8.42616415682074660e+01</item>
+								<item>-2.90334966132265606e+01</item>
+								<item>-1.43512687624874875e+01</item>
+								<item>-2.62231810185785967e+01</item>
+								<item>-4.77560189769582593e+01</item>
+								<item>-8.01321381020614343e-01</item>
+								<item>4.26634609899314583e+01</item>
+								<item>-8.24081707666526171e+01</item>
+								<item>1.64578293010562966e+01</item>
+								<item>-8.45857606147821528e+00</item>
+								<item>2.58771125144124774e+01</item>
+								<item>-9.15441805767733570e+01</item>
+								<item>5.92769857321209486e+01</item>
+								<item>-7.03919737513036381e+01</item>
+								<item>-3.24535577243972782e+01</item>
+								<item>-1.66359962010244793e+01</item>
+								<item>2.48028458121687763e+01</item>
+								<item>-2.15529183777999371e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>9.34197183252336458e+01</item>
+								<item>2.76233972145661149e+01</item>
+								<item>2.25766284985226449e+01</item>
+								<item>-2.39170403241009595e+01</item>
+								<item>1.53702168542746431e+00</item>
+								<item>9.70855949483436831e+01</item>
+								<item>-9.34197183252336458e+01</item>
+								<item>-2.76233972145661149e+01</item>
+								<item>-2.25766284985226449e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>2.64713318548450189e+01</item>
+								<item>-9.60967506769340645e+01</item>
+								<item>8.04258037981846208e+00</item>
+								<item>2.39170403241009595e+01</item>
+								<item>-1.53702168542746431e+00</item>
+								<item>-9.70855949483436831e+01</item>
+								<item>-2.64713318548450189e+01</item>
+								<item>9.60967506769340645e+01</item>
+								<item>-8.04258037981846208e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-6.19976450903283478e+01</item>
+								<item>1.85138049290959714e+02</item>
+								<item>2.04096238885501400e+01</item>
+								<item>-5.38401179783636792e+01</item>
+								<item>1.95403129638118465e+01</item>
+								<item>3.38132128740523674e+00</item>
+								<item>-1.81977553628765463e+02</item>
+								<item>-6.94268461743989320e+01</item>
+								<item>-2.32863678513998718e+00</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_29">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>13</item>
+								<item>22</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.61359876726555882e+01</item>
+								<item>1.08711432986265653e+01</item>
+								<item>8.51184696478907732e+00</item>
+								<item>6.31122752697449414e+01</item>
+								<item>-1.20771880436756209e+01</item>
+								<item>7.50149628126125521e+01</item>
+								<item>-4.10840851091186110e+01</item>
+								<item>-8.68858010361213076e+01</item>
+								<item>2.35731234098279678e+01</item>
+								<item>4.08177879353251924e+01</item>
+								<item>-2.17654374458011439e+01</item>
+								<item>8.78457730201184290e+01</item>
+								<item>-2.17842465512247614e+01</item>
+								<item>1.46672234096332783e+01</item>
+								<item>6.07972286227193015e-01</item>
+								<item>6.89954693463132855e+01</item>
+								<item>-4.93810369540159471e+01</item>
+								<item>-4.74803457645439977e+01</item>
+								<item>-6.20341282977744939e+01</item>
+								<item>-7.54126088063309794e+01</item>
+								<item>1.25298075512294904e+01</item>
+								<item>-5.28542109714301915e+01</item>
+								<item>5.80638264069757923e+01</item>
+								<item>5.82126617561848008e+01</item>
+								<item>-2.29292298928228675e+01</item>
+								<item>-1.00045379878072604e+00</item>
+								<item>1.50754924253946800e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>9.78145711723120286e+01</item>
+								<item>1.76232674770143696e+01</item>
+								<item>-1.10331368980137903e+01</item>
+								<item>-1.40616464377447681e+01</item>
+								<item>1.69815554662718142e+01</item>
+								<item>-9.75392068524539155e+01</item>
+								<item>-9.78145711723120286e+01</item>
+								<item>-1.76232674770143696e+01</item>
+								<item>1.10331368980137903e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-1.53159970565603700e+01</item>
+								<item>9.69589976091932186e+01</item>
+								<item>1.90885572211179095e+01</item>
+								<item>1.40616464377447681e+01</item>
+								<item>-1.69815554662718142e+01</item>
+								<item>9.75392068524539155e+01</item>
+								<item>1.53159970565603700e+01</item>
+								<item>-9.69589976091932186e+01</item>
+								<item>-1.90885572211179095e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>8.03183434009733901e+01</item>
+								<item>-1.57959181406014523e+02</item>
+								<item>-6.44060214101481705e+01</item>
+								<item>-6.64898917378079801e+01</item>
+								<item>-6.21866664385735888e+01</item>
+								<item>-6.61756460634393022e+01</item>
+								<item>-1.52823432141161049e+02</item>
+								<item>-1.42511660560011357e+01</item>
+								<item>-7.24093903591090111e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_30">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>20</item>
+								<item>15</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>4.71343070271561331e+01</item>
+								<item>-6.72091990499062604e+01</item>
+								<item>5.47388912655424136e+01</item>
+								<item>8.35500845890857278e+01</item>
+								<item>2.23841738875626213e+01</item>
+								<item>-3.32945376880477681e+01</item>
+								<item>-2.59734415135038539e+01</item>
+								<item>-2.12006909898996589e+01</item>
+								<item>2.34658474313030787e+01</item>
+								<item>6.32652845860905977e+00</item>
+								<item>9.30238234940692976e+00</item>
+								<item>3.37625583972403689e+01</item>
+								<item>3.44554933454442613e+00</item>
+								<item>8.79955033997688929e+01</item>
+								<item>4.09722885299349642e+01</item>
+								<item>-1.92439510927881194e+01</item>
+								<item>4.62720459594682580e+01</item>
+								<item>-8.23910435376807584e+01</item>
+								<item>-5.55999617765192156e+00</item>
+								<item>6.20368695338309522e+01</item>
+								<item>7.15470037167418553e+01</item>
+								<item>3.41059120969163914e+01</item>
+								<item>-1.39920442875337869e+01</item>
+								<item>-2.48395357584865195e+01</item>
+								<item>9.22321060162172301e+01</item>
+								<item>2.27421056426905714e+01</item>
+								<item>1.25600461547882558e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-8.15059339771809164e+01</item>
+								<item>-5.54598536753216109e+01</item>
+								<item>-1.67626774955357369e+01</item>
+								<item>-1.04808841952049807e+01</item>
+								<item>4.25686446848321935e+01</item>
+								<item>-8.98780371179923492e+01</item>
+								<item>8.15059339771809164e+01</item>
+								<item>5.54598536753216109e+01</item>
+								<item>1.67626774955357369e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>5.69818724946287603e+01</item>
+								<item>-7.14990567770542356e+01</item>
+								<item>-4.05086544703406517e+01</item>
+								<item>1.04808841952049807e+01</item>
+								<item>-4.25686446848321935e+01</item>
+								<item>8.98780371179923492e+01</item>
+								<item>-5.69818724946287603e+01</item>
+								<item>7.14990567770542356e+01</item>
+								<item>4.05086544703406517e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>-1.44773765128431023e+02</item>
+								<item>3.21567907629271019e+01</item>
+								<item>6.77990836285083986e+01</item>
+								<item>3.24643872643349098e+01</item>
+								<item>3.07230481384375675e+00</item>
+								<item>-8.66387073171390654e+00</item>
+								<item>6.03331707496851593e+01</item>
+								<item>1.38479259212844596e+02</item>
+								<item>6.83329111783498746e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_31">
+					<Base>
+						<Base>
+							<keys_>
+								<count>27</count>
+								<item_version>0</item_version>
+								<item>0</item>
+								<item>1</item>
+								<item>2</item>
+								<item>3</item>
+								<item>4</item>
+								<item>5</item>
+								<item>6</item>
+								<item>7</item>
+								<item>8</item>
+								<item>9</item>
+								<item>10</item>
+								<item>11</item>
+								<item>12</item>
+								<item>13</item>
+								<item>14</item>
+								<item>15</item>
+								<item>16</item>
+								<item>17</item>
+								<item>18</item>
+								<item>19</item>
+								<item>20</item>
+								<item>21</item>
+								<item>22</item>
+								<item>23</item>
+								<item>24</item>
+								<item>25</item>
+								<item>26</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>3</rows>
+							<cols>82</cols>
+							<data>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>29</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>9</item>
+							<item>12</item>
+							<item>15</item>
+							<item>18</item>
+							<item>21</item>
+							<item>24</item>
+							<item>27</item>
+							<item>30</item>
+							<item>33</item>
+							<item>36</item>
+							<item>39</item>
+							<item>42</item>
+							<item>45</item>
+							<item>48</item>
+							<item>51</item>
+							<item>54</item>
+							<item>57</item>
+							<item>60</item>
+							<item>63</item>
+							<item>66</item>
+							<item>69</item>
+							<item>72</item>
+							<item>75</item>
+							<item>78</item>
+							<item>81</item>
+							<item>82</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>3</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+		</factors_>
+	</Base>
+</graph>
+</boost_serialization>
+
diff --git a/examples/Data/toy3D.xml b/examples/Data/toy3D.xml
new file mode 100644
index 000000000..13dbcbe6c
--- /dev/null
+++ b/examples/Data/toy3D.xml
@@ -0,0 +1,169 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<!DOCTYPE boost_serialization>
+<boost_serialization signature="serialization::archive" version="17">
+<graph class_id="0" tracking_level="0" version="0">
+	<Base class_id="1" tracking_level="0" version="0">
+		<factors_ class_id="2" tracking_level="0" version="0">
+			<count>2</count>
+			<item_version>1</item_version>
+			<item class_id="3" tracking_level="0" version="1">
+				<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
+					<Base class_id="5" tracking_level="0" version="0">
+						<Base class_id="6" tracking_level="0" version="0">
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>0</item>
+								<item>1</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_ class_id="8" tracking_level="0" version="0">
+						<matrix_ class_id="9" tracking_level="0" version="0">
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.22427709071730959e+01</item>
+								<item>1.51514613104920048e+01</item>
+								<item>3.60934366857813060e+00</item>
+								<item>-1.18407259026383116e+01</item>
+								<item>7.84826117220921216e+00</item>
+								<item>1.23509165494819051e+01</item>
+								<item>-6.09875015991639735e+00</item>
+								<item>6.16547190708139126e-01</item>
+								<item>3.94972084922329048e+00</item>
+								<item>-4.89208482920378174e+00</item>
+								<item>3.02091647632478866e+00</item>
+								<item>-8.95328692238917512e+00</item>
+								<item>7.89831607220345955e+00</item>
+								<item>-2.36793602009719084e+00</item>
+								<item>1.48517612051941725e+01</item>
+								<item>-3.97284286249233731e-01</item>
+								<item>-1.95744531643153863e+01</item>
+								<item>-3.85954855417462017e+00</item>
+								<item>4.79268277145419042e+00</item>
+								<item>-9.01707953629520453e+00</item>
+								<item>1.37848069005841385e+01</item>
+								<item>1.04829326688375950e+01</item>
+								<item>-5.00630568442241675e+00</item>
+								<item>4.70463561852773182e+00</item>
+								<item>-1.59179134598689274e+01</item>
+								<item>-2.04767784956723942e+00</item>
+								<item>9.54135497908261954e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>-1.76201757312015372e+00</item>
+								<item>1.98634190821282672e+01</item>
+								<item>1.52966546661624236e+00</item>
+								<item>1.94817649567575373e+01</item>
+								<item>1.39684693294110307e+00</item>
+								<item>4.30228460420588288e+00</item>
+								<item>1.76201757312015372e+00</item>
+								<item>-1.98634190821282672e+01</item>
+								<item>-1.52966546661624236e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>4.16606867942304948e+00</item>
+								<item>1.86906420801308037e+00</item>
+								<item>-1.94717865319198360e+01</item>
+								<item>-1.94817649567575373e+01</item>
+								<item>-1.39684693294110307e+00</item>
+								<item>-4.30228460420588288e+00</item>
+								<item>-4.16606867942304948e+00</item>
+								<item>-1.86906420801308037e+00</item>
+								<item>1.94717865319198360e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00891462904330531e+01</item>
+								<item>-1.08132497987816869e+01</item>
+								<item>8.66487736568128497e+00</item>
+								<item>2.88370015604634311e+01</item>
+								<item>1.89391698948574643e+01</item>
+								<item>2.12398960190661290e+00</item>
+								<item>1.22150946112124039e+01</item>
+								<item>-2.33658532501596561e+01</item>
+								<item>1.51576204760307363e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_ class_id="11" tracking_level="0" version="1">
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_1">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>0</item>
+								<item>1</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>3</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>3</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+		</factors_>
+	</Base>
+</graph>
+</boost_serialization>
+
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index c32a049e2..b66f22ced 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -66,6 +66,7 @@ string findExampleDataFile(const string& name) {
   namesToSearch.push_back(name + ".graph");
   namesToSearch.push_back(name + ".txt");
   namesToSearch.push_back(name + ".out");
+  namesToSearch.push_back(name + ".xml");
 
   // Find first name that exists
   for(const fs::path& root: rootsToSearch) {

From 798e730d876b263ac91d869d1693b3b454166f4e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 6 Apr 2019 22:56:20 -0400
Subject: [PATCH 246/281] Fixed issue where number of extra "augmentation"
 edges asked for are larger than the number of remaining edges after taking
 out the spanning tree.

---
 gtsam/linear/SubgraphPreconditioner.cpp | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index c75bcd4e2..39f5a65cd 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -397,23 +397,30 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
   const SubgraphBuilderParameters &p = parameters_;
   const Ordering inverse_ordering = Ordering::Natural(gfg);
   const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
-  const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;
+  const size_t n = inverse_ordering.size(), m = gfg.size();
 
-  vector<double> w = weights(gfg);
-  const vector<size_t> tree = buildTree(gfg, forward_ordering, w);
+  // Make sure the subgraph preconditioner does not include more than half of
+  // the edges beyond the spanning tree, or we might as well solve the whole thing.
+  size_t numExtraEdges = n * p.complexity_;
+  const size_t numRemaining = m - (n - 1);
+  numExtraEdges = std::min(numExtraEdges, numRemaining/2);
 
-  /* sanity check */
+  // Calculate weights 
+  vector<double> weights = this->weights(gfg);
+
+  // Build spanning tree.
+  const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
   if ( tree.size() != n-1 ) {
     throw std::runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
   }
 
-  /* down weight the tree edges to zero */
+  // Downweight the tree edges to zero.
   for ( const size_t id: tree ) {
-    w[id] = 0.0;
+    weights[id] = 0.0;
   }
 
   /* decide how many edges to augment */
-  vector<size_t> offTree = sample(w, t);
+  vector<size_t> offTree = sample(weights, numExtraEdges);
 
   vector<size_t> subgraph = unary(gfg);
   subgraph.insert(subgraph.end(), tree.begin(), tree.end());

From 490a558fe1639e0bedef157762f89c575859dde4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 6 Apr 2019 22:57:20 -0400
Subject: [PATCH 247/281] Fixed dimension mismatch (on account of three extra
 dummy factors)

---
 tests/testSubgraphPreconditioner.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index f51263bfb..ce0c80dbd 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -133,7 +133,7 @@ TEST( SubgraphPreconditioner, system )
   Matrix R1 = Rc1->matrix(ordering).first;
   Matrix Abar(13 * 2, 9 * 2);
   Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2);
-  Abar.bottomRows(4 * 2) = A2 * R1.inverse();
+  Abar.bottomRows(8) = A2.topRows(8) * R1.inverse();
 
   // Helper function to vectorize in correct order, which is the order in which
   // we eliminated the spanning tree.

From 42284355f4c67793dbcc1d2d76c86ab8b2d30993 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 6 Apr 2019 22:57:43 -0400
Subject: [PATCH 248/281] Some cleanup, renaming

---
 gtsam/linear/SubgraphPreconditioner.cpp | 45 +++++++++++++------------
 gtsam/linear/SubgraphPreconditioner.h   | 10 +++---
 2 files changed, 29 insertions(+), 26 deletions(-)

diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 39f5a65cd..80adfc098 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -78,16 +78,19 @@ static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
 
   /* compute the sum of the weights */
   const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
+  if (sum==0.0) {
+    throw std::runtime_error("Weight vector has no non-zero weights");
+  }
 
   /* make a normalized and accumulated version of the weight vector */
   const size_t m = weight.size();
-  vector<double> w; w.reserve(m);
+  vector<double> cdf; cdf.reserve(m);
   for ( size_t i = 0 ; i < m ; ++i ) {
-    w.push_back(weight[i]/sum);
+    cdf.push_back(weight[i]/sum);
   }
 
   vector<double> acc(m);
-  std::partial_sum(w.begin(),w.end(),acc.begin());
+  std::partial_sum(cdf.begin(),cdf.end(),acc.begin());
 
   /* iid sample n times */
   vector<size_t> result; result.reserve(n);
@@ -95,18 +98,18 @@ static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
   for ( size_t i = 0 ; i < n ; ++i ) {
     const double value = rand() / denominator;
     /* binary search the interval containing "value" */
-    vector<double>::iterator it = std::lower_bound(acc.begin(), acc.end(), value);
-    size_t idx = it - acc.begin();
+    const auto it = std::lower_bound(acc.begin(), acc.end(), value);
+    const size_t idx = it - acc.begin();
     result.push_back(idx);
   }
   return result;
 }
 
 /*****************************************************************************/
-vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
+static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n) {
 
   const size_t m = weight.size();
-  if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size");
+  if ( n > m ) throw std::invalid_argument("UniqueSampler: invalid input size");
 
   vector<size_t> result;
 
@@ -221,11 +224,11 @@ SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslato
 }
 
 /****************************************************************/
-std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) {
-  if ( w == NATURALCHAIN )return "NATURALCHAIN";
-  else if ( w == BFS )    return "BFS";
-  else if ( w == KRUSKAL )return "KRUSKAL";
-  else                    return "UNKNOWN";
+std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) {
+  if ( s == NATURALCHAIN ) return "NATURALCHAIN";
+  else if ( s == BFS )     return "BFS";
+  else if ( s == KRUSKAL ) return "KRUSKAL";
+  else                     return "UNKNOWN";
 }
 
 /****************************************************************/
@@ -271,7 +274,7 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(Augmentation
 /****************************************************************/
 vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
                                           const FastMap<Key, size_t> &ordering,
-                                          const vector<double> &w) const {
+                                          const vector<double> &weights) const {
   const SubgraphBuilderParameters &p = parameters_;
   switch (p.skeleton_) {
   case SubgraphBuilderParameters::NATURALCHAIN:
@@ -281,7 +284,7 @@ vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
     return bfs(gfg);
     break;
   case SubgraphBuilderParameters::KRUSKAL:
-    return kruskal(gfg, ordering, w);
+    return kruskal(gfg, ordering, weights);
     break;
   default:
     std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
@@ -357,10 +360,10 @@ vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
 /****************************************************************/
 vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
                                         const FastMap<Key, size_t> &ordering,
-                                        const vector<double> &w) const {
+                                        const vector<double> &weights) const {
   const VariableIndex variableIndex(gfg);
   const size_t n = variableIndex.size();
-  const vector<size_t> idx = sort_idx(w) ;
+  const vector<size_t> idx = sort_idx(weights) ;
 
   /* initialize buffer */
   vector<size_t> result;
@@ -379,7 +382,7 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
     if ( dsf.find(u) != dsf.find(v) ) {
       dsf.merge(u, v) ;
       result.push_back(id) ;
-      sum += w[id] ;
+      sum += weights[id] ;
       if ( ++count == n-1 ) break ;
     }
   }
@@ -388,14 +391,14 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
 
 /****************************************************************/
 vector<size_t> SubgraphBuilder::sample(const vector<double> &weights, const size_t t) const {
-  return uniqueSampler(weights, t);
+  return UniqueSampler(weights, t);
 }
 
 /****************************************************************/
 Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
 
-  const SubgraphBuilderParameters &p = parameters_;
-  const Ordering inverse_ordering = Ordering::Natural(gfg);
+  const auto &p = parameters_;
+  const auto inverse_ordering = Ordering::Natural(gfg);
   const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
   const size_t n = inverse_ordering.size(), m = gfg.size();
 
@@ -411,7 +414,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
   // Build spanning tree.
   const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
   if ( tree.size() != n-1 ) {
-    throw std::runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
+    throw std::runtime_error("SubgraphBuilder::operator() failure: tree.size() != n-1");
   }
 
   // Downweight the tree edges to zero.
diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h
index 7995afedc..6bba3dd1c 100644
--- a/gtsam/linear/SubgraphPreconditioner.h
+++ b/gtsam/linear/SubgraphPreconditioner.h
@@ -129,11 +129,11 @@ namespace gtsam {
 
     enum AugmentationWeight {   /* how to weigh the graph edges */
       SKELETON = 0,             /* use the same weights in building the skeleton */
-//      STRETCH,                  /* stretch in the laplacian sense */
-//      GENERALIZED_STRETCH       /* the generalized stretch defined in jian2013iros */
+//      STRETCH,                /* stretch in the laplacian sense */
+//      GENERALIZED_STRETCH     /* the generalized stretch defined in jian2013iros */
     } augmentationWeight_ ;
 
-    double complexity_;
+    double complexity_; /* factor multiplied with n, yields number of extra edges. */
 
     SubgraphBuilderParameters()
       : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
@@ -145,7 +145,7 @@ namespace gtsam {
     friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
 
     static Skeleton skeletonTranslator(const std::string &s);
-    static std::string skeletonTranslator(Skeleton w);
+    static std::string skeletonTranslator(Skeleton s);
     static SkeletonWeight skeletonWeightTranslator(const std::string &s);
     static std::string skeletonWeightTranslator(SkeletonWeight w);
     static AugmentationWeight augmentationWeightTranslator(const std::string &s);
@@ -170,7 +170,7 @@ namespace gtsam {
     std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
     std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
     std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
-    std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const ;
+    std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
     std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
     Weights weights(const GaussianFactorGraph &gfg) const;
     SubgraphBuilderParameters parameters_;

From 1c2646000b0c295b67415b144c962bb7866890aa Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 7 Apr 2019 12:03:33 -0400
Subject: [PATCH 249/281] Added tests for matrix/vector conversion

---
 gtsam/linear/tests/testGaussianBayesNet.cpp | 34 +++++++++++++++++----
 1 file changed, 28 insertions(+), 6 deletions(-)

diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 74f07b92e..184933732 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <CppUnitLite/TestHarness.h>
+#include <boost/tuple/tuple.hpp>
 #include <boost/assign/list_of.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
@@ -28,13 +30,11 @@ using namespace boost::assign;
 // STL/C++
 #include <iostream>
 #include <sstream>
-#include <CppUnitLite/TestHarness.h>
-#include <boost/tuple/tuple.hpp>
 
 using namespace std;
 using namespace gtsam;
 
-static const Key _x_=0, _y_=1;
+static const Key _x_ = 11, _y_ = 22, _z_ = 33;
 
 static GaussianBayesNet smallBayesNet =
     list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))(
@@ -42,9 +42,9 @@ static GaussianBayesNet smallBayesNet =
 
 static GaussianBayesNet noisyBayesNet =
     list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1,
-                                noiseModel::Diagonal::Sigmas(Vector1::Constant(2))))(
+                                noiseModel::Isotropic::Sigma(1, 2.0)))(
         GaussianConditional(_y_, Vector1::Constant(5), I_1x1,
-                            noiseModel::Diagonal::Sigmas(Vector1::Constant(3))));
+                            noiseModel::Isotropic::Sigma(1, 3.0)));
 
 /* ************************************************************************* */
 TEST( GaussianBayesNet, Matrix )
@@ -140,11 +140,33 @@ TEST( GaussianBayesNet, optimize3 )
 TEST(GaussianBayesNet, ordering) 
 {
   Ordering expected;
-  expected += 0, 1;
+  expected += _x_, _y_;
   const auto actual = noisyBayesNet.ordering();
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST( GaussianBayesNet, MatrixStress )
+{
+  GaussianBayesNet bn;
+  using GC = GaussianConditional;
+  bn.emplace_shared<GC>(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2);
+  bn.emplace_shared<GC>(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2);
+  bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
+
+  const VectorValues expected = bn.optimize();
+  for (const auto keys :
+       {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
+        KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
+        KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
+    const Ordering ordering(keys);
+    Matrix R;
+    Vector d;
+    boost::tie(R, d) = bn.matrix(ordering);
+    EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
+  }
+}
+
 /* ************************************************************************* */
 TEST( GaussianBayesNet, backSubstituteTranspose )
 {

From d0129220434cc5db668944877ced179ef0af8888 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 7 Apr 2019 13:14:25 -0400
Subject: [PATCH 250/281] Cleanup, made solve faster (eliminating copy) and no
 longer in-place

---
 gtsam/linear/SubgraphPreconditioner.cpp | 35 +++++++++++--------------
 gtsam/linear/SubgraphPreconditioner.h   |  4 +--
 2 files changed, 17 insertions(+), 22 deletions(-)

diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 80adfc098..657846f95 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -587,25 +587,22 @@ void SubgraphPreconditioner::print(const std::string& s) const {
 
 /*****************************************************************************/
 void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
-  /* copy first */
   assert(x.size() == y.size());
-  std::copy(y.data(), y.data() + y.rows(), x.data());
 
-  /* in place back substitute */
+  /* back substitute */
   for (const auto &cg : boost::adaptors::reverse(*Rc1_)) {
     /* collect a subvector of x that consists of the parents of cg (S) */
-    const Vector xParent = getSubvector(
-        x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
-    const Vector rhsFrontal = getSubvector(
-        x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
+    const KeyVector parentKeys(cg->beginParents(), cg->endParents());
+    const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
+    const Vector xParent = getSubvector(x, keyInfo_, parentKeys);
+    const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys);
 
     /* compute the solution for the current pivot */
     const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(
         rhsFrontal - cg->get_S() * xParent);
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_,
-                 KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_, frontalKeys, x);
   }
 }
 
@@ -657,25 +654,24 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
 }
 
 /*****************************************************************************/
-Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
-
+Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
+                    const KeyVector &keys) {
   /* a cache of starting index and dim */
-  typedef vector<std::pair<size_t, size_t> > Cache;
-  Cache cache;
+  vector<std::pair<size_t, size_t> > cache;
 
   /* figure out dimension by traversing the keys */
-  size_t d = 0;
-  for ( const Key &key: keys ) {
+  size_t dim = 0;
+  for (const Key &key : keys) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
     cache.emplace_back(entry.colstart(), entry.dim());
-    d += entry.dim();
+    dim += entry.dim();
   }
 
   /* use the cache to fill the result */
-  Vector result = Vector::Zero(d, 1);
+  Vector result = Vector::Zero(dim);
   size_t idx = 0;
-  for ( const Cache::value_type &p: cache ) {
-    result.segment(idx, p.second) = src.segment(p.first, p.second) ;
+  for (const auto &p : cache) {
+    result.segment(idx, p.second) = src.segment(p.first, p.second);
     idx += p.second;
   }
 
@@ -684,7 +680,6 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &
 
 /*****************************************************************************/
 void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
-  /* use the cache  */
   size_t idx = 0;
   for ( const Key &key: keys ) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h
index 6bba3dd1c..a9746d104 100644
--- a/gtsam/linear/SubgraphPreconditioner.h
+++ b/gtsam/linear/SubgraphPreconditioner.h
@@ -249,8 +249,8 @@ namespace gtsam {
 
     /* A zero VectorValues with the structure of xbar */
     VectorValues zero() const {
-      VectorValues V(VectorValues::Zero(*xbar_));
-      return V ;
+      assert(xbar_);
+      return VectorValues::Zero(*xbar_);
     }
 
     /**

From 93a7c99f03e0ee5c38fee453c7ff738b60017ea2 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 7 Apr 2019 13:14:51 -0400
Subject: [PATCH 251/281] Created more unit tests attesting problems with
 solve.

---
 tests/testSubgraphPreconditioner.cpp | 72 +++++++++++++++++++++++-----
 1 file changed, 59 insertions(+), 13 deletions(-)

diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index ce0c80dbd..8a92eced8 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -15,21 +15,29 @@
  *  @author Frank Dellaert
  **/
 
-#include <CppUnitLite/TestHarness.h>
 
 #include <tests/smallExample.h>
-#include <gtsam/inference/Symbol.h>
+
+#include <gtsam/slam/dataset.h>
 #include <gtsam/linear/iterative.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/numericalDerivative.h>
 
-#include <boost/tuple/tuple.hpp>
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/archive/xml_iarchive.hpp>
 #include <boost/assign/std/list.hpp>
+#include <boost/serialization/export.hpp>
+#include <boost/tuple/tuple.hpp>
 using namespace boost::assign;
 
+#include <fstream>
+
 using namespace std;
 using namespace gtsam;
 using namespace example;
@@ -139,14 +147,11 @@ TEST( SubgraphPreconditioner, system )
   // we eliminated the spanning tree.
   auto vec = [ordering](const VectorValues& x) { return x.vector(ordering);};
 
-  // Create zero config
-  const VectorValues zeros = VectorValues::Zero(xbar);
-
   // Set up y0 as all zeros
-  const VectorValues y0 = zeros;
+  const VectorValues y0 = system.zero();
 
   // y1 = perturbed y0
-  VectorValues y1 = zeros;
+  VectorValues y1 = system.zero();
   y1[key(3,3)] = Vector2(1.0, -1.0);
 
   // Check backSubstituteTranspose works with R1
@@ -181,13 +186,13 @@ TEST( SubgraphPreconditioner, system )
   ee2 << Vector::Zero(9*2), Vector::Ones(4*2);
 
   // Check transposeMultiplyAdd for e1
-  VectorValues y = zeros;
+  VectorValues y = system.zero();
   system.transposeMultiplyAdd(alpha, e1, y);
   Vector expected_y = alpha * Abar.transpose() * ee1;
   EXPECT(assert_equal(expected_y, vec(y)));
 
   // Check transposeMultiplyAdd for e2
-  y = zeros;
+  y = system.zero();
   system.transposeMultiplyAdd(alpha, e2, y);
   expected_y = alpha * Abar.transpose() * ee2;
   EXPECT(assert_equal(expected_y, vec(y)));
@@ -214,9 +219,8 @@ TEST( SubgraphPreconditioner, RawVectorAPI )
   KeyInfo keyInfo(Ab);
   std::map<Key,Vector> lambda;
   system.build(Ab, keyInfo, lambda);
-  const auto ordering1 = system.Rc1()->ordering(); // build changed R1 !
-  const auto ordering2 = keyInfo.ordering();
-  const Matrix R1 = system.Rc1()->matrix(ordering1).first;
+  const auto ordering = system.Rc1()->ordering(); // build changed R1 !
+  const Matrix R1 = system.Rc1()->matrix(ordering).first;
 
   // Test that 'solve' does implement x = R^{-1} y
   Vector y2 = Vector::Zero(18), x2(18), x3(18);
@@ -230,6 +234,48 @@ TEST( SubgraphPreconditioner, RawVectorAPI )
   // EXPECT(assert_equal(R1.transpose().inverse() * y2, x3));
 }
 
+/* ************************************************************************* */
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor");
+
+TEST( SubgraphSolver, toy3D )
+{
+  // Read from file
+  string inputFile = findExampleDataFile("randomGrid3D");
+  ifstream is(inputFile);
+  if (!is.is_open())
+    throw runtime_error("Cannot find file " + inputFile);
+  boost::archive::xml_iarchive in_archive(is);
+  GaussianFactorGraph Ab;
+  in_archive >> boost::serialization::make_nvp("graph", Ab);
+
+  // Create solver, leaving splitting to constructor
+  SubgraphPreconditioner system;
+  
+  // Call build, a non-const method needed to make solve work :-(
+  KeyInfo keyInfo(Ab);
+  std::map<Key,Vector> lambda;
+  system.build(Ab, keyInfo, lambda);
+
+  // Solve the VectorValues way
+  const VectorValues xbar = system.Rc1()->optimize(); // merely for use in zero below
+  auto y = VectorValues::Zero(xbar);
+  y[12] = Vector3(100,200,-300);
+  auto values_x = system.Rc1()->backSubstitute(y);
+
+  // Solve the matrix way, this really just checks BN::backSubstitute
+  const auto ordering = system.Rc1()->ordering();
+  const Matrix R1 = system.Rc1()->matrix(ordering).first;
+  auto y2 = y.vector(ordering);
+  auto matrix_x = R1.inverse() * y2;
+  EXPECT(assert_equal(matrix_x, values_x.vector(ordering)));
+
+  // Test that 'solve' does implement x = R^{-1} y
+  const size_t N = R1.cols();
+  Vector solve_x = Vector::Zero(N);
+  system.solve(y2, solve_x);
+  EXPECT(assert_equal(solve_x, matrix_x));
+}
+
 /* ************************************************************************* */
 TEST( SubgraphPreconditioner, conjugateGradients )
 {

From 1304d26e81ddc2eedd3c516de6f36e1a48ed18b7 Mon Sep 17 00:00:00 2001
From: jlblancoc <joseluisblancoc@gmail.com>
Date: Wed, 27 Feb 2019 08:58:33 +0100
Subject: [PATCH 252/281] exit()-> throw (Closes #427)

---
 gtsam/geometry/Cal3_S2.cpp                        | 3 +--
 gtsam/linear/JacobianFactor.cpp                   | 7 ++++---
 gtsam/slam/SmartProjectionFactor.h                | 9 +++------
 gtsam_unstable/slam/SmartStereoProjectionFactor.h | 9 +++------
 4 files changed, 11 insertions(+), 17 deletions(-)

diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp
index 54deedfdc..980f7f969 100644
--- a/gtsam/geometry/Cal3_S2.cpp
+++ b/gtsam/geometry/Cal3_S2.cpp
@@ -44,8 +44,7 @@ Cal3_S2::Cal3_S2(const std::string &path) :
   if (infile)
     infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
   else {
-    printf("Unable to load the calibration\n");
-    exit(0);
+    throw std::runtime_error("Cal3_S2: Unable to load the calibration");
   }
 
   infile.close();
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 1f5e5557c..2373e7df0 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -164,9 +164,10 @@ boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
             n += vardim;
           } else {
             if(!(varDims[jointVarpos] == vardim)) {
-              cout << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) <<
-              " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos] << endl;
-              exit(1);
+              std::stringstream ss;
+              ss << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) <<
+              " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos];
+              throw std::runtime_error(ss.str());
             }
           }
 #else
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index e554e0c85..cbeed77c5 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -183,12 +183,9 @@ public:
     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
     std::vector<Vector> gs(numKeys);
 
-    if (this->measured_.size() != cameras.size()) {
-      std::cout
-          << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
-          << std::endl;
-      exit(1);
-    }
+    if (this->measured_.size() != cameras.size())
+      throw std::runtime_error("SmartProjectionHessianFactor: this->measured_"
+                               ".size() inconsistent with input");
 
     triangulateSafe(cameras);
 
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index f32dd3b3e..42ffa9e2f 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -206,12 +206,9 @@ public:
     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
     std::vector<Vector> gs(numKeys);
 
-    if (this->measured_.size() != cameras.size()) {
-      std::cout
-          << "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input"
-          << std::endl;
-      exit(1);
-    }
+    if (this->measured_.size() != cameras.size())
+      throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
+                               "measured_.size() inconsistent with input");
 
     triangulateSafe(cameras);
 

From 2de0957b2ddbe024626c1096956bba76d78e0ede Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 7 Apr 2019 18:14:53 -0400
Subject: [PATCH 253/281] Renamed some variables, used subVector as a test

---
 tests/testSubgraphPreconditioner.cpp | 46 ++++++++++++++++++----------
 1 file changed, 29 insertions(+), 17 deletions(-)

diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index 8a92eced8..1ffa9db93 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -219,19 +219,19 @@ TEST( SubgraphPreconditioner, RawVectorAPI )
   KeyInfo keyInfo(Ab);
   std::map<Key,Vector> lambda;
   system.build(Ab, keyInfo, lambda);
-  const auto ordering = system.Rc1()->ordering(); // build changed R1 !
+  const auto ordering = keyInfo.ordering(); // build changed R1 !
   const Matrix R1 = system.Rc1()->matrix(ordering).first;
 
   // Test that 'solve' does implement x = R^{-1} y
-  Vector y2 = Vector::Zero(18), x2(18), x3(18);
-  y2.head(2) << 100, -100;
-  system.solve(y2, x2);
-  EXPECT(assert_equal(R1.inverse() * y2, x2));
+  Vector vector_y = Vector::Zero(18), solve_x(18), solveT_x(18);
+  vector_y.head(2) << 100, -100;
+  system.solve(vector_y, solve_x);
+  EXPECT(assert_equal(R1.inverse() * vector_y, solve_x));
 
   // I can't get test below to pass!
   // Test that transposeSolve does implement x = R^{-T} y
-  // system.transposeSolve(y2, x3);
-  // EXPECT(assert_equal(R1.transpose().inverse() * y2, x3));
+  system.transposeSolve(vector_y, solveT_x);
+  EXPECT(assert_equal(R1.transpose().inverse() * vector_y, solveT_x));
 }
 
 /* ************************************************************************* */
@@ -257,23 +257,35 @@ TEST( SubgraphSolver, toy3D )
   system.build(Ab, keyInfo, lambda);
 
   // Solve the VectorValues way
-  const VectorValues xbar = system.Rc1()->optimize(); // merely for use in zero below
-  auto y = VectorValues::Zero(xbar);
-  y[12] = Vector3(100,200,-300);
-  auto values_x = system.Rc1()->backSubstitute(y);
+  const auto xbar = system.Rc1()->optimize();  // merely for use in zero below
+  auto values_y = VectorValues::Zero(xbar);
+  const size_t n = values_y.size();
+  values_y[0] = Vector3(100, 200, -300);
+  values_y[n - 1] = Vector3(10, 20, -100);
+  auto values_x = system.Rc1()->backSubstitute(values_y);
+
+  // Check YD's sub-vector machinery
+  const auto ordering = keyInfo.ordering();
+  auto vector_y = values_y.vector(ordering);
+  for (size_t j = 0; j < n; j++) {
+    EXPECT(assert_equal(values_y[j], getSubvector(vector_y, keyInfo, {j})));
+  }
 
   // Solve the matrix way, this really just checks BN::backSubstitute
-  const auto ordering = system.Rc1()->ordering();
   const Matrix R1 = system.Rc1()->matrix(ordering).first;
-  auto y2 = y.vector(ordering);
-  auto matrix_x = R1.inverse() * y2;
-  EXPECT(assert_equal(matrix_x, values_x.vector(ordering)));
+  auto vector_x = R1.inverse() * vector_y;
+  EXPECT(assert_equal(vector_x, values_x.vector(ordering)));
 
   // Test that 'solve' does implement x = R^{-1} y
   const size_t N = R1.cols();
+  EXPECT_LONGS_EQUAL(n * 3, N);
   Vector solve_x = Vector::Zero(N);
-  system.solve(y2, solve_x);
-  EXPECT(assert_equal(solve_x, matrix_x));
+  system.solve(vector_y, solve_x);
+  EXPECT(assert_equal(vector_x, solve_x));
+  for (size_t j = 0; j < n; j++) {
+    cout << j << endl;
+    EXPECT(assert_equal(values_x[j], getSubvector(vector_x, keyInfo, {j}), 1e-3));
+  }
 }
 
 /* ************************************************************************* */

From c1c2fd7008ccb8ffc42b7c7604d2ffac4cfc63a5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 7 Apr 2019 22:31:27 -0400
Subject: [PATCH 254/281] Fixed the tests - key was choosing right ordering

---
 tests/testSubgraphPreconditioner.cpp | 118 ++++++++++++---------------
 1 file changed, 51 insertions(+), 67 deletions(-)

diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index 1ffa9db93..de753fca6 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -30,6 +30,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/range/adaptor/reversed.hpp>
 #include <boost/archive/xml_iarchive.hpp>
 #include <boost/assign/std/list.hpp>
 #include <boost/serialization/export.hpp>
@@ -203,88 +204,71 @@ TEST( SubgraphPreconditioner, system )
   EXPECT(assert_equal(expected_g, vec(g)));
 }
 
-/* ************************************************************************* */
-  // Test raw vector interface
-TEST( SubgraphPreconditioner, RawVectorAPI )
-{
-  // Build a planar graph
-  GaussianFactorGraph Ab;
-  VectorValues xtrue;
-  size_t N = 3;
-  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
-
-  SubgraphPreconditioner system;
-  
-  // Call build, a non-const method needed to make solve work :-(
-  KeyInfo keyInfo(Ab);
-  std::map<Key,Vector> lambda;
-  system.build(Ab, keyInfo, lambda);
-  const auto ordering = keyInfo.ordering(); // build changed R1 !
-  const Matrix R1 = system.Rc1()->matrix(ordering).first;
-
-  // Test that 'solve' does implement x = R^{-1} y
-  Vector vector_y = Vector::Zero(18), solve_x(18), solveT_x(18);
-  vector_y.head(2) << 100, -100;
-  system.solve(vector_y, solve_x);
-  EXPECT(assert_equal(R1.inverse() * vector_y, solve_x));
-
-  // I can't get test below to pass!
-  // Test that transposeSolve does implement x = R^{-T} y
-  system.transposeSolve(vector_y, solveT_x);
-  EXPECT(assert_equal(R1.transpose().inverse() * vector_y, solveT_x));
-}
-
 /* ************************************************************************* */
 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor");
 
-TEST( SubgraphSolver, toy3D )
-{
-  // Read from file
-  string inputFile = findExampleDataFile("randomGrid3D");
+// Read from XML file
+static GaussianFactorGraph read(const string& name) {
+  auto inputFile = findExampleDataFile(name);
   ifstream is(inputFile);
   if (!is.is_open())
     throw runtime_error("Cannot find file " + inputFile);
   boost::archive::xml_iarchive in_archive(is);
   GaussianFactorGraph Ab;
   in_archive >> boost::serialization::make_nvp("graph", Ab);
+  return Ab;
+}
 
-  // Create solver, leaving splitting to constructor
+TEST(SubgraphSolver, Solves) {
+  // Create preconditioner
   SubgraphPreconditioner system;
-  
-  // Call build, a non-const method needed to make solve work :-(
-  KeyInfo keyInfo(Ab);
-  std::map<Key,Vector> lambda;
-  system.build(Ab, keyInfo, lambda);
 
-  // Solve the VectorValues way
-  const auto xbar = system.Rc1()->optimize();  // merely for use in zero below
-  auto values_y = VectorValues::Zero(xbar);
-  const size_t n = values_y.size();
-  values_y[0] = Vector3(100, 200, -300);
-  values_y[n - 1] = Vector3(10, 20, -100);
-  auto values_x = system.Rc1()->backSubstitute(values_y);
+  // We test on three different graphs
+  const auto Ab1 = planarGraph(3).get<0>();
+  const auto Ab2 = read("toy3D");
+  const auto Ab3 = read("randomGrid3D");
 
-  // Check YD's sub-vector machinery
-  const auto ordering = keyInfo.ordering();
-  auto vector_y = values_y.vector(ordering);
-  for (size_t j = 0; j < n; j++) {
-    EXPECT(assert_equal(values_y[j], getSubvector(vector_y, keyInfo, {j})));
-  }
+  // For all graphs, test solve and solveTranspose
+  for (const auto& Ab : {Ab1,Ab2,Ab3}) {
+    // Call build, a non-const method needed to make solve work :-(
+    KeyInfo keyInfo(Ab);
+    std::map<Key, Vector> lambda;
+    system.build(Ab, keyInfo, lambda);
 
-  // Solve the matrix way, this really just checks BN::backSubstitute
-  const Matrix R1 = system.Rc1()->matrix(ordering).first;
-  auto vector_x = R1.inverse() * vector_y;
-  EXPECT(assert_equal(vector_x, values_x.vector(ordering)));
+    // Create a perturbed (non-zero) RHS
+    const auto xbar = system.Rc1()->optimize();  // merely for use in zero below
+    auto values_y = VectorValues::Zero(xbar);
+    values_y.begin()->second.setConstant(100);
+    (--values_y.end())->second.setConstant(-100);
 
-  // Test that 'solve' does implement x = R^{-1} y
-  const size_t N = R1.cols();
-  EXPECT_LONGS_EQUAL(n * 3, N);
-  Vector solve_x = Vector::Zero(N);
-  system.solve(vector_y, solve_x);
-  EXPECT(assert_equal(vector_x, solve_x));
-  for (size_t j = 0; j < n; j++) {
-    cout << j << endl;
-    EXPECT(assert_equal(values_x[j], getSubvector(vector_x, keyInfo, {j}), 1e-3));
+    // Solve the VectorValues way
+    auto values_x = system.Rc1()->backSubstitute(values_y);
+
+    // Solve the matrix way, this really just checks BN::backSubstitute
+    // This only works with Rc1 ordering, not with keyInfo !
+    // TODO(frank): why does this not work with an arbitrary ordering?
+    const auto ord = system.Rc1()->ordering();
+    const Matrix R1 = system.Rc1()->matrix(ord).first;
+    auto ord_y = values_y.vector(ord);
+    auto vector_x = R1.inverse() * ord_y;
+    EXPECT(assert_equal(vector_x, values_x.vector(ord)));
+
+    // Test that 'solve' does implement x = R^{-1} y
+    // We do this by asserting it gives same answer as backSubstitute
+    // Only works with keyInfo ordering:
+    const auto ordering = keyInfo.ordering();
+    auto vector_y = values_y.vector(ordering);
+    const size_t N = R1.cols();
+    Vector solve_x = Vector::Zero(N);
+    system.solve(vector_y, solve_x);
+    EXPECT(assert_equal(values_x.vector(ordering), solve_x));
+
+    // Test that transposeSolve does implement x = R^{-T} y
+    // We do this by asserting it gives same answer as backSubstituteTranspose
+    auto values_x2 = system.Rc1()->backSubstituteTranspose(values_y);
+    Vector solveT_x = Vector::Zero(N);
+    system.transposeSolve(vector_y, solveT_x);
+    EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
   }
 }
 

From a7227cab436f3e1df3e4f0f766e38b6d2d6e1a5a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sun, 7 Apr 2019 22:33:58 -0400
Subject: [PATCH 255/281] Cleaned up formatting

---
 tests/testSubgraphPreconditioner.cpp | 132 +++++++++++++--------------
 1 file changed, 65 insertions(+), 67 deletions(-)

diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index de753fca6..d06aab7d2 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -15,24 +15,23 @@
  *  @author Frank Dellaert
  **/
 
-
 #include <tests/smallExample.h>
 
-#include <gtsam/slam/dataset.h>
-#include <gtsam/linear/iterative.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/GaussianEliminationTree.h>
-#include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/symbolic/SymbolicFactorGraph.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/inference/Ordering.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianEliminationTree.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/linear/iterative.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/range/adaptor/reversed.hpp>
 #include <boost/archive/xml_iarchive.hpp>
 #include <boost/assign/std/list.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 #include <boost/serialization/export.hpp>
 #include <boost/tuple/tuple.hpp>
 using namespace boost::assign;
@@ -45,50 +44,46 @@ using namespace example;
 
 // define keys
 // Create key for simulated planar graph
-Symbol key(int x, int y) {
-  return symbol_shorthand::X(1000*x+y);
-}
+Symbol key(int x, int y) { return symbol_shorthand::X(1000 * x + y); }
 
 /* ************************************************************************* */
-TEST( SubgraphPreconditioner, planarOrdering ) {
+TEST(SubgraphPreconditioner, planarOrdering) {
   // Check canonical ordering
   Ordering expected, ordering = planarOrdering(3);
   expected +=
       key(3, 3), key(2, 3), key(1, 3),
       key(3, 2), key(2, 2), key(1, 2),
       key(3, 1), key(2, 1), key(1, 1);
-  EXPECT(assert_equal(expected,ordering));
+  EXPECT(assert_equal(expected, ordering));
 }
 
 /* ************************************************************************* */
 /** unnormalized error */
 static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
   double total_error = 0.;
-  for(const GaussianFactor::shared_ptr& factor: fg)
+  for (const GaussianFactor::shared_ptr& factor : fg)
     total_error += factor->error(x);
   return total_error;
 }
 
 /* ************************************************************************* */
-TEST( SubgraphPreconditioner, planarGraph )
-  {
+TEST(SubgraphPreconditioner, planarGraph) {
   // Check planar graph construction
   GaussianFactorGraph A;
   VectorValues xtrue;
   boost::tie(A, xtrue) = planarGraph(3);
-  LONGS_EQUAL(13,A.size());
-  LONGS_EQUAL(9,xtrue.size());
-  DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue
+  LONGS_EQUAL(13, A.size());
+  LONGS_EQUAL(9, xtrue.size());
+  DOUBLES_EQUAL(0, error(A, xtrue), 1e-9);  // check zero error for xtrue
 
   // Check that xtrue is optimal
   GaussianBayesNet::shared_ptr R1 = A.eliminateSequential();
   VectorValues actual = R1->optimize();
-  EXPECT(assert_equal(xtrue,actual));
+  EXPECT(assert_equal(xtrue, actual));
 }
 
 /* ************************************************************************* */
-TEST( SubgraphPreconditioner, splitOffPlanarTree )
-{
+TEST(SubgraphPreconditioner, splitOffPlanarTree) {
   // Build a planar graph
   GaussianFactorGraph A;
   VectorValues xtrue;
@@ -97,48 +92,48 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree )
   // Get the spanning tree and constraints, and check their sizes
   GaussianFactorGraph::shared_ptr T, C;
   boost::tie(T, C) = splitOffPlanarTree(3, A);
-  LONGS_EQUAL(9,T->size());
-  LONGS_EQUAL(4,C->size());
+  LONGS_EQUAL(9, T->size());
+  LONGS_EQUAL(4, C->size());
 
   // Check that the tree can be solved to give the ground xtrue
   GaussianBayesNet::shared_ptr R1 = T->eliminateSequential();
   VectorValues xbar = R1->optimize();
-  EXPECT(assert_equal(xtrue,xbar));
+  EXPECT(assert_equal(xtrue, xbar));
 }
 
 /* ************************************************************************* */
-TEST( SubgraphPreconditioner, system )
-{
+TEST(SubgraphPreconditioner, system) {
   // Build a planar graph
   GaussianFactorGraph Ab;
   VectorValues xtrue;
   size_t N = 3;
-  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+  boost::tie(Ab, xtrue) = planarGraph(N);  // A*x-b
 
   // Get the spanning tree and remaining graph
-  GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
+  GaussianFactorGraph::shared_ptr Ab1, Ab2;  // A1*x-b1 and A2*x-b2
   boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
 
   // Eliminate the spanning tree to build a prior
   const Ordering ord = planarOrdering(N);
-  auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1
-  VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1
+  auto Rc1 = Ab1->eliminateSequential(ord);  // R1*x-c1
+  VectorValues xbar = Rc1->optimize();       // xbar = inv(R1)*c1
 
   // Create Subgraph-preconditioned system
-  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
+  VectorValues::shared_ptr xbarShared(
+      new VectorValues(xbar));  // TODO: horrible
   const SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
 
   // Get corresponding matrices for tests. Add dummy factors to Ab2 to make
   // sure it works with the ordering.
-  Ordering ordering = Rc1->ordering(); // not ord in general!
-  Ab2->add(key(1,1),Z_2x2, Z_2x1);
-  Ab2->add(key(1,2),Z_2x2, Z_2x1);
-  Ab2->add(key(1,3),Z_2x2, Z_2x1);
+  Ordering ordering = Rc1->ordering();  // not ord in general!
+  Ab2->add(key(1, 1), Z_2x2, Z_2x1);
+  Ab2->add(key(1, 2), Z_2x2, Z_2x1);
+  Ab2->add(key(1, 3), Z_2x2, Z_2x1);
   Matrix A, A1, A2;
   Vector b, b1, b2;
-  std::tie(A,b) = Ab.jacobian(ordering);
-  std::tie(A1,b1) = Ab1->jacobian(ordering);
-  std::tie(A2,b2) = Ab2->jacobian(ordering);
+  std::tie(A, b) = Ab.jacobian(ordering);
+  std::tie(A1, b1) = Ab1->jacobian(ordering);
+  std::tie(A2, b2) = Ab2->jacobian(ordering);
   Matrix R1 = Rc1->matrix(ordering).first;
   Matrix Abar(13 * 2, 9 * 2);
   Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2);
@@ -146,14 +141,14 @@ TEST( SubgraphPreconditioner, system )
 
   // Helper function to vectorize in correct order, which is the order in which
   // we eliminated the spanning tree.
-  auto vec = [ordering](const VectorValues& x) { return x.vector(ordering);};
+  auto vec = [ordering](const VectorValues& x) { return x.vector(ordering); };
 
   // Set up y0 as all zeros
   const VectorValues y0 = system.zero();
 
   // y1 = perturbed y0
   VectorValues y1 = system.zero();
-  y1[key(3,3)] = Vector2(1.0, -1.0);
+  y1[key(3, 3)] = Vector2(1.0, -1.0);
 
   // Check backSubstituteTranspose works with R1
   VectorValues actual = Rc1->backSubstituteTranspose(y1);
@@ -169,22 +164,22 @@ TEST( SubgraphPreconditioner, system )
   EXPECT(assert_equal(expected_x1, vec(x1)));
 
   // Check errors
-  DOUBLES_EQUAL(0,error(Ab,xbar),1e-9);
-  DOUBLES_EQUAL(0,system.error(y0),1e-9);
-  DOUBLES_EQUAL(2,error(Ab,x1),1e-9);
-  DOUBLES_EQUAL(2,system.error(y1),1e-9);
+  DOUBLES_EQUAL(0, error(Ab, xbar), 1e-9);
+  DOUBLES_EQUAL(0, system.error(y0), 1e-9);
+  DOUBLES_EQUAL(2, error(Ab, x1), 1e-9);
+  DOUBLES_EQUAL(2, system.error(y1), 1e-9);
 
   // Check that transposeMultiplyAdd <=> y += alpha * Abar' * e
   // We check for e1 =[1;0] and e2=[0;1] corresponding to T and C
   const double alpha = 0.5;
-  Errors e1,e2;
-  for (size_t i=0;i<13;i++) {
-    e1 += i<9 ? Vector2(1, 1) : Vector2(0, 0);
-    e2 += i>=9 ? Vector2(1, 1) : Vector2(0, 0);
+  Errors e1, e2;
+  for (size_t i = 0; i < 13; i++) {
+    e1 += i < 9 ? Vector2(1, 1) : Vector2(0, 0);
+    e2 += i >= 9 ? Vector2(1, 1) : Vector2(0, 0);
   }
-  Vector ee1(13*2), ee2(13*2);
-  ee1 << Vector::Ones(9*2), Vector::Zero(4*2);
-  ee2 << Vector::Zero(9*2), Vector::Ones(4*2);
+  Vector ee1(13 * 2), ee2(13 * 2);
+  ee1 << Vector::Ones(9 * 2), Vector::Zero(4 * 2);
+  ee2 << Vector::Zero(9 * 2), Vector::Ones(4 * 2);
 
   // Check transposeMultiplyAdd for e1
   VectorValues y = system.zero();
@@ -211,8 +206,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor");
 static GaussianFactorGraph read(const string& name) {
   auto inputFile = findExampleDataFile(name);
   ifstream is(inputFile);
-  if (!is.is_open())
-    throw runtime_error("Cannot find file " + inputFile);
+  if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile);
   boost::archive::xml_iarchive in_archive(is);
   GaussianFactorGraph Ab;
   in_archive >> boost::serialization::make_nvp("graph", Ab);
@@ -229,7 +223,7 @@ TEST(SubgraphSolver, Solves) {
   const auto Ab3 = read("randomGrid3D");
 
   // For all graphs, test solve and solveTranspose
-  for (const auto& Ab : {Ab1,Ab2,Ab3}) {
+  for (const auto& Ab : {Ab1, Ab2, Ab3}) {
     // Call build, a non-const method needed to make solve work :-(
     KeyInfo keyInfo(Ab);
     std::map<Key, Vector> lambda;
@@ -273,24 +267,25 @@ TEST(SubgraphSolver, Solves) {
 }
 
 /* ************************************************************************* */
-TEST( SubgraphPreconditioner, conjugateGradients )
-{
+TEST(SubgraphPreconditioner, conjugateGradients) {
   // Build a planar graph
   GaussianFactorGraph Ab;
   VectorValues xtrue;
   size_t N = 3;
-  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+  boost::tie(Ab, xtrue) = planarGraph(N);  // A*x-b
 
   // Get the spanning tree
-  GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
+  GaussianFactorGraph::shared_ptr Ab1, Ab2;  // A1*x-b1 and A2*x-b2
   boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab);
 
   // Eliminate the spanning tree to build a prior
-  SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1->eliminateSequential(); // R1*x-c1
-  VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1
+  SubgraphPreconditioner::sharedBayesNet Rc1 =
+      Ab1->eliminateSequential();       // R1*x-c1
+  VectorValues xbar = Rc1->optimize();  // xbar = inv(R1)*c1
 
   // Create Subgraph-preconditioned system
-  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
+  VectorValues::shared_ptr xbarShared(
+      new VectorValues(xbar));  // TODO: horrible
   SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
 
   // Create zero config y0 and perturbed config y1
@@ -308,9 +303,12 @@ TEST( SubgraphPreconditioner, conjugateGradients )
 
   // Compare with non preconditioned version:
   VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
-  EXPECT(assert_equal(xtrue,actual2,1e-4));
+  EXPECT(assert_equal(xtrue, actual2, 1e-4));
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

From 285ebd7dbdfbe2d3793124428587257866953576 Mon Sep 17 00:00:00 2001
From: jlblancoc <joseluisblancoc@gmail.com>
Date: Wed, 27 Feb 2019 08:50:50 +0100
Subject: [PATCH 256/281] Type for Factor indices, dual to "Key"

This avoids a couple of confusing uses of KeySet to refer to lists of
Factors, and makes code more readable where formerly using size_t to
index factors.
---
 gtsam.h                                       | 38 +++++++++++++++++--
 gtsam/base/types.h                            |  3 ++
 gtsam/inference/Factor.h                      |  3 ++
 gtsam/inference/VariableIndex.cpp             |  4 +-
 gtsam/inference/VariableIndex.h               |  5 ++-
 gtsam/nonlinear/ISAM2.cpp                     | 17 ++++-----
 gtsam/nonlinear/ISAM2.h                       |  2 +-
 gtsam/nonlinear/ISAM2Result.h                 |  2 -
 .../nonlinear/ConcurrentIncrementalFilter.cpp |  2 +-
 9 files changed, 55 insertions(+), 21 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index ba3b7f7c7..c5cea7549 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -209,6 +209,38 @@ class KeyGroupMap {
   bool insert2(size_t key, int val);
 };
 
+// Actually a FastSet<FactorIndex>
+class FactorIndexSet {
+  FactorIndexSet();
+  FactorIndexSet(const gtsam::FactorIndexSet& set);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(size_t factorIdx);
+  bool erase(size_t factorIdx); // returns true if value was removed
+  bool count(size_t factorIdx) const; // returns true if value exists
+};
+
+// Actually a vector<FactorIndex>
+class FactorIndices {
+  FactorIndices();
+  FactorIndices(const gtsam::FactorIndices& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  size_t at(size_t i) const;
+  size_t front() const;
+  size_t back() const;
+  void push_back(size_t factorIdx) const;
+};
 //*************************************************************************
 // base
 //*************************************************************************
@@ -1219,7 +1251,7 @@ namespace noiseModel {
 #include <gtsam/linear/NoiseModel.h>
 virtual class Base {
   void print(string s) const;
-  // Methods below are available for all noise models. However, can't add them 
+  // Methods below are available for all noise models. However, can't add them
   // because wrap (incorrectly) thinks robust classes derive from this Base as well.
   // bool isConstrained() const;
   // bool isUnit() const;
@@ -1257,7 +1289,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
   Vector sigmas() const;
   Vector invsigmas() const;
   Vector precisions() const;
-    
+
   // enabling serialization functionality
   void serializable() const;
 };
@@ -2053,7 +2085,7 @@ virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
   bool getUseFixedLambdaFactor();
   string getLogFile() const;
   string getVerbosityLM() const;
-  
+
   void setDiagonalDamping(bool flag);
   void setlambdaFactor(double value);
   void setlambdaInitial(double value);
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index c4775b672..3b6c9f337 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -56,6 +56,9 @@ namespace gtsam {
   /// Integer nonlinear key type
   typedef std::uint64_t Key;
 
+  /// Integer nonlinear factor index type
+  typedef std::uint64_t FactorIndex;
+
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
 
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index d44d82954..40326a4cc 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -28,6 +28,9 @@
 #include <gtsam/inference/Key.h>
 
 namespace gtsam {
+/// Define collection types:
+typedef FastVector<FactorIndex> FactorIndices;
+typedef FastSet<FactorIndex> FactorIndexSet;
 
   /**
    * This is the base class for all factor types.  It is templated on a KEY type,
diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp
index 72c56be5b..6d7471e31 100644
--- a/gtsam/inference/VariableIndex.cpp
+++ b/gtsam/inference/VariableIndex.cpp
@@ -35,7 +35,7 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c
   cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
   for(KeyMap::value_type key_factors: index_) {
     cout << "var " << keyFormatter(key_factors.first) << ":";
-    for(const size_t factor: key_factors.second)
+    for(const FactorIndex factor: key_factors.second)
       cout << " " << factor;
     cout << "\n";
   }
@@ -48,7 +48,7 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
   // run over variables, which will be hyper-edges.
   for(KeyMap::value_type key_factors: index_) {
     // every variable is a hyper-edge covering its factors
-    for(const size_t factor: key_factors.second)
+    for(const FactorIndex factor: key_factors.second)
       os << (factor+1) << " "; // base 1
     os << "\n";
   }
diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
index c16946f80..5f9e05cb0 100644
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include <gtsam/inference/Factor.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/base/FastVector.h>
@@ -43,7 +44,7 @@ class GTSAM_EXPORT VariableIndex {
 public:
 
   typedef boost::shared_ptr<VariableIndex> shared_ptr;
-  typedef FastVector<size_t> Factors;
+  typedef FactorIndices Factors;
   typedef Factors::iterator Factor_iterator;
   typedef Factors::const_iterator Factor_const_iterator;
 
@@ -122,7 +123,7 @@ public:
    * solving problems incrementally.
    */
   template<class FG>
-  void augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices = boost::none);
+  void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
 
   /**
    * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 32a24b51d..4b4edba5f 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -96,7 +96,7 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
 }
 
 /* ************************************************************************* */
-KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
+FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const {
   static const bool debug = false;
   if (debug) cout << "Getting affected factors for ";
   if (debug) {
@@ -106,15 +106,14 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
   }
   if (debug) cout << endl;
 
-  NonlinearFactorGraph allAffected;
-  KeySet indices;
+  FactorIndexSet indices;
   for (const Key key : keys) {
     const VariableIndex::Factors& factors(variableIndex_[key]);
     indices.insert(factors.begin(), factors.end());
   }
   if (debug) cout << "Affected factors are: ";
   if (debug) {
-    for (const size_t index : indices) {
+    for (const FactorIndex index : indices) {
       cout << index << " ";
     }
   }
@@ -132,8 +131,6 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
   KeySet candidates = getAffectedFactors(affectedKeys);
   gttoc(getAffectedFactors);
 
-  NonlinearFactorGraph nonlinearAffectedFactors;
-
   gttic(affectedKeysSet);
   // for fast lookup below
   KeySet affectedKeysSet;
@@ -589,7 +586,7 @@ ISAM2Result ISAM2::update(
   // Remove the removed factors
   NonlinearFactorGraph removeFactors;
   removeFactors.reserve(removeFactorIndices.size());
-  for (size_t index : removeFactorIndices) {
+  for (FactorIndex index : removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
     if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
@@ -823,7 +820,7 @@ void ISAM2::marginalizeLeaves(
   KeySet leafKeysRemoved;
 
   // Keep track of factors that get summarized by removing cliques
-  KeySet factorIndicesToRemove;
+  FactorIndexSet factorIndicesToRemove;
 
   // Remove the subtree and throw away the cliques
   auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) {
@@ -937,7 +934,7 @@ void ISAM2::marginalizeLeaves(
           }
         }
         // Create factor graph from factor indices
-        for (size_t i : factorsFromMarginalizedInClique_step1) {
+        for (FactorIndex i : factorsFromMarginalizedInClique_step1) {
           graph.push_back(nonlinearFactors_[i]->linearize(theta_));
         }
 
@@ -1011,7 +1008,7 @@ void ISAM2::marginalizeLeaves(
   // Remove the factors to remove that have been summarized in the newly-added
   // marginal factors
   NonlinearFactorGraph removedFactors;
-  for (size_t i : factorIndicesToRemove) {
+  for (FactorIndex i : factorIndicesToRemove) {
     removedFactors.push_back(nonlinearFactors_[i]);
     nonlinearFactors_.remove(i);
     if (params_.cacheLinearizedFactors) linearFactors_.remove(i);
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 04bd3d3eb..86b878837 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -292,7 +292,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
    */
   void expmapMasked(const KeySet& mask);
 
-  FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const;
+  FactorIndexSet getAffectedFactors(const FastList<Key>& keys) const;
   GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
       const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
   GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans);
diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h
index 1539d90c4..763a5e198 100644
--- a/gtsam/nonlinear/ISAM2Result.h
+++ b/gtsam/nonlinear/ISAM2Result.h
@@ -31,8 +31,6 @@
 
 namespace gtsam {
 
-typedef FastVector<size_t> FactorIndices;
-
 /**
  * @addtogroup ISAM2
  * This struct is returned from ISAM2::update() and contains information about
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
index e7c8229ef..78bd977f5 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

From a0fce4257fe4e5fe483ec9d048ee62bac803df8f Mon Sep 17 00:00:00 2001
From: jlblancoc <joseluisblancoc@gmail.com>
Date: Mon, 8 Apr 2019 10:22:59 +0200
Subject: [PATCH 257/281] Fix cmake handling newer boost versions (Closes:
 #442)

---
 CMakeLists.txt             | 37 +++++++++++++++++++++++++++++--------
 CppUnitLite/CMakeLists.txt |  2 +-
 gtsam/CMakeLists.txt       |  3 +++
 wrap/CMakeLists.txt        | 13 ++++++++++---
 4 files changed, 43 insertions(+), 12 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 186dabaf4..a299f9f19 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -142,22 +142,43 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE
   message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
 endif()
 
-option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
 # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
+option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
+
+# JLBC: This was once updated to target-based names (Boost::xxx), but it caused
+# problems with Boost versions newer than FindBoost.cmake was prepared to handle,
+# so we downgraded this to classic filenames-based variables, and manually adding
+# the target_include_directories(xxx ${Boost_INCLUDE_DIR})
 set(GTSAM_BOOST_LIBRARIES
-  Boost::serialization
-  Boost::system
-  Boost::filesystem
-  Boost::thread
-  Boost::date_time
-  Boost::regex
+  optimized
+    ${Boost_SERIALIZATION_LIBRARY_RELEASE}
+    ${Boost_SYSTEM_LIBRARY_RELEASE}
+    ${Boost_FILESYSTEM_LIBRARY_RELEASE}
+    ${Boost_THREAD_LIBRARY_RELEASE}
+    ${Boost_DATE_TIME_LIBRARY_RELEASE}
+    ${Boost_REGEX_LIBRARY_RELEASE}
+  debug
+    ${Boost_SERIALIZATION_LIBRARY_DEBUG}
+    ${Boost_SYSTEM_LIBRARY_DEBUG}
+    ${Boost_FILESYSTEM_LIBRARY_DEBUG}
+    ${Boost_THREAD_LIBRARY_DEBUG}
+    ${Boost_DATE_TIME_LIBRARY_DEBUG}
+    ${Boost_REGEX_LIBRARY_DEBUG}
 )
+message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}")
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
     list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
 else()
     if(Boost_TIMER_LIBRARY)
-      list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
+      list(APPEND GTSAM_BOOST_LIBRARIES
+        optimized
+          ${Boost_TIMER_LIBRARY_RELEASE}
+          ${Boost_CHRONO_LIBRARY_RELEASE}
+        debug
+          ${Boost_TIMER_LIBRARY_DEBUG}
+          ${Boost_CHRONO_LIBRARY_DEBUG}
+        )
     else()
       list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
       message("WARNING:  GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt
index f52841274..72651aca9 100644
--- a/CppUnitLite/CMakeLists.txt
+++ b/CppUnitLite/CMakeLists.txt
@@ -6,7 +6,7 @@ file(GLOB cppunitlite_src "*.cpp")
 add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
 list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
+target_include_directories(CppUnitLite PUBLIC ${Boost_INCLUDE_DIR}) # boost/lexical_cast.h
 
 gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
 
diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index b4adc9978..60915eead 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -101,7 +101,10 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
 
 # BUILD_SHARED_LIBS automatically defines static/shared libs:
 add_library(gtsam ${gtsam_srcs})
+# Boost:
 target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
+target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR})
+# Other deps:
 target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
 target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
 target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})
diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt
index df8dc209f..3027db19c 100644
--- a/wrap/CMakeLists.txt
+++ b/wrap/CMakeLists.txt
@@ -1,9 +1,14 @@
 # Build/install Wrap
 
 set(WRAP_BOOST_LIBRARIES
-  Boost::system
-  Boost::filesystem
-  Boost::thread
+  optimized
+    ${Boost_FILESYSTEM_LIBRARY_RELEASE}
+    ${Boost_SYSTEM_LIBRARY_RELEASE}
+    ${Boost_THREAD_LIBRARY_RELEASE}
+  optimized
+    ${Boost_FILESYSTEM_LIBRARY_DEBUG}
+    ${Boost_SYSTEM_LIBRARY_DEBUG}
+    ${Boost_THREAD_LIBRARY_DEBUG}
 )
 
 # Allow for disabling serialization to handle errors related to Clang's linker
@@ -22,6 +27,8 @@ if (NOT GTSAM_WRAP_SERIALIZATION)
 endif()
 
 target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES})
+target_include_directories(wrap_lib PUBLIC ${Boost_INCLUDE_DIR})
+
 gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers})
 add_executable(wrap wrap.cpp)
 target_link_libraries(wrap PRIVATE wrap_lib)

From 913262b27d2416f54b4b11e0165c8fe2bec93022 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:46:56 -0400
Subject: [PATCH 258/281] Fixed issue with test with tbb iterators

---
 tests/testSubgraphPreconditioner.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index d06aab7d2..d9b07184b 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -232,8 +232,10 @@ TEST(SubgraphSolver, Solves) {
     // Create a perturbed (non-zero) RHS
     const auto xbar = system.Rc1()->optimize();  // merely for use in zero below
     auto values_y = VectorValues::Zero(xbar);
-    values_y.begin()->second.setConstant(100);
-    (--values_y.end())->second.setConstant(-100);
+    auto it = values_y.begin();
+    it->second.setConstant(100);
+    ++it;
+    it->second.setConstant(-100);
 
     // Solve the VectorValues way
     auto values_x = system.Rc1()->backSubstitute(values_y);

From 4fb718a943e5565d0e5e1e8dfc3e186e96e61920 Mon Sep 17 00:00:00 2001
From: jlblancoc <joseluisblancoc@gmail.com>
Date: Tue, 9 Apr 2019 00:29:31 +0200
Subject: [PATCH 259/281] prefer auto in range for loops

---
 gtsam.h                                          |  8 ++++----
 gtsam/inference/VariableIndex.cpp                |  8 ++++----
 gtsam/nonlinear/ISAM2.cpp                        | 16 ++++++++--------
 .../discrete/tests/testLoopyBelief.cpp           | 10 +++++-----
 4 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index c5cea7549..8e6d308e1 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -220,9 +220,9 @@ class FactorIndexSet {
   void clear();
 
   // structure specific methods
-  void insert(size_t factorIdx);
-  bool erase(size_t factorIdx); // returns true if value was removed
-  bool count(size_t factorIdx) const; // returns true if value exists
+  void insert(size_t factorIndex);
+  bool erase(size_t factorIndex); // returns true if value was removed
+  bool count(size_t factorIndex) const; // returns true if value exists
 };
 
 // Actually a vector<FactorIndex>
@@ -239,7 +239,7 @@ class FactorIndices {
   size_t at(size_t i) const;
   size_t front() const;
   size_t back() const;
-  void push_back(size_t factorIdx) const;
+  void push_back(size_t factorIndex) const;
 };
 //*************************************************************************
 // base
diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp
index 6d7471e31..b71c81988 100644
--- a/gtsam/inference/VariableIndex.cpp
+++ b/gtsam/inference/VariableIndex.cpp
@@ -35,8 +35,8 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c
   cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
   for(KeyMap::value_type key_factors: index_) {
     cout << "var " << keyFormatter(key_factors.first) << ":";
-    for(const FactorIndex factor: key_factors.second)
-      cout << " " << factor;
+    for(const auto index: key_factors.second)
+      cout << " " << index;
     cout << "\n";
   }
   cout.flush();
@@ -48,8 +48,8 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
   // run over variables, which will be hyper-edges.
   for(KeyMap::value_type key_factors: index_) {
     // every variable is a hyper-edge covering its factors
-    for(const FactorIndex factor: key_factors.second)
-      os << (factor+1) << " "; // base 1
+    for(const auto index: key_factors.second)
+      os << (index+1) << " "; // base 1
     os << "\n";
   }
   os << flush;
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 4b4edba5f..c0b2d0757 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -113,7 +113,7 @@ FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const {
   }
   if (debug) cout << "Affected factors are: ";
   if (debug) {
-    for (const FactorIndex index : indices) {
+    for (const auto index : indices) {
       cout << index << " ";
     }
   }
@@ -586,7 +586,7 @@ ISAM2Result ISAM2::update(
   // Remove the removed factors
   NonlinearFactorGraph removeFactors;
   removeFactors.reserve(removeFactorIndices.size());
-  for (FactorIndex index : removeFactorIndices) {
+  for (const auto index : removeFactorIndices) {
     removeFactors.push_back(nonlinearFactors_[index]);
     nonlinearFactors_.remove(index);
     if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
@@ -934,8 +934,8 @@ void ISAM2::marginalizeLeaves(
           }
         }
         // Create factor graph from factor indices
-        for (FactorIndex i : factorsFromMarginalizedInClique_step1) {
-          graph.push_back(nonlinearFactors_[i]->linearize(theta_));
+        for (const auto index: factorsFromMarginalizedInClique_step1) {
+          graph.push_back(nonlinearFactors_[index]->linearize(theta_));
         }
 
         // Reeliminate the linear graph to get the marginal and discard the
@@ -1008,10 +1008,10 @@ void ISAM2::marginalizeLeaves(
   // Remove the factors to remove that have been summarized in the newly-added
   // marginal factors
   NonlinearFactorGraph removedFactors;
-  for (FactorIndex i : factorIndicesToRemove) {
-    removedFactors.push_back(nonlinearFactors_[i]);
-    nonlinearFactors_.remove(i);
-    if (params_.cacheLinearizedFactors) linearFactors_.remove(i);
+  for (const auto index: factorIndicesToRemove) {
+    removedFactors.push_back(nonlinearFactors_[index]);
+    nonlinearFactors_.remove(index);
+    if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
   }
   variableIndex_.remove(factorIndicesToRemove.begin(),
                         factorIndicesToRemove.end(), removedFactors);
diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
index d2efc3a2d..262c575c1 100644
--- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
+++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp
@@ -175,19 +175,19 @@ private:
 
       // collect all factors involving this key in the original graph
       DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph());
-      for(size_t factorIdx: varIndex[key]) {
-        star->push_back(graph.at(factorIdx));
+      for(size_t factorIndex: varIndex[key]) {
+        star->push_back(graph.at(factorIndex));
 
         // accumulate unary factors
-        if (graph.at(factorIdx)->size() == 1) {
+        if (graph.at(factorIndex)->size() == 1) {
           if (!prodOfUnaries)
             prodOfUnaries = boost::dynamic_pointer_cast<DecisionTreeFactor>(
-                graph.at(factorIdx));
+                graph.at(factorIndex));
           else
             prodOfUnaries = boost::make_shared<DecisionTreeFactor>(
                 *prodOfUnaries
                     * (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
-                        graph.at(factorIdx))));
+                        graph.at(factorIndex))));
         }
       }
 

From 7eb8cc18c37854f37edf371a47c4caa344046774 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 9 Apr 2019 21:08:35 -0400
Subject: [PATCH 260/281] removed duplicate declaration of FactorIndices class

---
 gtsam.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 8e6d308e1..73684c96a 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -2241,8 +2241,6 @@ class ISAM2Result {
   size_t getCliques() const;
 };
 
-class FactorIndices {};
-
 class ISAM2 {
   ISAM2();
   ISAM2(const gtsam::ISAM2Params& params);

From 50d9e1ef1c17f0d4e29cebccd1ff1a4496986530 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:49:17 -0400
Subject: [PATCH 261/281] numerical expected results rather than regression

---
 gtsam/linear/tests/testJacobianFactor.cpp | 21 ++++++++++++---------
 1 file changed, 12 insertions(+), 9 deletions(-)

diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index 2ea1b15bd..f6ab4be73 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -322,27 +322,30 @@ TEST(JacobianFactor, matrices)
 /* ************************************************************************* */
 TEST(JacobianFactor, operators )
 {
-  SharedDiagonal  sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
+  const double sigma = 0.1;
+  SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2, sigma);
 
   Matrix I = I_2x2;
   Vector b = Vector2(0.2,-0.1);
   JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
 
-  VectorValues c;
-  c.insert(1, Vector2(10.,20.));
-  c.insert(2, Vector2(30.,60.));
+  VectorValues x;
+  Vector2 x1(10,20), x2(30,60);
+  x.insert(1, x1);
+  x.insert(2, x2);
 
   // test A*x
-  Vector expectedE = Vector2(200.,400.);
-  Vector actualE = lf * c;
+  Vector expectedE = (x2 - x1)/sigma;
+  Vector actualE = lf * x;
   EXPECT(assert_equal(expectedE, actualE));
 
   // test A^e
   VectorValues expectedX;
-  expectedX.insert(1, Vector2(-2000.,-4000.));
-  expectedX.insert(2, Vector2(2000., 4000.));
+  const double alpha = 0.5;
+  expectedX.insert(1, - alpha * expectedE /sigma);
+  expectedX.insert(2,   alpha * expectedE /sigma);
   VectorValues actualX = VectorValues::Zero(expectedX);
-  lf.transposeMultiplyAdd(1.0, actualE, actualX);
+  lf.transposeMultiplyAdd(alpha, actualE, actualX);
   EXPECT(assert_equal(expectedX, actualX));
 
   // test gradient at zero

From 1a862c24a699487a627ffa790a4e246b7a3066bf Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:51:39 -0400
Subject: [PATCH 262/281] Reserve memory for cache

---
 gtsam/linear/SubgraphPreconditioner.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 657846f95..9578d45bd 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -658,6 +658,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
                     const KeyVector &keys) {
   /* a cache of starting index and dim */
   vector<std::pair<size_t, size_t> > cache;
+  cache.reserve(3);
 
   /* figure out dimension by traversing the keys */
   size_t dim = 0;

From 60d07287c9db798894420a1816e97d8b36cc3b4a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:52:41 -0400
Subject: [PATCH 263/281] made KeyInfoEntry into a struct

---
 gtsam/linear/IterativeSolver.h | 23 +++++------------------
 1 file changed, 5 insertions(+), 18 deletions(-)

diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h
index 92dcbfa07..f0fbfbfd2 100644
--- a/gtsam/linear/IterativeSolver.h
+++ b/gtsam/linear/IterativeSolver.h
@@ -32,8 +32,8 @@
 namespace gtsam {
 
 // Forward declarations
+struct KeyInfoEntry;
 class KeyInfo;
-class KeyInfoEntry;
 class GaussianFactorGraph;
 class Values;
 class VectorValues;
@@ -109,27 +109,14 @@ public:
 
 /**
  * Handy data structure for iterative solvers
- * key to (index, dimension, colstart)
+ * key to (index, dimension, start)
  */
-class GTSAM_EXPORT KeyInfoEntry: public boost::tuple<Key, size_t, Key> {
-
-public:
-
-  typedef boost::tuple<Key, size_t, Key> Base;
-
+struct GTSAM_EXPORT KeyInfoEntry {
+  size_t index, dim, start;
   KeyInfoEntry() {
   }
   KeyInfoEntry(size_t idx, size_t d, Key start) :
-      Base(idx, d, start) {
-  }
-  size_t index() const {
-    return this->get<0>();
-  }
-  size_t dim() const {
-    return this->get<1>();
-  }
-  size_t colstart() const {
-    return this->get<2>();
+      index(idx), dim(d), start(start) {
   }
 };
 

From d8147105755d5fda691e736794373dff582be921 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:52:52 -0400
Subject: [PATCH 264/281] Added emplace

---
 gtsam/linear/VectorValues.cpp | 27 ++++++++++++++++----------
 gtsam/linear/VectorValues.h   | 36 ++++++++++++++++-------------------
 2 files changed, 33 insertions(+), 30 deletions(-)

diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp
index ca95313cf..e8304e6e7 100644
--- a/gtsam/linear/VectorValues.cpp
+++ b/gtsam/linear/VectorValues.cpp
@@ -51,7 +51,7 @@ namespace gtsam {
       Key key;
       size_t n;
       boost::tie(key, n) = v;
-      values_.insert(make_pair(key, x.segment(j, n)));
+      values_.emplace(key, x.segment(j, n));
       j += n;
     }
   }
@@ -60,7 +60,7 @@ namespace gtsam {
   VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
     size_t j = 0;
     for (const SlotEntry& v : scatter) {
-      values_.insert(make_pair(v.key, x.segment(j, v.dimension)));
+      values_.emplace(v.key, x.segment(j, v.dimension));
       j += v.dimension;
     }
   }
@@ -70,14 +70,12 @@ namespace gtsam {
   {
     VectorValues result;
     for(const KeyValuePair& v: other)
-      result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size())));
+      result.values_.emplace(v.first, Vector::Zero(v.second.size()));
     return result;
   }
 
   /* ************************************************************************* */
   VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
-    // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
-    // it is inserted into the values_ map.
     std::pair<iterator, bool> result = values_.insert(key_value);
     if(!result.second)
       throw std::invalid_argument(
@@ -86,6 +84,16 @@ namespace gtsam {
     return result.first;
   }
 
+  /* ************************************************************************* */
+  VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
+    std::pair<iterator, bool> result = values_.emplace(j, value);
+    if(!result.second)
+      throw std::invalid_argument(
+      "Requested to emplace variable '" + DefaultKeyFormatter(j)
+      + "' already in this VectorValues.");
+    return result.first;
+  }
+
   /* ************************************************************************* */
   void VectorValues::update(const VectorValues& values)
   {
@@ -132,8 +140,7 @@ namespace gtsam {
   bool VectorValues::equals(const VectorValues& x, double tol) const {
     if(this->size() != x.size())
       return false;
-    typedef boost::tuple<value_type, value_type> ValuePair;
-    for(const ValuePair& values: boost::combine(*this, x)) {
+    for(const auto& values: boost::combine(*this, x)) {
       if(values.get<0>().first != values.get<1>().first ||
         !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
         return false;
@@ -240,7 +247,7 @@ namespace gtsam {
     VectorValues result;
     // The result.end() hint here should result in constant-time inserts
     for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
-      result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second));
+      result.values_.emplace(j1->first, j1->second + j2->second);
 
     return result;
   }
@@ -298,7 +305,7 @@ namespace gtsam {
     VectorValues result;
     // The result.end() hint here should result in constant-time inserts
     for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
-      result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second));
+      result.values_.emplace(j1->first, j1->second - j2->second);
 
     return result;
   }
@@ -314,7 +321,7 @@ namespace gtsam {
   {
     VectorValues result;
     for(const VectorValues::KeyValuePair& key_v: v)
-      result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second));
+      result.values_.emplace(key_v.first, a * key_v.second);
     return result;
   }
 
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 39abe1b56..e0bacfd12 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -50,9 +50,9 @@ namespace gtsam {
    * Example:
    * \code
      VectorValues values;
-     values.insert(3, Vector3(1.0, 2.0, 3.0));
-     values.insert(4, Vector2(4.0, 5.0));
-     values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished());
+     values.emplace(3, Vector3(1.0, 2.0, 3.0));
+     values.emplace(4, Vector2(4.0, 5.0));
+     values.emplace(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished());
 
      // Prints [ 3.0 4.0 ]
      gtsam::print(values[1]);
@@ -64,18 +64,7 @@ namespace gtsam {
    *
    * <h2>Advanced Interface and Performance Information</h2>
    *
-   * For advanced usage, or where speed is important:
-   *  - Allocate space ahead of time using a pre-allocating constructor
-   *    (\ref AdvancedConstructors "Advanced Constructors"), Zero(),
-   *    SameStructure(), resize(), or append().  Do not use
-   *    insert(Key, const Vector&), which always has to re-allocate the
-   *    internal vector.
-   *  - The vector() function permits access to the underlying Vector, for
-   *    doing mathematical or other operations that require all values.
-   *  - operator[]() returns a SubVector view of the underlying Vector,
-   *    without copying any data.
-   *
-   * Access is through the variable index j, and returns a SubVector,
+   * Access is through the variable Key j, and returns a SubVector,
    * which is a view on the underlying data structure.
    *
    * This class is additionally used in gradient descent and dog leg to store the gradient.
@@ -183,15 +172,21 @@ namespace gtsam {
      *  j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated. */
-    iterator insert(Key j, const Vector& value) {
-      return insert(std::make_pair(j, value));
-    }
+    iterator insert(const std::pair<Key, Vector>& key_value);
+
+    /** Emplace a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
+     *  j is already used.
+     * @param value The vector to be inserted.
+     * @param j The index with which the value will be associated. */
+    iterator emplace(Key j, const Vector& value);
 
     /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
      *  j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated. */
-    iterator insert(const std::pair<Key, Vector>& key_value);
+    iterator insert(Key j, const Vector& value) {
+      return insert(std::make_pair(j, value));
+    }
 
     /** Insert all values from \c values.  Throws an invalid_argument exception if any keys to be
      *  inserted are already used. */
@@ -202,7 +197,8 @@ namespace gtsam {
      *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
      *  returned. */
     std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
-      return values_.insert(std::make_pair(j, value)); }
+      return values_.emplace(j, value); 
+    }
 
     /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */
     void erase(Key var) {

From 2cedda703c925287e1b5150861e487cc664c4744 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:53:27 -0400
Subject: [PATCH 265/281] Fixed up iterative methods to use struct

---
 gtsam/linear/IterativeSolver.cpp        | 10 +--
 gtsam/linear/PCGSolver.cpp              |  9 ++-
 gtsam/linear/SubgraphPreconditioner.cpp | 84 ++++++++++++-------------
 3 files changed, 51 insertions(+), 52 deletions(-)

diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp
index 411feb7a9..c7d4e5405 100644
--- a/gtsam/linear/IterativeSolver.cpp
+++ b/gtsam/linear/IterativeSolver.cpp
@@ -117,7 +117,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
   for (size_t i = 0; i < n; ++i) {
     const size_t key = ordering_[i];
     const size_t dim = colspec.find(key)->second;
-    insert(make_pair(key, KeyInfoEntry(i, dim, start)));
+    this->emplace(key, KeyInfoEntry(i, dim, start));
     start += dim;
   }
   numCols_ = start;
@@ -126,8 +126,8 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
 /****************************************************************************/
 vector<size_t> KeyInfo::colSpec() const {
   std::vector<size_t> result(size(), 0);
-  for ( const KeyInfo::value_type &item: *this ) {
-    result[item.second.index()] = item.second.dim();
+  for ( const auto &item: *this ) {
+    result[item.second.index] = item.second.dim;
   }
   return result;
 }
@@ -135,8 +135,8 @@ vector<size_t> KeyInfo::colSpec() const {
 /****************************************************************************/
 VectorValues KeyInfo::x0() const {
   VectorValues result;
-  for ( const KeyInfo::value_type &item: *this ) {
-    result.insert(item.first, Vector::Zero(item.second.dim()));
+  for ( const auto &item: *this ) {
+    result.emplace(item.first, Vector::Zero(item.second.dim));
   }
   return result;
 }
diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp
index f9ef756f1..08307c5ab 100644
--- a/gtsam/linear/PCGSolver.cpp
+++ b/gtsam/linear/PCGSolver.cpp
@@ -89,7 +89,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
   VectorValues vvX = buildVectorValues(x, keyInfo_);
 
   // VectorValues form of A'Ax for multiplyHessianAdd
-  VectorValues vvAtAx;
+  VectorValues vvAtAx = keyInfo_.x0(); // crucial for performance
 
   // vvAtAx += 1.0 * A'Ax for each factor
   gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
@@ -132,14 +132,14 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
 
   DenseIndex offset = 0;
   for (size_t i = 0; i < ordering.size(); ++i) {
-    const Key &key = ordering[i];
+    const Key key = ordering[i];
     map<Key, size_t>::const_iterator it = dimensions.find(key);
     if (it == dimensions.end()) {
       throw invalid_argument(
           "buildVectorValues: inconsistent ordering and dimensions");
     }
     const size_t dim = it->second;
-    result.insert(key, v.segment(offset, dim));
+    result.emplace(key, v.segment(offset, dim));
     offset += dim;
   }
 
@@ -150,8 +150,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
 VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
   VectorValues result;
   for ( const KeyInfo::value_type &item: keyInfo ) {
-    result.insert(item.first,
-        v.segment(item.second.colstart(), item.second.dim()));
+    result.emplace(item.first, v.segment(item.second.start, item.second.dim));
   }
   return result;
 }
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 9578d45bd..e011b60d8 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -99,8 +99,8 @@ static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
     const double value = rand() / denominator;
     /* binary search the interval containing "value" */
     const auto it = std::lower_bound(acc.begin(), acc.end(), value);
-    const size_t idx = it - acc.begin();
-    result.push_back(idx);
+    const size_t index = it - acc.begin();
+    result.push_back(index);
   }
   return result;
 }
@@ -129,10 +129,10 @@ static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n
 
     /* sampling and cache results */
     vector<size_t> samples = iidSampler(localWeights, n-count);
-    for ( const size_t &id: samples ) {
-      if ( touched[id] == false ) {
-        touched[id] = true ;
-        result.push_back(id);
+    for ( const size_t &index: samples ) {
+      if ( touched[index] == false ) {
+        touched[index] = true ;
+        result.push_back(index);
         if ( ++count >= n ) break;
       }
     }
@@ -143,8 +143,8 @@ static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n
 /****************************************************************************/
 Subgraph::Subgraph(const vector<size_t> &indices) {
   edges_.reserve(indices.size());
-  for ( const size_t &idx: indices ) {
-    edges_.emplace_back(idx, 1.0);
+  for ( const size_t &index: indices ) {
+    edges_.emplace_back(index, 1.0);
   }
 }
 
@@ -296,12 +296,12 @@ vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
 /****************************************************************/
 vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
   vector<size_t> result ;
-  size_t idx = 0;
+  size_t index = 0;
   for (const auto &factor : gfg) {
     if ( factor->size() == 1 ) {
-      result.push_back(idx);
+      result.push_back(index);
     }
-    idx++;
+    index++;
   }
   return result;
 }
@@ -309,14 +309,14 @@ vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
 /****************************************************************/
 vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
   vector<size_t> result ;
-  size_t idx = 0;
+  size_t index = 0;
   for ( const GaussianFactor::shared_ptr &gf: gfg ) {
     if ( gf->size() == 2 ) {
       const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
       if ( (k1-k0) == 1 || (k0-k1) == 1 )
-        result.push_back(idx);
+        result.push_back(index);
     }
-    idx++;
+    index++;
   }
   return result;
 }
@@ -343,13 +343,13 @@ vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
   /* traversal */
   while ( !q.empty() ) {
     const size_t head = q.front(); q.pop();
-    for ( const size_t id: variableIndex[head] ) {
-      const GaussianFactor &gf = *gfg[id];
+    for ( const size_t index: variableIndex[head] ) {
+      const GaussianFactor &gf = *gfg[index];
       for ( const size_t key: gf.keys() ) {
         if ( flags.count(key) == 0 ) {
           q.push(key);
           flags.insert(key);
-          result.push_back(id);
+          result.push_back(index);
         }
       }
     }
@@ -363,7 +363,7 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
                                         const vector<double> &weights) const {
   const VariableIndex variableIndex(gfg);
   const size_t n = variableIndex.size();
-  const vector<size_t> idx = sort_idx(weights) ;
+  const vector<size_t> sortedIndices = sort_idx(weights) ;
 
   /* initialize buffer */
   vector<size_t> result;
@@ -373,16 +373,16 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
   DSFVector dsf(n);
 
   size_t count = 0 ; double sum = 0.0 ;
-  for (const size_t id: idx) {
-    const GaussianFactor &gf = *gfg[id];
+  for (const size_t index: sortedIndices) {
+    const GaussianFactor &gf = *gfg[index];
     const auto keys = gf.keys();
     if ( keys.size() != 2 ) continue;
     const size_t u = ordering.find(keys[0])->second,
                  v = ordering.find(keys[1])->second;
     if ( dsf.find(u) != dsf.find(v) ) {
       dsf.merge(u, v) ;
-      result.push_back(id) ;
-      sum += weights[id] ;
+      result.push_back(index) ;
+      sum += weights[index] ;
       if ( ++count == n-1 ) break ;
     }
   }
@@ -418,8 +418,8 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
   }
 
   // Downweight the tree edges to zero.
-  for ( const size_t id: tree ) {
-    weights[id] = 0.0;
+  for ( const size_t index: tree ) {
+    weights[index] = 0.0;
   }
 
   /* decide how many edges to augment */
@@ -614,8 +614,8 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
 
   /* in place back substitute */
   for (const auto &cg : *Rc1_) {
-    const Vector rhsFrontal = getSubvector(
-        x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
+    const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
+    const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
     const Vector solFrontal =
         cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
             rhsFrontal);
@@ -625,13 +625,12 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
       throw IndeterminantLinearSystemException(cg->keys().front());
 
     /* assign subvector of sol to the frontal variables */
-    setSubvector(solFrontal, keyInfo_,
-                 KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
+    setSubvector(solFrontal, keyInfo_, frontalKeys, x);
 
     /* substract from parent variables */
     for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
-      const KeyInfoEntry &info = keyInfo_.find(*it)->second;
-      Eigen::Map<Vector> rhsParent(x.data() + info.colstart(), info.dim(), 1);
+      const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
+      Eigen::Map<Vector> rhsParent(x.data() + entry.start, entry.dim, 1);
       rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
     }
   }
@@ -664,16 +663,16 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
   size_t dim = 0;
   for (const Key &key : keys) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
-    cache.emplace_back(entry.colstart(), entry.dim());
-    dim += entry.dim();
+    cache.emplace_back(entry.start, entry.dim);
+    dim += entry.dim;
   }
 
   /* use the cache to fill the result */
-  Vector result = Vector::Zero(dim);
-  size_t idx = 0;
+  Vector result(dim);
+  size_t index = 0;
   for (const auto &p : cache) {
-    result.segment(idx, p.second) = src.segment(p.first, p.second);
-    idx += p.second;
+    result.segment(index, p.second) = src.segment(p.first, p.second);
+    index += p.second;
   }
 
   return result;
@@ -681,11 +680,12 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
 
 /*****************************************************************************/
 void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
-  size_t idx = 0;
+  size_t index = 0;
   for ( const Key &key: keys ) {
     const KeyInfoEntry &entry = keyInfo.find(key)->second;
-    dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
-    idx += entry.dim();
+    const size_t keyDim = entry.dim;
+    dst.segment(entry.start, keyDim) = src.segment(index, keyDim) ;
+    index += keyDim;
   }
 }
 
@@ -696,9 +696,9 @@ GaussianFactorGraph::shared_ptr buildFactorSubgraph(
   auto result = boost::make_shared<GaussianFactorGraph>();
   result->reserve(subgraph.size());
   for ( const SubgraphEdge &e: subgraph ) {
-    const size_t idx = e.index();
-    if ( clone ) result->push_back(gfg[idx]->clone());
-    else result->push_back(gfg[idx]);
+    const size_t index = e.index();
+    if ( clone ) result->push_back(gfg[index]->clone());
+    else result->push_back(gfg[index]);
   }
   return result;
 }

From 9ec714063a4a889f3f5ecc9a15d7327fc04a8ad5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:53:59 -0400
Subject: [PATCH 266/281] many performance tweaks

---
 gtsam/linear/JacobianFactor.cpp | 50 +++++++++++++++++++--------------
 1 file changed, 29 insertions(+), 21 deletions(-)

diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 2373e7df0..d617ecc15 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -430,10 +430,9 @@ Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
 
 /* ************************************************************************* */
 Vector JacobianFactor::error_vector(const VectorValues& c) const {
-  if (model_)
-    return model_->whiten(unweighted_error(c));
-  else
-    return unweighted_error(c);
+  Vector e = unweighted_error(c);
+  if (model_) model_->whitenInPlace(e);
+  return e;
 }
 
 /* ************************************************************************* */
@@ -474,10 +473,10 @@ VectorValues JacobianFactor::hessianDiagonal() const {
     for (size_t k = 0; k < nj; ++k) {
       Vector column_k = Ab_(pos).col(k);
       if (model_)
-        column_k = model_->whiten(column_k);
+        model_->whitenInPlace(column_k);
       dj(k) = dot(column_k, column_k);
     }
-    d.insert(j, dj);
+    d.emplace(j, dj);
   }
   return d;
 }
@@ -496,7 +495,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
     Matrix Aj = Ab_(pos);
     if (model_)
       Aj = model_->Whiten(Aj);
-    blocks.insert(make_pair(j, Aj.transpose() * Aj));
+    blocks.emplace(j, Aj.transpose() * Aj);
   }
   return blocks;
 }
@@ -541,29 +540,38 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys,
 
 /* ************************************************************************* */
 Vector JacobianFactor::operator*(const VectorValues& x) const {
-  Vector Ax = Vector::Zero(Ab_.rows());
+  Vector Ax(Ab_.rows());
+  Ax.setZero();
   if (empty())
     return Ax;
 
   // Just iterate over all A matrices and multiply in correct config part
-  for (size_t pos = 0; pos < size(); ++pos)
-    Ax += Ab_(pos) * x[keys_[pos]];
+  for (size_t pos = 0; pos < size(); ++pos) {
+    // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
+    Ax.noalias() += Ab_(pos) * x[keys_[pos]];
+  }
 
-  return model_ ? model_->whiten(Ax) : Ax;
+  if (model_) model_->whitenInPlace(Ax);
+  return Ax;
 }
 
 /* ************************************************************************* */
 void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
-    VectorValues& x) const {
-  Vector E = alpha * (model_ ? model_->whiten(e) : e);
+                                          VectorValues& x) const {
+  Vector E(e.size());
+  E.noalias() = alpha * e;
+  if (model_) model_->whitenInPlace(E);
   // Just iterate over all A matrices and insert Ai^e into VectorValues
   for (size_t pos = 0; pos < size(); ++pos) {
-    Key j = keys_[pos];
-    // Create the value as a zero vector if it does not exist.
-    pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
-    if (xi.second)
-      xi.first->second = Vector::Zero(getDim(begin() + pos));
-    xi.first->second += Ab_(pos).transpose()*E;
+    const Key j = keys_[pos];
+    // To avoid another malloc if key exists, we explicitly check
+    auto it = x.find(j);
+    if (it != x.end()) {
+      // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html
+      it->second.noalias() += Ab_(pos).transpose() * E;
+    } else {
+      x.emplace(j, Ab_(pos).transpose() * E);
+    }
   }
 }
 
@@ -625,8 +633,8 @@ VectorValues JacobianFactor::gradientAtZero() const {
   Vector b = getb();
   // Gradient is really -A'*b / sigma^2
   // transposeMultiplyAdd will divide by sigma once, so we need one more
-  Vector b_sigma = model_ ? model_->whiten(b) : b;
-  this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2
+  if (model_) model_->whitenInPlace(b);
+  this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2
   return g;
 }
 

From 35d8ebca22ddd89e671078e48396fe9bd59fc632 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 8 Apr 2019 16:54:20 -0400
Subject: [PATCH 267/281] use emplace where possible

---
 gtsam/linear/GaussianConditional.cpp | 4 ++--
 gtsam/linear/GaussianFactorGraph.cpp | 4 ++--
 gtsam/linear/HessianFactor.cpp       | 6 +++---
 gtsam/linear/linearAlgorithms-inst.h | 6 +++---
 4 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 93217c027..ed4a6e091 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -134,7 +134,7 @@ namespace gtsam {
     VectorValues result;
     DenseIndex vectorPosition = 0;
     for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
-      result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal)));
+      result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal)));
       vectorPosition += getDim(frontal);
     }
 
@@ -162,7 +162,7 @@ namespace gtsam {
     VectorValues result;
     DenseIndex vectorPosition = 0;
     for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
-      result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal)));
+      result.emplace(*frontal, soln.segment(vectorPosition, getDim(frontal)));
       vectorPosition += getDim(frontal);
     }
 
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index a4df04cf9..9281c7e7a 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -61,7 +61,7 @@ namespace gtsam {
       for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) {
         map<Key,size_t>::iterator it2 = spec.find(*it);
         if ( it2 == spec.end() ) {
-          spec.insert(make_pair(*it, gf->getDim(it)));
+          spec.emplace(*it, gf->getDim(it));
         }
       }
     }
@@ -246,7 +246,7 @@ namespace gtsam {
         if (blocks.count(j))
           blocks[j] += Bj;
         else
-          blocks.insert(make_pair(j,Bj));
+          blocks.emplace(j,Bj);
       }
     }
     return blocks;
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index cded6d2ee..da7d7bd7b 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -305,7 +305,7 @@ Matrix HessianFactor::information() const {
 VectorValues HessianFactor::hessianDiagonal() const {
   VectorValues d;
   for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
-    d.insert(keys_[j], info_.diagonal(j));
+    d.emplace(keys_[j], info_.diagonal(j));
   }
   return d;
 }
@@ -436,7 +436,7 @@ VectorValues HessianFactor::gradientAtZero() const {
   VectorValues g;
   size_t n = size();
   for (size_t j = 0; j < n; ++j)
-    g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n));
+    g.emplace(keys_[j], -info_.aboveDiagonalBlock(j, n));
   return g;
 }
 
@@ -513,7 +513,7 @@ VectorValues HessianFactor::solve() {
   std::size_t offset = 0;
   for (DenseIndex j = 0; j < (DenseIndex)n; ++j) {
     const DenseIndex dim = info_.getDim(j);
-    delta.insert(keys_[j], solution.segment(offset, dim));
+    delta.emplace(keys_[j], solution.segment(offset, dim));
     offset += dim;
   }
 
diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h
index ac8c2d7c7..8b83ce0c3 100644
--- a/gtsam/linear/linearAlgorithms-inst.h
+++ b/gtsam/linear/linearAlgorithms-inst.h
@@ -56,7 +56,7 @@ namespace gtsam
           myData.parentData = parentData;
           // Take any ancestor results we'll need
           for(Key parent: clique->conditional_->parents())
-            myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent)));
+            myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));
 
           // Solve and store in our results
           {
@@ -99,8 +99,8 @@ namespace gtsam
             DenseIndex vectorPosition = 0;
             for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
               VectorValues::const_iterator r =
-                collectedResult.insert(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
-              myData.cliqueResults.insert(make_pair(r->first, r));
+                collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
+              myData.cliqueResults.emplace(r->first, r);
               vectorPosition += c.getDim(frontal);
             }
           }

From d8570ce09b380a899fee3a5ed0c74186f4c3c7c1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 10 Apr 2019 22:25:33 -0400
Subject: [PATCH 268/281] Fixed issue with << operator (surfaced in debug mode)

---
 gtsam/slam/InitializePose3.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp
index 2f247811d..a1baab5fa 100644
--- a/gtsam/slam/InitializePose3.cpp
+++ b/gtsam/slam/InitializePose3.cpp
@@ -82,7 +82,11 @@ Values InitializePose3::normalizeRelaxedRotations(
   for(const auto& it: relaxedRot3) {
     Key key = it.first;
     if (key != kAnchorKey) {
-      Matrix3 R; R << it.second;
+      Matrix3 R;
+      R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
+
+      // ClosestTo finds rotation matrix closest to H in Frobenius sense
+      // Rot3 initRot = Rot3::ClosestTo(M.transpose());
 
       Matrix U, V; Vector s;
       svd(R.transpose(), U, s, V);

From 35d9f65d9c72e682bd1482f96751b937f7dcd206 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 10 Apr 2019 22:26:34 -0400
Subject: [PATCH 269/281] Cut out middle-man, added noalias for better
 performance

---
 gtsam/linear/GaussianConditional.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index ed4a6e091..445941fa8 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -172,13 +172,13 @@ namespace gtsam {
   /* ************************************************************************* */
   void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
     Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
-    frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
+    frontalVec = get_R().triangularView<Eigen::Upper>().solve<Eigen::OnTheLeft>(frontalVec);
 
     // Check for indeterminant solution
     if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
 
     for (const_iterator it = beginParents(); it!= endParents(); it++)
-      gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec;
+      gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec;
 
     // Scale by sigmas
     if (model_)

From 8acba8eabe34cec5f2b36652c75f979bfb73ed6c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 10 Apr 2019 22:30:39 -0400
Subject: [PATCH 270/281] ignore debug directory

---
 .gitignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitignore b/.gitignore
index ad0e08aa1..03ce578e7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
 /build*
+/debug*
 .idea
 *.pyc
 *.DS_Store

From 97de0e3c8228e43cd44c243e62f95c42cbc6f322 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 11 Apr 2019 10:49:07 -0400
Subject: [PATCH 271/281] Compilation fixes

---
 gtsam/inference/VariableIndex-inl.h                      | 2 +-
 gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp       | 2 +-
 gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp       | 2 +-
 gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp | 2 +-
 gtsam_unstable/partition/GenericGraph.cpp                | 1 -
 5 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h
index 352ea166f..9ee614b69 100644
--- a/gtsam/inference/VariableIndex-inl.h
+++ b/gtsam/inference/VariableIndex-inl.h
@@ -25,7 +25,7 @@ namespace gtsam {
 /* ************************************************************************* */
 template<class FG>
 void VariableIndex::augment(const FG& factors,
-    boost::optional<const FastVector<size_t>&> newFactorIndices) {
+    boost::optional<const FactorIndices&> newFactorIndices) {
   gttic(VariableIndex_augment);
 
   // Augment index for each factor
diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
index a0ede5f74..56de7feab 100644
--- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
@@ -309,7 +309,7 @@ void BatchFixedLagSmoother::marginalize(const KeyVector& marginalizeKeys) {
   set<size_t> removedFactorSlots;
   const VariableIndex variableIndex(factors_);
   for(Key key: marginalizeKeys) {
-    const FastVector<size_t>& slots = variableIndex[key];
+    const auto& slots = variableIndex[key];
     removedFactorSlots.insert(slots.begin(), slots.end());
   }
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index 51a959075..5968cea52 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -536,7 +536,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
   std::vector<size_t> removedFactorSlots;
   VariableIndex variableIndex(factors_);
   for(Key key: keysToMove) {
-    const FastVector<size_t>& slots = variableIndex[key];
+    const auto& slots = variableIndex[key];
     removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
   }
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
index 78bd977f5..b70b9efc2 100644
--- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
@@ -291,7 +291,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam
   FactorIndices removedFactorSlots;
   const VariableIndex& variableIndex = isam2.getVariableIndex();
   for(Key key: keys) {
-    const FastVector<size_t>& slots = variableIndex[key];
+    const auto& slots = variableIndex[key];
     removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
   }
 
diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp
index f941407e9..67d767799 100644
--- a/gtsam_unstable/partition/GenericGraph.cpp
+++ b/gtsam_unstable/partition/GenericGraph.cpp
@@ -354,7 +354,6 @@ namespace gtsam { namespace partition {
       const std::vector<int>& dictionary,  GenericGraph3D& reducedGraph) {
 
     typedef size_t CameraKey;
-    typedef pair<CameraKey, CameraKey> CameraPair;
     typedef size_t LandmarkKey;
     // get a mapping from each landmark to its connected cameras
     vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());

From 7b6eaa43339e0dbc8246b99d93e5b4f3ec138325 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 13 Apr 2019 11:00:52 -0400
Subject: [PATCH 272/281] removed unused typedefs

---
 gtsam/slam/tests/testSmartProjectionCameraFactor.cpp     | 1 -
 gtsam_unstable/examples/SmartProjectionFactorExample.cpp | 2 --
 gtsam_unstable/partition/GenericGraph.cpp                | 1 -
 tests/testExpressionFactor.cpp                           | 1 -
 4 files changed, 5 deletions(-)

diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
index aaffbf0e6..ddb4d8adb 100644
--- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
@@ -46,7 +46,6 @@ static double rankTol = 1.0;
 template<class CALIBRATION>
 PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
     const PinholeCamera<CALIBRATION>& camera) {
-  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
       Point3(0.5, 0.1, 0.3));
   Pose3 cameraPose = camera.pose();
diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp
index 94a70470a..e290cef7e 100644
--- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp
+++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp
@@ -43,8 +43,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv){
-
-  typedef PinholePose<Cal3_S2> Camera;
   typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
   Values initial_estimate;
diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp
index 67d767799..92f0266d0 100644
--- a/gtsam_unstable/partition/GenericGraph.cpp
+++ b/gtsam_unstable/partition/GenericGraph.cpp
@@ -353,7 +353,6 @@ namespace gtsam { namespace partition {
   void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys,  const std::vector<size_t>& landmarkKeys,
       const std::vector<int>& dictionary,  GenericGraph3D& reducedGraph) {
 
-    typedef size_t CameraKey;
     typedef size_t LandmarkKey;
     // get a mapping from each landmark to its connected cameras
     vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index 769b458e4..161e6976b 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -243,7 +243,6 @@ TEST(ExpressionFactor, Shallow) {
 
   // traceExecution of shallow tree
   typedef internal::UnaryExpression<Point2, Point3> Unary;
-  typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t size = expression.traceSize();
   internal::ExecutionTraceStorage traceStorage[size];
   internal::ExecutionTrace<Point2> trace;

From 948367603b2968acc17b22cc6a3a0096b3755519 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Sat, 13 Apr 2019 22:06:18 -0400
Subject: [PATCH 273/281] Fixed missing transpose

---
 gtsam/linear/GaussianConditional.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 445941fa8..85569de14 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -172,7 +172,7 @@ namespace gtsam {
   /* ************************************************************************* */
   void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
     Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
-    frontalVec = get_R().triangularView<Eigen::Upper>().solve<Eigen::OnTheLeft>(frontalVec);
+    frontalVec = get_R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);
 
     // Check for indeterminant solution
     if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());

From b6abcb04f286010a407fc9ac1bb63ef9edaaef47 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 10 Apr 2019 22:30:21 -0400
Subject: [PATCH 274/281] Split off builder, now also used with same parameters
 in SubGraphSolver

---
 gtsam/linear/SubgraphBuilder.cpp        | 545 +++++++++++++++++++++++
 gtsam/linear/SubgraphBuilder.h          | 182 ++++++++
 gtsam/linear/SubgraphPreconditioner.cpp | 548 +++---------------------
 gtsam/linear/SubgraphPreconditioner.h   | 187 +-------
 gtsam/linear/SubgraphSolver.cpp         |  50 +--
 gtsam/linear/SubgraphSolver.h           |  41 +-
 tests/smallExample.h                    |   9 +-
 tests/testSubgraphPreconditioner.cpp    |   2 +-
 tests/testSubgraphSolver.cpp            |  35 +-
 9 files changed, 856 insertions(+), 743 deletions(-)
 create mode 100644 gtsam/linear/SubgraphBuilder.cpp
 create mode 100644 gtsam/linear/SubgraphBuilder.h

diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp
new file mode 100644
index 000000000..22ad89cd8
--- /dev/null
+++ b/gtsam/linear/SubgraphBuilder.cpp
@@ -0,0 +1,545 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SubgraphBuilder.cpp
+ * @date Dec 31, 2009
+ * @author Frank Dellaert, Yong-Dian Jian
+ */
+
+#include <gtsam/base/DSFVector.h>
+#include <gtsam/base/FastMap.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/inference/VariableIndex.h>
+#include <gtsam/linear/Errors.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/SubgraphBuilder.h>
+
+#include <boost/algorithm/string.hpp>
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+#include <boost/serialization/vector.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <numeric>  // accumulate
+#include <queue>
+#include <set>
+#include <stdexcept>
+#include <vector>
+
+using std::cout;
+using std::endl;
+using std::ostream;
+using std::vector;
+
+namespace gtsam {
+
+/*****************************************************************************/
+/* sort the container and return permutation index with default comparator */
+template <typename Container>
+static vector<size_t> sort_idx(const Container &src) {
+  typedef typename Container::value_type T;
+  const size_t n = src.size();
+  vector<std::pair<size_t, T> > tmp;
+  tmp.reserve(n);
+  for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]);
+
+  /* sort */
+  std::stable_sort(tmp.begin(), tmp.end());
+
+  /* copy back */
+  vector<size_t> idx;
+  idx.reserve(n);
+  for (size_t i = 0; i < n; i++) {
+    idx.push_back(tmp[i].first);
+  }
+  return idx;
+}
+
+/*****************************************************************************/
+static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
+  /* compute the sum of the weights */
+  const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
+  if (sum == 0.0) {
+    throw std::runtime_error("Weight vector has no non-zero weights");
+  }
+
+  /* make a normalized and accumulated version of the weight vector */
+  const size_t m = weight.size();
+  vector<double> cdf;
+  cdf.reserve(m);
+  for (size_t i = 0; i < m; ++i) {
+    cdf.push_back(weight[i] / sum);
+  }
+
+  vector<double> acc(m);
+  std::partial_sum(cdf.begin(), cdf.end(), acc.begin());
+
+  /* iid sample n times */
+  vector<size_t> samples;
+  samples.reserve(n);
+  const double denominator = (double)RAND_MAX;
+  for (size_t i = 0; i < n; ++i) {
+    const double value = rand() / denominator;
+    /* binary search the interval containing "value" */
+    const auto it = std::lower_bound(acc.begin(), acc.end(), value);
+    const size_t index = it - acc.begin();
+    samples.push_back(index);
+  }
+  return samples;
+}
+
+/*****************************************************************************/
+static vector<size_t> UniqueSampler(const vector<double> &weight,
+                                    const size_t n) {
+  const size_t m = weight.size();
+  if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size");
+
+  vector<size_t> samples;
+
+  size_t count = 0;
+  vector<bool> touched(m, false);
+  while (count < n) {
+    vector<size_t> localIndices;
+    localIndices.reserve(n - count);
+    vector<double> localWeights;
+    localWeights.reserve(n - count);
+
+    /* collect data */
+    for (size_t i = 0; i < m; ++i) {
+      if (!touched[i]) {
+        localIndices.push_back(i);
+        localWeights.push_back(weight[i]);
+      }
+    }
+
+    /* sampling and cache results */
+    vector<size_t> samples = iidSampler(localWeights, n - count);
+    for (const size_t &index : samples) {
+      if (touched[index] == false) {
+        touched[index] = true;
+        samples.push_back(index);
+        if (++count >= n) break;
+      }
+    }
+  }
+  return samples;
+}
+
+/****************************************************************************/
+Subgraph::Subgraph(const vector<size_t> &indices) {
+  edges_.reserve(indices.size());
+  for (const size_t &index : indices) {
+    const Edge e {index,1.0};
+    edges_.push_back(e);
+  }
+}
+
+/****************************************************************************/
+vector<size_t> Subgraph::edgeIndices() const {
+  vector<size_t> eid;
+  eid.reserve(size());
+  for (const Edge &edge : edges_) {
+    eid.push_back(edge.index);
+  }
+  return eid;
+}
+
+/****************************************************************************/
+void Subgraph::save(const std::string &fn) const {
+  std::ofstream os(fn.c_str());
+  boost::archive::text_oarchive oa(os);
+  oa << *this;
+  os.close();
+}
+
+/****************************************************************************/
+Subgraph Subgraph::load(const std::string &fn) {
+  std::ifstream is(fn.c_str());
+  boost::archive::text_iarchive ia(is);
+  Subgraph subgraph;
+  ia >> subgraph;
+  is.close();
+  return subgraph;
+}
+
+/****************************************************************************/
+ostream &operator<<(ostream &os, const Subgraph::Edge &edge) {
+  if (edge.weight != 1.0)
+    os << edge.index << "(" << std::setprecision(2) << edge.weight << ")";
+  else
+    os << edge.index;
+  return os;
+}
+
+/****************************************************************************/
+ostream &operator<<(ostream &os, const Subgraph &subgraph) {
+  os << "Subgraph" << endl;
+  for (const auto &e : subgraph.edges()) {
+    os << e << ", ";
+  }
+  return os;
+}
+
+/*****************************************************************************/
+void SubgraphBuilderParameters::print() const { print(cout); }
+
+/***************************************************************************************/
+void SubgraphBuilderParameters::print(ostream &os) const {
+  os << "SubgraphBuilderParameters" << endl
+     << "skeleton:            " << skeletonTranslator(skeletonType) << endl
+     << "skeleton weight:     " << skeletonWeightTranslator(skeletonWeight)
+     << endl
+     << "augmentation weight: "
+     << augmentationWeightTranslator(augmentationWeight) << endl;
+}
+
+/*****************************************************************************/
+ostream &operator<<(ostream &os, const SubgraphBuilderParameters &p) {
+  p.print(os);
+  return os;
+}
+
+/*****************************************************************************/
+SubgraphBuilderParameters::Skeleton
+SubgraphBuilderParameters::skeletonTranslator(const std::string &src) {
+  std::string s = src;
+  boost::algorithm::to_upper(s);
+  if (s == "NATURALCHAIN")
+    return NATURALCHAIN;
+  else if (s == "BFS")
+    return BFS;
+  else if (s == "KRUSKAL")
+    return KRUSKAL;
+  throw std::invalid_argument(
+      "SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
+  return KRUSKAL;
+}
+
+/****************************************************************/
+std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) {
+  if (s == NATURALCHAIN)
+    return "NATURALCHAIN";
+  else if (s == BFS)
+    return "BFS";
+  else if (s == KRUSKAL)
+    return "KRUSKAL";
+  else
+    return "UNKNOWN";
+}
+
+/****************************************************************/
+SubgraphBuilderParameters::SkeletonWeight
+SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
+  std::string s = src;
+  boost::algorithm::to_upper(s);
+  if (s == "EQUAL")
+    return EQUAL;
+  else if (s == "RHS")
+    return RHS_2NORM;
+  else if (s == "LHS")
+    return LHS_FNORM;
+  else if (s == "RANDOM")
+    return RANDOM;
+  throw std::invalid_argument(
+      "SubgraphBuilderParameters::skeletonWeightTranslator undefined string " +
+      s);
+  return EQUAL;
+}
+
+/****************************************************************/
+std::string SubgraphBuilderParameters::skeletonWeightTranslator(
+    SkeletonWeight w) {
+  if (w == EQUAL)
+    return "EQUAL";
+  else if (w == RHS_2NORM)
+    return "RHS";
+  else if (w == LHS_FNORM)
+    return "LHS";
+  else if (w == RANDOM)
+    return "RANDOM";
+  else
+    return "UNKNOWN";
+}
+
+/****************************************************************/
+SubgraphBuilderParameters::AugmentationWeight
+SubgraphBuilderParameters::augmentationWeightTranslator(
+    const std::string &src) {
+  std::string s = src;
+  boost::algorithm::to_upper(s);
+  if (s == "SKELETON") return SKELETON;
+  //  else if (s == "STRETCH")  return STRETCH;
+  //  else if (s == "GENERALIZED_STRETCH")  return GENERALIZED_STRETCH;
+  throw std::invalid_argument(
+      "SubgraphBuilder::Parameters::augmentationWeightTranslator undefined "
+      "string " +
+      s);
+  return SKELETON;
+}
+
+/****************************************************************/
+std::string SubgraphBuilderParameters::augmentationWeightTranslator(
+    AugmentationWeight w) {
+  if (w == SKELETON) return "SKELETON";
+  //  else if ( w == STRETCH )              return "STRETCH";
+  //  else if ( w == GENERALIZED_STRETCH )  return "GENERALIZED_STRETCH";
+  else
+    return "UNKNOWN";
+}
+
+/****************************************************************/
+vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
+                                          const FastMap<Key, size_t> &ordering,
+                                          const vector<double> &weights) const {
+  const SubgraphBuilderParameters &p = parameters_;
+  switch (p.skeletonType) {
+    case SubgraphBuilderParameters::NATURALCHAIN:
+      return natural_chain(gfg);
+      break;
+    case SubgraphBuilderParameters::BFS:
+      return bfs(gfg);
+      break;
+    case SubgraphBuilderParameters::KRUSKAL:
+      return kruskal(gfg, ordering, weights);
+      break;
+    default:
+      std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
+      break;
+  }
+  return vector<size_t>();
+}
+
+/****************************************************************/
+vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
+  vector<size_t> unaryFactorIndices;
+  size_t index = 0;
+  for (const auto &factor : gfg) {
+    if (factor->size() == 1) {
+      unaryFactorIndices.push_back(index);
+    }
+    index++;
+  }
+  return unaryFactorIndices;
+}
+
+/****************************************************************/
+vector<size_t> SubgraphBuilder::natural_chain(
+    const GaussianFactorGraph &gfg) const {
+  vector<size_t> chainFactorIndices;
+  size_t index = 0;
+  for (const GaussianFactor::shared_ptr &gf : gfg) {
+    if (gf->size() == 2) {
+      const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
+      if ((k1 - k0) == 1 || (k0 - k1) == 1) chainFactorIndices.push_back(index);
+    }
+    index++;
+  }
+  return chainFactorIndices;
+}
+
+/****************************************************************/
+vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
+  const VariableIndex variableIndex(gfg);
+  /* start from the first key of the first factor */
+  Key seed = gfg[0]->keys()[0];
+
+  const size_t n = variableIndex.size();
+
+  /* each vertex has self as the predecessor */
+  vector<size_t> bfsFactorIndices;
+  bfsFactorIndices.reserve(n - 1);
+
+  /* Initialize */
+  std::queue<size_t> q;
+  q.push(seed);
+
+  std::set<size_t> flags;
+  flags.insert(seed);
+
+  /* traversal */
+  while (!q.empty()) {
+    const size_t head = q.front();
+    q.pop();
+    for (const size_t index : variableIndex[head]) {
+      const GaussianFactor &gf = *gfg[index];
+      for (const size_t key : gf.keys()) {
+        if (flags.count(key) == 0) {
+          q.push(key);
+          flags.insert(key);
+          bfsFactorIndices.push_back(index);
+        }
+      }
+    }
+  }
+  return bfsFactorIndices;
+}
+
+/****************************************************************/
+vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
+                                        const FastMap<Key, size_t> &ordering,
+                                        const vector<double> &weights) const {
+  const VariableIndex variableIndex(gfg);
+  const size_t n = variableIndex.size();
+  const vector<size_t> sortedIndices = sort_idx(weights);
+
+  /* initialize buffer */
+  vector<size_t> treeIndices;
+  treeIndices.reserve(n - 1);
+
+  // container for acsendingly sorted edges
+  DSFVector dsf(n);
+
+  size_t count = 0;
+  double sum = 0.0;
+  for (const size_t index : sortedIndices) {
+    const GaussianFactor &gf = *gfg[index];
+    const auto keys = gf.keys();
+    if (keys.size() != 2) continue;
+    const size_t u = ordering.find(keys[0])->second,
+                 v = ordering.find(keys[1])->second;
+    if (dsf.find(u) != dsf.find(v)) {
+      dsf.merge(u, v);
+      treeIndices.push_back(index);
+      sum += weights[index];
+      if (++count == n - 1) break;
+    }
+  }
+  return treeIndices;
+}
+
+/****************************************************************/
+vector<size_t> SubgraphBuilder::sample(const vector<double> &weights,
+                                       const size_t t) const {
+  return UniqueSampler(weights, t);
+}
+
+/****************************************************************/
+Subgraph SubgraphBuilder::operator()(
+    const GaussianFactorGraph &gfg) const {
+  const auto &p = parameters_;
+  const auto inverse_ordering = Ordering::Natural(gfg);
+  const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
+  const size_t n = inverse_ordering.size(), m = gfg.size();
+
+  // Make sure the subgraph preconditioner does not include more than half of
+  // the edges beyond the spanning tree, or we might as well solve the whole
+  // thing.
+  size_t numExtraEdges = n * p.augmentationFactor;
+  const size_t numRemaining = m - (n - 1);
+  numExtraEdges = std::min(numExtraEdges, numRemaining / 2);
+
+  // Calculate weights
+  vector<double> weights = this->weights(gfg);
+
+  // Build spanning tree.
+  const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
+  if (tree.size() != n - 1) {
+    throw std::runtime_error(
+        "SubgraphBuilder::operator() failure: tree.size() != n-1");
+  }
+
+  // Downweight the tree edges to zero.
+  for (const size_t index : tree) {
+    weights[index] = 0.0;
+  }
+
+  /* decide how many edges to augment */
+  vector<size_t> offTree = sample(weights, numExtraEdges);
+
+  vector<size_t> subgraph = unary(gfg);
+  subgraph.insert(subgraph.end(), tree.begin(), tree.end());
+  subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
+
+  return Subgraph(subgraph);
+}
+
+/****************************************************************/
+SubgraphBuilder::Weights SubgraphBuilder::weights(
+    const GaussianFactorGraph &gfg) const {
+  const size_t m = gfg.size();
+  Weights weight;
+  weight.reserve(m);
+
+  for (const GaussianFactor::shared_ptr &gf : gfg) {
+    switch (parameters_.skeletonWeight) {
+      case SubgraphBuilderParameters::EQUAL:
+        weight.push_back(1.0);
+        break;
+      case SubgraphBuilderParameters::RHS_2NORM: {
+        if (JacobianFactor::shared_ptr jf =
+                boost::dynamic_pointer_cast<JacobianFactor>(gf)) {
+          weight.push_back(jf->getb().norm());
+        } else if (HessianFactor::shared_ptr hf =
+                       boost::dynamic_pointer_cast<HessianFactor>(gf)) {
+          weight.push_back(hf->linearTerm().norm());
+        }
+      } break;
+      case SubgraphBuilderParameters::LHS_FNORM: {
+        if (JacobianFactor::shared_ptr jf =
+                boost::dynamic_pointer_cast<JacobianFactor>(gf)) {
+          weight.push_back(std::sqrt(jf->getA().squaredNorm()));
+        } else if (HessianFactor::shared_ptr hf =
+                       boost::dynamic_pointer_cast<HessianFactor>(gf)) {
+          weight.push_back(std::sqrt(hf->information().squaredNorm()));
+        }
+      } break;
+
+      case SubgraphBuilderParameters::RANDOM:
+        weight.push_back(std::rand() % 100 + 1.0);
+        break;
+
+      default:
+        throw std::invalid_argument(
+            "SubgraphBuilder::weights: undefined weight scheme ");
+        break;
+    }
+  }
+  return weight;
+}
+
+/*****************************************************************************/
+GaussianFactorGraph::shared_ptr buildFactorSubgraph(
+    const GaussianFactorGraph &gfg, const Subgraph &subgraph,
+    const bool clone) {
+  auto subgraphFactors = boost::make_shared<GaussianFactorGraph>();
+  subgraphFactors->reserve(subgraph.size());
+  for (const auto &e : subgraph) {
+    const auto factor = gfg[e.index];
+    subgraphFactors->push_back(clone ? factor->clone(): factor);
+  }
+  return subgraphFactors;
+}
+
+/**************************************************************************************************/
+std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
+splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) {
+
+  // Get the subgraph by calling cheaper method
+  auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false);
+
+  // Now, copy all factors then set subGraph factors to zero
+  auto remaining = boost::make_shared<GaussianFactorGraph>(factorGraph);
+
+  for (const auto &e : subgraph) {
+    remaining->remove(e.index);
+  }
+
+  return std::make_pair(subgraphFactors, remaining);
+}
+
+/*****************************************************************************/
+
+}  // namespace gtsam
diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h
new file mode 100644
index 000000000..5247ea2a2
--- /dev/null
+++ b/gtsam/linear/SubgraphBuilder.h
@@ -0,0 +1,182 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SubgraphBuilder.h
+ * @date Dec 31, 2009
+ * @author Frank Dellaert, Yong-Dian Jian
+ */
+
+#pragma once
+
+#include <gtsam/base/FastMap.h>
+#include <gtsam/base/types.h>
+#include <gtsam/dllexport.h>
+
+#include <boost/serialization/nvp.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include <vector>
+
+namespace boost {
+namespace serialization {
+class access;
+} /* namespace serialization */
+} /* namespace boost */
+
+namespace gtsam {
+
+// Forward declarations
+class GaussianFactorGraph;
+struct PreconditionerParameters;
+
+/**************************************************************************/
+class GTSAM_EXPORT Subgraph {
+ public:
+  struct GTSAM_EXPORT Edge {
+    size_t index;  /* edge id */
+    double weight; /* edge weight */
+    inline bool isUnitWeight() const { return (weight == 1.0); }
+    friend std::ostream &operator<<(std::ostream &os, const Edge &edge);
+
+   private:
+    friend class boost::serialization::access;
+    template <class Archive>
+    void serialize(Archive &ar, const unsigned int /*version*/) {
+      ar &BOOST_SERIALIZATION_NVP(index);
+      ar &BOOST_SERIALIZATION_NVP(weight);
+    }
+  };
+
+  typedef std::vector<Edge> Edges;
+  typedef std::vector<size_t> EdgeIndices;
+  typedef Edges::iterator iterator;
+  typedef Edges::const_iterator const_iterator;
+
+ protected:
+  Edges edges_; /* index to the factors */
+
+ public:
+  Subgraph() {}
+  Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
+  Subgraph(const Edges &edges) : edges_(edges) {}
+  Subgraph(const std::vector<size_t> &indices);
+
+  inline const Edges &edges() const { return edges_; }
+  inline size_t size() const { return edges_.size(); }
+  EdgeIndices edgeIndices() const;
+
+  iterator begin() { return edges_.begin(); }
+  const_iterator begin() const { return edges_.begin(); }
+  iterator end() { return edges_.end(); }
+  const_iterator end() const { return edges_.end(); }
+
+  void save(const std::string &fn) const;
+  static Subgraph load(const std::string &fn);
+  friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
+
+ private:
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive &ar, const unsigned int /*version*/) {
+    ar &BOOST_SERIALIZATION_NVP(edges_);
+  }
+};
+
+/****************************************************************************/
+struct GTSAM_EXPORT SubgraphBuilderParameters {
+  typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
+
+  enum Skeleton {
+    /* augmented tree */
+    NATURALCHAIN = 0, /* natural ordering of the graph */
+    BFS,              /* breadth-first search tree */
+    KRUSKAL,          /* maximum weighted spanning tree */
+  } skeletonType;
+
+  enum SkeletonWeight {            /* how to weigh the graph edges */
+                        EQUAL = 0, /* every block edge has equal weight */
+                        RHS_2NORM, /* use the 2-norm of the rhs */
+                        LHS_FNORM, /* use the frobenius norm of the lhs */
+                        RANDOM,    /* bounded random edge weight */
+  } skeletonWeight;
+
+  enum AugmentationWeight {               /* how to weigh the graph edges */
+                            SKELETON = 0, /* use the same weights in building
+                                             the skeleton */
+                            //      STRETCH,                /* stretch in the
+                            //      laplacian sense */ GENERALIZED_STRETCH /*
+                            //      the generalized stretch defined in
+                            //      jian2013iros */
+  } augmentationWeight;
+
+  /// factor multiplied with n, yields number of extra edges.
+  double augmentationFactor; 
+
+  SubgraphBuilderParameters()
+      : skeletonType(KRUSKAL),
+        skeletonWeight(RANDOM),
+        augmentationWeight(SKELETON),
+        augmentationFactor(1.0) {}
+  virtual ~SubgraphBuilderParameters() {}
+
+  /* for serialization */
+  void print() const;
+  virtual void print(std::ostream &os) const;
+  friend std::ostream &operator<<(std::ostream &os,
+                                  const PreconditionerParameters &p);
+
+  static Skeleton skeletonTranslator(const std::string &s);
+  static std::string skeletonTranslator(Skeleton s);
+  static SkeletonWeight skeletonWeightTranslator(const std::string &s);
+  static std::string skeletonWeightTranslator(SkeletonWeight w);
+  static AugmentationWeight augmentationWeightTranslator(const std::string &s);
+  static std::string augmentationWeightTranslator(AugmentationWeight w);
+};
+
+/*****************************************************************************/
+class GTSAM_EXPORT SubgraphBuilder {
+ public:
+  typedef SubgraphBuilder Base;
+  typedef std::vector<double> Weights;
+
+  SubgraphBuilder(
+      const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
+      : parameters_(p) {}
+  virtual ~SubgraphBuilder() {}
+  virtual Subgraph operator()(const GaussianFactorGraph &jfg) const;
+
+ private:
+  std::vector<size_t> buildTree(const GaussianFactorGraph &gfg,
+                                const FastMap<Key, size_t> &ordering,
+                                const std::vector<double> &weights) const;
+  std::vector<size_t> unary(const GaussianFactorGraph &gfg) const;
+  std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const;
+  std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const;
+  std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
+                              const FastMap<Key, size_t> &ordering,
+                              const std::vector<double> &weights) const;
+  std::vector<size_t> sample(const std::vector<double> &weights,
+                             const size_t t) const;
+  Weights weights(const GaussianFactorGraph &gfg) const;
+  SubgraphBuilderParameters parameters_;
+};
+
+/** Select the factors in a factor graph according to the subgraph. */
+boost::shared_ptr<GaussianFactorGraph> buildFactorSubgraph(
+    const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
+
+/** Split the graph into a subgraph and the remaining edges. 
+ * Note that the remaining factorgraph has null factors. */
+std::pair<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> > 
+splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
+
+}  // namespace gtsam
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index e011b60d8..d74669c07 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -12,45 +12,22 @@
 /**
  * @file SubgraphPreconditioner.cpp
  * @date Dec 31, 2009
- * @author: Frank Dellaert
+ * @author Frank Dellaert, Yong-Dian Jian
  */
 
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/linear/GaussianConditional.h>
-#include <gtsam/linear/HessianFactor.h>
+
+#include <gtsam/linear/SubgraphBuilder.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/GaussianEliminationTree.h>
-#include <gtsam/inference/Ordering.h>
-#include <gtsam/inference/VariableIndex.h>
-#include <gtsam/base/DSFVector.h>
-#include <gtsam/base/FastMap.h>
-#include <gtsam/base/FastVector.h>
 #include <gtsam/base/types.h>
 #include <gtsam/base/Vector.h>
 
 #include <boost/algorithm/string.hpp>
-#include <boost/archive/text_iarchive.hpp>
-#include <boost/archive/text_oarchive.hpp>
-#include <boost/serialization/vector.hpp>
 #include <boost/range/adaptor/reversed.hpp>
-#include <boost/shared_ptr.hpp>
 
-#include <algorithm>
-#include <cmath>
-#include <cstdlib>
-#include <fstream>
-#include <iomanip>
-#include <iostream>
-#include <iterator>
-#include <list>
-#include <map>
-#include <numeric> // accumulate
-#include <queue>
-#include <set>
 #include <stdexcept>
-#include <string>
-#include <utility>
-#include <vector>
 
 using std::cout;
 using std::endl;
@@ -60,422 +37,60 @@ using std::ostream;
 namespace gtsam {
 
 /* ************************************************************************* */
-// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
-static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
+static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
+                           const KeyVector &keys) {
+  /* a cache of starting index and dim */
+  vector<std::pair<size_t, size_t> > cache;
+  cache.reserve(3);
+
+  /* figure out dimension by traversing the keys */
+  size_t dim = 0;
+  for (const Key &key : keys) {
+    const KeyInfoEntry &entry = keyInfo.find(key)->second;
+    cache.emplace_back(entry.start, entry.dim);
+    dim += entry.dim;
+  }
+
+  /* use the cache to fill the result */
+  Vector result(dim);
+  size_t index = 0;
+  for (const auto &p : cache) {
+    result.segment(index, p.second) = src.segment(p.first, p.second);
+    index += p.second;
+  }
+
+  return result;
+}
+
+/*****************************************************************************/
+static void setSubvector(const Vector &src, const KeyInfo &keyInfo,
+                         const KeyVector &keys, Vector &dst) {
+  size_t index = 0;
+  for (const Key &key : keys) {
+    const KeyInfoEntry &entry = keyInfo.find(key)->second;
+    const size_t keyDim = entry.dim;
+    dst.segment(entry.start, keyDim) = src.segment(index, keyDim);
+    index += keyDim;
+  }
+}
+
+/* ************************************************************************* */
+// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with
+// Cholesky)
+static GaussianFactorGraph::shared_ptr convertToJacobianFactors(
+    const GaussianFactorGraph &gfg) {
   auto result = boost::make_shared<GaussianFactorGraph>();
-  for (const auto &factor : gfg) {
-    auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
-    if( !jf ) {
-      jf = boost::make_shared<JacobianFactor>(*factor); 
-    }
-    result->push_back(jf);
-  }
-  return result;
-}
-
-/*****************************************************************************/
-static vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
-
-  /* compute the sum of the weights */
-  const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
-  if (sum==0.0) {
-    throw std::runtime_error("Weight vector has no non-zero weights");
-  }
-
-  /* make a normalized and accumulated version of the weight vector */
-  const size_t m = weight.size();
-  vector<double> cdf; cdf.reserve(m);
-  for ( size_t i = 0 ; i < m ; ++i ) {
-    cdf.push_back(weight[i]/sum);
-  }
-
-  vector<double> acc(m);
-  std::partial_sum(cdf.begin(),cdf.end(),acc.begin());
-
-  /* iid sample n times */
-  vector<size_t> result; result.reserve(n);
-  const double denominator = (double)RAND_MAX;
-  for ( size_t i = 0 ; i < n ; ++i ) {
-    const double value = rand() / denominator;
-    /* binary search the interval containing "value" */
-    const auto it = std::lower_bound(acc.begin(), acc.end(), value);
-    const size_t index = it - acc.begin();
-    result.push_back(index);
-  }
-  return result;
-}
-
-/*****************************************************************************/
-static vector<size_t> UniqueSampler(const vector<double> &weight, const size_t n) {
-
-  const size_t m = weight.size();
-  if ( n > m ) throw std::invalid_argument("UniqueSampler: invalid input size");
-
-  vector<size_t> result;
-
-  size_t count = 0;
-  vector<bool> touched(m, false);
-  while ( count < n ) {
-    vector<size_t> localIndices; localIndices.reserve(n-count);
-    vector<double> localWeights; localWeights.reserve(n-count);
-
-    /* collect data */
-    for ( size_t i = 0 ; i < m ; ++i ) {
-      if ( !touched[i] ) {
-        localIndices.push_back(i);
-        localWeights.push_back(weight[i]);
+  for (const auto &factor : gfg) 
+    if (factor) {
+      auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
+      if (!jf) {
+        jf = boost::make_shared<JacobianFactor>(*factor);
       }
+      result->push_back(jf);
     }
-
-    /* sampling and cache results */
-    vector<size_t> samples = iidSampler(localWeights, n-count);
-    for ( const size_t &index: samples ) {
-      if ( touched[index] == false ) {
-        touched[index] = true ;
-        result.push_back(index);
-        if ( ++count >= n ) break;
-      }
-    }
-  }
   return result;
 }
 
-/****************************************************************************/
-Subgraph::Subgraph(const vector<size_t> &indices) {
-  edges_.reserve(indices.size());
-  for ( const size_t &index: indices ) {
-    edges_.emplace_back(index, 1.0);
-  }
-}
-
-/****************************************************************************/
-vector<size_t> Subgraph::edgeIndices() const {
-  vector<size_t> eid; eid.reserve(size());
-  for ( const SubgraphEdge &edge: edges_ ) {
-    eid.push_back(edge.index_);
-  }
-  return eid;
-}
-
-/****************************************************************************/
-void Subgraph::save(const std::string &fn) const {
-  std::ofstream os(fn.c_str());
-  boost::archive::text_oarchive oa(os);
-  oa << *this;
-  os.close();
-}
-
-/****************************************************************************/
-Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
-  std::ifstream is(fn.c_str());
-  boost::archive::text_iarchive ia(is);
-  Subgraph::shared_ptr subgraph(new Subgraph());
-  ia >> *subgraph;
-  is.close();
-  return subgraph;
-}
-
-/****************************************************************************/
-ostream &operator<<(ostream &os, const SubgraphEdge &edge) {
-  if ( edge.weight() != 1.0 )
-    os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
-  else
-    os << edge.index() ;
-  return os;
-}
-
-/****************************************************************************/
-ostream &operator<<(ostream &os, const Subgraph &subgraph) {
-  os << "Subgraph" << endl;
-  for ( const SubgraphEdge &e: subgraph.edges() ) {
-    os << e << ", " ;
-  }
-  return os;
-}
-
-/*****************************************************************************/
-void SubgraphBuilderParameters::print() const {
-  print(cout);
-}
-
-/***************************************************************************************/
-void SubgraphBuilderParameters::print(ostream &os) const {
-  os << "SubgraphBuilderParameters" << endl
-      << "skeleton:            " << skeletonTranslator(skeleton_) << endl
-      << "skeleton weight:     " << skeletonWeightTranslator(skeletonWeight_) << endl
-      << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl
-      ;
-}
-
-/*****************************************************************************/
-ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) {
-  p.print(os);
-  return os;
-}
-
-/*****************************************************************************/
-SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){
-  std::string s = src;  boost::algorithm::to_upper(s);
-  if (s == "NATURALCHAIN")    return NATURALCHAIN;
-  else if (s == "BFS")        return BFS;
-  else if (s == "KRUSKAL")    return KRUSKAL;
-  throw std::invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
-  return KRUSKAL;
-}
-
-/****************************************************************/
-std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) {
-  if ( s == NATURALCHAIN ) return "NATURALCHAIN";
-  else if ( s == BFS )     return "BFS";
-  else if ( s == KRUSKAL ) return "KRUSKAL";
-  else                     return "UNKNOWN";
-}
-
-/****************************************************************/
-SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
-  std::string s = src;  boost::algorithm::to_upper(s);
-  if (s == "EQUAL")       return EQUAL;
-  else if (s == "RHS")    return RHS_2NORM;
-  else if (s == "LHS")    return LHS_FNORM;
-  else if (s == "RANDOM") return RANDOM;
-  throw std::invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
-  return EQUAL;
-}
-
-/****************************************************************/
-std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) {
-  if ( w == EQUAL )           return "EQUAL";
-  else if ( w == RHS_2NORM )  return "RHS";
-  else if ( w == LHS_FNORM )  return "LHS";
-  else if ( w == RANDOM )     return "RANDOM";
-  else                        return "UNKNOWN";
-}
-
-/****************************************************************/
-SubgraphBuilderParameters::AugmentationWeight
-SubgraphBuilderParameters::augmentationWeightTranslator(
-    const std::string &src) {
-  std::string s = src;  boost::algorithm::to_upper(s);
-  if (s == "SKELETON")      return SKELETON;
-//  else if (s == "STRETCH")  return STRETCH;
-//  else if (s == "GENERALIZED_STRETCH")  return GENERALIZED_STRETCH;
-  throw std::invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
-  return SKELETON;
-}
-
-/****************************************************************/
-std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) {
-  if ( w == SKELETON )                  return "SKELETON";
-//  else if ( w == STRETCH )              return "STRETCH";
-//  else if ( w == GENERALIZED_STRETCH )  return "GENERALIZED_STRETCH";
-  else                                  return "UNKNOWN";
-}
-
-/****************************************************************/
-vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg,
-                                          const FastMap<Key, size_t> &ordering,
-                                          const vector<double> &weights) const {
-  const SubgraphBuilderParameters &p = parameters_;
-  switch (p.skeleton_) {
-  case SubgraphBuilderParameters::NATURALCHAIN:
-    return natural_chain(gfg);
-    break;
-  case SubgraphBuilderParameters::BFS:
-    return bfs(gfg);
-    break;
-  case SubgraphBuilderParameters::KRUSKAL:
-    return kruskal(gfg, ordering, weights);
-    break;
-  default:
-    std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
-    break;
-  }
-  return vector<size_t>();
-}
-
-/****************************************************************/
-vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
-  vector<size_t> result ;
-  size_t index = 0;
-  for (const auto &factor : gfg) {
-    if ( factor->size() == 1 ) {
-      result.push_back(index);
-    }
-    index++;
-  }
-  return result;
-}
-
-/****************************************************************/
-vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
-  vector<size_t> result ;
-  size_t index = 0;
-  for ( const GaussianFactor::shared_ptr &gf: gfg ) {
-    if ( gf->size() == 2 ) {
-      const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
-      if ( (k1-k0) == 1 || (k0-k1) == 1 )
-        result.push_back(index);
-    }
-    index++;
-  }
-  return result;
-}
-
-/****************************************************************/
-vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
-  const VariableIndex variableIndex(gfg);
-  /* start from the first key of the first factor */
-  Key seed = gfg[0]->keys()[0];
-
-  const size_t n = variableIndex.size();
-
-  /* each vertex has self as the predecessor */
-  vector<size_t> result;
-  result.reserve(n-1);
-
-  /* Initialize */
-  std::queue<size_t> q;
-  q.push(seed);
-
-  std::set<size_t> flags;
-  flags.insert(seed);
-
-  /* traversal */
-  while ( !q.empty() ) {
-    const size_t head = q.front(); q.pop();
-    for ( const size_t index: variableIndex[head] ) {
-      const GaussianFactor &gf = *gfg[index];
-      for ( const size_t key: gf.keys() ) {
-        if ( flags.count(key) == 0 ) {
-          q.push(key);
-          flags.insert(key);
-          result.push_back(index);
-        }
-      }
-    }
-  }
-  return result;
-}
-
-/****************************************************************/
-vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
-                                        const FastMap<Key, size_t> &ordering,
-                                        const vector<double> &weights) const {
-  const VariableIndex variableIndex(gfg);
-  const size_t n = variableIndex.size();
-  const vector<size_t> sortedIndices = sort_idx(weights) ;
-
-  /* initialize buffer */
-  vector<size_t> result;
-  result.reserve(n-1);
-
-  // container for acsendingly sorted edges
-  DSFVector dsf(n);
-
-  size_t count = 0 ; double sum = 0.0 ;
-  for (const size_t index: sortedIndices) {
-    const GaussianFactor &gf = *gfg[index];
-    const auto keys = gf.keys();
-    if ( keys.size() != 2 ) continue;
-    const size_t u = ordering.find(keys[0])->second,
-                 v = ordering.find(keys[1])->second;
-    if ( dsf.find(u) != dsf.find(v) ) {
-      dsf.merge(u, v) ;
-      result.push_back(index) ;
-      sum += weights[index] ;
-      if ( ++count == n-1 ) break ;
-    }
-  }
-  return result;
-}
-
-/****************************************************************/
-vector<size_t> SubgraphBuilder::sample(const vector<double> &weights, const size_t t) const {
-  return UniqueSampler(weights, t);
-}
-
-/****************************************************************/
-Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
-
-  const auto &p = parameters_;
-  const auto inverse_ordering = Ordering::Natural(gfg);
-  const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
-  const size_t n = inverse_ordering.size(), m = gfg.size();
-
-  // Make sure the subgraph preconditioner does not include more than half of
-  // the edges beyond the spanning tree, or we might as well solve the whole thing.
-  size_t numExtraEdges = n * p.complexity_;
-  const size_t numRemaining = m - (n - 1);
-  numExtraEdges = std::min(numExtraEdges, numRemaining/2);
-
-  // Calculate weights 
-  vector<double> weights = this->weights(gfg);
-
-  // Build spanning tree.
-  const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
-  if ( tree.size() != n-1 ) {
-    throw std::runtime_error("SubgraphBuilder::operator() failure: tree.size() != n-1");
-  }
-
-  // Downweight the tree edges to zero.
-  for ( const size_t index: tree ) {
-    weights[index] = 0.0;
-  }
-
-  /* decide how many edges to augment */
-  vector<size_t> offTree = sample(weights, numExtraEdges);
-
-  vector<size_t> subgraph = unary(gfg);
-  subgraph.insert(subgraph.end(), tree.begin(), tree.end());
-  subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
-
-  return boost::make_shared<Subgraph>(subgraph);
-}
-
-/****************************************************************/
-SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const
-{
-  const size_t m = gfg.size() ;
-  Weights weight; weight.reserve(m);
-
-  for(const GaussianFactor::shared_ptr &gf: gfg ) {
-    switch ( parameters_.skeletonWeight_ ) {
-    case SubgraphBuilderParameters::EQUAL:
-      weight.push_back(1.0);
-      break;
-    case SubgraphBuilderParameters::RHS_2NORM:
-    {
-      if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
-        weight.push_back(jf->getb().norm());
-      }
-      else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
-        weight.push_back(hf->linearTerm().norm());
-      }
-    }
-      break;
-    case SubgraphBuilderParameters::LHS_FNORM:
-    {
-      if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
-        weight.push_back(std::sqrt(jf->getA().squaredNorm()));
-      }
-      else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
-        weight.push_back(std::sqrt(hf->information().squaredNorm()));
-      }
-    }
-      break;
-
-    case SubgraphBuilderParameters::RANDOM:
-      weight.push_back(std::rand()%100 + 1.0);
-      break;
-
-    default:
-      throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
-      break;
-    }
-  }
-  return weight;
-}
-
 /* ************************************************************************* */
 SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
          parameters_(p) {}
@@ -640,67 +255,18 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
 void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
 {
   /* identify the subgraph structure */
-  const SubgraphBuilder builder(parameters_.builderParams_);
-  Subgraph::shared_ptr subgraph = builder(gfg);
+  const SubgraphBuilder builder(parameters_.builderParams);
+  auto subgraph = builder(gfg);
 
   keyInfo_ = keyInfo;
 
   /* build factor subgraph */
-  GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true);
+  GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true);
 
   /* factorize and cache BayesNet */
   Rc1_ = gfg_subgraph->eliminateSequential();
 }
 
 /*****************************************************************************/
-Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
-                    const KeyVector &keys) {
-  /* a cache of starting index and dim */
-  vector<std::pair<size_t, size_t> > cache;
-  cache.reserve(3);
-
-  /* figure out dimension by traversing the keys */
-  size_t dim = 0;
-  for (const Key &key : keys) {
-    const KeyInfoEntry &entry = keyInfo.find(key)->second;
-    cache.emplace_back(entry.start, entry.dim);
-    dim += entry.dim;
-  }
-
-  /* use the cache to fill the result */
-  Vector result(dim);
-  size_t index = 0;
-  for (const auto &p : cache) {
-    result.segment(index, p.second) = src.segment(p.first, p.second);
-    index += p.second;
-  }
-
-  return result;
-}
-
-/*****************************************************************************/
-void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
-  size_t index = 0;
-  for ( const Key &key: keys ) {
-    const KeyInfoEntry &entry = keyInfo.find(key)->second;
-    const size_t keyDim = entry.dim;
-    dst.segment(entry.start, keyDim) = src.segment(index, keyDim) ;
-    index += keyDim;
-  }
-}
-
-/*****************************************************************************/
-GaussianFactorGraph::shared_ptr buildFactorSubgraph(
-    const GaussianFactorGraph &gfg, const Subgraph &subgraph,
-    const bool clone) {
-  auto result = boost::make_shared<GaussianFactorGraph>();
-  result->reserve(subgraph.size());
-  for ( const SubgraphEdge &e: subgraph ) {
-    const size_t index = e.index();
-    if ( clone ) result->push_back(gfg[index]->clone());
-    else result->push_back(gfg[index]);
-  }
-  return result;
-}
 
 } // nsamespace gtsam
diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h
index a9746d104..8906337a9 100644
--- a/gtsam/linear/SubgraphPreconditioner.h
+++ b/gtsam/linear/SubgraphPreconditioner.h
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -17,30 +17,16 @@
 
 #pragma once
 
+#include <gtsam/linear/SubgraphBuilder.h>
 #include <gtsam/linear/Errors.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/Preconditioner.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/base/FastMap.h>
-#include <gtsam/base/FastVector.h>
-#include <gtsam/base/types.h>
-#include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
 
-#include <boost/serialization/nvp.hpp>
 #include <boost/shared_ptr.hpp>
 
 #include <map>
-#include <utility>
-#include <vector>
-
-namespace boost {
-namespace serialization {
-class access;
-} /* namespace serialization */
-} /* namespace boost */
 
 namespace gtsam {
 
@@ -49,142 +35,11 @@ namespace gtsam {
   class GaussianFactorGraph;
   class VectorValues;
 
-  struct GTSAM_EXPORT SubgraphEdge {
-    size_t index_;   /* edge id */
-    double weight_; /* edge weight */
-    SubgraphEdge() : index_(0), weight_(1.0) {}
-    SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {}
-    SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {}
-    inline size_t index() const { return index_; }
-    inline double weight() const { return weight_; }
-    inline bool isUnitWeight() const { return (weight_ == 1.0); }
-    friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge);
-  private:
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(index_);
-      ar & BOOST_SERIALIZATION_NVP(weight_);
-    }
-  };
-
-  /**************************************************************************/
-  class GTSAM_EXPORT Subgraph {
-  public:
-    typedef boost::shared_ptr<Subgraph> shared_ptr;
-    typedef std::vector<shared_ptr> vector_shared_ptr;
-    typedef std::vector<SubgraphEdge> Edges;
-    typedef std::vector<size_t> EdgeIndices;
-    typedef Edges::iterator iterator;
-    typedef Edges::const_iterator const_iterator;
-
-  protected:
-    Edges edges_;                 /* index to the factors */
-
-  public:
-    Subgraph() {}
-    Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
-    Subgraph(const Edges &edges) : edges_(edges) {}
-    Subgraph(const std::vector<size_t> &indices) ;
-
-    inline const Edges& edges() const { return edges_; }
-    inline size_t size() const { return edges_.size(); }
-    EdgeIndices edgeIndices() const;
-
-    iterator begin() { return edges_.begin(); }
-    const_iterator begin() const { return edges_.begin(); }
-    iterator end() { return edges_.end(); }
-    const_iterator end() const { return edges_.end(); }
-
-    void save(const std::string &fn) const;
-    static shared_ptr load(const std::string &fn);
-    friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
-
-  private:
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(edges_);
-    }
-  };
-
-  /****************************************************************************/
-  struct GTSAM_EXPORT SubgraphBuilderParameters {
-  public:
-    typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
-
-    enum Skeleton {
-      /* augmented tree */
-      NATURALCHAIN = 0,  /* natural ordering of the graph */
-      BFS,      /* breadth-first search tree */
-      KRUSKAL,  /* maximum weighted spanning tree */
-    } skeleton_ ;
-
-    enum SkeletonWeight {   /* how to weigh the graph edges */
-      EQUAL = 0,    /* every block edge has equal weight */
-      RHS_2NORM,    /* use the 2-norm of the rhs */
-      LHS_FNORM,    /* use the frobenius norm of the lhs */
-      RANDOM,       /* bounded random edge weight */
-    } skeletonWeight_ ;
-
-    enum AugmentationWeight {   /* how to weigh the graph edges */
-      SKELETON = 0,             /* use the same weights in building the skeleton */
-//      STRETCH,                /* stretch in the laplacian sense */
-//      GENERALIZED_STRETCH     /* the generalized stretch defined in jian2013iros */
-    } augmentationWeight_ ;
-
-    double complexity_; /* factor multiplied with n, yields number of extra edges. */
-
-    SubgraphBuilderParameters()
-      : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
-    virtual ~SubgraphBuilderParameters() {}
-
-    /* for serialization */
-    void print() const ;
-    virtual void print(std::ostream &os) const ;
-    friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
-
-    static Skeleton skeletonTranslator(const std::string &s);
-    static std::string skeletonTranslator(Skeleton s);
-    static SkeletonWeight skeletonWeightTranslator(const std::string &s);
-    static std::string skeletonWeightTranslator(SkeletonWeight w);
-    static AugmentationWeight augmentationWeightTranslator(const std::string &s);
-    static std::string augmentationWeightTranslator(AugmentationWeight w);
-  };
-
-  /*****************************************************************************/
-  class GTSAM_EXPORT SubgraphBuilder {
-
-  public:
-    typedef SubgraphBuilder Base;
-    typedef boost::shared_ptr<SubgraphBuilder> shared_ptr;
-    typedef std::vector<double> Weights;
-
-    SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
-      : parameters_(p) {}
-    virtual ~SubgraphBuilder() {}
-    virtual boost::shared_ptr<Subgraph> operator() (const GaussianFactorGraph &jfg) const ;
-
-  private:
-    std::vector<size_t> buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
-    std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
-    std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
-    std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
-    std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
-    std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
-    Weights weights(const GaussianFactorGraph &gfg) const;
-    SubgraphBuilderParameters parameters_;
-
-  };
-
-  /*******************************************************************************************/
   struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
-    typedef PreconditionerParameters Base;
     typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
     SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
-      : Base(), builderParams_(p) {}
-    virtual ~SubgraphPreconditionerParameters() {}
-    SubgraphBuilderParameters builderParams_;
+      : builderParams(p) {}
+    SubgraphBuilderParameters builderParams;
   };
 
   /**
@@ -300,38 +155,4 @@ namespace gtsam {
     /*****************************************************************************/
   };
 
-  /* get subvectors */
-  Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys);
-
-  /* set subvectors */
-  void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst);
-
-
-  /* build a factor subgraph, which is defined as a set of weighted edges (factors) */
-  boost::shared_ptr<GaussianFactorGraph>
-  buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
-
-
-  /* sort the container and return permutation index with default comparator */
-  template <typename Container>
-  std::vector<size_t> sort_idx(const Container &src)
-  {
-     typedef typename Container::value_type T;
-     const size_t n = src.size() ;
-     std::vector<std::pair<size_t,T> > tmp;
-     tmp.reserve(n);
-     for ( size_t i = 0 ; i < n ; i++ )
-       tmp.push_back(std::make_pair(i, src[i]));
-
-     /* sort */
-     std::stable_sort(tmp.begin(), tmp.end()) ;
-
-     /* copy back */
-     std::vector<size_t> idx; idx.reserve(n);
-     for ( size_t i = 0 ; i < n ; i++ ) {
-       idx.push_back(tmp[i].first) ;
-     }
-     return idx;
-  }
-
 } // namespace gtsam
diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp
index edf39e462..56b843e8d 100644
--- a/gtsam/linear/SubgraphSolver.cpp
+++ b/gtsam/linear/SubgraphSolver.cpp
@@ -18,13 +18,12 @@
  */
 
 #include <gtsam/linear/SubgraphSolver.h>
+
+#include <gtsam/linear/SubgraphBuilder.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/base/DSFMap.h>
-
-#include <iostream>
 
 using namespace std;
 
@@ -36,7 +35,7 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab,
     const Parameters &parameters, const Ordering& ordering) :
     parameters_(parameters) {
   GaussianFactorGraph::shared_ptr Ab1,Ab2;
-  boost::tie(Ab1, Ab2) = splitGraph(Ab);
+  std::tie(Ab1, Ab2) = splitGraph(Ab);
   if (parameters_.verbosity())
     cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
          << " factors" << endl;
@@ -91,49 +90,20 @@ VectorValues SubgraphSolver::optimize() const {
 }
 
 VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
-    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
+    const KeyInfo &keyInfo, const map<Key, Vector> &lambda,
     const VectorValues &initial) {
   return VectorValues();
 }
 /**************************************************************************************************/
-// Run Kruskal algorithm to create a spanning tree of factor "edges".
-// Edges are not weighted, and will only work if factors are binary.
-// Unary factors are ignored for this purpose and added to tree graph.
-boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
+pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
 SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
 
-  // Create disjoint set forest data structure for Kruskal algorithm
-  DSFMap<Key> dsf;
+  /* identify the subgraph structure */
+  const SubgraphBuilder builder(parameters_.builderParams);
+  auto subgraph = builder(factorGraph);
 
-  // Allocate two output graphs
-  auto tree = boost::make_shared<GaussianFactorGraph>();
-  auto constraints = boost::make_shared<GaussianFactorGraph>();
-
-  // Loop over all "edges"
-  for ( const auto &factor: factorGraph ) {
-
-    // Fail on > binary factors
-    const auto& keys = factor->keys();
-    if (keys.size() > 2) {
-      throw runtime_error(
-          "SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
-    }
-
-    // check whether this factor should be augmented to the "tree" graph
-    if (keys.size() == 1)
-      tree->push_back(factor);
-    else if (dsf.find(keys[0]) != dsf.find(keys[1])) {
-      // We merge two trees joined by this edge if they are still disjoint
-      tree->push_back(factor);
-      // Record this fact in DSF
-      dsf.merge(keys[0], keys[1]);
-    } else {
-      // This factor would create a loop, so we add it to non-tree edges...
-      constraints->push_back(factor);
-    }
-  }
-
-  return boost::tie(tree, constraints);
+  /* build factor subgraph */
+  return splitFactorGraph(factorGraph, subgraph);
 }
 
 /****************************************************************************/
diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h
index 76fe31d32..5ab1a8520 100644
--- a/gtsam/linear/SubgraphSolver.h
+++ b/gtsam/linear/SubgraphSolver.h
@@ -20,6 +20,10 @@
 #pragma once
 
 #include <gtsam/linear/ConjugateGradientSolver.h>
+#include <gtsam/linear/SubgraphBuilder.h>
+
+#include <map>
+#include <utility>  // pair
 
 namespace gtsam {
 
@@ -28,13 +32,15 @@ class GaussianFactorGraph;
 class GaussianBayesNet;
 class SubgraphPreconditioner;
 
-class GTSAM_EXPORT SubgraphSolverParameters
+struct GTSAM_EXPORT SubgraphSolverParameters
     : public ConjugateGradientParameters {
- public:
-  typedef ConjugateGradientParameters Base;
-  SubgraphSolverParameters() : Base() {}
+  SubgraphBuilderParameters builderParams;
+  explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
+    : builderParams(p) {}
   void print() const { Base::print(); }
-  virtual void print(std::ostream &os) const { Base::print(os); }
+  virtual void print(std::ostream &os) const {
+    Base::print(os);
+  }
 };
 
 /**
@@ -115,10 +121,19 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
   VectorValues optimize() const;
 
   /// Interface that IterativeSolver subclasses have to implement
-  virtual VectorValues optimize(const GaussianFactorGraph &gfg,
-                                const KeyInfo &keyInfo,
-                                const std::map<Key, Vector> &lambda,
-                                const VectorValues &initial) override;
+  VectorValues optimize(const GaussianFactorGraph &gfg,
+                        const KeyInfo &keyInfo,
+                        const std::map<Key, Vector> &lambda,
+                        const VectorValues &initial) override;
+
+  /// @}
+  /// @name Implement interface
+  /// @{
+
+  /// Split graph using Kruskal algorithm, treating binary factors as edges.
+  std::pair < boost::shared_ptr<GaussianFactorGraph>,
+      boost::shared_ptr<GaussianFactorGraph> > splitGraph(
+          const GaussianFactorGraph &gfg);
 
   /// @}
 
@@ -127,7 +142,7 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
   /// @{
   SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
                  const Parameters &parameters, const Ordering &ordering)
-      : SubgraphSolver(*A, parameters, ordering){};
+      : SubgraphSolver(*A, parameters, ordering) {}
   SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &,
                  const Parameters &, const Ordering &);
   SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
@@ -138,12 +153,6 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
                  const GaussianFactorGraph &, const Parameters &);
   /// @}
 #endif
-
- protected:
-  /// Split graph using Kruskal algorithm, treating binary factors as edges.
-  static boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
-                      boost::shared_ptr<GaussianFactorGraph>>
-  splitGraph(const GaussianFactorGraph &gfg);
 };
 
 }  // namespace gtsam
diff --git a/tests/smallExample.h b/tests/smallExample.h
index 8cd219bff..553650be8 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -26,7 +26,6 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <boost/tuple/tuple.hpp>
 #include <boost/assign/list_of.hpp>
 
 namespace gtsam {
@@ -131,7 +130,7 @@ namespace example {
  * -x11-x21-x31
  * with x11 clamped at (1,1), and others related by 2D odometry.
  */
-// inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N);
+// inline std::pair<GaussianFactorGraph, VectorValues> planarGraph(size_t N);
 
 /*
  * Create canonical ordering for planar graph that also works for tree
@@ -399,7 +398,7 @@ inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
 inline GaussianFactorGraph createSmoother(int T) {
   NonlinearFactorGraph nlfg;
   Values poses;
-  boost::tie(nlfg, poses) = createNonlinearSmoother(T);
+  std::tie(nlfg, poses) = createNonlinearSmoother(T);
 
   return *nlfg.linearize(poses);
 }
@@ -563,7 +562,7 @@ inline Symbol key(size_t x, size_t y) {
 } // \namespace impl
 
 /* ************************************************************************* */
-inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
+inline std::pair<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
   using namespace impl;
 
   // create empty graph
@@ -601,7 +600,7 @@ inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
 
   // linearize around zero
   boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);
-  return boost::make_tuple(*gfg, xtrue);
+  return std::make_pair(*gfg, xtrue);
 }
 
 /* ************************************************************************* */
diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp
index d9b07184b..fb9f7a5a2 100644
--- a/tests/testSubgraphPreconditioner.cpp
+++ b/tests/testSubgraphPreconditioner.cpp
@@ -218,7 +218,7 @@ TEST(SubgraphSolver, Solves) {
   SubgraphPreconditioner system;
 
   // We test on three different graphs
-  const auto Ab1 = planarGraph(3).get<0>();
+  const auto Ab1 = planarGraph(3).first;
   const auto Ab2 = read("toy3D");
   const auto Ab3 = read("randomGrid3D");
 
diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp
index 93101131d..cca13c822 100644
--- a/tests/testSubgraphSolver.cpp
+++ b/tests/testSubgraphSolver.cpp
@@ -15,18 +15,19 @@
  *  @author Yong-Dian Jian
  **/
 
+#include <gtsam/linear/SubgraphSolver.h>
+
 #include <tests/smallExample.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/iterative.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/SubgraphSolver.h>
+#include <gtsam/linear/SubgraphBuilder.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/numericalDerivative.h>
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/list.hpp>
 using namespace boost::assign;
 
@@ -53,13 +54,33 @@ TEST( SubgraphSolver, Parameters )
   LONGS_EQUAL(500, kParameters.maxIterations());
 }
 
+/* ************************************************************************* */
+TEST( SubgraphSolver, splitFactorGraph )
+{
+  // Build a planar graph
+  GaussianFactorGraph Ab;
+  VectorValues xtrue;
+  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
+
+  SubgraphBuilderParameters params;
+  params.augmentationFactor = 0.0;
+  SubgraphBuilder builder(params);
+  auto subgraph = builder(Ab);
+  EXPECT_LONGS_EQUAL(9, subgraph.size());
+
+  GaussianFactorGraph::shared_ptr Ab1, Ab2;
+  std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph);
+  EXPECT_LONGS_EQUAL(9, Ab1->size());
+  EXPECT_LONGS_EQUAL(13, Ab2->size());
+}
+
 /* ************************************************************************* */
 TEST( SubgraphSolver, constructor1 )
 {
   // Build a planar graph
   GaussianFactorGraph Ab;
   VectorValues xtrue;
-  boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
+  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
   // The first constructor just takes a factor graph (and kParameters)
   // and it will split the graph into A1 and A2, where A1 is a spanning tree
@@ -75,11 +96,11 @@ TEST( SubgraphSolver, constructor2 )
   GaussianFactorGraph Ab;
   VectorValues xtrue;
   size_t N = 3;
-  boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
+  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
   // Get the spanning tree
   GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
+  std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
 
   // The second constructor takes two factor graphs, so the caller can specify
   // the preconditioner (Ab1) and the constraints that are left out (Ab2)
@@ -95,11 +116,11 @@ TEST( SubgraphSolver, constructor3 )
   GaussianFactorGraph Ab;
   VectorValues xtrue;
   size_t N = 3;
-  boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
+  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b
 
   // Get the spanning tree and corresponding kOrdering
   GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2
-  boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
+  std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);
 
   // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
   auto Rc1 = Ab1->eliminateSequential();

From dab022a5b7c62597d2b2d600d455e2bc5d2fdf4a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 14 Apr 2019 15:13:01 -0400
Subject: [PATCH 275/281] Fixed another FactorIndices issue on Mac

---
 gtsam/symbolic/tests/testVariableIndex.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/symbolic/tests/testVariableIndex.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp
index 580f5a1a4..7aa5f971b 100644
--- a/gtsam/symbolic/tests/testVariableIndex.cpp
+++ b/gtsam/symbolic/tests/testVariableIndex.cpp
@@ -79,7 +79,7 @@ TEST(VariableIndex, augment2) {
 
   VariableIndex expected(fgCombined);
 
-  FastVector<size_t> newIndices = list_of(5)(6)(7)(8);
+  FactorIndices newIndices = list_of(5)(6)(7)(8);
   VariableIndex actual(fg1);
   actual.augment(fg2, newIndices);
 

From 9a322db8157707e50774251a7af6018abae5c35f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 15 Apr 2019 12:39:52 -0400
Subject: [PATCH 276/281] Added deprecation notice for python folder

---
 python/README.md | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/python/README.md b/python/README.md
index f1c218cfb..396bcea89 100644
--- a/python/README.md
+++ b/python/README.md
@@ -1,6 +1,14 @@
 Python Wrapper and Packaging
 ============================
 
+# Deprecation Notice
+
+We are in the process of deprecating the old Python bindings and Cython bindings in favor of the new Pybind11 binding system.
+
+As such, the examples in this directory are no longer guaranteed to work. Please refer to the [cython examples](https://bitbucket.org/gtborg/gtsam/src/develop/cython/gtsam/examples).
+
+---
+
 This directory contains the basic setup script and directory structure for the gtsam python module.
 During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run.
 

From 53980ae318d7fea2477ed45c9bd5345eb26755b1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 15 Apr 2019 15:19:06 -0400
Subject: [PATCH 277/281] Added comments to python example

---
 cython/gtsam/examples/PlanarManipulatorExample.py | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py
index 73959b6c5..e42ae09d7 100644
--- a/cython/gtsam/examples/PlanarManipulatorExample.py
+++ b/cython/gtsam/examples/PlanarManipulatorExample.py
@@ -115,7 +115,7 @@ class ThreeLinkArm(object):
 
         l1Zl1 = expmap(0.0, 0.0, q[0])
         l2Zl2 = expmap(0.0, self.L1, q[1])
-        l3Zl3 = expmap(0.0, 7.0, q[2])
+        l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
         return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
 
     def ik(self, sTt_desired, e=1e-9):
@@ -297,12 +297,18 @@ def run_example():
     """ Use trajectory interpolation and then trajectory tracking a la Murray
         to move a 3-link arm on a straight line.
     """
+    # Create arm
     arm = ThreeLinkArm()
+
+    # Get initial pose using forward kinematics
     q = np.radians(vector3(30, -30, 45))
     sTt_initial = arm.fk(q)
+
+    # Create interpolated trajectory in task space to desired goal pose
     sTt_goal = Pose2(2.4, 4.3, math.radians(0))
     poses = trajectory(sTt_initial, sTt_goal, 50)
 
+    # Setup figure and plot initial pose
     fignum = 0
     fig = plt.figure(fignum)
     axes = fig.gca()
@@ -310,6 +316,8 @@ def run_example():
     axes.set_ylim(0, 10)
     gtsam_plot.plot_pose2(fignum, arm.fk(q))
 
+    # For all poses in interpolated trajectory, calculate dq to move to next pose.
+    # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
     for pose in poses:
         sTt = arm.fk(q)
         error = delta(sTt, pose)

From 9b7eb34add3f5a21fcf571c238c244476c8b15fd Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 15 Apr 2019 15:19:40 -0400
Subject: [PATCH 278/281] Show how expressions make (optimization-based)
 inverse kinematics easy.

---
 .../InverseKinematicsExampleExpressions.cpp   | 92 +++++++++++++++++++
 1 file changed, 92 insertions(+)
 create mode 100644 examples/InverseKinematicsExampleExpressions.cpp

diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp
new file mode 100644
index 000000000..2ccb05c96
--- /dev/null
+++ b/examples/InverseKinematicsExampleExpressions.cpp
@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file InverseKinematicsExampleExpressions.cpp
+ * @brief Implement inverse kinematics on a three-link arm using expressions.
+ * @date April 15, 2019
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/expressions.h>
+
+#include <cmath>
+
+using namespace std;
+using namespace gtsam;
+
+// Scalar multiplication of a vector, with derivatives.
+inline Vector3 scalarMultiply(const double& s, const Vector3& v,
+                              OptionalJacobian<3, 1> Hs,
+                              OptionalJacobian<3, 3> Hv) {
+  if (Hs) *Hs = v;
+  if (Hv) *Hv = s * I_3x3;
+  return s * v;
+}
+
+// Expression version of scalar product, using above function.
+inline Vector3_ operator*(const Double_& s, const Vector3_& v) {
+  return Vector3_(&scalarMultiply, s, v);
+}
+
+// Expression version of Pose2::Expmap
+inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); }
+
+// Main function
+int main(int argc, char** argv) {
+  // Three-link planar manipulator specification.
+  const double L1 = 3.5, L2 = 3.5, L3 = 2.5;    // link lengths
+  const Pose2 sXt0(0, L1 + L2 + L3, M_PI / 2);  // end-effector pose at rest
+  const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1),
+      xi3(L1 + L2, 0, 1);  // screw axes at rest
+
+  // Create Expressions for unknowns
+  using symbol_shorthand::Q;
+  Double_ q1(Q(1)), q2(Q(2)), q3(Q(3));
+
+  // Forward kinematics expression as product of exponentials
+  Pose2_ l1Zl1 = Expmap(q1 * Vector3_(xi1));
+  Pose2_ l2Zl2 = Expmap(q2 * Vector3_(xi2));
+  Pose2_ l3Zl3 = Expmap(q3 * Vector3_(xi3));
+  Pose2_ forward = compose(compose(l1Zl1, l2Zl2), compose(l3Zl3, Pose2_(sXt0)));
+
+  // Create a factor graph with a a single expression factor.
+  ExpressionFactorGraph graph;
+  Pose2 desiredEndEffectorPose(3, 2, 0);
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  graph.addExpressionFactor(forward, desiredEndEffectorPose, model);
+
+  // Create initial estimate
+  Values initial;
+  initial.insert(Q(1), 0.1);
+  initial.insert(Q(2), 0.2);
+  initial.insert(Q(3), 0.3);
+  initial.print("\nInitial Estimate:\n");  // print
+  GTSAM_PRINT(forward.value(initial));
+
+  // Optimize the initial values using a Gauss-Newton nonlinear optimizer
+  LevenbergMarquardtParams params;
+  params.setlambdaInitial(1e6);
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+
+  GTSAM_PRINT(forward.value(result));
+
+  return 0;
+}

From 63de2f887e8f461d0fc0eca0ab3414a23efc0d19 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 17 Apr 2019 09:02:16 -0400
Subject: [PATCH 279/281] Fixed comment

---
 examples/InverseKinematicsExampleExpressions.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp
index 2ccb05c96..9e86886e7 100644
--- a/examples/InverseKinematicsExampleExpressions.cpp
+++ b/examples/InverseKinematicsExampleExpressions.cpp
@@ -79,7 +79,7 @@ int main(int argc, char** argv) {
   initial.print("\nInitial Estimate:\n");  // print
   GTSAM_PRINT(forward.value(initial));
 
-  // Optimize the initial values using a Gauss-Newton nonlinear optimizer
+  // Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer
   LevenbergMarquardtParams params;
   params.setlambdaInitial(1e6);
   LevenbergMarquardtOptimizer optimizer(graph, initial, params);

From 85934fd8ca22b102caff1a27e1b4c2d796efdc13 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 17 Apr 2019 20:05:28 -0400
Subject: [PATCH 280/281] Added DSFMap to wrapper, as well as IndexPair

---
 cython/gtsam/tests/test_dsf_map.py | 38 ++++++++++++++++++++++++++++++
 gtsam.h                            | 17 +++++++++++++
 gtsam/base/DSFMap.h                | 36 ++++++++++++++--------------
 gtsam/base/tests/testDSFMap.cpp    |  6 +++++
 4 files changed, 79 insertions(+), 18 deletions(-)
 create mode 100644 cython/gtsam/tests/test_dsf_map.py

diff --git a/cython/gtsam/tests/test_dsf_map.py b/cython/gtsam/tests/test_dsf_map.py
new file mode 100644
index 000000000..6eb551034
--- /dev/null
+++ b/cython/gtsam/tests/test_dsf_map.py
@@ -0,0 +1,38 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Disjoint Set Forest.
+Author: Frank Dellaert & Varun Agrawal
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestDSFMap(GtsamTestCase):
+    """Tests for DSFMap."""
+
+    def test_all(self):
+        """Test everything in DFSMap."""
+        def key(index_pair):
+            return index_pair.i(), index_pair.j()
+
+        dsf = gtsam.DSFMapIndexPair()
+        pair1 = gtsam.IndexPair(1, 18)
+        self.assertEqual(key(dsf.find(pair1)), key(pair1))
+        pair2 = gtsam.IndexPair(2, 2)
+        dsf.merge(pair1, pair2)
+        self.assertTrue(dsf.find(pair1), dsf.find(pair1))
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/gtsam.h b/gtsam.h
index 73684c96a..aefe4e028 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -241,11 +241,28 @@ class FactorIndices {
   size_t back() const;
   void push_back(size_t factorIndex) const;
 };
+
 //*************************************************************************
 // base
 //*************************************************************************
 
 /** gtsam namespace functions */
+
+#include <gtsam/base/DSFMap.h>
+class IndexPair { 
+  IndexPair(); 
+  IndexPair(size_t i, size_t j); 
+  size_t i() const;
+  size_t j() const;
+};
+
+template<KEY = {gtsam::IndexPair}>
+class DSFMap {
+  DSFMap();
+  KEY find(const KEY& key) const;
+  void merge(const KEY& x, const KEY& y);
+};
+
 #include <gtsam/base/Matrix.h>
 bool linear_independent(Matrix A, Matrix B, double tol);
 
diff --git a/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h
index 6ddb74cfd..dfce185dc 100644
--- a/gtsam/base/DSFMap.h
+++ b/gtsam/base/DSFMap.h
@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -18,9 +18,9 @@
 
 #pragma once
 
+#include <cstdlib>  // Provides size_t
 #include <map>
 #include <set>
-#include <cstdlib>        // Provides size_t
 
 namespace gtsam {
 
@@ -29,11 +29,9 @@ namespace gtsam {
  * Uses rank compression and union by rank, iterator version
  * @addtogroup base
  */
-template<class KEY>
+template <class KEY>
 class DSFMap {
-
-protected:
-
+ protected:
   /// We store the forest in an STL map, but parents are done with pointers
   struct Entry {
     typename std::map<KEY, Entry>::iterator parent_;
@@ -41,7 +39,7 @@ protected:
     Entry() {}
   };
 
-    typedef typename std::map<KEY, Entry> Map;
+  typedef typename std::map<KEY, Entry> Map;
   typedef typename Map::iterator iterator;
   mutable Map entries_;
 
@@ -62,8 +60,7 @@ protected:
   iterator find_(const iterator& it) const {
     // follow parent pointers until we reach set representative
     iterator& parent = it->second.parent_;
-    if (parent != it)
-      parent = find_(parent); // not yet, recurse!
+    if (parent != it) parent = find_(parent);  // not yet, recurse!
     return parent;
   }
 
@@ -73,13 +70,11 @@ protected:
     return find_(initial);
   }
 
-public:
-
+ public:
   typedef std::set<KEY> Set;
 
   /// constructor
-  DSFMap() {
-  }
+  DSFMap() {}
 
   /// Given key, find the representative key for the set in which it lives
   inline KEY find(const KEY& key) const {
@@ -89,12 +84,10 @@ public:
 
   /// Merge two sets
   void merge(const KEY& x, const KEY& y) {
-
     // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
     iterator xRoot = find_(x);
     iterator yRoot = find_(y);
-    if (xRoot == yRoot)
-      return;
+    if (xRoot == yRoot) return;
 
     // Merge sets
     if (xRoot->second.rank_ < yRoot->second.rank_)
@@ -117,7 +110,14 @@ public:
     }
     return sets;
   }
-
 };
 
-}
+/// Small utility class for representing a wrappable pairs of ints.
+class IndexPair : public std::pair<size_t,size_t> {
+ public:
+  IndexPair(): std::pair<size_t,size_t>(0,0) {}
+  IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
+  inline size_t i() const { return first; };
+  inline size_t j() const { return second; };
+};
+}  // namespace gtsam
diff --git a/gtsam/base/tests/testDSFMap.cpp b/gtsam/base/tests/testDSFMap.cpp
index 5ffa0d12a..8bc4cd7ca 100644
--- a/gtsam/base/tests/testDSFMap.cpp
+++ b/gtsam/base/tests/testDSFMap.cpp
@@ -139,6 +139,12 @@ TEST(DSFMap, sets){
   EXPECT(s2 == sets[4]);
 }
 
+/* ************************************************************************* */
+TEST(DSFMap, findIndexPair) {
+  DSFMap<IndexPair> dsf;
+  EXPECT(dsf.find(IndexPair(1,2))==IndexPair(1,2));
+  EXPECT(dsf.find(IndexPair(1,2)) != dsf.find(IndexPair(1,3)));
+}
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

From 898417196f8130b8d971de0f07d05172f0aabcb9 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <jlblanco@ual.es>
Date: Thu, 18 Apr 2019 20:25:55 +0200
Subject: [PATCH 281/281] alternative solution to DLL build in MSVC

---
 CMakeLists.txt | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 186dabaf4..7856ba582 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -130,6 +130,14 @@ if(MSVC)
 	endif()
 endif()
 
+# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() 
+# or explicit instantiation will generate build errors. 
+# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017
+#
+if(MSVC AND BUILD_SHARED_LIBS)
+	list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
+endif()
+
 # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
 set(BOOST_FIND_MINIMUM_VERSION 1.43)
 set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)