diff --git a/.cproject b/.cproject
index 2bc98db3c..dd5f6dc09 100644
--- a/.cproject
+++ b/.cproject
@@ -307,6 +307,14 @@
 				true
 				true
 			
+			
+				make
+				-j2
+				testGaussianFactor.run
+				true
+				true
+				true
+			
 			
 				make
 				-j2
@@ -333,7 +341,6 @@
 			
 			
 				make
-				
 				tests/testBayesTree.run
 				true
 				false
@@ -341,7 +348,6 @@
 			
 			
 				make
-				
 				testBinaryBayesNet.run
 				true
 				false
@@ -389,7 +395,6 @@
 			
 			
 				make
-				
 				testSymbolicBayesNet.run
 				true
 				false
@@ -397,7 +402,6 @@
 			
 			
 				make
-				
 				tests/testSymbolicFactor.run
 				true
 				false
@@ -405,7 +409,6 @@
 			
 			
 				make
-				
 				testSymbolicFactorGraph.run
 				true
 				false
@@ -421,20 +424,11 @@
 			
 			
 				make
-				
 				tests/testBayesTree
 				true
 				false
 				true
 			
-			
-				make
-				-j2
-				testGaussianFactor.run
-				true
-				true
-				true
-			
 			
 				make
 				-j5
@@ -523,22 +517,6 @@
 				false
 				true
 			
-			
-				make
-				-j2
-				tests/testPose2.run
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				tests/testPose3.run
-				true
-				true
-				true
-			
 			
 				make
 				-j2
@@ -555,6 +533,22 @@
 				true
 				true
 			
+			
+				make
+				-j2
+				tests/testPose2.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				tests/testPose3.run
+				true
+				true
+				true
+			
 			
 				make
 				-j2
@@ -579,26 +573,34 @@
 				true
 				true
 			
-			
+			
 				make
-				-j2
-				all
+				-j5
+				testValues.run
 				true
 				true
 				true
 			
-			
+			
 				make
-				-j2
-				check
+				-j5
+				testOrdering.run
 				true
 				true
 				true
 			
-			
+			
 				make
-				-j2
-				clean
+				-j5
+				testKey.run
+				true
+				true
+				true
+			
+			
+				make
+				-j5
+				testLinearContainerFactor.run
 				true
 				true
 				true
@@ -683,26 +685,26 @@
 				true
 				true
 			
-			
+			
 				make
-				-j5
-				testValues.run
+				-j2
+				all
 				true
 				true
 				true
 			
-			
+			
 				make
-				-j5
-				testOrdering.run
+				-j2
+				check
 				true
 				true
 				true
 			
-			
+			
 				make
-				-j5
-				testKey.run
+				-j2
+				clean
 				true
 				true
 				true
@@ -1045,7 +1047,6 @@
 			
 			
 				make
-				
 				testGraph.run
 				true
 				false
@@ -1053,7 +1054,6 @@
 			
 			
 				make
-				
 				testJunctionTree.run
 				true
 				false
@@ -1061,7 +1061,6 @@
 			
 			
 				make
-				
 				testSymbolicBayesNetB.run
 				true
 				false
@@ -1221,7 +1220,6 @@
 			
 			
 				make
-				
 				testErrors.run
 				true
 				false
@@ -1267,10 +1265,10 @@
 				true
 				true
 			
-			
+			
 				make
-				-j5
-				testLinearContainerFactor.run
+				-j2
+				testGaussianFactor.run
 				true
 				true
 				true
@@ -1355,14 +1353,6 @@
 				true
 				true
 			
-			
-				make
-				-j2
-				testGaussianFactor.run
-				true
-				true
-				true
-			
 			
 				make
 				-j2
@@ -1693,6 +1683,7 @@
 			
 			
 				make
+				
 				testSimulated2DOriented.run
 				true
 				false
@@ -1732,6 +1723,7 @@
 			
 			
 				make
+				
 				testSimulated2D.run
 				true
 				false
@@ -1739,6 +1731,7 @@
 			
 			
 				make
+				
 				testSimulated3D.run
 				true
 				false
@@ -1832,6 +1825,14 @@
 				true
 				true
 			
+			
+				make
+				-j5
+				testJacobianFactor.run
+				true
+				true
+				true
+			
 			
 				make
 				-j2
@@ -1930,6 +1931,7 @@
 			
 			
 				make
+				
 				tests/testGaussianISAM2
 				true
 				false
@@ -1951,6 +1953,102 @@
 				true
 				true
 			
+			
+				make
+				-j2
+				testRot3.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testRot2.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testPose3.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				timeRot3.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testPose2.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testCal3_S2.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testSimpleCamera.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testHomography2.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testCalibratedCamera.run
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				check
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				clean
+				true
+				true
+				true
+			
+			
+				make
+				-j2
+				testPoint2.run
+				true
+				true
+				true
+			
 			
 				make
 				-j3
@@ -2152,7 +2250,6 @@
 			
 			
 				cpack
-				
 				-G DEB
 				true
 				false
@@ -2160,7 +2257,6 @@
 			
 			
 				cpack
-				
 				-G RPM
 				true
 				false
@@ -2168,7 +2264,6 @@
 			
 			
 				cpack
-				
 				-G TGZ
 				true
 				false
@@ -2176,7 +2271,6 @@
 			
 			
 				cpack
-				
 				--config CPackSourceConfig.cmake
 				true
 				false
@@ -2262,14 +2356,6 @@
 				true
 				true
 			
-			
-				make
-				-j5
-				check.nonlinear_unstable
-				true
-				true
-				true
-			
 			
 				make
 				-j5
@@ -2310,98 +2396,34 @@
 				true
 				true
 			
-			
+			
 				make
-				-j2
-				testRot3.run
+				-j5
+				testSpirit.run
 				true
 				true
 				true
 			
-			
+			
 				make
-				-j2
-				testRot2.run
+				-j5
+				testWrap.run
 				true
 				true
 				true
 			
-			
+			
 				make
-				-j2
-				testPose3.run
+				-j5
+				check.wrap
 				true
 				true
 				true
 			
-			
+			
 				make
-				-j2
-				timeRot3.run
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				testPose2.run
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				testCal3_S2.run
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				testSimpleCamera.run
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				testHomography2.run
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				testCalibratedCamera.run
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				check
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				clean
-				true
-				true
-				true
-			
-			
-				make
-				-j2
-				testPoint2.run
+				-j5
+				wrap
 				true
 				true
 				true
@@ -2445,38 +2467,6 @@
 				false
 				true
 			
-			
-				make
-				-j5
-				testSpirit.run
-				true
-				true
-				true
-			
-			
-				make
-				-j5
-				testWrap.run
-				true
-				true
-				true
-			
-			
-				make
-				-j5
-				check.wrap
-				true
-				true
-				true
-			
-			
-				make
-				-j5
-				wrap
-				true
-				true
-				true
-			
 		
 	
 
diff --git a/CMakeLists.txt b/CMakeLists.txt
index bda8ef9c8..8dcbe1c65 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,7 +46,7 @@ endif()
 
 # Configurable Options
 option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
-option(GTSAM_BUILD_TIMING                "Enable/Disable building of timing scripts" ON) # These do not currently work
+option(GTSAM_BUILD_TIMING                "Enable/Disable building of timing scripts" OFF) # These do not currently work
 option(GTSAM_BUILD_EXAMPLES              "Enable/Disable building of examples"       ON)
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
@@ -69,8 +69,6 @@ option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab
 option(GTSAM_BUILD_WRAP                  "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
 option(GTSAM_INSTALL_WRAP                "Enable/Disable installation of wrap utility for wrapping other libraries"    ON)
 set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/toolbox")
-set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
-set(MEX_COMMAND "mex" CACHE FILEPATH "Command to use for executing mex (if on path, 'mex' will work)")
 
 # Check / set dependent variables for MATLAB wrapper
 set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap")
diff --git a/LICENSE b/LICENSE
index 7317867ff..81c000246 100644
--- a/LICENSE
+++ b/LICENSE
@@ -2,6 +2,8 @@ Copyright (c) 2010, Georgia Tech Research Corporation
 Atlanta, Georgia 30332-0415
 All Rights Reserved
 
+See also README for licensing of 3rd-party code included in GTSAM.
+
 Redistribution and use in source and binary forms, with or without modification, 
 are permitted provided that the following conditions are met:
 
diff --git a/README b/README
index d7486c8b8..d60faea45 100644
--- a/README
+++ b/README
@@ -33,12 +33,13 @@ The library is organized according to the following directory structure:
   slam          SLAM and visual SLAM application code
 
 This library contains unchanged copies of two third party libraries, with documentation 
-of licensing in LICENSE and as follows: 
+of licensing as follows: 
   - CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library
     - http://www.cise.ufl.edu/research/sparse
     - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
   - Eigen 3.1:  General C++ matrix and linear algebra library
-    - Licenced under LGPL v3, provided in gtsam/3rdparty/Eigen/COPYING.LGPL
+    - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to
+	  Eigen is BSD and LGPL)
 
 
 There are two supporting libraries:
diff --git a/doc/LieGroups.lyx b/doc/LieGroups.lyx
index 03116dbc9..0ceb6da09 100644
--- a/doc/LieGroups.lyx
+++ b/doc/LieGroups.lyx
@@ -1744,7 +1744,7 @@ For small angles
  we have 
 \begin_inset Formula 
 \[
-e^{\skew{\omega}}\approx\skew{\omega}=\omega\skew 1
+e^{\skew{\omega}}\approx I+\skew{\omega}=I+\omega\skew 1
 \]
 
 \end_inset
@@ -2122,7 +2122,7 @@ Analoguous to
 \begin_inset Formula $\SEthree$
 \end_inset
 
-, we can compute a velocity 
+ (see below), we can compute a velocity 
 \begin_inset Formula $\xihat\hat{p}$
 \end_inset
 
@@ -3143,8 +3143,7 @@ p\\
 
 \end_inset
 
-We would now like to know what an incremental rotation parameterized by
- 
+We would now like to know what an incremental pose parameterized by 
 \begin_inset Formula $\xi$
 \end_inset
 
@@ -3227,6 +3226,17 @@ v
 
 \end_inset
 
+yielding the derivative
+\begin_inset Formula 
+\[
+\deriv{\hat{q}(\xi)}{\xi}=T\deriv{}{\xi}\left(\xihat\hat{p}\right)=T\left[\begin{array}{cc}
+-\Skew p & I_{3}\\
+0 & 0
+\end{array}\right]
+\]
+
+\end_inset
+
 The inverse action 
 \begin_inset Formula $T^{-1}p$
 \end_inset
diff --git a/doc/LieGroups.pdf b/doc/LieGroups.pdf
index f271f35c0..4daeb602c 100644
Binary files a/doc/LieGroups.pdf and b/doc/LieGroups.pdf differ
diff --git a/gtsam.h b/gtsam.h
index 62ca26269..c4950e4e4 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -765,6 +765,7 @@ class IndexConditional {
   gtsam::IndexFactor* toFactor() const;
 
   //Advanced interface
+  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
@@ -787,6 +788,7 @@ virtual class BayesNet {
   void push_front(This& conditional);
   void pop_front();
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
+  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
 #include 
@@ -830,6 +832,7 @@ virtual class BayesTreeClique {
 //  derived_ptr parent() const { return parent_.lock(); }
 
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
+  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 
   // FIXME: need wrapped versions graphs, BayesNet
 //  BayesNet shortcut(derived_ptr root, Eliminate function) const;
@@ -856,6 +859,7 @@ virtual class SymbolicBayesNet  : gtsam::SymbolicBayesNetBase {
   //Advanced Interface
   void pop_front();
   void permuteWithInverse(const gtsam::Permutation& inversePermutation);
+  bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
 #include 
@@ -1517,6 +1521,8 @@ class KeyList {
   size_t back() const;
   void push_back(size_t key);
   void push_front(size_t key);
+  void pop_back();
+  void pop_front();
   void sort();
   void remove(size_t key);
 };
@@ -1584,6 +1590,42 @@ class JointMarginal {
   void print() const;
 };
 
+#include 
+virtual class LinearContainerFactor : gtsam::NonlinearFactor {
+
+  LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering,
+      const gtsam::Values& linearizationPoint);
+  LinearContainerFactor(gtsam::GaussianFactor* factor,  const gtsam::Values& linearizationPoint);
+  LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering,
+      const gtsam::Values& linearizationPoint);
+
+  LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering);
+  LinearContainerFactor(gtsam::GaussianFactor* factor);
+  LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::InvertedOrdering& ordering);
+
+  gtsam::GaussianFactor* factor() const;
+//  const boost::optional& linearizationPoint() const;
+
+  gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const;
+  gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const;
+  gtsam::NonlinearFactor* negate() const;
+
+  bool isJacobian() const;
+  gtsam::JacobianFactor* toJacobian() const;
+  gtsam::HessianFactor* toHessian() const;
+
+  static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
+      const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint);
+  static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
+      const gtsam::InvertedOrdering& invOrdering, const gtsam::Values& linearizationPoint);
+
+  static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
+      const gtsam::Ordering& ordering);
+  static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
+      const gtsam::InvertedOrdering& invOrdering);
+
+}; // \class LinearContainerFactor
+
 //*************************************************************************
 // Nonlinear optimizers
 //*************************************************************************
@@ -1764,6 +1806,7 @@ virtual class ISAM2Clique {
     void print(string s);
 
     void permuteWithInverse(const gtsam::Permutation& inversePermutation);
+    bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
 };
 
 class ISAM2Result {
@@ -1957,6 +2000,7 @@ namespace utilities {
   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 perturbPoint3(gtsam::Values& values, double sigma, int seed);
diff --git a/gtsam/3rdparty/Eigen/COPYING.LGPL b/gtsam/3rdparty/Eigen/COPYING.LGPL
index 0e4fa8aaf..4362b4915 100644
--- a/gtsam/3rdparty/Eigen/COPYING.LGPL
+++ b/gtsam/3rdparty/Eigen/COPYING.LGPL
@@ -1,165 +1,502 @@
                   GNU LESSER GENERAL PUBLIC LICENSE
-                       Version 3, 29 June 2007
+                       Version 2.1, February 1999
 
- Copyright (C) 2007 Free Software Foundation, Inc. 
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  Everyone is permitted to copy and distribute verbatim copies
  of this license document, but changing it is not allowed.
 
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
 
-  This version of the GNU Lesser General Public License incorporates
-the terms and conditions of version 3 of the GNU General Public
-License, supplemented by the additional permissions listed below.
+                            Preamble
 
-  0. Additional Definitions. 
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
 
-  As used herein, "this License" refers to version 3 of the GNU Lesser
-General Public License, and the "GNU GPL" refers to version 3 of the GNU
-General Public License.
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
 
-  "The Library" refers to a covered work governed by this License,
-other than an Application or a Combined Work as defined below.
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
 
-  An "Application" is any work that makes use of an interface provided
-by the Library, but which is not otherwise based on the Library.
-Defining a subclass of a class defined by the Library is deemed a mode
-of using an interface provided by the Library.
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
 
-  A "Combined Work" is a work produced by combining or linking an
-Application with the Library.  The particular version of the Library
-with which the Combined Work was made is also called the "Linked
-Version".
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
 
-  The "Minimal Corresponding Source" for a Combined Work means the
-Corresponding Source for the Combined Work, excluding any source code
-for portions of the Combined Work that, considered in isolation, are
-based on the Application, and not on the Linked Version.
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
 
-  The "Corresponding Application Code" for a Combined Work means the
-object code and/or source code for the Application, including any data
-and utility programs needed for reproducing the Combined Work from the
-Application, but excluding the System Libraries of the Combined Work.
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
 
-  1. Exception to Section 3 of the GNU GPL.
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
 
-  You may convey a covered work under sections 3 and 4 of this License
-without being bound by section 3 of the GNU GPL.
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
 
-  2. Conveying Modified Versions.
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
 
-  If you modify a copy of the Library, and, in your modifications, a
-facility refers to a function or data to be supplied by an Application
-that uses the facility (other than as an argument passed when the
-facility is invoked), then you may convey a copy of the modified
-version:
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
 
-   a) under this License, provided that you make a good faith effort to
-   ensure that, in the event an Application does not supply the
-   function or data, the facility still operates, and performs
-   whatever part of its purpose remains meaningful, or
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
 
-   b) under the GNU GPL, with none of the additional permissions of
-   this License applicable to that copy.
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
 
-  3. Object Code Incorporating Material from Library Header Files.
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+                  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
 
-  The object code form of an Application may incorporate material from
-a header file that is part of the Library.  You may convey such object
-code under terms of your choice, provided that, if the incorporated
-material is not limited to numerical parameters, data structure
-layouts and accessors, or small macros, inline functions and templates
-(ten or fewer lines in length), you do both of the following:
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
 
-   a) Give prominent notice with each copy of the object code that the
-   Library is used in it and that the Library and its use are
-   covered by this License.
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
 
-   b) Accompany the object code with a copy of the GNU GPL and this license
-   document.
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
 
-  4. Combined Works.
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
 
-  You may convey a Combined Work under terms of your choice that,
-taken together, effectively do not restrict modification of the
-portions of the Library contained in the Combined Work and reverse
-engineering for debugging such modifications, if you also do each of
-the following:
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
 
-   a) Give prominent notice with each copy of the Combined Work that
-   the Library is used in it and that the Library and its use are
-   covered by this License.
-
-   b) Accompany the Combined Work with a copy of the GNU GPL and this license
-   document.
-
-   c) For a Combined Work that displays copyright notices during
-   execution, include the copyright notice for the Library among
-   these notices, as well as a reference directing the user to the
-   copies of the GNU GPL and this license document.
-
-   d) Do one of the following:
-
-       0) Convey the Minimal Corresponding Source under the terms of this
-       License, and the Corresponding Application Code in a form
-       suitable for, and under terms that permit, the user to
-       recombine or relink the Application with a modified version of
-       the Linked Version to produce a modified Combined Work, in the
-       manner specified by section 6 of the GNU GPL for conveying
-       Corresponding Source.
-
-       1) Use a suitable shared library mechanism for linking with the
-       Library.  A suitable mechanism is one that (a) uses at run time
-       a copy of the Library already present on the user's computer
-       system, and (b) will operate properly with a modified version
-       of the Library that is interface-compatible with the Linked
-       Version. 
-
-   e) Provide Installation Information, but only if you would otherwise
-   be required to provide such information under section 6 of the
-   GNU GPL, and only to the extent that such information is
-   necessary to install and execute a modified version of the
-   Combined Work produced by recombining or relinking the
-   Application with a modified version of the Linked Version. (If
-   you use option 4d0, the Installation Information must accompany
-   the Minimal Corresponding Source and Corresponding Application
-   Code. If you use option 4d1, you must provide the Installation
-   Information in the manner specified by section 6 of the GNU GPL
-   for conveying Corresponding Source.)
-
-  5. Combined Libraries.
-
-  You may place library facilities that are a work based on the
-Library side by side in a single library together with other library
-facilities that are not Applications and are not covered by this
-License, and convey such a combined library under terms of your
-choice, if you do both of the following:
-
-   a) Accompany the combined library with a copy of the same work based
-   on the Library, uncombined with any other library facilities,
-   conveyed under the terms of this License.
-
-   b) Give prominent notice with the combined library that part of it
-   is a work based on the Library, and explaining where to find the
-   accompanying uncombined form of the same work.
-
-  6. Revised Versions of the GNU Lesser General Public License.
-
-  The Free Software Foundation may publish revised and/or new versions
-of the GNU Lesser General Public License from time to time. Such new
-versions will be similar in spirit to the present version, but may
-differ in detail to address new problems or concerns.
-
-  Each version is given a distinguishing version number. If the
-Library as you received it specifies that a certain numbered version
-of the GNU Lesser General Public License "or any later version"
-applies to it, you have the option of following the terms and
-conditions either of that published version or of any later version
-published by the Free Software Foundation. If the Library as you
-received it does not specify a version number of the GNU Lesser
-General Public License, you may choose any version of the GNU Lesser
-General Public License ever published by the Free Software Foundation.
-
-  If the Library as you received it specifies that a proxy can decide
-whether future versions of the GNU Lesser General Public License shall
-apply, that proxy's public statement of acceptance of any version is
-permanent authorization for you to choose that version for the
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
 Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+                            NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    
+    Copyright (C)   
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  , 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
diff --git a/gtsam/3rdparty/Eigen/COPYING.MINPACK b/gtsam/3rdparty/Eigen/COPYING.MINPACK
new file mode 100644
index 000000000..ae7984dae
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/COPYING.MINPACK
@@ -0,0 +1,52 @@
+Minpack Copyright Notice (1999) University of Chicago.  All rights reserved
+
+Redistribution and use in source and binary forms, with or
+without modification, are permitted provided that the
+following conditions are met:
+
+1. Redistributions of source code must retain the above
+copyright notice, this list of conditions and the following
+disclaimer.
+
+2. Redistributions in binary form must reproduce the above
+copyright notice, this list of conditions and the following
+disclaimer in the documentation and/or other materials
+provided with the distribution.
+
+3. The end-user documentation included with the
+redistribution, if any, must include the following
+acknowledgment:
+
+   "This product includes software developed by the
+   University of Chicago, as Operator of Argonne National
+   Laboratory.
+
+Alternately, this acknowledgment may appear in the software
+itself, if and wherever such third-party acknowledgments
+normally appear.
+
+4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS"
+WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE
+UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND
+THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES
+OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE
+OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY
+OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR
+USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF
+THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4)
+DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION
+UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL
+BE CORRECTED.
+
+5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT
+HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF
+ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT,
+INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF
+ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF
+PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER
+SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT
+(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE,
+EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE
+POSSIBILITY OF SUCH LOSS OR DAMAGES.
+
diff --git a/gtsam/3rdparty/Eigen/COPYING.MPL2 b/gtsam/3rdparty/Eigen/COPYING.MPL2
new file mode 100644
index 000000000..14e2f777f
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/COPYING.MPL2
@@ -0,0 +1,373 @@
+Mozilla Public License Version 2.0
+==================================
+
+1. Definitions
+--------------
+
+1.1. "Contributor"
+    means each individual or legal entity that creates, contributes to
+    the creation of, or owns Covered Software.
+
+1.2. "Contributor Version"
+    means the combination of the Contributions of others (if any) used
+    by a Contributor and that particular Contributor's Contribution.
+
+1.3. "Contribution"
+    means Covered Software of a particular Contributor.
+
+1.4. "Covered Software"
+    means Source Code Form to which the initial Contributor has attached
+    the notice in Exhibit A, the Executable Form of such Source Code
+    Form, and Modifications of such Source Code Form, in each case
+    including portions thereof.
+
+1.5. "Incompatible With Secondary Licenses"
+    means
+
+    (a) that the initial Contributor has attached the notice described
+        in Exhibit B to the Covered Software; or
+
+    (b) that the Covered Software was made available under the terms of
+        version 1.1 or earlier of the License, but not also under the
+        terms of a Secondary License.
+
+1.6. "Executable Form"
+    means any form of the work other than Source Code Form.
+
+1.7. "Larger Work"
+    means a work that combines Covered Software with other material, in 
+    a separate file or files, that is not Covered Software.
+
+1.8. "License"
+    means this document.
+
+1.9. "Licensable"
+    means having the right to grant, to the maximum extent possible,
+    whether at the time of the initial grant or subsequently, any and
+    all of the rights conveyed by this License.
+
+1.10. "Modifications"
+    means any of the following:
+
+    (a) any file in Source Code Form that results from an addition to,
+        deletion from, or modification of the contents of Covered
+        Software; or
+
+    (b) any new file in Source Code Form that contains any Covered
+        Software.
+
+1.11. "Patent Claims" of a Contributor
+    means any patent claim(s), including without limitation, method,
+    process, and apparatus claims, in any patent Licensable by such
+    Contributor that would be infringed, but for the grant of the
+    License, by the making, using, selling, offering for sale, having
+    made, import, or transfer of either its Contributions or its
+    Contributor Version.
+
+1.12. "Secondary License"
+    means either the GNU General Public License, Version 2.0, the GNU
+    Lesser General Public License, Version 2.1, the GNU Affero General
+    Public License, Version 3.0, or any later versions of those
+    licenses.
+
+1.13. "Source Code Form"
+    means the form of the work preferred for making modifications.
+
+1.14. "You" (or "Your")
+    means an individual or a legal entity exercising rights under this
+    License. For legal entities, "You" includes any entity that
+    controls, is controlled by, or is under common control with You. For
+    purposes of this definition, "control" means (a) the power, direct
+    or indirect, to cause the direction or management of such entity,
+    whether by contract or otherwise, or (b) ownership of more than
+    fifty percent (50%) of the outstanding shares or beneficial
+    ownership of such entity.
+
+2. License Grants and Conditions
+--------------------------------
+
+2.1. Grants
+
+Each Contributor hereby grants You a world-wide, royalty-free,
+non-exclusive license:
+
+(a) under intellectual property rights (other than patent or trademark)
+    Licensable by such Contributor to use, reproduce, make available,
+    modify, display, perform, distribute, and otherwise exploit its
+    Contributions, either on an unmodified basis, with Modifications, or
+    as part of a Larger Work; and
+
+(b) under Patent Claims of such Contributor to make, use, sell, offer
+    for sale, have made, import, and otherwise transfer either its
+    Contributions or its Contributor Version.
+
+2.2. Effective Date
+
+The licenses granted in Section 2.1 with respect to any Contribution
+become effective for each Contribution on the date the Contributor first
+distributes such Contribution.
+
+2.3. Limitations on Grant Scope
+
+The licenses granted in this Section 2 are the only rights granted under
+this License. No additional rights or licenses will be implied from the
+distribution or licensing of Covered Software under this License.
+Notwithstanding Section 2.1(b) above, no patent license is granted by a
+Contributor:
+
+(a) for any code that a Contributor has removed from Covered Software;
+    or
+
+(b) for infringements caused by: (i) Your and any other third party's
+    modifications of Covered Software, or (ii) the combination of its
+    Contributions with other software (except as part of its Contributor
+    Version); or
+
+(c) under Patent Claims infringed by Covered Software in the absence of
+    its Contributions.
+
+This License does not grant any rights in the trademarks, service marks,
+or logos of any Contributor (except as may be necessary to comply with
+the notice requirements in Section 3.4).
+
+2.4. Subsequent Licenses
+
+No Contributor makes additional grants as a result of Your choice to
+distribute the Covered Software under a subsequent version of this
+License (see Section 10.2) or under the terms of a Secondary License (if
+permitted under the terms of Section 3.3).
+
+2.5. Representation
+
+Each Contributor represents that the Contributor believes its
+Contributions are its original creation(s) or it has sufficient rights
+to grant the rights to its Contributions conveyed by this License.
+
+2.6. Fair Use
+
+This License is not intended to limit any rights You have under
+applicable copyright doctrines of fair use, fair dealing, or other
+equivalents.
+
+2.7. Conditions
+
+Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
+in Section 2.1.
+
+3. Responsibilities
+-------------------
+
+3.1. Distribution of Source Form
+
+All distribution of Covered Software in Source Code Form, including any
+Modifications that You create or to which You contribute, must be under
+the terms of this License. You must inform recipients that the Source
+Code Form of the Covered Software is governed by the terms of this
+License, and how they can obtain a copy of this License. You may not
+attempt to alter or restrict the recipients' rights in the Source Code
+Form.
+
+3.2. Distribution of Executable Form
+
+If You distribute Covered Software in Executable Form then:
+
+(a) such Covered Software must also be made available in Source Code
+    Form, as described in Section 3.1, and You must inform recipients of
+    the Executable Form how they can obtain a copy of such Source Code
+    Form by reasonable means in a timely manner, at a charge no more
+    than the cost of distribution to the recipient; and
+
+(b) You may distribute such Executable Form under the terms of this
+    License, or sublicense it under different terms, provided that the
+    license for the Executable Form does not attempt to limit or alter
+    the recipients' rights in the Source Code Form under this License.
+
+3.3. Distribution of a Larger Work
+
+You may create and distribute a Larger Work under terms of Your choice,
+provided that You also comply with the requirements of this License for
+the Covered Software. If the Larger Work is a combination of Covered
+Software with a work governed by one or more Secondary Licenses, and the
+Covered Software is not Incompatible With Secondary Licenses, this
+License permits You to additionally distribute such Covered Software
+under the terms of such Secondary License(s), so that the recipient of
+the Larger Work may, at their option, further distribute the Covered
+Software under the terms of either this License or such Secondary
+License(s).
+
+3.4. Notices
+
+You may not remove or alter the substance of any license notices
+(including copyright notices, patent notices, disclaimers of warranty,
+or limitations of liability) contained within the Source Code Form of
+the Covered Software, except that You may alter any license notices to
+the extent required to remedy known factual inaccuracies.
+
+3.5. Application of Additional Terms
+
+You may choose to offer, and to charge a fee for, warranty, support,
+indemnity or liability obligations to one or more recipients of Covered
+Software. However, You may do so only on Your own behalf, and not on
+behalf of any Contributor. You must make it absolutely clear that any
+such warranty, support, indemnity, or liability obligation is offered by
+You alone, and You hereby agree to indemnify every Contributor for any
+liability incurred by such Contributor as a result of warranty, support,
+indemnity or liability terms You offer. You may include additional
+disclaimers of warranty and limitations of liability specific to any
+jurisdiction.
+
+4. Inability to Comply Due to Statute or Regulation
+---------------------------------------------------
+
+If it is impossible for You to comply with any of the terms of this
+License with respect to some or all of the Covered Software due to
+statute, judicial order, or regulation then You must: (a) comply with
+the terms of this License to the maximum extent possible; and (b)
+describe the limitations and the code they affect. Such description must
+be placed in a text file included with all distributions of the Covered
+Software under this License. Except to the extent prohibited by statute
+or regulation, such description must be sufficiently detailed for a
+recipient of ordinary skill to be able to understand it.
+
+5. Termination
+--------------
+
+5.1. The rights granted under this License will terminate automatically
+if You fail to comply with any of its terms. However, if You become
+compliant, then the rights granted under this License from a particular
+Contributor are reinstated (a) provisionally, unless and until such
+Contributor explicitly and finally terminates Your grants, and (b) on an
+ongoing basis, if such Contributor fails to notify You of the
+non-compliance by some reasonable means prior to 60 days after You have
+come back into compliance. Moreover, Your grants from a particular
+Contributor are reinstated on an ongoing basis if such Contributor
+notifies You of the non-compliance by some reasonable means, this is the
+first time You have received notice of non-compliance with this License
+from such Contributor, and You become compliant prior to 30 days after
+Your receipt of the notice.
+
+5.2. If You initiate litigation against any entity by asserting a patent
+infringement claim (excluding declaratory judgment actions,
+counter-claims, and cross-claims) alleging that a Contributor Version
+directly or indirectly infringes any patent, then the rights granted to
+You by any and all Contributors for the Covered Software under Section
+2.1 of this License shall terminate.
+
+5.3. In the event of termination under Sections 5.1 or 5.2 above, all
+end user license agreements (excluding distributors and resellers) which
+have been validly granted by You or Your distributors under this License
+prior to termination shall survive termination.
+
+************************************************************************
+*                                                                      *
+*  6. Disclaimer of Warranty                                           *
+*  -------------------------                                           *
+*                                                                      *
+*  Covered Software is provided under this License on an "as is"       *
+*  basis, without warranty of any kind, either expressed, implied, or  *
+*  statutory, including, without limitation, warranties that the       *
+*  Covered Software is free of defects, merchantable, fit for a        *
+*  particular purpose or non-infringing. The entire risk as to the     *
+*  quality and performance of the Covered Software is with You.        *
+*  Should any Covered Software prove defective in any respect, You     *
+*  (not any Contributor) assume the cost of any necessary servicing,   *
+*  repair, or correction. This disclaimer of warranty constitutes an   *
+*  essential part of this License. No use of any Covered Software is   *
+*  authorized under this License except under this disclaimer.         *
+*                                                                      *
+************************************************************************
+
+************************************************************************
+*                                                                      *
+*  7. Limitation of Liability                                          *
+*  --------------------------                                          *
+*                                                                      *
+*  Under no circumstances and under no legal theory, whether tort      *
+*  (including negligence), contract, or otherwise, shall any           *
+*  Contributor, or anyone who distributes Covered Software as          *
+*  permitted above, be liable to You for any direct, indirect,         *
+*  special, incidental, or consequential damages of any character      *
+*  including, without limitation, damages for lost profits, loss of    *
+*  goodwill, work stoppage, computer failure or malfunction, or any    *
+*  and all other commercial damages or losses, even if such party      *
+*  shall have been informed of the possibility of such damages. This   *
+*  limitation of liability shall not apply to liability for death or   *
+*  personal injury resulting from such party's negligence to the       *
+*  extent applicable law prohibits such limitation. Some               *
+*  jurisdictions do not allow the exclusion or limitation of           *
+*  incidental or consequential damages, so this exclusion and          *
+*  limitation may not apply to You.                                    *
+*                                                                      *
+************************************************************************
+
+8. Litigation
+-------------
+
+Any litigation relating to this License may be brought only in the
+courts of a jurisdiction where the defendant maintains its principal
+place of business and such litigation shall be governed by laws of that
+jurisdiction, without reference to its conflict-of-law provisions.
+Nothing in this Section shall prevent a party's ability to bring
+cross-claims or counter-claims.
+
+9. Miscellaneous
+----------------
+
+This License represents the complete agreement concerning the subject
+matter hereof. If any provision of this License is held to be
+unenforceable, such provision shall be reformed only to the extent
+necessary to make it enforceable. Any law or regulation which provides
+that the language of a contract shall be construed against the drafter
+shall not be used to construe this License against a Contributor.
+
+10. Versions of the License
+---------------------------
+
+10.1. New Versions
+
+Mozilla Foundation is the license steward. Except as provided in Section
+10.3, no one other than the license steward has the right to modify or
+publish new versions of this License. Each version will be given a
+distinguishing version number.
+
+10.2. Effect of New Versions
+
+You may distribute the Covered Software under the terms of the version
+of the License under which You originally received the Covered Software,
+or under the terms of any subsequent version published by the license
+steward.
+
+10.3. Modified Versions
+
+If you create software not governed by this License, and you want to
+create a new license for such software, you may create and use a
+modified version of this License if you rename the license and remove
+any references to the name of the license steward (except to note that
+such modified license differs from this License).
+
+10.4. Distributing Source Code Form that is Incompatible With Secondary
+Licenses
+
+If You choose to distribute Source Code Form that is Incompatible With
+Secondary Licenses under the terms of this version of the License, the
+notice described in Exhibit B of this License must be attached.
+
+Exhibit A - Source Code Form License Notice
+-------------------------------------------
+
+  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/.
+
+If it is not possible or desirable to put the notice in a particular
+file, then You may include the notice in a location (such as a LICENSE
+file in a relevant directory) where a recipient would be likely to look
+for such a notice.
+
+You may add additional accurate notices of copyright ownership.
+
+Exhibit B - "Incompatible With Secondary Licenses" Notice
+---------------------------------------------------------
+
+  This Source Code Form is "Incompatible With Secondary Licenses", as
+  defined by the Mozilla Public License, v. 2.0.
diff --git a/gtsam/3rdparty/Eigen/COPYING.README b/gtsam/3rdparty/Eigen/COPYING.README
new file mode 100644
index 000000000..de5b63215
--- /dev/null
+++ b/gtsam/3rdparty/Eigen/COPYING.README
@@ -0,0 +1,18 @@
+Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links:
+  http://www.mozilla.org/MPL/2.0/
+  http://www.mozilla.org/MPL/2.0/FAQ.html
+
+Some files contain third-party code under BSD or LGPL licenses, whence the other
+COPYING.* files here.
+
+All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later.
+For this reason, the COPYING.LGPL file contains the LGPL 2.1 text.
+
+If you want to guarantee that the Eigen code that you are #including is licensed
+under the MPL2 and possibly more permissive licenses (like BSD), #define this
+preprocessor symbol:
+  EIGEN_MPL2_ONLY
+For example, with most compilers, you could add this to your project CXXFLAGS:
+  -DEIGEN_MPL2_ONLY
+This will cause a compilation error to be generated if you #include any code that is
+LGPL licensed.
diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core
index 0cf101636..d48017022 100644
--- a/gtsam/3rdparty/Eigen/Eigen/Core
+++ b/gtsam/3rdparty/Eigen/Eigen/Core
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Gael Guennebaud 
 // Copyright (C) 2007-2011 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_CORE_H
 #define EIGEN_CORE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support
index c2aa2f618..36156d29a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/Eigen2Support
+++ b/gtsam/3rdparty/Eigen/Eigen/Eigen2Support
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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 EIGEN2SUPPORT_H
 #define EIGEN2SUPPORT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/StdDeque b/gtsam/3rdparty/Eigen/Eigen/StdDeque
index a4f96232d..f27234778 100644
--- a/gtsam/3rdparty/Eigen/Eigen/StdDeque
+++ b/gtsam/3rdparty/Eigen/Eigen/StdDeque
@@ -4,24 +4,9 @@
 // Copyright (C) 2009 Gael Guennebaud 
 // Copyright (C) 2009 Hauke Heibel 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_STDDEQUE_MODULE_H
 #define EIGEN_STDDEQUE_MODULE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/StdList b/gtsam/3rdparty/Eigen/Eigen/StdList
index d914ded4f..225c1e18f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/StdList
+++ b/gtsam/3rdparty/Eigen/Eigen/StdList
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Hauke Heibel 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_STDLIST_MODULE_H
 #define EIGEN_STDLIST_MODULE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/StdVector b/gtsam/3rdparty/Eigen/Eigen/StdVector
index 3d8995e5a..6b22627f6 100644
--- a/gtsam/3rdparty/Eigen/Eigen/StdVector
+++ b/gtsam/3rdparty/Eigen/Eigen/StdVector
@@ -4,24 +4,9 @@
 // Copyright (C) 2009 Gael Guennebaud 
 // Copyright (C) 2009 Hauke Heibel 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_STDVECTOR_MODULE_H
 #define EIGEN_STDVECTOR_MODULE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
index ef257aded..68e54b1d4 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
@@ -6,24 +6,9 @@
 // Copyright (C) 2009 Benoit Jacob 
 // Copyright (C) 2011 Timothy E. Holy 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_LDLT_H
 #define EIGEN_LDLT_H
@@ -370,7 +355,7 @@ template<> struct ldlt_inplace
     for (Index j = 0; j < size; j++)
     {
       // Check for termination due to an original decomposition of low-rank
-      if (!isfinite(alpha))
+      if (!(isfinite)(alpha))
         break;
 
       // Update the diagonal terms
@@ -514,12 +499,12 @@ struct solve_retval, Rhs>
     typedef typename LDLTType::RealScalar RealScalar;
     const Diagonal vectorD = dec().vectorD();
     RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits::epsilon(),
-         RealScalar(1) / NumTraits::highest()); // motivated by LAPACK's xGELSS
+				 RealScalar(1) / NumTraits::highest()); // motivated by LAPACK's xGELSS
     for (Index i = 0; i < vectorD.size(); ++i) {
       if(abs(vectorD(i)) > tolerance)
-  dst.row(i) /= vectorD(i);
+	dst.row(i) /= vectorD(i);
       else
-  dst.row(i).setZero();
+	dst.row(i).setZero();
     }
 
     // dst = L^-T (D^-1 L^-1 P b)
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h
index 17c6d6b8d..41d14e532 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_LLT_H
 #define EIGEN_LLT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h
index a06c429f0..37f142150 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_CHOLMODSUPPORT_H
 #define EIGEN_CHOLMODSUPPORT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h
index 4762933d7..aaa389978 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_ARRAY_H
 #define EIGEN_ARRAY_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h
index ec3a4be43..004b117c9 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_ARRAYBASE_H
 #define EIGEN_ARRAYBASE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h
index f8a442cee..65ffd64ca 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_ARRAYWRAPPER_H
 #define EIGEN_ARRAYWRAPPER_H
@@ -136,6 +121,13 @@ class ArrayWrapper : public ArrayBase >
       return m_expression;
     }
 
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index)  */
+    void resize(Index newSize) { m_expression.const_cast_derived().resize(newSize); }
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index,Index)*/
+    void resize(Index nbRows, Index nbCols) { m_expression.const_cast_derived().resize(nbRows,nbCols); }
+
   protected:
     NestedExpressionType m_expression;
 };
@@ -246,6 +238,13 @@ class MatrixWrapper : public MatrixBase >
       return m_expression;
     }
 
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index)  */
+    void resize(Index newSize) { m_expression.const_cast_derived().resize(newSize); }
+    /** Forwards the resizing request to the nested expression
+      * \sa DenseBase::resize(Index,Index)*/
+    void resize(Index nbRows, Index nbCols) { m_expression.const_cast_derived().resize(nbRows,nbCols); }
+
   protected:
     NestedExpressionType m_expression;
 };
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h
index 75390acf3..cd29a88f0 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h
@@ -5,24 +5,9 @@
 // Copyright (C) 2006-2010 Benoit Jacob 
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_ASSIGN_H
 #define EIGEN_ASSIGN_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/BandMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/BandMatrix.h
index 8ef917de1..ffd7fe8b3 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/BandMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/BandMatrix.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_BANDMATRIX_H
 #define EIGEN_BANDMATRIX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h
index 84ad9b63b..5f29cb3d1 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Gael Guennebaud 
 // Copyright (C) 2006-2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_BLOCK_H
 #define EIGEN_BLOCK_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h
index 2c554a57d..57efd8e69 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_ALLANDANY_H
 #define EIGEN_ALLANDANY_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h
index f9ec1d587..f20c1774c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_COMMAINITIALIZER_H
 #define EIGEN_COMMAINITIALIZER_H
@@ -80,6 +65,8 @@ struct CommaInitializer
   template
   CommaInitializer& operator,(const DenseBase& other)
   {
+    if(other.cols()==0 || other.rows()==0)
+      return *this;
     if (m_col==m_xpr.cols())
     {
       m_row+=m_currentBlockRows;
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h
index 32599a7d9..1b93af31b 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008-2009 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_CWISE_BINARY_OP_H
 #define EIGEN_CWISE_BINARY_OP_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h
index a6d5e0934..2635a62b0 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_CWISE_NULLARY_OP_H
 #define EIGEN_CWISE_NULLARY_OP_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h
index 9110c9800..063355ae5 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008-2010 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_CWISE_UNARY_OP_H
 #define EIGEN_CWISE_UNARY_OP_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h
index bf16243d5..66f73a950 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_CWISE_UNARY_VIEW_H
 #define EIGEN_CWISE_UNARY_VIEW_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h
index 1882dcca4..1cc0314ef 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2007-2010 Benoit Jacob 
 // Copyright (C) 2008-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_DENSEBASE_H
 #define EIGEN_DENSEBASE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h
index e1aa1a5f8..72704c2d7 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2006-2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_DENSECOEFFSBASE_H
 #define EIGEN_DENSECOEFFSBASE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h
index 0ea05bc90..1fc2daf2c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h
@@ -5,24 +5,9 @@
 // Copyright (C) 2006-2009 Benoit Jacob 
 // Copyright (C) 2010 Hauke Heibel 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_MATRIXSTORAGE_H
 #define EIGEN_MATRIXSTORAGE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
index 77b765f15..16261968a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2007-2009 Benoit Jacob 
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_DIAGONAL_H
 #define EIGEN_DIAGONAL_H
@@ -56,16 +41,15 @@ struct traits >
   typedef typename remove_reference::type _MatrixTypeNested;
   typedef typename MatrixType::StorageKind StorageKind;
   enum {
-    AbsDiagIndex = DiagIndex<0 ? -DiagIndex : DiagIndex, // only used if DiagIndex != Dynamic
-    // FIXME these computations are broken in the case where the matrix is rectangular and DiagIndex!=0
     RowsAtCompileTime = (int(DiagIndex) == Dynamic || int(MatrixType::SizeAtCompileTime) == Dynamic) ? Dynamic
-                      : (EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime,
-                                        MatrixType::ColsAtCompileTime) - AbsDiagIndex),
+    : (EIGEN_PLAIN_ENUM_MIN(MatrixType::RowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0),
+                            MatrixType::ColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))),
     ColsAtCompileTime = 1,
     MaxRowsAtCompileTime = int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic
                          : DiagIndex == Dynamic ? EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::MaxRowsAtCompileTime,
-                                                                    MatrixType::MaxColsAtCompileTime)
-                         : (EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::MaxRowsAtCompileTime, MatrixType::MaxColsAtCompileTime) - AbsDiagIndex),
+                                                                              MatrixType::MaxColsAtCompileTime)
+                         : (EIGEN_PLAIN_ENUM_MIN(MatrixType::MaxRowsAtCompileTime - EIGEN_PLAIN_ENUM_MAX(-DiagIndex, 0),
+                                                 MatrixType::MaxColsAtCompileTime - EIGEN_PLAIN_ENUM_MAX( DiagIndex, 0))),
     MaxColsAtCompileTime = 1,
     MaskLvalueBit = is_lvalue::value ? LvalueBit : 0,
     Flags = (unsigned int)_MatrixTypeNested::Flags & (HereditaryBits | LinearAccessBit | MaskLvalueBit | DirectAccessBit) & ~RowMajorBit,
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h
index 844f9864b..6e8b50fab 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2009 Gael Guennebaud 
 // Copyright (C) 2007-2009 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_DIAGONALMATRIX_H
 #define EIGEN_DIAGONALMATRIX_H
@@ -35,6 +20,7 @@ class DiagonalBase : public EigenBase
   public:
     typedef typename internal::traits::DiagonalVectorType DiagonalVectorType;
     typedef typename DiagonalVectorType::Scalar Scalar;
+    typedef typename DiagonalVectorType::RealScalar RealScalar;
     typedef typename internal::traits::StorageKind StorageKind;
     typedef typename internal::traits::Index Index;
 
@@ -80,6 +66,17 @@ class DiagonalBase : public EigenBase
       return diagonal().cwiseInverse();
     }
     
+    inline const DiagonalWrapper, const DiagonalVectorType> >
+    operator*(const Scalar& scalar) const
+    {
+      return diagonal() * scalar;
+    }
+    friend inline const DiagonalWrapper, const DiagonalVectorType> >
+    operator*(const Scalar& scalar, const DiagonalBase& other)
+    {
+      return other.diagonal() * scalar;
+    }
+    
     #ifdef EIGEN2_SUPPORT
     template
     bool isApprox(const DiagonalBase& other, typename NumTraits::Real precision = NumTraits::dummy_precision()) const
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h
index 9f6a99895..598c6b3e1 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Gael Guennebaud 
 // Copyright (C) 2007-2009 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_DIAGONALPRODUCT_H
 #define EIGEN_DIAGONALPRODUCT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h
index 67dbbf8ba..ae9274e36 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2006-2008, 2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_DOT_H
 #define EIGEN_DOT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h
index 77d4c25d5..0bbd28bec 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2009 Benoit Jacob 
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_EIGENBASE_H
 #define EIGEN_EIGENBASE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Flagged.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Flagged.h
index 47f411b05..1f2955fc1 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Flagged.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Flagged.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_FLAGGED_H
 #define EIGEN_FLAGGED_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ForceAlignedAccess.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ForceAlignedAccess.h
index 238b7b770..807c7a293 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ForceAlignedAccess.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ForceAlignedAccess.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_FORCEALIGNEDACCESS_H
 #define EIGEN_FORCEALIGNEDACCESS_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h
index f33f636bb..2f46abfdd 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_FUNCTORS_H
 #define EIGEN_FUNCTORS_H
@@ -462,7 +447,7 @@ struct functor_traits >
  * indeed it seems better to declare m_other as a Packet and do the pset1() once
  * in the constructor. However, in practice:
  *  - GCC does not like m_other as a Packet and generate a load every time it needs it
- *  - on the other hand GCC is able to moves the pset1() away the loop :)
+ *  - on the other hand GCC is able to moves the pset1() outside the loop :)
  *  - simpler code ;)
  * (ICC and gcc 4.4 seems to perform well in both cases, the issue is visible with y = a*x + b*y)
  */
@@ -493,33 +478,6 @@ template
 struct functor_traits >
 { enum { Cost = NumTraits::MulCost, PacketAccess = false }; };
 
-template
-struct scalar_quotient1_impl {
-  typedef typename packet_traits::type Packet;
-  // FIXME default copy constructors seems bugged with std::complex<>
-  EIGEN_STRONG_INLINE scalar_quotient1_impl(const scalar_quotient1_impl& other) : m_other(other.m_other) { }
-  EIGEN_STRONG_INLINE scalar_quotient1_impl(const Scalar& other) : m_other(static_cast(1) / other) {}
-  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; }
-  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
-  { return internal::pmul(a, pset1(m_other)); }
-  const Scalar m_other;
-};
-template
-struct functor_traits >
-{ enum { Cost = NumTraits::MulCost, PacketAccess = packet_traits::HasMul }; };
-
-template
-struct scalar_quotient1_impl {
-  // FIXME default copy constructors seems bugged with std::complex<>
-  EIGEN_STRONG_INLINE scalar_quotient1_impl(const scalar_quotient1_impl& other) : m_other(other.m_other) { }
-  EIGEN_STRONG_INLINE scalar_quotient1_impl(const Scalar& other) : m_other(other) {}
-  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a / m_other; }
-  typename add_const_on_value_type::Nested>::type m_other;
-};
-template
-struct functor_traits >
-{ enum { Cost = 2 * NumTraits::MulCost, PacketAccess = false }; };
-
 /** \internal
   * \brief Template functor to divide a scalar by a fixed other one
   *
@@ -529,14 +487,19 @@ struct functor_traits >
   * \sa class CwiseUnaryOp, MatrixBase::operator/
   */
 template
-struct scalar_quotient1_op : scalar_quotient1_impl::IsInteger > {
-  EIGEN_STRONG_INLINE scalar_quotient1_op(const Scalar& other)
-    : scalar_quotient1_impl::IsInteger >(other) {}
+struct scalar_quotient1_op {
+  typedef typename packet_traits::type Packet;
+  // FIXME default copy constructors seems bugged with std::complex<>
+  EIGEN_STRONG_INLINE scalar_quotient1_op(const scalar_quotient1_op& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE scalar_quotient1_op(const Scalar& other) : m_other(other) {}
+  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a / m_other; }
+  EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::pdiv(a, pset1(m_other)); }
+  typename add_const_on_value_type::Nested>::type m_other;
 };
 template
 struct functor_traits >
-: functor_traits::IsInteger> >
-{};
+{ enum { Cost = 2 * NumTraits::MulCost, PacketAccess = packet_traits::HasDiv }; };
 
 // nullary functors
 
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h
index 887641163..d74edcfdb 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2006-2008 Benoit Jacob 
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_FUZZY_H
 #define EIGEN_FUZZY_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
index 4fbe1f14b..bfc2a67b1 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2006-2008 Benoit Jacob 
 // Copyright (C) 2008-2011 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_GENERAL_PRODUCT_H
 #define EIGEN_GENERAL_PRODUCT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h
index d92ac9529..858fb243e 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_GENERIC_PACKET_MATH_H
 #define EIGEN_GENERIC_PACKET_MATH_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h
index 94605252d..e63726c47 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2010 Gael Guennebaud 
 // Copyright (C) 2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_GLOBAL_FUNCTIONS_H
 #define EIGEN_GLOBAL_FUNCTIONS_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h
index 2f1906f2a..cc8e18a00 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2006-2008 Benoit Jacob 
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_IO_H
 #define EIGEN_IO_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
index 360a2280e..15a19226e 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2007-2010 Benoit Jacob 
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_MAP_H
 #define EIGEN_MAP_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
index 2b736cb74..a388d61ea 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2007-2010 Benoit Jacob 
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_MAPBASE_H
 #define EIGEN_MAPBASE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h
index ab153c1eb..05e913f2f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2006-2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_MATHFUNCTIONS_H
 #define EIGEN_MATHFUNCTIONS_H
@@ -845,7 +830,7 @@ template<> struct scalar_fuzzy_impl
 
 // std::isfinite is non standard, so let's define our own version,
 // even though it is not very efficient.
-template bool isfinite(const T& x)
+template bool (isfinite)(const T& x)
 {
   return x::highest() && x>NumTraits::lowest();
 }
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h
index 8742a0130..99160b591 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2006-2010 Benoit Jacob 
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_MATRIX_H
 #define EIGEN_MATRIX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
index 5a744c5ec..36ea2cee8 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2006-2009 Benoit Jacob 
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_MATRIXBASE_H
 #define EIGEN_MATRIXBASE_H
@@ -252,7 +237,7 @@ template class MatrixBase
     
     // huuuge hack. make Eigen2's matrix.part() work in eigen3. Problem: Diagonal is now a class template instead
     // of an integer constant. Solution: overload the part() method template wrt template parameters list.
-    template class U>
+    template class U>
     const DiagonalWrapper part() const
     { return diagonal().asDiagonal(); }
     #endif // EIGEN2_SUPPORT
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/NestByValue.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/NestByValue.h
index cfe3e7990..a893b1761 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/NestByValue.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/NestByValue.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_NESTBYVALUE_H
 #define EIGEN_NESTBYVALUE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h
index 5278cfb73..ecb3fa285 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_NOALIAS_H
 #define EIGEN_NOALIAS_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h
index e8867235e..c94ef026b 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2006-2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_NUMTRAITS_H
 #define EIGEN_NUMTRAITS_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h
index e0d618dfb..bc29f8142 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2009 Benoit Jacob 
 // Copyright (C) 2009-2011 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PERMUTATIONMATRIX_H
 #define EIGEN_PERMUTATIONMATRIX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
index f9c432732..71c74309a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008-2009 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_DENSESTORAGEBASE_H
 #define EIGEN_DENSESTORAGEBASE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h
index 53eb0fbae..30aa8943b 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Product.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2011 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PRODUCT_H
 #define EIGEN_PRODUCT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h
index 6cf02a649..ec12e5c9f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PRODUCTBASE_H
 #define EIGEN_PRODUCTBASE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h
index 1470e9184..a9f7f4346 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_RANDOM_H
 #define EIGEN_RANDOM_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h
index d66ff00c1..b7ce7c658 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_REDUX_H
 #define EIGEN_REDUX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h
index 79e3578df..b61fdc29e 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_REPLICATE_H
 #define EIGEN_REPLICATE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h
index 24b6a3f6a..613912ffa 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2009-2010 Gael Guennebaud 
 // Copyright (C) 2009-2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_RETURNBYVALUE_H
 #define EIGEN_RETURNBYVALUE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h
index 9e4e8a2bc..e30ae3d28 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h
@@ -5,24 +5,9 @@
 // Copyright (C) 2009 Ricard Marxer 
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_REVERSE_H
 #define EIGEN_REVERSE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h
index 92508a168..2bf6e91d0 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SELECT_H
 #define EIGEN_SELECT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h
index 086f05c49..82cc4da73 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SELFADJOINTMATRIX_H
 #define EIGEN_SELFADJOINTMATRIX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h
index 99389d7e2..0caf2bab1 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SELFCWISEBINARYOP_H
 #define EIGEN_SELFCWISEBINARYOP_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h
index ef09226a4..ef17f288e 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SOLVETRIANGULAR_H
 #define EIGEN_SOLVETRIANGULAR_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h
index 16514c86a..7499b195e 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_STABLENORM_H
 #define EIGEN_STABLENORM_H
@@ -146,7 +131,6 @@ MatrixBase::blueNorm() const
     abig = internal::sqrt(abig);
     if(abig > overfl)
     {
-      eigen_assert(false && "overflow");
       return rbig;
     }
     if(amed > RealScalar(0))
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Stride.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Stride.h
index 73c54e6bf..1e3f5fe9f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Stride.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Stride.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2010 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_STRIDE_H
 #define EIGEN_STRIDE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Swap.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Swap.h
index deb1d2831..fd73cf3ad 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Swap.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Swap.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SWAP_H
 #define EIGEN_SWAP_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h
index c62f74764..045a1cce6 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2006-2008 Benoit Jacob 
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_TRANSPOSE_H
 #define EIGEN_TRANSPOSE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h
index fa37822f8..2cd268a5f 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2010-2011 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_TRANSPOSITIONS_H
 #define EIGEN_TRANSPOSITIONS_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h
index 5e97e4052..301b0ef24 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008 Benoit Jacob 
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_TRIANGULARMATRIX_H
 #define EIGEN_TRIANGULARMATRIX_H
@@ -526,6 +511,7 @@ template
 struct triangular_assignment_selector
 {
   typedef typename Derived1::Index Index;
+  typedef typename Derived1::Scalar Scalar;
   static inline void run(Derived1 &dst, const Derived2 &src)
   {
     for(Index j = 0; j < dst.cols(); ++j)
@@ -535,7 +521,7 @@ struct triangular_assignment_selector
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_VECTORBLOCK_H
 #define EIGEN_VECTORBLOCK_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h
index 7b5be7cd5..862c0f336 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008-2010 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PARTIAL_REDUX_H
 #define EIGEN_PARTIAL_REDUX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h
index fd04fd978..916bfd096 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_VISITOR_H
 #define EIGEN_VISITOR_H
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 b2d866b71..68d9a2bff 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_COMPLEX_ALTIVEC_H
 #define EIGEN_COMPLEX_ALTIVEC_H
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 5b62b4c31..75de19311 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008 Konstantinos Margaritis 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PACKET_MATH_ALTIVEC_H
 #define EIGEN_PACKET_MATH_ALTIVEC_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/Settings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/Settings.h
index 957adc8fe..097373c84 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/Settings.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/Settings.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2008-2010 Gael Guennebaud 
 // Copyright (C) 2006-2008 Benoit Jacob 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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/.
 
 
 /* All the parameters defined in this file can be specialized in the
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 72abb6f4a..795b4be73 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_COMPLEX_NEON_H
 #define EIGEN_COMPLEX_NEON_H
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 1eb082a5b..a20250f7c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
@@ -5,24 +5,9 @@
 // Copyright (C) 2010 Konstantinos Margaritis 
 // Heavily based on Gael's SSE version.
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PACKET_MATH_NEON_H
 #define EIGEN_PACKET_MATH_NEON_H
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 1615886ac..12df98775 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_COMPLEX_SSE_H
 #define EIGEN_COMPLEX_SSE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h
index de2f06d5f..3f41a4e26 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2007 Julien Pommier
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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/.
 
 /* The sin, cos, exp, and log functions of this file come from
  * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/
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 8faeeefc9..10d918219 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PACKET_MATH_SSE_H
 #define EIGEN_PACKET_MATH_SSE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h
index 8f53c43ad..403d25fa9 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h
@@ -4,24 +4,9 @@
 // Copyright (C) 2006-2008 Benoit Jacob 
 // Copyright (C) 2008-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_COEFFBASED_PRODUCT_H
 #define EIGEN_COEFFBASED_PRODUCT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
index d631fa28e..5eb03c98c 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_GENERAL_BLOCK_PANEL_H
 #define EIGEN_GENERAL_BLOCK_PANEL_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h
index 76fc32032..73a465ec5 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_GENERAL_MATRIX_MATRIX_H
 #define EIGEN_GENERAL_MATRIX_MATRIX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
index 74331ee4f..432d3a9dc 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_GENERAL_MATRIX_MATRIX_TRIANGULAR_H
 #define EIGEN_GENERAL_MATRIX_MATRIX_TRIANGULAR_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h
index d868a66a1..71eb7661d 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_GENERAL_MATRIX_VECTOR_H
 #define EIGEN_GENERAL_MATRIX_VECTOR_H
@@ -96,14 +81,13 @@ EIGEN_DONT_INLINE static void run(
   const Index peels = 2;
   const Index LhsPacketAlignedMask = LhsPacketSize-1;
   const Index ResPacketAlignedMask = ResPacketSize-1;
-  const Index PeelAlignedMask = ResPacketSize*peels-1;
   const Index size = rows;
   
   // How many coeffs of the result do we have to skip to be aligned.
   // Here we assume data are at least aligned on the base scalar type.
   Index alignedStart = internal::first_aligned(res,size);
   Index alignedSize = ResPacketSize>1 ? alignedStart + ((size-alignedStart) & ~ResPacketAlignedMask) : 0;
-  const Index peeledSize  = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
+  const Index peeledSize = alignedSize - RhsPacketSize*peels - RhsPacketSize + 1;
 
   const Index alignmentStep = LhsPacketSize>1 ? (LhsPacketSize - lhsStride % LhsPacketSize) & LhsPacketAlignedMask : 0;
   Index alignmentPattern = alignmentStep==0 ? AllAligned
@@ -192,6 +176,8 @@ EIGEN_DONT_INLINE static void run(
               _EIGEN_ACCUMULATE_PACKETS(d,du,d);
             break;
           case FirstAligned:
+          {
+            Index j = alignedStart;
             if(peels>1)
             {
               LhsPacket A00, A01, A02, A03, A10, A11, A12, A13;
@@ -201,7 +187,7 @@ EIGEN_DONT_INLINE static void run(
               A02 = pload(&lhs2[alignedStart-2]);
               A03 = pload(&lhs3[alignedStart-3]);
 
-              for (Index j = alignedStart; j(&lhs1[j-1+LhsPacketSize]);  palign<1>(A01,A11);
                 A12 = pload(&lhs2[j-2+LhsPacketSize]);  palign<2>(A02,A12);
@@ -225,9 +211,10 @@ EIGEN_DONT_INLINE static void run(
                 pstore(&res[j+ResPacketSize],T1);
               }
             }
-            for (Index j = peeledSize; j1 ? alignedStart + ((depth-alignedStart) & ~RhsPacketAlignedMask) : 0;
-  const Index peeledSize  = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
+  const Index peeledSize = alignedSize - RhsPacketSize*peels - RhsPacketSize + 1;
 
   const Index alignmentStep = LhsPacketSize>1 ? (LhsPacketSize - lhsStride % LhsPacketSize) & LhsPacketAlignedMask : 0;
   Index alignmentPattern = alignmentStep==0 ? AllAligned
@@ -445,10 +431,12 @@ EIGEN_DONT_INLINE static void run(
               _EIGEN_ACCUMULATE_PACKETS(d,du,d);
             break;
           case FirstAligned:
+          {
+            Index j = alignedStart;
             if (peels>1)
             {
               /* Here we proccess 4 rows with with two peeled iterations to hide
-               * tghe overhead of unaligned loads. Moreover unaligned loads are handled
+               * the overhead of unaligned loads. Moreover unaligned loads are handled
                * using special shift/move operations between the two aligned packets
                * overlaping the desired unaligned packet. This is *much* more efficient
                * than basic unaligned loads.
@@ -458,7 +446,7 @@ EIGEN_DONT_INLINE static void run(
               A02 = pload(&lhs2[alignedStart-2]);
               A03 = pload(&lhs3[alignedStart-3]);
 
-              for (Index j = alignedStart; j(&rhs[j]);
                 A11 = pload(&lhs1[j-1+LhsPacketSize]);  palign<1>(A01,A11);
@@ -480,9 +468,10 @@ EIGEN_DONT_INLINE static void run(
                 ptmp3 = pcj.pmadd(A13, b, ptmp3);
               }
             }
-            for (Index j = peeledSize; j
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_PARALLELIZER_H
 #define EIGEN_PARALLELIZER_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
index 91ba12081..48209636e 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SELFADJOINT_MATRIX_MATRIX_H
 #define EIGEN_SELFADJOINT_MATRIX_MATRIX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h
index 7f39ef01a..c3145c69a 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SELFADJOINT_MATRIX_VECTOR_H
 #define EIGEN_SELFADJOINT_MATRIX_VECTOR_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h
index a3ff9e3e7..6a55f3d77 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SELFADJOINT_PRODUCT_H
 #define EIGEN_SELFADJOINT_PRODUCT_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointRank2Update.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointRank2Update.h
index 001cfb591..57a98cc2d 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointRank2Update.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointRank2Update.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_SELFADJOINTRANK2UPTADE_H
 #define EIGEN_SELFADJOINTRANK2UPTADE_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h
index 06053bfd9..92cba66f6 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_TRIANGULAR_MATRIX_MATRIX_H
 #define EIGEN_TRIANGULAR_MATRIX_MATRIX_H
@@ -76,7 +61,7 @@ struct product_triangular_matrix_matrix& blocking)
   {
     product_triangular_matrix_matrix
-      ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resStride, alpha);
+      ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resStride, alpha, blocking);
   }
 };
 
@@ -111,7 +96,7 @@ struct product_triangular_matrix_matrix& blocking)
   {
     // strip zeros
     Index diagSize  = (std::min)(_rows,_depth);
@@ -122,15 +107,16 @@ struct product_triangular_matrix_matrix lhs(_lhs,lhsStride);
     const_blas_data_mapper rhs(_rhs,rhsStride);
 
-    Index kc = depth; // cache block size along the K direction
-    Index mc = rows;  // cache block size along the M direction
-    Index nc = cols;  // cache block size along the N direction
-    computeProductBlockingSizes(kc, mc, nc);
+    Index kc = blocking.kc();                   // cache block size along the K direction
+    Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
+
+    std::size_t sizeA = kc*mc;
+    std::size_t sizeB = kc*cols;
     std::size_t sizeW = kc*Traits::WorkSpaceFactor;
-    std::size_t sizeB = sizeW + kc*cols;
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, kc*mc, 0);
-    ei_declare_aligned_stack_constructed_variable(Scalar, allocatedBlockB, sizeB, 0);    
-    Scalar* blockB = allocatedBlockB + sizeW;
+
+    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+    ei_declare_aligned_stack_constructed_variable(Scalar, blockW, sizeW, blocking.blockW());
 
     Matrix triangularBuffer;
     triangularBuffer.setZero();
@@ -188,7 +174,7 @@ struct product_triangular_matrix_matrix0)
@@ -198,7 +184,7 @@ struct product_triangular_matrix_matrix()
             (blockA, &lhs(i2, actual_k2), lhsStride, actual_kc, actual_mc);
 
-          gebp_kernel(res+i2, resStride, blockA, blockB, actual_mc, actual_kc, cols, alpha);
+          gebp_kernel(res+i2, resStride, blockA, blockB, actual_mc, actual_kc, cols, alpha, -1, -1, 0, 0, blockW);
         }
       }
     }
@@ -239,7 +225,7 @@ struct product_triangular_matrix_matrix& blocking)
   {
     // strip zeros
     Index diagSize  = (std::min)(_cols,_depth);
@@ -250,16 +236,16 @@ struct product_triangular_matrix_matrix lhs(_lhs,lhsStride);
     const_blas_data_mapper rhs(_rhs,rhsStride);
 
-    Index kc = depth; // cache block size along the K direction
-    Index mc = rows;  // cache block size along the M direction
-    Index nc = cols;  // cache block size along the N direction
-    computeProductBlockingSizes(kc, mc, nc);
+    Index kc = blocking.kc();                   // cache block size along the K direction
+    Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
 
+    std::size_t sizeA = kc*mc;
+    std::size_t sizeB = kc*cols;
     std::size_t sizeW = kc*Traits::WorkSpaceFactor;
-    std::size_t sizeB = sizeW + kc*cols;
-    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, kc*mc, 0);
-    ei_declare_aligned_stack_constructed_variable(Scalar, allocatedBlockB, sizeB, 0);
-    Scalar* blockB = allocatedBlockB + sizeW;
+
+    ei_declare_aligned_stack_constructed_variable(Scalar, blockA, sizeA, blocking.blockA());
+    ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
+    ei_declare_aligned_stack_constructed_variable(Scalar, blockW, sizeW, blocking.blockW());
 
     Matrix triangularBuffer;
     triangularBuffer.setZero();
@@ -347,13 +333,13 @@ struct product_triangular_matrix_matrix
     Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(m_lhs)
                                * RhsBlasTraits::extractScalarFactor(m_rhs);
 
+    typedef internal::gemm_blocking_space<(Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,Scalar,Scalar,
+              Lhs::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime, Lhs::MaxColsAtCompileTime,4> BlockingType;
+
+    enum { IsLower = (Mode&Lower) == Lower };
+    Index stripedRows  = ((!LhsIsTriangular) || (IsLower))  ? lhs.rows() : (std::min)(lhs.rows(),lhs.cols());
+    Index stripedCols  = ((LhsIsTriangular)  || (!IsLower)) ? rhs.cols() : (std::min)(rhs.cols(),rhs.rows());
+    Index stripedDepth = LhsIsTriangular ? ((!IsLower) ? lhs.cols() : (std::min)(lhs.cols(),lhs.rows()))
+                                         : ((IsLower)  ? rhs.rows() : (std::min)(rhs.rows(),rhs.cols()));
+
+    BlockingType blocking(stripedRows, stripedCols, stripedDepth);
+
     internal::product_triangular_matrix_matrix::Flags&RowMajorBit) ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate,
       (internal::traits<_ActualRhsType>::Flags&RowMajorBit) ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate,
       (internal::traits::Flags&RowMajorBit) ? RowMajor : ColMajor>
       ::run(
-        lhs.rows(), rhs.cols(), lhs.cols(),// LhsIsTriangular ? rhs.cols() : lhs.rows(),           // sizes
+        stripedRows, stripedCols, stripedDepth,   // sizes
         &lhs.coeffRef(0,0),    lhs.outerStride(), // lhs info
         &rhs.coeffRef(0,0),    rhs.outerStride(), // rhs info
-        &dst.coeffRef(0,0), dst.outerStride(), // result info
-        actualAlpha                            // alpha
+        &dst.coeffRef(0,0), dst.outerStride(),    // result info
+        actualAlpha, blocking
       );
   }
 };
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_MKL.h
index 8173da5bb..4d20de617 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_MKL.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_MKL.h
@@ -57,11 +57,11 @@ template  { \
   static inline void run(Index _rows, Index _cols, Index _depth, const Scalar* _lhs, Index lhsStride,\
-    const Scalar* _rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha) { \
+    const Scalar* _rhs, Index rhsStride, Scalar* res, Index resStride, Scalar alpha, level3_blocking& blocking) { \
       product_triangular_matrix_matrix_trmm::run( \
-        _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha); \
+        _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \
   } \
 };
 
@@ -96,7 +96,7 @@ struct product_triangular_matrix_matrix_trmm& blocking) \
   { \
    Index diagSize  = (std::min)(_rows,_depth); \
    Index rows      = IsLower ? _rows : diagSize; \
@@ -115,16 +115,16 @@ struct product_triangular_matrix_matrix_trmm::run( \
-           _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha); \
+           _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \
      /*std::cout << "TRMM_L: A is not square! Go to Eigen TRMM implementation!\n";*/ \
      } else { \
      /* Make sense to call GEMM */ \
        Map > lhsMap(_lhs,rows,depth,OuterStride<>(lhsStride)); \
        MatrixLhs aa_tmp=lhsMap.template triangularView(); \
        MKL_INT aStride = aa_tmp.outerStride(); \
-       gemm_blocking_space blocking(_rows,_cols,_depth); \
+       gemm_blocking_space gemm_blocking(_rows,_cols,_depth); \
        general_matrix_matrix_product::run( \
-       rows, cols, depth, aa_tmp.data(), aStride, _rhs, rhsStride, res, resStride, alpha, blocking, 0); \
+       rows, cols, depth, aa_tmp.data(), aStride, _rhs, rhsStride, res, resStride, alpha, gemm_blocking, 0); \
 \
      /*std::cout << "TRMM_L: A is not square! Go to MKL GEMM implementation! " << nthr<<" \n";*/ \
      } \
@@ -210,7 +210,7 @@ struct product_triangular_matrix_matrix_trmm& blocking) \
   { \
    Index diagSize  = (std::min)(_cols,_depth); \
    Index rows      = _rows; \
@@ -229,16 +229,16 @@ struct product_triangular_matrix_matrix_trmm::run( \
-           _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha); \
+           _rows, _cols, _depth, _lhs, lhsStride, _rhs, rhsStride, res, resStride, alpha, blocking); \
        /*std::cout << "TRMM_R: A is not square! Go to Eigen TRMM implementation!\n";*/ \
      } else { \
      /* Make sense to call GEMM */ \
        Map > rhsMap(_rhs,depth,cols, OuterStride<>(rhsStride)); \
        MatrixRhs aa_tmp=rhsMap.template triangularView(); \
        MKL_INT aStride = aa_tmp.outerStride(); \
-       gemm_blocking_space blocking(_rows,_cols,_depth); \
+       gemm_blocking_space gemm_blocking(_rows,_cols,_depth); \
        general_matrix_matrix_product::run( \
-       rows, cols, depth, _lhs, lhsStride, aa_tmp.data(), aStride, res, resStride, alpha, blocking, 0); \
+       rows, cols, depth, _lhs, lhsStride, aa_tmp.data(), aStride, res, resStride, alpha, gemm_blocking, 0); \
 \
      /*std::cout << "TRMM_R: A is not square! Go to MKL GEMM implementation! " << nthr<<" \n";*/ \
      } \
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h
index e1dc0c23e..b1c10c201 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_TRIANGULARMATRIXVECTOR_H
 #define EIGEN_TRIANGULARMATRIXVECTOR_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h
index 0dd94638b..a49ea3183 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2009 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see .
+// 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_TRIANGULAR_SOLVER_MATRIX_H
 #define EIGEN_TRIANGULAR_SOLVER_MATRIX_H
diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverVector.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverVector.h
index f83a81061..ce4d10088 100644
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverVector.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverVector.h
@@ -3,24 +3,9 @@
 //
 // Copyright (C) 2008-2010 Gael Guennebaud 
 //
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see