Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues

Conflicts: .cproject examples/Pose2SLAMExample_graph.cpp examples/SFMExample_SmartFactor.cpp examples/SFMExample_SmartFactorPCG.cpp gtsam.h gtsam/geometry/CameraSet.h gtsam/geometry/StereoCamera.cpp gtsam/geometry/StereoCamera.h gtsam/geometry/tests/testCameraSet.cpp gtsam/linear/RegularJacobianFactor.h gtsam/linear/tests/testRegularJacobianFactor.cpp gtsam/nonlinear/ExpressionFactor.h gtsam/nonlinear/expressions.h gtsam/slam/JacobianFactorQR.h gtsam/slam/JacobianSchurFactor.h gtsam/slam/RegularHessianFactor.h gtsam/slam/RegularImplicitSchurFactor.h gtsam/slam/SmartFactorBase.h gtsam/slam/SmartProjectionFactor.h gtsam/slam/SmartProjectionPoseFactor.h gtsam/slam/tests/testRegularHessianFactor.cpp gtsam/slam/tests/testRegularImplicitSchurFactor.cpp gtsam/slam/tests/testSmartFactorBase.cpp gtsam/slam/tests/testSmartProjectionPoseFactor.cpp gtsam_unstable/examples/SmartProjectionFactorExample.cpp gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp gtsam_unstable/slam/SmartStereoProjectionFactor.h gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
2015-08-31 21:06:37 -07:00 · 2015-08-31 21:06:37 -07:00 · 63ca5dcfb0
parent 69ff1c4e7d c73b835848
commit 63ca5dcfb0
512 changed files with 20995 additions and 13753 deletions
--- a/.settings/.gitignore
+++ b/.settings/.gitignore
@ -0,0 +1 @@
+/org.eclipse.cdt.codan.core.prefs
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -158,6 +158,12 @@ else()
 	set(GTSAM_USE_TBB 0)  # This will go into config.h
 endif()

+###############################################################################
+# Prohibit Timing build mode in combination with TBB
+if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE  STREQUAL "Timing"))
+      message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
+endif()
+

 ###############################################################################
 # Find Google perftools
@ -173,6 +179,11 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
    set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
    include_directories(${MKL_INCLUDE_DIR})
    list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
+    
+    # --no-as-needed is required with gcc according to the MKL link advisor
+    if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+        set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")  
+    endif()
 else()
    set(GTSAM_USE_EIGEN_MKL 0)
    set(EIGEN_USE_MKL_ALL 0)
@ -192,36 +203,40 @@ endif()

 ###############################################################################
 # Option for using system Eigen or GTSAM-bundled Eigen
-### Disabled until our patches are included in Eigen ###
+### These patches only affect usage of MKL. If you want to enable MKL, you *must*
+### use our patched version of Eigen
 ### See:  http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
 ###       http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
-###       http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
-# option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
-set(GTSAM_USE_SYSTEM_EIGEN OFF)
+option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)

 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
-	# Use generic Eigen include paths e.g. <Eigen/Core>
-	set(GTSAM_EIGEN_INCLUDE_PREFIX "")
-
 	find_package(Eigen3 REQUIRED)
 	include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
-else()
-	# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
-	set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
 	
+	# Use generic Eigen include paths e.g. <Eigen/Core>
+    set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
+	
+	# check if MKL is also enabled - can have one or the other, but not both!
+	if(EIGEN_USE_MKL_ALL)
+	  message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL")
+	endif()
+else()
+	# Use bundled Eigen include path. 
 	# Clear any variables set by FindEigen3
 	if(EIGEN3_INCLUDE_DIR)
 		set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
 	endif()
+	# Add the bundled version of eigen to the include path so that it can still be included
+	# with  #include <Eigen/Core>
+	include_directories(BEFORE "gtsam/3rdparty/Eigen/")
+	
+	# set full path to be used by external projects
+	# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
+	set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
+	
 endif()

-# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen
-configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h)
-
-# Install the configuration file for Eigen
-install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
-
 ###############################################################################
 # Global compile options

@ -389,6 +404,11 @@ if(NOT MSVC AND NOT XCODE_VERSION)
    message(STATUS "  C compilation flags            : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
    message(STATUS "  C++ compilation flags          : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}")
 endif()
+if(GTSAM_USE_SYSTEM_EIGEN)
+    message(STATUS "  Use System Eigen               : Yes")
+else()
+    message(STATUS "  Use System Eigen               : No")
+endif()
 if(GTSAM_USE_TBB)
 	message(STATUS "  Use Intel TBB                  : Yes")
 elseif(TBB_FOUND)
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@ -53,9 +53,10 @@ if(NOT FIRST_PASS_DONE)
  endif()
 endif()

-# Clang on Mac uses a template depth that is less than standard and is too small
+# Clang uses a template depth that is less than standard and is too small
 if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
-	if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
+    # Apple Clang before 5.0 does not support -ftemplate-depth.
+    if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0"))
        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
    endif()
 endif()
@ -97,7 +98,8 @@ if(    NOT cmake_build_type_tolower STREQUAL ""
   AND NOT cmake_build_type_tolower STREQUAL "release"
   AND NOT cmake_build_type_tolower STREQUAL "timing"
   AND NOT cmake_build_type_tolower STREQUAL "profiling"
-   AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
+   AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo"
+   AND NOT cmake_build_type_tolower STREQUAL "minsizerel")
  message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
 endif()

--- a/cmake/GtsamMatlabWrap.cmake
+++ b/cmake/GtsamMatlabWrap.cmake
@ -270,7 +270,7 @@ function(install_wrapped_library_internal interfaceHeader)
 	if(GTSAM_BUILD_TYPE_POSTFIXES)
 		foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
 			string(TOUPPER "${build_type}" build_type_upper)
-			if("${build_type_upper}" STREQUAL "RELEASE")
+			if(${build_type_upper} STREQUAL "RELEASE")
 				set(build_type_tag "") # Don't create release mode tag on installed directory
 			else()
 				set(build_type_tag "${build_type}")
@ -367,13 +367,18 @@ endfunction()
 # should be installed to all build type toolboxes
 function(install_matlab_scripts source_directory patterns)
 	set(patterns_args "")
+	set(exclude_patterns "")
+	if(NOT GTSAM_WRAP_SERIALIZATION)
+	    set(exclude_patterns "testSerialization.m")
+	endif()
+
 	foreach(pattern ${patterns})
 		list(APPEND patterns_args PATTERN "${pattern}")
 	endforeach()
 	if(GTSAM_BUILD_TYPE_POSTFIXES)
 		foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
 			string(TOUPPER "${build_type}" build_type_upper)
-			if("${build_type_upper}" STREQUAL "RELEASE")
+			if(${build_type_upper} STREQUAL "RELEASE")
 				set(build_type_tag "") # Don't create release mode tag on installed directory
 			else()
 				set(build_type_tag "${build_type}")
@ -381,10 +386,10 @@ function(install_matlab_scripts source_directory patterns)
 			# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
 			get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
 			get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
-			install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
+			install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
 		endforeach()
 	else()
-		install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
+		install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
 	endif()

 endfunction()
--- a/cmake/GtsamTesting.cmake
+++ b/cmake/GtsamTesting.cmake
@ -195,7 +195,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 			add_test(NAME ${target_name} COMMAND ${target_name})
 			add_dependencies(check.${groupName} ${target_name})
 			add_dependencies(check ${target_name})
+			if(NOT XCODE_VERSION)
 				add_dependencies(all.tests ${script_name})
+			endif()
 		
 			# Add TOPSRCDIR
 			set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
--- a/examples/Data/.gitignore
+++ b/examples/Data/.gitignore
@ -0,0 +1 @@
+*.txt
--- a/examples/PlanarSLAMExample.cpp
+++ b/examples/PlanarSLAMExample.cpp
@ -42,7 +42,7 @@
 // Also, we will initialize the robot at the origin using a Prior factor.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>

 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
 // are nonlinear factors, we will need a Nonlinear Factor Graph.
--- a/examples/Pose2SLAMExample_g2o.cpp
+++ b/examples/Pose2SLAMExample_g2o.cpp
@ -20,6 +20,7 @@

 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <fstream>

--- a/examples/Pose2SLAMExample_graph.cpp
+++ b/examples/Pose2SLAMExample_graph.cpp
@ -19,6 +19,7 @@
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>

--- a/examples/Pose2SLAMExample_lago.cpp
+++ b/examples/Pose2SLAMExample_lago.cpp
@ -22,6 +22,7 @@
 #include <gtsam/slam/lago.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <fstream>

 using namespace std;
--- a/examples/RangeISAMExample_plaza2.cpp
+++ b/examples/RangeISAMExample_plaza2.cpp
@ -39,7 +39,7 @@
 // have been provided with the library for solving robotics SLAM problems.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/dataset.h>

 // Standard headers, added last, so we know headers above work on their own
--- a/examples/SFMExample.cpp
+++ b/examples/SFMExample.cpp
@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
  // Intentionally initialize the variables off from the ground truth
  Values initialEstimate;
  for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
  for (size_t j = 0; j < points.size(); ++j)
    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
  initialEstimate.print("Initial Estimates:\n");
--- a/examples/SFMExampleExpressions.cpp
+++ b/examples/SFMExampleExpressions.cpp
@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {

  // Create perturbed initial
  Values initial;
-  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
  for (size_t i = 0; i < poses.size(); ++i)
    initial.insert(Symbol('x', i), poses[i].compose(delta));
  for (size_t j = 0; j < points.size(); ++j)
--- a/examples/SFMExample_SmartFactor.cpp
+++ b/examples/SFMExample_SmartFactor.cpp
@ -34,6 +34,9 @@ using namespace gtsam;
 // Make the typename short so it looks much cleaner
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;

+// create a typedef to the camera type
+typedef PinholePose<Cal3_S2> Camera;
+
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {

@ -55,12 +58,12 @@ int main(int argc, char* argv[]) {
  for (size_t j = 0; j < points.size(); ++j) {

    // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
-    SmartFactor::shared_ptr smartfactor(new SmartFactor());
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));

    for (size_t i = 0; i < poses.size(); ++i) {

      // generate the 2D measurement
-      SimpleCamera camera(poses[i], *K);
+      Camera camera(poses[i], K);
      Point2 measurement = camera.project(points[j]);

      // call add() function to add measurement into a single factor, here we need to add:
@ -68,7 +71,7 @@ int main(int argc, char* argv[]) {
      //    2. the corresponding camera's key
      //    3. camera noise model
      //    4. camera calibration
-      smartfactor->add(measurement, i, measurementNoise, K);
+      smartfactor->add(measurement, i);
    }

    // insert the smart factor in the graph
@ -77,24 +80,24 @@ int main(int argc, char* argv[]) {

  // Add a prior on pose x0. This indirectly specifies where the origin is.
  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));

  // Because the structure-from-motion problem has a scale ambiguity, the problem is
  // still under-constrained. Here we add a prior on the second pose x1, so this will
  // fix the scale by indicating the distance between x0 and x1.
  // Because these two are fixed, the rest of the poses will be also be fixed.
-  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
+  graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph

  graph.print("Factor Graph:\n");

  // Create the initial estimate to the solution
  // Intentionally initialize the variables off from the ground truth
  Values initialEstimate;
-  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
  for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(i, poses[i].compose(delta));
+    initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
  initialEstimate.print("Initial Estimates:\n");

  // Optimize the graph and print results
--- a/examples/SFMExample_SmartFactorPCG.cpp
+++ b/examples/SFMExample_SmartFactorPCG.cpp
@ -30,6 +30,9 @@ using namespace gtsam;
 // Make the typename short so it looks much cleaner
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;

+// create a typedef to the camera type
+typedef PinholePose<Cal3_S2> Camera;
+
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {

@ -51,16 +54,16 @@ int main(int argc, char* argv[]) {
  for (size_t j = 0; j < points.size(); ++j) {

    // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
-    SmartFactor::shared_ptr smartfactor(new SmartFactor());
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));

    for (size_t i = 0; i < poses.size(); ++i) {

      // generate the 2D measurement
-      SimpleCamera camera(poses[i], *K);
+      Camera camera(poses[i], K);
      Point2 measurement = camera.project(points[j]);

      // call add() function to add measurement into a single factor, here we need to add:
-      smartfactor->add(measurement, i, measurementNoise, K);
+      smartfactor->add(measurement, i);
    }

    // insert the smart factor in the graph
@ -69,18 +72,18 @@ int main(int argc, char* argv[]) {

  // Add a prior on pose x0. This indirectly specifies where the origin is.
  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));

  // Fix the scale ambiguity by adding a prior
-  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
+  graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));

  // Create the initial estimate to the solution
  Values initialEstimate;
-  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
  for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(i, poses[i].compose(delta));
+    initialEstimate.insert(i, Camera(poses[i].compose(delta),K));

  // We will use LM in the outer optimization loop, but by specifying "Iterative" below
  // We indicate that an iterative linear solver should be used.
--- a/examples/SFMExample_bal_COLAMD_METIS.cpp
+++ b/examples/SFMExample_bal_COLAMD_METIS.cpp
@ -0,0 +1,144 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SFMExample.cpp
+ * @brief   This file is to compare the ordering performance for COLAMD vs METIS.
+ * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
+ * @author  Frank Dellaert, Zhaoyang Lv
+ */
+
+// For an explanation of headers, see SFMExample.cpp
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+
+#include <gtsam/base/timing.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::C;
+using symbol_shorthand::P;
+
+// We will be using a projection factor that ties a SFM_Camera to a 3D point.
+// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+// and has a total of 9 free parameters
+typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
+
+/* ************************************************************************* */
+int main (int argc, char* argv[]) {
+
+  // Find default file, but if an argument is given, try loading a file
+  string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  if (argc>1) filename = string(argv[1]);
+
+  // Load the SfM data from file
+  SfM_data mydata;
+  readBAL(filename, mydata);
+  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+
+  // Create a factor graph
+  NonlinearFactorGraph graph;
+
+  // We share *one* noiseModel between all projection factors
+  noiseModel::Isotropic::shared_ptr noise =
+      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+
+  // Add measurements to the factor graph
+  size_t j = 0;
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+      size_t i = m.first;
+      Point2 uv = m.second;
+      graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
+    }
+    j += 1;
+  }
+
+  // Add a prior on pose x1. This indirectly specifies where the origin is.
+  // and a prior on the position of the first landmark to fix the scale
+  graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1)));
+  graph.push_back(PriorFactor<Point3>    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
+
+  // Create initial estimate
+  Values initial;
+  size_t i = 0; j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+
+  /** ---------------  COMPARISON  -----------------------**/
+  /** ----------------------------------------------------**/
+
+  LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
+  try {
+    params_using_METIS.setVerbosity("ERROR");
+    gttic_(METIS_ORDERING);
+    params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
+    gttoc_(METIS_ORDERING);
+
+    params_using_COLAMD.setVerbosity("ERROR");
+    gttic_(COLAMD_ORDERING);
+    params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
+    gttoc_(COLAMD_ORDERING);
+  } catch (exception& e) {
+    cout << e.what();
+  }
+
+  // expect they have different ordering results
+  if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
+    cout << "COLAMD and METIS produce the same ordering. "
+         << "Problem here!!!" << endl;
+  }
+
+  /* Optimize the graph with METIS and COLAMD and time the results */
+
+  Values result_METIS, result_COLAMD;
+  try {
+    gttic_(OPTIMIZE_WITH_METIS);
+    LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
+    result_METIS = lm_METIS.optimize();
+    gttoc_(OPTIMIZE_WITH_METIS);
+
+    gttic_(OPTIMIZE_WITH_COLAMD);
+    LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
+    result_COLAMD = lm_COLAMD.optimize();
+    gttoc_(OPTIMIZE_WITH_COLAMD);
+  } catch (exception& e) {
+    cout << e.what();
+  }
+
+
+  { // printing the result
+
+    cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
+    cout << "METIS final error: " << graph.error(result_METIS) << endl;
+
+    cout << endl << endl;
+
+    cout << "Time comparison by solving " << filename << " results:" << endl;
+    cout << boost::format("%1% point tracks and %2% cameras\n") \
+            % mydata.number_tracks() % mydata.number_cameras() \
+         << endl;
+
+    tictoc_print_();
+  }
+
+
+  return 0;
+}
+/* ************************************************************************* */
+
--- a/examples/SelfCalibrationExample.cpp
+++ b/examples/SelfCalibrationExample.cpp
@ -82,7 +82,7 @@ int main(int argc, char* argv[]) {
  Values initialEstimate;
  initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
  for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
  for (size_t j = 0; j < points.size(); ++j)
    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));

--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@ -31,28 +31,30 @@
 *
 */

-#include <gtsam/base/timing.h>
-#include <gtsam/base/treeTraversal-inst.h>
-#include <gtsam/slam/dataset.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/linear/GaussianJunctionTree.h>
-#include <gtsam/linear/GaussianEliminationTree.h>
+#include <gtsam/sam/BearingRangeFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
+#include <gtsam/linear/GaussianEliminationTree.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/config.h> // for GTSAM_USE_TBB

-#include <fstream>
-#include <iostream>
-#include <boost/archive/binary_oarchive.hpp>
 #include <boost/archive/binary_iarchive.hpp>
-#include <boost/serialization/export.hpp>
+#include <boost/archive/binary_oarchive.hpp>
 #include <boost/program_options.hpp>
 #include <boost/range/algorithm/set_algorithm.hpp>
 #include <boost/random.hpp>
+#include <boost/serialization/export.hpp>
+
+#include <fstream>
+#include <iostream>

 #ifdef GTSAM_USE_TBB
 #include <tbb/tbb.h>
@ -72,23 +74,6 @@ typedef NoiseModelFactor1<Pose> NM1;
 typedef NoiseModelFactor2<Pose,Pose> NM2;
 typedef BearingRangeFactor<Pose,Point2> BR;

-//GTSAM_VALUE_EXPORT(Value);
-//GTSAM_VALUE_EXPORT(Pose);
-//GTSAM_VALUE_EXPORT(Rot2);
-//GTSAM_VALUE_EXPORT(Point2);
-//GTSAM_VALUE_EXPORT(NonlinearFactor);
-//GTSAM_VALUE_EXPORT(NoiseModelFactor);
-//GTSAM_VALUE_EXPORT(NM1);
-//GTSAM_VALUE_EXPORT(NM2);
-//GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
-//GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
-//GTSAM_VALUE_EXPORT(BR);
-//GTSAM_VALUE_EXPORT(noiseModel::Base);
-//GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
-//GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
-//GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
-//GTSAM_VALUE_EXPORT(noiseModel::Unit);
-
 double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
  // Compute degrees of freedom (observations - variables)
  // In ocaml, +1 was added to the observations to account for the prior, but
@ -269,12 +254,12 @@ void runIncremental()
      boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
    {
      Key key1 = measurement->key1(), key2 = measurement->key2();
-      if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) {
+      if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
        // We found an odometry starting at firstStep
        firstPose = std::min(key1, key2);
        break;
      }
-      if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) {
+      if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) {
        // We found an odometry joining firstStep with a previous pose
        havePreviousPose = true;
        firstPose = std::max(key1, key2);
@ -303,7 +288,9 @@ void runIncremental()

  cout << "Playing forward time steps..." << endl;

-  for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step)
+  for (size_t step = firstPose;
+      nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep);
+      ++step)
  {
    Values newVariables;
    NonlinearFactorGraph newFactors;
@ -589,7 +576,7 @@ void runStats()
 {
  cout << "Gathering statistics..." << endl;
  GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
-  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
+  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
  treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);

  ofstream file;
--- a/examples/TimeTBB.cpp
+++ b/examples/TimeTBB.cpp
@ -17,6 +17,7 @@

 #include <gtsam/global_includes.h>
 #include <gtsam/base/Matrix.h>
+
 #include <boost/assign/list_of.hpp>
 #include <boost/foreach.hpp>
 #include <map>
--- a/examples/VisualISAM2Example.cpp
+++ b/examples/VisualISAM2Example.cpp
@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {

    // Add an initial guess for the current pose
    // Intentionally initialize the variables off from the ground truth
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));

    // If this is the first iteration, add a prior on the first pose to set the coordinate frame
    // and a prior on the first landmark to set the scale
--- a/examples/VisualISAMExample.cpp
+++ b/examples/VisualISAMExample.cpp
@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
    }

    // Intentionally initialize the variables off from the ground truth
-    Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+    Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
    Pose3 initial_xi = poses[i].compose(noise);

    // Add an initial guess for the current pose
--- a/gtsam.h
+++ b/gtsam.h
@ -1,4 +1,5 @@
 /**
+
 * GTSAM Wrap Module Definition
 *
 * These are the current classes available through the matlab toolbox interface,
@ -156,7 +157,7 @@ virtual class Value {
  size_t dim() const;
 };

-#include <gtsam/base/LieScalar.h>
+#include <gtsam/base/deprecated/LieScalar.h>
 class LieScalar {
  // Standard constructors
  LieScalar();
@ -185,7 +186,7 @@ class LieScalar {
  static Vector Logmap(const gtsam::LieScalar& p);
 };

-#include <gtsam/base/LieVector.h>
+#include <gtsam/base/deprecated/LieVector.h>
 class LieVector {
  // Standard constructors
  LieVector();
@ -217,7 +218,7 @@ class LieVector {
  void serialize() const;
 };

-#include <gtsam/base/LieMatrix.h>
+#include <gtsam/base/deprecated/LieMatrix.h>
 class LieMatrix {
  // Standard constructors
  LieMatrix();
@ -288,6 +289,32 @@ class Point2 {
  void serialize() const;
 };

+// std::vector<gtsam::Point2>
+class Point2Vector
+{
+  // Constructors
+  Point2Vector();
+  Point2Vector(const gtsam::Point2Vector& v);
+
+  //Capacity
+  size_t size() const;
+  size_t max_size() const;
+  void resize(size_t sz);
+  size_t capacity() const;
+  bool empty() const;
+  void reserve(size_t n);
+
+  //Element access
+  gtsam::Point2 at(size_t n) const;
+  gtsam::Point2 front() const;
+  gtsam::Point2 back() const;
+
+  //Modifiers
+  void assign(size_t n, const gtsam::Point2& u);
+  void push_back(const gtsam::Point2& x);
+  void pop_back();
+};
+
 class StereoPoint2 {
  // Standard Constructors
  StereoPoint2();
@ -304,8 +331,6 @@ class StereoPoint2 {
  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;

  // Manifold
-  static size_t Dim();
-  size_t dim() const;
  gtsam::StereoPoint2 retract(Vector v) const;
  Vector localCoordinates(const gtsam::StereoPoint2& p) const;

@ -414,7 +439,7 @@ class Rot3 {
  static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft)
  static gtsam::Rot3 ypr(double y, double p, double r);
  static gtsam::Rot3 quaternion(double w, double x, double y, double z);
-  static gtsam::Rot3 rodriguez(Vector v);
+  static gtsam::Rot3 Rodrigues(Vector v);

  // Testable
  void print(string s) const;
@ -550,6 +575,16 @@ class Pose3 {
  void serialize() const;
 };

+// std::vector<gtsam::Pose3>
+class Pose3Vector
+{
+  Pose3Vector();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Pose3 at(size_t n) const;
+  void push_back(const gtsam::Pose3& x);
+};
+
 #include <gtsam/geometry/Unit3.h>
 class Unit3 {
  // Standard Constructors
@ -778,7 +813,7 @@ class CalibratedCamera {

  // Action on Point3
  gtsam::Point2 project(const gtsam::Point3& point) const;
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);

  // Standard Interface
  gtsam::Pose3 pose() const;
@ -788,56 +823,16 @@ class CalibratedCamera {
  void serialize() const;
 };

-class SimpleCamera {
-  // Standard Constructors and Named Constructors
-  SimpleCamera();
-  SimpleCamera(const gtsam::Pose3& pose);
-  SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
-  static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K,
-      const gtsam::Pose2& pose, double height);
-  static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
-  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye,
-      const gtsam::Point3& target, const gtsam::Point3& upVector,
-      const gtsam::Cal3_S2& K);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::SimpleCamera& camera, double tol) const;
-
-  // Standard Interface
-  gtsam::Pose3 pose() const;
-  gtsam::Cal3_S2 calibration();
-
-  // Manifold
-  gtsam::SimpleCamera retract(const Vector& d) const;
-  Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
-  size_t dim() const;
-  static size_t Dim();
-
-  // Transformations and measurement functions
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
-  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
-  gtsam::Point2 project(const gtsam::Point3& point);
-  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
-  double range(const gtsam::Point3& point);
-  double range(const gtsam::Pose3& point);
-
-  // enabling serialization functionality
-  void serialize() const;
-};
-
-template<CALIBRATION = {gtsam::Cal3DS2}>
+template<CALIBRATION>
 class PinholeCamera {
  // Standard Constructors and Named Constructors
  PinholeCamera();
  PinholeCamera(const gtsam::Pose3& pose);
-  PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K);
-  static This Level(const gtsam::Cal3DS2& K,
-    const gtsam::Pose2& pose, double height);
+  PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
+  static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height);
  static This Level(const gtsam::Pose2& pose, double height);
-  static This Lookat(const gtsam::Point3& eye,
-    const gtsam::Point3& target, const gtsam::Point3& upVector,
-    const gtsam::Cal3DS2& K);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+      const gtsam::Point3& upVector, const CALIBRATION& K);

  // Testable
  void print(string s) const;
@ -854,7 +849,7 @@ class PinholeCamera {
  static size_t Dim();

  // Transformations and measurement functions
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
  gtsam::Point2 project(const gtsam::Point3& point);
  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@ -865,6 +860,50 @@ class PinholeCamera {
  void serialize() const;
 };

+virtual class SimpleCamera {
+  // Standard Constructors and Named Constructors
+  SimpleCamera();
+  SimpleCamera(const gtsam::Pose3& pose);
+  SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
+  static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
+  static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
+  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+      const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::SimpleCamera& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  gtsam::Cal3_S2 calibration() const;
+
+  // Manifold
+  gtsam::SimpleCamera retract(const Vector& d) const;
+  Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Pose3& point);
+
+  // enabling serialization functionality
+  void serialize() const;
+
+};
+
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+
 class StereoCamera {
  // Standard Constructors and Named Constructors
  StereoCamera();
@ -877,7 +916,7 @@ class StereoCamera {
  // Standard Interface
  gtsam::Pose3 pose() const;
  double baseline() const;
-  gtsam::Cal3_S2Stereo* calibration() const;
+  gtsam::Cal3_S2Stereo calibration() const;

  // Manifold
  gtsam::StereoCamera retract(const Vector& d) const;
@ -893,6 +932,16 @@ class StereoCamera {
  void serialize() const;
 };

+#include <gtsam/geometry/triangulation.h>
+
+// Templates appear not yet supported for free functions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+    gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
+    double rank_tol, bool optimize);
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+    gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
+    double rank_tol, bool optimize);
+
 //*************************************************************************
 // Symbolic
 //*************************************************************************
@ -1606,12 +1655,12 @@ char symbolChr(size_t key);
 size_t symbolIndex(size_t key);

 // Default keyformatter
-void printKeyList  (const gtsam::KeyList& keys);
-void printKeyList  (const gtsam::KeyList& keys, string s);
-void printKeyVector(const gtsam::KeyVector& keys);
-void printKeyVector(const gtsam::KeyVector& keys, string s);
-void printKeySet   (const gtsam::KeySet& keys);
-void printKeySet   (const gtsam::KeySet& keys, string s);
+void PrintKeyList  (const gtsam::KeyList& keys);
+void PrintKeyList  (const gtsam::KeyList& keys, string s);
+void PrintKeyVector(const gtsam::KeyVector& keys);
+void PrintKeyVector(const gtsam::KeyVector& keys, string s);
+void PrintKeySet   (const gtsam::KeySet& keys);
+void PrintKeySet   (const gtsam::KeySet& keys, string s);

 #include <gtsam/inference/LabeledSymbol.h>
 class LabeledSymbol {
@ -1728,7 +1777,7 @@ class Values {
  void swap(gtsam::Values& values);

  bool exists(size_t j) const;
-  gtsam::KeyList keys() const;
+  gtsam::KeyVector keys() const;

  gtsam::VectorValues zeroVectors() const;

@ -1836,8 +1885,6 @@ class KeySet {
 class KeyVector {
  KeyVector();
  KeyVector(const gtsam::KeyVector& other);
-  KeyVector(const gtsam::KeySet& other);
-  KeyVector(const gtsam::KeyList& other);

  // Note: no print function

@ -2167,9 +2214,6 @@ class NonlinearISAM {
 //*************************************************************************
 // Nonlinear factor types
 //*************************************************************************
-#include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/Cal3DS2.h>
-#include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
@ -2210,7 +2254,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
 };


-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NoiseModelFactor {
  RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
@ -2220,20 +2264,16 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
-
-// Commented out by Frank Dec 2014: not poses!
-// If needed, we need a RangeFactor that takes a camera, extracts the pose
-// Should be easy with Expressions
-//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
-//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
+typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
+typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;


-#include <gtsam/slam/BearingFactor.h>
-template<POSE, POINT, ROTATION>
+#include <gtsam/sam/BearingFactor.h>
+template<POSE, POINT, BEARING>
 virtual class BearingFactor : gtsam::NoiseModelFactor {
-  BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
+  BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel);

  // enabling serialization functionality
  void serialize() const;
@ -2241,19 +2281,18 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {

 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;

-
-#include <gtsam/slam/BearingRangeFactor.h>
-template<POSE, POINT, ROTATION>
+#include <gtsam/sam/BearingRangeFactor.h>
+template<POSE, POINT, BEARING, RANGE>
 virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
-  BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
-
-  pair<ROTATION, double> measured() const;
+  BearingRangeFactor(size_t poseKey, size_t pointKey,
+      const BEARING& measuredBearing, const RANGE& measuredRange,
+      const gtsam::noiseModel::Base* noiseModel);

  // enabling serialization functionality
  void serialize() const;
 };

-typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;


 #include <gtsam/slam/ProjectionFactor.h>
@ -2300,18 +2339,33 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
  void serialize() const;
 };

+#include <gtsam/slam/SmartProjectionFactor.h>
+class SmartProjectionParams {
+  SmartProjectionParams();
+  // TODO(frank): make these work:
+  //  void setLinearizationMode(LinearizationMode linMode);
+  //  void setDegeneracyMode(DegeneracyMode degMode);
+  void setRankTolerance(double rankTol);
+  void setEnableEPI(bool enableEPI);
+  void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
+  void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
+};
+
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
 template<CALIBRATION>
-virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
+virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {

-  SmartProjectionPoseFactor(double rankTol, double linThreshold,
-      bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K,
+      const gtsam::Pose3& body_P_sensor);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K,
+      const gtsam::Pose3& body_P_sensor,
+      const gtsam::SmartProjectionParams& params);

-  SmartProjectionPoseFactor(double rankTol);
-  SmartProjectionPoseFactor();
-
-  void add(const gtsam::Point2& measured_i, size_t poseKey_i,
-      const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
+  void add(const gtsam::Point2& measured_i, size_t poseKey_i);

  // enabling serialization functionality
  //void serialize() const;
--- a/gtsam/3rdparty/CMakeLists.txt
+++ b/gtsam/3rdparty/CMakeLists.txt
@ -58,10 +58,10 @@ add_subdirectory(ceres)
 include(GeographicLib/cmake/FindGeographicLib.cmake)

 # Set up the option to install GeographicLib
-if(GEOGRAPHICLIB_FOUND)
-  set(install_geographiclib_default OFF)
-else()
+if(GEOGRAPHICLIB-NOTFOUND)
  set(install_geographiclib_default ON)
+else()
+  set(install_geographiclib_default OFF)
 endif()
 option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default})

--- a/gtsam/3rdparty/Eigen/.hg_archival.txt
+++ b/gtsam/3rdparty/Eigen/.hg_archival.txt
@ -1,4 +1,4 @@
 repo: 8a21fd850624c931e448cbcfb38168cb2717c790
-node: ffa86ffb557094721ca71dcea6aed2651b9fd610
+node: 10219c95fe653d4962aa9db4946f6fbea96dd740
 branch: 3.2
-tag: 3.2.0
+tag: 3.2.4
--- a/gtsam/3rdparty/Eigen/.hgtags
+++ b/gtsam/3rdparty/Eigen/.hgtags
@ -23,3 +23,7 @@ bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2
 da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1
 4b687cad1d23066f66863f4f87298447298443df 3.2-rc1
 1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2
+ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
+6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1
+1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2
+36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3
--- a/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
@ -442,6 +442,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
  m_transpositions.resize(size);
  m_isInitialized = false;
  m_temporary.resize(size);
+  m_sign = internal::ZeroSign;

  internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign);

@ -502,7 +503,6 @@ struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
    using std::abs;
    using std::max;
    typedef typename LDLTType::MatrixType MatrixType;
-    typedef typename LDLTType::Scalar Scalar;
    typedef typename LDLTType::RealScalar RealScalar;
    const typename Diagonal<const MatrixType>::RealReturnType vectorD(dec().vectorD());
    // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h
@ -29,6 +29,11 @@ struct traits<ArrayWrapper<ExpressionType> >
  : public traits<typename remove_all<typename ExpressionType::Nested>::type >
 {
  typedef ArrayXpr XprKind;
+  // Let's remove NestByRefBit
+  enum {
+    Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
+    Flags = Flags0 & ~NestByRefBit
+  };
 };
 }

@ -149,6 +154,11 @@ struct traits<MatrixWrapper<ExpressionType> >
 : public traits<typename remove_all<typename ExpressionType::Nested>::type >
 {
  typedef MatrixXpr XprKind;
+  // Let's remove NestByRefBit
+  enum {
+    Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
+    Flags = Flags0 & ~NestByRefBit
+  };
 };
 }

--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h
@ -462,8 +462,10 @@ template<typename Derived> class DenseBase
    template<int p> RealScalar lpNorm() const;

    template<int RowFactor, int ColFactor>
-    const Replicate<Derived,RowFactor,ColFactor> replicate() const;
-    const Replicate<Derived,Dynamic,Dynamic> replicate(Index rowFacor,Index colFactor) const;
+    inline const Replicate<Derived,RowFactor,ColFactor> replicate() const;
+    
+    typedef Replicate<Derived,Dynamic,Dynamic> ReplicateReturnType;
+    inline const ReplicateReturnType replicate(Index rowFacor,Index colFactor) const;

    typedef Reverse<Derived, BothDirections> ReverseReturnType;
    typedef const Reverse<const Derived, BothDirections> ConstReverseReturnType;
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
@ -190,18 +190,18 @@ MatrixBase<Derived>::diagonal() const
  *
  * \sa MatrixBase::diagonal(), class Diagonal */
 template<typename Derived>
-inline typename MatrixBase<Derived>::template DiagonalIndexReturnType<DynamicIndex>::Type
+inline typename MatrixBase<Derived>::DiagonalDynamicIndexReturnType
 MatrixBase<Derived>::diagonal(Index index)
 {
-  return typename DiagonalIndexReturnType<DynamicIndex>::Type(derived(), index);
+  return DiagonalDynamicIndexReturnType(derived(), index);
 }

 /** This is the const version of diagonal(Index). */
 template<typename Derived>
-inline typename MatrixBase<Derived>::template ConstDiagonalIndexReturnType<DynamicIndex>::Type
+inline typename MatrixBase<Derived>::ConstDiagonalDynamicIndexReturnType
 MatrixBase<Derived>::diagonal(Index index) const
 {
-  return typename ConstDiagonalIndexReturnType<DynamicIndex>::Type(derived(), index);
+  return ConstDiagonalDynamicIndexReturnType(derived(), index);
 }

 /** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
@ -232,7 +232,7 @@ EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest&
  // FIXME not very good if rhs is real and lhs complex while alpha is real too
  const Index cols = dest.cols();
  for (Index j=0; j<cols; ++j)
-    func(dest.col(j), prod.rhs().coeff(j) * prod.lhs());
+    func(dest.col(j), prod.rhs().coeff(0,j) * prod.lhs());
 }

 // Row major
@ -243,7 +243,7 @@ EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest&
  // FIXME not very good if lhs is real and rhs complex while alpha is real too
  const Index rows = dest.rows();
  for (Index i=0; i<rows; ++i)
-    func(dest.row(i), prod.lhs().coeff(i) * prod.rhs());
+    func(dest.row(i), prod.lhs().coeff(i,0) * prod.rhs());
 }

 template<typename Lhs, typename Rhs>
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
@ -168,6 +168,7 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
 template<typename Derived> class MapBase<Derived, WriteAccessors>
  : public MapBase<Derived, ReadOnlyAccessors>
 {
+    typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
  public:

    typedef MapBase<Derived, ReadOnlyAccessors> Base;
@ -230,11 +231,13 @@ template<typename Derived> class MapBase<Derived, WriteAccessors>

    Derived& operator=(const MapBase& other)
    {
-      Base::Base::operator=(other);
+      ReadOnlyMapBase::Base::operator=(other);
      return derived();
    }

-    using Base::Base::operator=;
+    // In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base,
+    // see bugs 821 and 920.
+    using ReadOnlyMapBase::Base::operator=;
 };

 #undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
@ -224,15 +224,11 @@ template<typename Derived> class MatrixBase
    template<int Index> typename DiagonalIndexReturnType<Index>::Type diagonal();
    template<int Index> typename ConstDiagonalIndexReturnType<Index>::Type diagonal() const;
    
-    // Note: The "MatrixBase::" prefixes are added to help MSVC9 to match these declarations with the later implementations.
-    // On the other hand they confuse MSVC8...
-    #if (defined _MSC_VER) && (_MSC_VER >= 1500) // 2008 or later
-    typename MatrixBase::template DiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index);
-    typename MatrixBase::template ConstDiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index) const;
-    #else
-    typename DiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index);
-    typename ConstDiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index) const;
-    #endif
+    typedef Diagonal<Derived,DynamicIndex> DiagonalDynamicIndexReturnType;
+    typedef typename internal::add_const<Diagonal<const Derived,DynamicIndex> >::type ConstDiagonalDynamicIndexReturnType;
+
+    DiagonalDynamicIndexReturnType diagonal(Index index);
+    ConstDiagonalDynamicIndexReturnType diagonal(Index index) const;

    #ifdef EIGEN2_SUPPORT
    template<unsigned int Mode> typename internal::eigen2_part_return_type<Derived, Mode>::type part();
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h
@ -555,7 +555,10 @@ struct permut_matrix_product_retval
      const Index n = Side==OnTheLeft ? rows() : cols();
      // FIXME we need an is_same for expression that is not sensitive to constness. For instance
      // is_same_xpr<Block<const Matrix>, Block<Matrix> >::value should be true.
-      if(is_same<MatrixTypeNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_matrix))
+      if(    is_same<MatrixTypeNestedCleaned,Dest>::value
+          && blas_traits<MatrixTypeNestedCleaned>::HasUsableDirectAccess
+          && blas_traits<Dest>::HasUsableDirectAccess
+          && extract_data(dst) == extract_data(m_matrix))
      {
        // apply the permutation inplace
        Matrix<bool,PermutationType::RowsAtCompileTime,1,0,PermutationType::MaxRowsAtCompileTime> mask(m_permutation.size());
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h
@ -85,7 +85,14 @@ class ProductBase : public MatrixBase<Derived>

  public:

+#ifndef EIGEN_NO_MALLOC
+    typedef typename Base::PlainObject BasePlainObject;
+    typedef Matrix<Scalar,RowsAtCompileTime==1?1:Dynamic,ColsAtCompileTime==1?1:Dynamic,BasePlainObject::Options> DynPlainObject;
+    typedef typename internal::conditional<(BasePlainObject::SizeAtCompileTime==Dynamic) || (BasePlainObject::SizeAtCompileTime*int(sizeof(Scalar)) < int(EIGEN_STACK_ALLOCATION_LIMIT)),
+                                           BasePlainObject, DynPlainObject>::type PlainObject;
+#else
    typedef typename Base::PlainObject PlainObject;
+#endif

    ProductBase(const Lhs& a_lhs, const Rhs& a_rhs)
      : m_lhs(a_lhs), m_rhs(a_rhs)
@ -180,7 +187,12 @@ namespace internal {
 template<typename Lhs, typename Rhs, int Mode, int N, typename PlainObject>
 struct nested<GeneralProduct<Lhs,Rhs,Mode>, N, PlainObject>
 {
-  typedef PlainObject const& type;
+  typedef typename GeneralProduct<Lhs,Rhs,Mode>::PlainObject const& type;
+};
+template<typename Lhs, typename Rhs, int Mode, int N, typename PlainObject>
+struct nested<const GeneralProduct<Lhs,Rhs,Mode>, N, PlainObject>
+{
+  typedef typename GeneralProduct<Lhs,Rhs,Mode>::PlainObject const& type;
 };
 }

--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h
@ -188,6 +188,8 @@ template<typename PlainObjectType, int Options, typename StrideType> class Ref
  : public RefBase<Ref<PlainObjectType, Options, StrideType> >
 {
    typedef internal::traits<Ref> Traits;
+    template<typename Derived>
+    inline Ref(const PlainObjectBase<Derived>& expr);
  public:

    typedef RefBase<Ref> Base;
@ -196,20 +198,21 @@ template<typename PlainObjectType, int Options, typename StrideType> class Ref

    #ifndef EIGEN_PARSED_BY_DOXYGEN
    template<typename Derived>
-    inline Ref(PlainObjectBase<Derived>& expr,
-               typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0)
+    inline Ref(PlainObjectBase<Derived>& expr)
    {
-      Base::construct(expr);
+      EIGEN_STATIC_ASSERT(static_cast<bool>(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+      Base::construct(expr.derived());
    }
    template<typename Derived>
-    inline Ref(const DenseBase<Derived>& expr,
-               typename internal::enable_if<bool(internal::is_lvalue<Derived>::value&&bool(Traits::template match<Derived>::MatchAtCompileTime)),Derived>::type* = 0,
-               int = Derived::ThisConstantIsPrivateInPlainObjectBase)
+    inline Ref(const DenseBase<Derived>& expr)
    #else
    template<typename Derived>
    inline Ref(DenseBase<Derived>& expr)
    #endif
    {
+      EIGEN_STATIC_ASSERT(static_cast<bool>(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
+      EIGEN_STATIC_ASSERT(static_cast<bool>(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
+      enum { THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY = Derived::ThisConstantIsPrivateInPlainObjectBase};
      Base::construct(expr.const_cast_derived());
    }

--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h
@ -135,7 +135,7 @@ template<typename MatrixType,int RowFactor,int ColFactor> class Replicate
  */
 template<typename Derived>
 template<int RowFactor, int ColFactor>
-inline const Replicate<Derived,RowFactor,ColFactor>
+const Replicate<Derived,RowFactor,ColFactor>
 DenseBase<Derived>::replicate() const
 {
  return Replicate<Derived,RowFactor,ColFactor>(derived());
@ -150,7 +150,7 @@ DenseBase<Derived>::replicate() const
  * \sa VectorwiseOp::replicate(), DenseBase::replicate<int,int>(), class Replicate
  */
 template<typename Derived>
-inline const Replicate<Derived,Dynamic,Dynamic>
+const typename DenseBase<Derived>::ReplicateReturnType
 DenseBase<Derived>::replicate(Index rowFactor,Index colFactor) const
 {
  return Replicate<Derived,Dynamic,Dynamic>(derived(),rowFactor,colFactor);
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h
@ -380,19 +380,19 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
    EIGEN_STRONG_INLINE TriangularView& operator=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
    {
      setZero();
-      return assignProduct(other,1);
+      return assignProduct(other.derived(),1);
    }
    
    template<typename ProductDerived, typename Lhs, typename Rhs>
    EIGEN_STRONG_INLINE TriangularView& operator+=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
    {
-      return assignProduct(other,1);
+      return assignProduct(other.derived(),1);
    }
    
    template<typename ProductDerived, typename Lhs, typename Rhs>
    EIGEN_STRONG_INLINE TriangularView& operator-=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
    {
-      return assignProduct(other,-1);
+      return assignProduct(other.derived(),-1);
    }
    
    
@ -400,19 +400,19 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
    EIGEN_STRONG_INLINE TriangularView& operator=(const ScaledProduct<ProductDerived>& other)
    {
      setZero();
-      return assignProduct(other,other.alpha());
+      return assignProduct(other.derived(),other.alpha());
    }
    
    template<typename ProductDerived>
    EIGEN_STRONG_INLINE TriangularView& operator+=(const ScaledProduct<ProductDerived>& other)
    {
-      return assignProduct(other,other.alpha());
+      return assignProduct(other.derived(),other.alpha());
    }
    
    template<typename ProductDerived>
    EIGEN_STRONG_INLINE TriangularView& operator-=(const ScaledProduct<ProductDerived>& other)
    {
-      return assignProduct(other,-other.alpha());
+      return assignProduct(other.derived(),-other.alpha());
    }
    
  protected:
@ -420,6 +420,15 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
    template<typename ProductDerived, typename Lhs, typename Rhs>
    EIGEN_STRONG_INLINE TriangularView& assignProduct(const ProductBase<ProductDerived, Lhs,Rhs>& prod, const Scalar& alpha);
    
+    template<int Mode, bool LhsIsTriangular,
+         typename Lhs, bool LhsIsVector,
+         typename Rhs, bool RhsIsVector>
+    EIGEN_STRONG_INLINE TriangularView& assignProduct(const TriangularProduct<Mode, LhsIsTriangular, Lhs, LhsIsVector, Rhs, RhsIsVector>& prod, const Scalar& alpha)
+    {
+      lazyAssign(alpha*prod.eval());
+      return *this;
+    }
+
    MatrixTypeNested m_matrix;
 };

--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
@ -110,7 +110,7 @@ template<> EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<
 template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> *   to, const Packet2cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); }
 template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> *   to, const Packet2cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); }

-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { __pld((float *)addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { EIGEN_ARM_PREFETCH((float *)addr); }

 template<> EIGEN_STRONG_INLINE std::complex<float>  pfirst<Packet2cf>(const Packet2cf& a)
 {
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
@ -49,8 +49,17 @@ typedef uint32x4_t  Packet4ui;
  #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {X, Y, Z, W}
 #endif

-#ifndef __pld
-#define __pld(x) asm volatile ( "   pld [%[addr]]\n" :: [addr] "r" (x) : "cc" );
+// arm64 does have the pld instruction. If available, let's trust the __builtin_prefetch built-in function
+// which available on LLVM and GCC (at least)
+#if EIGEN_HAS_BUILTIN(__builtin_prefetch) || defined(__GNUC__)
+  #define EIGEN_ARM_PREFETCH(ADDR) __builtin_prefetch(ADDR);
+#elif defined __pld
+  #define EIGEN_ARM_PREFETCH(ADDR) __pld(ADDR)
+#elif !defined(__aarch64__)
+  #define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__ ( "   pld [%[addr]]\n" :: [addr] "r" (ADDR) : "cc" );
+#else
+  // by default no explicit prefetching
+  #define EIGEN_ARM_PREFETCH(ADDR)
 #endif

 template<> struct packet_traits<float>  : default_packet_traits
@ -209,8 +218,8 @@ template<> EIGEN_STRONG_INLINE void pstore<int>(int*       to, const Packet4i& f
 template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*  to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from); }
 template<> EIGEN_STRONG_INLINE void pstoreu<int>(int*      to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from); }

-template<> EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) { __pld(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*     addr) { __pld(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*     addr) { EIGEN_ARM_PREFETCH(addr); }

 // FIXME only store the 2 first elements ?
 template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { float EIGEN_ALIGN16 x[4]; vst1q_f32(x, a); return x[0]; }
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h
@ -52,7 +52,7 @@ Packet4f plog<Packet4f>(const Packet4f& _x)

  Packet4i emm0;

-  Packet4f invalid_mask = _mm_cmplt_ps(x, _mm_setzero_ps());
+  Packet4f invalid_mask = _mm_cmpnge_ps(x, _mm_setzero_ps()); // not greater equal is true if x is NaN
  Packet4f iszero_mask = _mm_cmpeq_ps(x, _mm_setzero_ps());

  x = pmax(x, p4f_min_norm_pos);  /* cut off denormalized stuff */
@ -166,7 +166,7 @@ Packet4f pexp<Packet4f>(const Packet4f& _x)
  emm0 = _mm_cvttps_epi32(fx);
  emm0 = _mm_add_epi32(emm0, p4i_0x7f);
  emm0 = _mm_slli_epi32(emm0, 23);
-  return pmul(y, _mm_castsi128_ps(emm0));
+  return pmax(pmul(y, Packet4f(_mm_castsi128_ps(emm0))), _x);
 }
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet2d pexp<Packet2d>(const Packet2d& _x)
@ -239,7 +239,7 @@ Packet2d pexp<Packet2d>(const Packet2d& _x)
  emm0 = _mm_add_epi32(emm0, p4i_1023_0);
  emm0 = _mm_slli_epi32(emm0, 20);
  emm0 = _mm_shuffle_epi32(emm0, _MM_SHUFFLE(1,2,0,3));
-  return pmul(x, _mm_castsi128_pd(emm0));
+  return pmax(pmul(x, Packet2d(_mm_castsi128_pd(emm0))), _x);
 }

 /* evaluation of 4 sines at onces, using SSE2 intrinsics.
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h
@ -90,6 +90,7 @@ struct traits<CoeffBasedProduct<LhsNested,RhsNested,NestingFlags> >
            | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0),

      CoeffReadCost = InnerSize == Dynamic ? Dynamic
+                    : InnerSize == 0 ? 0
                    : InnerSize * (NumTraits<Scalar>::MulCost + LhsCoeffReadCost + RhsCoeffReadCost)
                      + (InnerSize - 1) * NumTraits<Scalar>::AddCost,

@ -133,7 +134,7 @@ class CoeffBasedProduct
    };

    typedef internal::product_coeff_impl<CanVectorizeInner ? InnerVectorizedTraversal : DefaultTraversal,
-                                   Unroll ? InnerSize-1 : Dynamic,
+                                   Unroll ? (InnerSize==0 ? 0 : InnerSize-1) : Dynamic,
                                   _LhsNested, _RhsNested, Scalar> ScalarCoeffImpl;

    typedef CoeffBasedProduct<LhsNested,RhsNested,NestByRefBit> LazyCoeffBasedProductType;
@ -184,7 +185,7 @@ class CoeffBasedProduct
    {
      PacketScalar res;
      internal::product_packet_impl<Flags&RowMajorBit ? RowMajor : ColMajor,
-                              Unroll ? InnerSize-1 : Dynamic,
+                              Unroll ? (InnerSize==0 ? 0 : InnerSize-1) : Dynamic,
                              _LhsNested, _RhsNested, PacketScalar, LoadMode>
        ::run(row, col, m_lhs, m_rhs, res);
      return res;
@ -262,10 +263,7 @@ struct product_coeff_impl<DefaultTraversal, Dynamic, Lhs, Rhs, RetScalar>
  typedef typename Lhs::Index Index;
  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar& res)
  {
-    eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix");
-    res = lhs.coeff(row, 0) * rhs.coeff(0, col);
-      for(Index i = 1; i < lhs.cols(); ++i)
-        res += lhs.coeff(row, i) * rhs.coeff(i, col);
+    res = (lhs.row(row).transpose().cwiseProduct( rhs.col(col) )).sum();
  }
 };

--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
@ -13,7 +13,7 @@

 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 2
-#define EIGEN_MINOR_VERSION 2
+#define EIGEN_MINOR_VERSION 4

 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                      (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@ -96,6 +96,13 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t
 #endif

+// Cross compiler wrapper around LLVM's __has_builtin
+#ifdef __has_builtin
+#  define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
+#else
+#  define EIGEN_HAS_BUILTIN(x) 0
+#endif
+
 /** Allows to disable some optimizations which might affect the accuracy of the result.
  * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
  * They currently include:
@ -247,7 +254,7 @@ namespace Eigen {

 #if !defined(EIGEN_ASM_COMMENT)
  #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) )
-    #define EIGEN_ASM_COMMENT(X)  asm("#" X)
+    #define EIGEN_ASM_COMMENT(X)  __asm__("#" X)
  #else
    #define EIGEN_ASM_COMMENT(X)
  #endif
@ -306,7 +313,7 @@ namespace Eigen {
 // just an empty macro !
 #define EIGEN_EMPTY

-#if defined(_MSC_VER) && (!defined(__INTEL_COMPILER))
+#if defined(_MSC_VER) && (_MSC_VER < 1900) && (!defined(__INTEL_COMPILER))
 #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
  using Base::operator =;
 #elif defined(__clang__) // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h
@ -63,7 +63,7 @@
 // Currently, let's include it only on unix systems:
 #if defined(__unix__) || defined(__unix)
  #include <unistd.h>
-  #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0)
+  #if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || (defined __PGI) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0)
    #define EIGEN_HAS_POSIX_MEMALIGN 1
  #endif
 #endif
@ -417,6 +417,8 @@ template<typename T, bool Align> inline T* conditional_aligned_realloc_new(T* pt

 template<typename T, bool Align> inline T* conditional_aligned_new_auto(size_t size)
 {
+  if(size==0)
+    return 0; // short-cut. Also fixes Bug 884
  check_size_for_overflow<T>(size);
  T *result = reinterpret_cast<T*>(conditional_aligned_malloc<Align>(sizeof(T)*size));
  if(NumTraits<T>::RequireInitialization)
@ -464,9 +466,8 @@ template<typename T, bool Align> inline void conditional_aligned_delete_auto(T *
 template<typename Scalar, typename Index>
 static inline Index first_aligned(const Scalar* array, Index size)
 {
-  enum { PacketSize = packet_traits<Scalar>::size,
-         PacketAlignedMask = PacketSize-1
-  };
+  static const Index PacketSize = packet_traits<Scalar>::size;
+  static const Index PacketAlignedMask = PacketSize-1;

  if(PacketSize==1)
  {
@ -612,7 +613,6 @@ template<typename T> class aligned_stack_memory_handler
      void* operator new(size_t size, const std::nothrow_t&) throw() { \
        try { return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); } \
        catch (...) { return 0; } \
-        return 0; \
      }
  #else
    #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h
@ -90,7 +90,9 @@
        YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED,
        THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE,
        THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH,
-        OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG
+        OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG,
+        IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY,
+        STORAGE_LAYOUT_DOES_NOT_MATCH
      };
    };

--- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h
@ -341,7 +341,7 @@ template<typename T, int n=1, typename PlainObject = typename eval<T>::type> str
 };

 template<typename T>
-T* const_cast_ptr(const T* ptr)
+inline T* const_cast_ptr(const T* ptr)
 {
  return const_cast<T*>(ptr);
 }
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h
@ -147,7 +147,6 @@ void fitHyperplane(int numPoints,

  // compute the covariance matrix
  CovMatrixType covMat = CovMatrixType::Zero(size, size);
-  VectorType remean = VectorType::Zero(size);
  for(int i = 0; i < numPoints; ++i)
  {
    VectorType diff = (*(points[i]) - mean).conjugate();
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h
@ -313,7 +313,7 @@ namespace Eigen {
      using std::abs;
      using std::sqrt;
      const Index dim=m_S.cols();
-      if (abs(m_S.coeff(i+1,i)==Scalar(0)))
+      if (abs(m_S.coeff(i+1,i))==Scalar(0))
        return;
      Index z = findSmallDiagEntry(i,i+1);
      if (z==i-1)
--- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
@ -563,7 +563,6 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
    if(computeEigenvectors)
    {
      Scalar safeNorm2 = Eigen::NumTraits<Scalar>::epsilon();
-      safeNorm2 *= safeNorm2;
      if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon())
      {
        eivecs.setIdentity();
@ -577,7 +576,7 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
        Scalar d0 = eivals(2) - eivals(1);
        Scalar d1 = eivals(1) - eivals(0);
        int k =  d0 > d1 ? 2 : 0;
-        d0 = d0 > d1 ? d1 : d0;
+        d0 = d0 > d1 ? d0 : d1;

        tmp.diagonal().array () -= eivals(k);
        VectorType cross;
@ -585,19 +584,25 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
        n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm();

        if(n>safeNorm2)
+        {
          eivecs.col(k) = cross / sqrt(n);
+        }
        else
        {
          n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm();

          if(n>safeNorm2)
+          {
            eivecs.col(k) = cross / sqrt(n);
+          }
          else
          {
            n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm();

            if(n>safeNorm2)
+            {
              eivecs.col(k) = cross / sqrt(n);
+            }
            else
            {
              // the input matrix and/or the eigenvaues probably contains some inf/NaN,
@ -617,12 +622,16 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
        tmp.diagonal().array() -= eivals(1);

        if(d0<=Eigen::NumTraits<Scalar>::epsilon())
+        {
          eivecs.col(1) = eivecs.col(k).unitOrthogonal();
+        }
        else
        {
-          n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm();
+          n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm();
          if(n>safeNorm2)
+          {
            eivecs.col(1) = cross / sqrt(n);
+          }
          else
          {
            n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm();
@ -636,13 +645,14 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
              else
              {
                // we should never reach this point,
-                // if so the last two eigenvalues are likely to ve very closed to each other
+                // if so the last two eigenvalues are likely to be very close to each other
                eivecs.col(1) = eivecs.col(k).unitOrthogonal();
              }
            }
          }

          // make sure that eivecs[1] is orthogonal to eivecs[2]
+          // FIXME: this step should not be needed
          Scalar d = eivecs.col(1).dot(eivecs.col(k));
          eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized();
        }
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h
@ -100,7 +100,17 @@ public:
  {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
    Hyperplane result(p0.size());
-    result.normal() = (p2 - p0).cross(p1 - p0).normalized();
+    VectorType v0(p2 - p0), v1(p1 - p0);
+    result.normal() = v0.cross(v1);
+    RealScalar norm = result.normal().norm();
+    if(norm <= v0.norm() * v1.norm() * NumTraits<RealScalar>::epsilon())
+    {
+      Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
+      JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
+      result.normal() = svd.matrixV().col(2);
+    }
+    else
+      result.normal() /= norm;
    result.offset() = -p0.dot(result.normal());
    return result;
  }
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h
@ -61,6 +61,9 @@ public:
  /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */
  inline Rotation2D(const Scalar& a) : m_angle(a) {}
  
+  /** Default constructor wihtout initialization. The represented rotation is undefined. */
+  Rotation2D() {}
+
  /** \returns the rotation angle */
  inline Scalar angle() const { return m_angle; }

@ -84,7 +87,7 @@ public:
  
  template<typename Derived>
  Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
-  Matrix2 toRotationMatrix(void) const;
+  Matrix2 toRotationMatrix() const;

  /** \returns the spherical interpolation between \c *this and \a other using
    * parameter \a t. It is in fact equivalent to a linear interpolation.
--- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h
@ -62,6 +62,8 @@ struct transform_construct_from_matrix;

 template<typename TransformType> struct transform_take_affine_part;

+template<int Mode> struct transform_make_affine;
+
 } // end namespace internal

 /** \geometry_module \ingroup Geometry_Module
@ -230,8 +232,7 @@ public:
  inline Transform()
  {
    check_template_params();
-    if (int(Mode)==Affine)
-      makeAffine();
+    internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix);
  }

  inline Transform(const Transform& other)
@ -591,11 +592,7 @@ public:
    */
  void makeAffine()
  {
-    if(int(Mode)!=int(AffineCompact))
-    {
-      matrix().template block<1,Dim>(Dim,0).setZero();
-      matrix().coeffRef(Dim,Dim) = Scalar(1);
-    }
+    internal::transform_make_affine<int(Mode)>::run(m_matrix);
  }

  /** \internal
@ -1079,6 +1076,24 @@ Transform<Scalar,Dim,Mode,Options>::fromPositionOrientationScale(const MatrixBas

 namespace internal {

+template<int Mode>
+struct transform_make_affine
+{
+  template<typename MatrixType>
+  static void run(MatrixType &mat)
+  {
+    static const int Dim = MatrixType::ColsAtCompileTime-1;
+    mat.template block<1,Dim>(Dim,0).setZero();
+    mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1);
+  }
+};
+
+template<>
+struct transform_make_affine<AffineCompact>
+{
+  template<typename MatrixType> static void run(MatrixType &) { }
+};
+    
 // selector needed to avoid taking the inverse of a 3x4 matrix
 template<typename TransformType, int Mode=TransformType::Mode>
 struct projective_transform_inverse
--- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
@ -39,7 +39,6 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
  int maxIters = iters;

  int n = mat.cols();
-  x = precond.solve(x);
  VectorType r  = rhs - mat * x;
  VectorType r0 = r;
  
@ -143,7 +142,7 @@ struct traits<BiCGSTAB<_MatrixType,_Preconditioner> >
  * SparseMatrix<double> A(n,n);
  * // fill A and b
  * BiCGSTAB<SparseMatrix<double> > solver;
-  * solver(A);
+  * solver.compute(A);
  * x = solver.solve(b);
  * std::cout << "#iterations:     " << solver.iterations() << std::endl;
  * std::cout << "estimated error: " << solver.error()      << std::endl;
--- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h
@ -219,7 +219,7 @@ class PardisoImpl
    void pardisoInit(int type)
    {
      m_type = type;
-      bool symmetric = abs(m_type) < 10;
+      bool symmetric = std::abs(m_type) < 10;
      m_iparm[0] = 1;   // No solver default
      m_iparm[1] = 3;   // use Metis for the ordering
      m_iparm[2] = 1;   // Numbers of processors, value of OMP_NUM_THREADS
--- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h
@ -762,6 +762,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD

    internal::qr_preconditioner_impl<MatrixType, QRPreconditioner, internal::PreconditionIfMoreColsThanRows> m_qr_precond_morecols;
    internal::qr_preconditioner_impl<MatrixType, QRPreconditioner, internal::PreconditionIfMoreRowsThanCols> m_qr_precond_morerows;
+    MatrixType m_scaledMatrix;
 };

 template<typename MatrixType, int QRPreconditioner>
@ -810,6 +811,7 @@ void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Index rows, Index cols, u
  
  if(m_cols>m_rows)   m_qr_precond_morecols.allocate(*this);
  if(m_rows>m_cols)   m_qr_precond_morerows.allocate(*this);
+  if(m_cols!=m_cols)  m_scaledMatrix.resize(rows,cols);
 }

 template<typename MatrixType, int QRPreconditioner>
@ -826,22 +828,27 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
  // limit for very small denormal numbers to be considered zero in order to avoid infinite loops (see bug 286)
  const RealScalar considerAsZero = RealScalar(2) * std::numeric_limits<RealScalar>::denorm_min();

+  // Scaling factor to reduce over/under-flows
+  RealScalar scale = matrix.cwiseAbs().maxCoeff();
+  if(scale==RealScalar(0)) scale = RealScalar(1);
+  
  /*** step 1. The R-SVD step: we use a QR decomposition to reduce to the case of a square matrix */

-  if(!m_qr_precond_morecols.run(*this, matrix) && !m_qr_precond_morerows.run(*this, matrix))
+  if(m_rows!=m_cols)
  {
-    m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize);
+    m_scaledMatrix = matrix / scale;
+    m_qr_precond_morecols.run(*this, m_scaledMatrix);
+    m_qr_precond_morerows.run(*this, m_scaledMatrix);
+  }
+  else
+  {
+    m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize) / scale;
    if(m_computeFullU) m_matrixU.setIdentity(m_rows,m_rows);
    if(m_computeThinU) m_matrixU.setIdentity(m_rows,m_diagSize);
    if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols);
    if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize);
  }

-  // Scaling factor to reduce over/under-flows
-  RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff();
-  if(scale==RealScalar(0)) scale = RealScalar(1);
-  m_workMatrix /= scale;
-
  /*** step 2. The main Jacobi SVD iteration. ***/

  bool finished = false;
@ -861,7 +868,8 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
        using std::max;
        RealScalar threshold = (max)(considerAsZero, precision * (max)(abs(m_workMatrix.coeff(p,p)),
                                                                       abs(m_workMatrix.coeff(q,q))));
-        if((max)(abs(m_workMatrix.coeff(p,q)),abs(m_workMatrix.coeff(q,p))) > threshold)
+        // We compare both values to threshold instead of calling max to be robust to NaN (See bug 791)
+        if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold)
        {
          finished = false;

--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h
@ -69,7 +69,7 @@ class AmbiVector
      delete[] m_buffer;
      if (size<1000)
      {
-        Index allocSize = (size * sizeof(ListEl))/sizeof(Scalar);
+        Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar);
        m_allocatedElements = (allocSize*sizeof(Scalar))/sizeof(ListEl);
        m_buffer = new Scalar[allocSize];
      }
@ -88,7 +88,7 @@ class AmbiVector
      Index copyElements = m_allocatedElements;
      m_allocatedElements = (std::min)(Index(m_allocatedElements*1.5),m_size);
      Index allocSize = m_allocatedElements * sizeof(ListEl);
-      allocSize = allocSize/sizeof(Scalar) + (allocSize%sizeof(Scalar)>0?1:0);
+      allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar);
      Scalar* newBuffer = new Scalar[allocSize];
      memcpy(newBuffer,  m_buffer,  copyElements * sizeof(ListEl));
      delete[] m_buffer;
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h
@ -68,6 +68,8 @@ public:
    const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;

    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
+  private:
+    Index nonZeros() const;
 };


@ -82,6 +84,7 @@ class BlockImpl<SparseMatrix<_Scalar, _Options, _Index>,BlockRows,BlockCols,true
    typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType;
    typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _MatrixTypeNested;
    typedef Block<SparseMatrixType, BlockRows, BlockCols, true> BlockType;
+    typedef Block<const SparseMatrixType, BlockRows, BlockCols, true> ConstBlockType;
 public:
    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
@ -245,6 +248,93 @@ public:

 };

+
+template<typename _Scalar, int _Options, typename _Index, int BlockRows, int BlockCols>
+class BlockImpl<const SparseMatrix<_Scalar, _Options, _Index>,BlockRows,BlockCols,true,Sparse>
+  : public SparseMatrixBase<Block<const SparseMatrix<_Scalar, _Options, _Index>,BlockRows,BlockCols,true> >
+{
+    typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType;
+    typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _MatrixTypeNested;
+    typedef Block<const SparseMatrixType, BlockRows, BlockCols, true> BlockType;
+public:
+    enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
+    EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
+protected:
+    enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
+public:
+    
+    class InnerIterator: public SparseMatrixType::InnerIterator
+    {
+      public:
+        inline InnerIterator(const BlockType& xpr, Index outer)
+          : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer)
+        {}
+        inline Index row() const { return IsRowMajor ? m_outer : this->index(); }
+        inline Index col() const { return IsRowMajor ? this->index() : m_outer; }
+      protected:
+        Index m_outer;
+    };
+    class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator
+    {
+      public:
+        inline ReverseInnerIterator(const BlockType& xpr, Index outer)
+          : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer)
+        {}
+        inline Index row() const { return IsRowMajor ? m_outer : this->index(); }
+        inline Index col() const { return IsRowMajor ? this->index() : m_outer; }
+      protected:
+        Index m_outer;
+    };
+
+    inline BlockImpl(const SparseMatrixType& xpr, int i)
+      : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize)
+    {}
+
+    inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols)
+      : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols)
+    {}
+
+    inline const Scalar* valuePtr() const
+    { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; }
+
+    inline const Index* innerIndexPtr() const
+    { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; }
+
+    inline const Index* outerIndexPtr() const
+    { return m_matrix.outerIndexPtr() + m_outerStart; }
+
+    Index nonZeros() const
+    {
+      if(m_matrix.isCompressed())
+        return  std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()])
+              - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]);
+      else if(m_outerSize.value()==0)
+        return 0;
+      else
+        return Map<const Matrix<Index,OuterSize,1> >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum();
+    }
+
+    const Scalar& lastCoeff() const
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl);
+      eigen_assert(nonZeros()>0);
+      if(m_matrix.isCompressed())
+        return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1];
+      else
+        return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1];
+    }
+
+    EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+  protected:
+
+    typename SparseMatrixType::Nested m_matrix;
+    Index m_outerStart;
+    const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
+
+};
+
 //----------

 /** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h
@ -306,15 +306,6 @@ class DenseTimeSparseProduct
    DenseTimeSparseProduct& operator=(const DenseTimeSparseProduct&);
 };

-// sparse * dense
-template<typename Derived>
-template<typename OtherDerived>
-inline const typename SparseDenseProductReturnType<Derived,OtherDerived>::Type
-SparseMatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
-{
-  return typename SparseDenseProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived());
-}
-
 } // end namespace Eigen

 #endif // EIGEN_SPARSEDENSEPRODUCT_H
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h
@ -358,7 +358,8 @@ template<typename Derived> class SparseMatrixBase : public EigenBase<Derived>
    /** sparse * dense (returns a dense object unless it is an outer product) */
    template<typename OtherDerived>
    const typename SparseDenseProductReturnType<Derived,OtherDerived>::Type
-    operator*(const MatrixBase<OtherDerived> &other) const;
+    operator*(const MatrixBase<OtherDerived> &other) const
+    { return typename SparseDenseProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived()); }
    
     /** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */
    SparseSymmetricPermutationProduct<Derived,Upper|Lower> twistedBy(const PermutationMatrix<Dynamic,Dynamic,Index>& perm) const
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h
@ -61,7 +61,7 @@ struct permut_sparsematrix_product_retval
        for(Index j=0; j<m_matrix.outerSize(); ++j)
        {
          Index jp = m_permutation.indices().coeff(j);
-          sizes[((Side==OnTheLeft) ^ Transposed) ? jp : j] = m_matrix.innerVector(((Side==OnTheRight) ^ Transposed) ? jp : j).size();
+          sizes[((Side==OnTheLeft) ^ Transposed) ? jp : j] = m_matrix.innerVector(((Side==OnTheRight) ^ Transposed) ? jp : j).nonZeros();
        }
        tmp.reserve(sizes);
        for(Index j=0; j<m_matrix.outerSize(); ++j)
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h
@ -260,14 +260,13 @@ class SparseLU : public internal::SparseLUImpl<typename _MatrixType::Scalar, typ
      eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
      // Initialize with the determinant of the row matrix
      Scalar det = Scalar(1.);
-      //Note that the diagonal blocks of U are stored in supernodes,
+      // Note that the diagonal blocks of U are stored in supernodes,
      // which are available in the  L part :)
      for (Index j = 0; j < this->cols(); ++j)
      {
        for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
        {
-          if(it.row() < j) continue;
-          if(it.row() == j)
+          if(it.index() == j)
          {
            det *= (std::abs)(it.value());
            break;
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
@ -189,8 +189,8 @@ class MappedSuperNodalMatrix<Scalar,Index>::InnerIterator
        m_idval(mat.colIndexPtr()[outer]),
        m_startidval(m_idval),
        m_endidval(mat.colIndexPtr()[outer+1]),
-        m_idrow(mat.rowIndexPtr()[outer]),
-        m_endidrow(mat.rowIndexPtr()[outer+1])
+        m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]),
+        m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1])
    {}
    inline InnerIterator& operator++()
    { 
--- a/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h
@ -75,7 +75,7 @@ class SparseQR
    typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
    typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
  public:
-    SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
+    SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
    { }
    
    /** Construct a QR factorization of the matrix \a mat.
@ -84,7 +84,7 @@ class SparseQR
      * 
      * \sa compute()
      */
-    SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
+    SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
    {
      compute(mat);
    }
@ -262,6 +262,7 @@ class SparseQR
    IndexVector m_etree;            // Column elimination tree
    IndexVector m_firstRowElt;      // First element in each row
    bool m_isQSorted;               // whether Q is sorted or not
+    bool m_isEtreeOk;               // whether the elimination tree match the initial input matrix
    
    template <typename, typename > friend struct SparseQR_QProduct;
    template <typename > friend struct SparseQRMatrixQReturnType;
@ -281,9 +282,11 @@ template <typename MatrixType, typename OrderingType>
 void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
 {
  eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
+  // Copy to a column major matrix if the input is rowmajor
+  typename internal::conditional<MatrixType::IsRowMajor,QRMatrixType,const MatrixType&>::type matCpy(mat);
  // Compute the column fill reducing ordering
  OrderingType ord; 
-  ord(mat, m_perm_c); 
+  ord(matCpy, m_perm_c); 
  Index n = mat.cols();
  Index m = mat.rows();
  Index diagSize = (std::min)(m,n);
@ -296,7 +299,8 @@ void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
  
  // Compute the column elimination tree of the permuted matrix
  m_outputPerm_c = m_perm_c.inverse();
-  internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
+  internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
+  m_isEtreeOk = true;
  
  m_R.resize(m, n);
  m_Q.resize(m, diagSize);
@ -331,14 +335,37 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
  ScalarVector tval(m);                                     // The dense vector used to compute the current column
  RealScalar pivotThreshold = m_threshold;
  
+  m_R.setZero();
+  m_Q.setZero();
  m_pmat = mat;
+  if(!m_isEtreeOk)
+  {
+    m_outputPerm_c = m_perm_c.inverse();
+    internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
+    m_isEtreeOk = true;
+  }
+
  m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
+  
  // Apply the fill-in reducing permutation lazily:
+  {
+    // If the input is row major, copy the original column indices,
+    // otherwise directly use the input matrix
+    // 
+    IndexVector originalOuterIndicesCpy;
+    const Index *originalOuterIndices = mat.outerIndexPtr();
+    if(MatrixType::IsRowMajor)
+    {
+      originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1);
+      originalOuterIndices = originalOuterIndicesCpy.data();
+    }
+    
    for (int i = 0; i < n; i++)
    {
      Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i;
-    m_pmat.outerIndexPtr()[p] = mat.outerIndexPtr()[i]; 
-    m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i]; 
+      m_pmat.outerIndexPtr()[p] = originalOuterIndices[i]; 
+      m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i]; 
+    }
  }
  
  /* Compute the default threshold as in MatLab, see:
@ -349,6 +376,8 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
  {
    RealScalar max2Norm = 0.0;
    for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm());
+    if(max2Norm==RealScalar(0))
+      max2Norm = RealScalar(1);
    pivotThreshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
  }
  
@ -373,7 +402,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
    // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k.
    // Note: if the diagonal entry does not exist, then its contribution must be explicitly added,
    // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
-    for (typename MatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
+    for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
    {
      Index curIdx = nonzeroCol;
      if(itp) curIdx = itp.row();
@ -447,7 +476,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
      }
    } // End update current column
    
-    Scalar tau;
+    Scalar tau = 0;
    RealScalar beta = 0;
    
    if(nonzeroCol < diagSize)
@ -461,7 +490,6 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
      for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
      if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
      {
-        tau = RealScalar(0);
        beta = numext::real(c0);
        tval(Qidx(0)) = 1;
      }
@ -514,6 +542,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
      
      // Recompute the column elimination tree
      internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data());
+      m_isEtreeOk = false;
    }
  }
  
@ -531,7 +560,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
  if(nonzeroCol<n)
  {
    // Permute the triangular factor to put the 'dead' columns to the end
-    MatrixType tempR(m_R);
+    QRMatrixType tempR(m_R);
    m_R = tempR * m_pivotperm;
    
    // Update the column permutation
--- a/gtsam/3rdparty/Eigen/Eigen/src/UmfPackSupport/UmfPackSupport.h
+++ b/gtsam/3rdparty/Eigen/Eigen/src/UmfPackSupport/UmfPackSupport.h
@ -107,6 +107,16 @@ inline int umfpack_get_determinant(std::complex<double> *Mx, double *Ex, void *N
  return umfpack_zi_get_determinant(&mx_real,0,Ex,NumericHandle,User_Info);
 }

+namespace internal {
+  template<typename T> struct umfpack_helper_is_sparse_plain : false_type {};
+  template<typename Scalar, int Options, typename StorageIndex>
+  struct umfpack_helper_is_sparse_plain<SparseMatrix<Scalar,Options,StorageIndex> >
+    : true_type {};
+  template<typename Scalar, int Options, typename StorageIndex>
+  struct umfpack_helper_is_sparse_plain<MappedSparseMatrix<Scalar,Options,StorageIndex> >
+    : true_type {};
+}
+
 /** \ingroup UmfPackSupport_Module
  * \brief A sparse LU factorization and solver based on UmfPack
  *
@ -192,10 +202,14 @@ class UmfPackLU : internal::noncopyable
     *  Note that the matrix should be column-major, and in compressed format for best performance.
     *  \sa SparseMatrix::makeCompressed().
     */
-    void compute(const MatrixType& matrix)
+    template<typename InputMatrixType>
+    void compute(const InputMatrixType& matrix)
    {
-      analyzePattern(matrix);
-      factorize(matrix);
+      if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar());
+      if(m_numeric)  umfpack_free_numeric(&m_numeric,Scalar());
+      grapInput(matrix.derived());
+      analyzePattern_impl();
+      factorize_impl();
    }

    /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
@ -230,23 +244,15 @@ class UmfPackLU : internal::noncopyable
      *
      * \sa factorize(), compute()
      */
-    void analyzePattern(const MatrixType& matrix)
+    template<typename InputMatrixType>
+    void analyzePattern(const InputMatrixType& matrix)
    {
-      if(m_symbolic)
-        umfpack_free_symbolic(&m_symbolic,Scalar());
-      if(m_numeric)
-        umfpack_free_numeric(&m_numeric,Scalar());
+      if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar());
+      if(m_numeric)  umfpack_free_numeric(&m_numeric,Scalar());
      
-      grapInput(matrix);
+      grapInput(matrix.derived());

-      int errorCode = 0;
-      errorCode = umfpack_symbolic(matrix.rows(), matrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
-                                   &m_symbolic, 0, 0);
-
-      m_isInitialized = true;
-      m_info = errorCode ? InvalidInput : Success;
-      m_analysisIsOk = true;
-      m_factorizationIsOk = false;
+      analyzePattern_impl();
    }

    /** Performs a numeric decomposition of \a matrix
@ -255,20 +261,16 @@ class UmfPackLU : internal::noncopyable
      *
      * \sa analyzePattern(), compute()
      */
-    void factorize(const MatrixType& matrix)
+    template<typename InputMatrixType>
+    void factorize(const InputMatrixType& matrix)
    {
      eigen_assert(m_analysisIsOk && "UmfPackLU: you must first call analyzePattern()");
      if(m_numeric)
        umfpack_free_numeric(&m_numeric,Scalar());

-      grapInput(matrix);
+      grapInput(matrix.derived());
      
-      int errorCode;
-      errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
-                                  m_symbolic, &m_numeric, 0, 0);
-
-      m_info = errorCode ? NumericalIssue : Success;
-      m_factorizationIsOk = true;
+      factorize_impl();
    }

    #ifndef EIGEN_PARSED_BY_DOXYGEN
@ -283,7 +285,6 @@ class UmfPackLU : internal::noncopyable

  protected:

-
    void init()
    {
      m_info                  = InvalidInput;
@ -293,9 +294,11 @@ class UmfPackLU : internal::noncopyable
      m_outerIndexPtr         = 0;
      m_innerIndexPtr         = 0;
      m_valuePtr              = 0;
+      m_extractedDataAreDirty = true;
    }
    
-    void grapInput(const MatrixType& mat)
+    template<typename InputMatrixType>
+    void grapInput_impl(const InputMatrixType& mat, internal::true_type)
    {
      m_copyMatrix.resize(mat.rows(), mat.cols());
      if( ((MatrixType::Flags&RowMajorBit)==RowMajorBit) || sizeof(typename MatrixType::Index)!=sizeof(int) || !mat.isCompressed() )
@ -314,6 +317,45 @@ class UmfPackLU : internal::noncopyable
      }
    }
    
+    template<typename InputMatrixType>
+    void grapInput_impl(const InputMatrixType& mat, internal::false_type)
+    {
+      m_copyMatrix = mat;
+      m_outerIndexPtr = m_copyMatrix.outerIndexPtr();
+      m_innerIndexPtr = m_copyMatrix.innerIndexPtr();
+      m_valuePtr      = m_copyMatrix.valuePtr();
+    }
+    
+    template<typename InputMatrixType>
+    void grapInput(const InputMatrixType& mat)
+    {
+      grapInput_impl(mat, internal::umfpack_helper_is_sparse_plain<InputMatrixType>());
+    }
+    
+    void analyzePattern_impl()
+    {
+      int errorCode = 0;
+      errorCode = umfpack_symbolic(m_copyMatrix.rows(), m_copyMatrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
+                                   &m_symbolic, 0, 0);
+
+      m_isInitialized = true;
+      m_info = errorCode ? InvalidInput : Success;
+      m_analysisIsOk = true;
+      m_factorizationIsOk = false;
+      m_extractedDataAreDirty = true;
+    }
+    
+    void factorize_impl()
+    {
+      int errorCode;
+      errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
+                                  m_symbolic, &m_numeric, 0, 0);
+
+      m_info = errorCode ? NumericalIssue : Success;
+      m_factorizationIsOk = true;
+      m_extractedDataAreDirty = true;
+    }
+
    // cached data to reduce reallocation, etc.
    mutable LUMatrixType m_l;
    mutable LUMatrixType m_u;
--- a/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake
@ -452,20 +452,12 @@ macro(ei_set_build_string)
 endmacro(ei_set_build_string)

 macro(ei_is_64bit_env VAR)
-
-  file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp"
-      "int main() { return (sizeof(int*) == 8 ? 1 : 0); }
-      ")
-  try_run(run_res compile_res
-         ${CMAKE_CURRENT_BINARY_DIR} "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp"
-          RUN_OUTPUT_VARIABLE run_output)
-
-  if(compile_res AND run_res)
-    set(${VAR} ${run_res})
-  elseif(CMAKE_CL_64)
-    set(${VAR} 1)
-  elseif("$ENV{Platform}" STREQUAL "X64") # nmake 64 bit
+  if(CMAKE_SIZEOF_VOID_P EQUAL 8)
    set(${VAR} 1)
+  elseif(CMAKE_SIZEOF_VOID_P EQUAL 4)
+    set(${VAR} 0)
+  else()
+    message(WARNING "Unsupported pointer size. Please contact the authors.")
  endif()
 endmacro(ei_is_64bit_env)

--- a/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake
@ -86,4 +86,4 @@ include(FindPackageHandleStandardArgs)
 find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
                                  CHOLMOD_INCLUDES CHOLMOD_LIBRARIES)

-mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY)
+mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY CAMD_LIBRARY CCOLAMD_LIBRARY CHOLMOD_METIS_LIBRARY)
--- a/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake
@ -115,5 +115,5 @@ include(FindPackageHandleStandardArgs)
 find_package_handle_standard_args(FFTW DEFAULT_MSG
                                  FFTW_INCLUDES FFTW_LIBRARIES)

-mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES)
+mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES FFTW_LIB FFTWF_LIB FFTWL_LIB)

--- a/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/FindMetis.cmake
@ -11,15 +11,49 @@ find_path(METIS_INCLUDES
  $ENV{METISDIR} 
  ${INCLUDE_INSTALL_DIR} 
  PATH_SUFFIXES
+  .
  metis
  include
 )

+macro(_metis_check_version)
+  file(READ "${METIS_INCLUDES}/metis.h" _metis_version_header)
+
+  string(REGEX MATCH "define[ \t]+METIS_VER_MAJOR[ \t]+([0-9]+)" _metis_major_version_match "${_metis_version_header}")
+  set(METIS_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+METIS_VER_MINOR[ \t]+([0-9]+)" _metis_minor_version_match "${_metis_version_header}")
+  set(METIS_MINOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+METIS_VER_SUBMINOR[ \t]+([0-9]+)" _metis_subminor_version_match "${_metis_version_header}")
+  set(METIS_SUBMINOR_VERSION "${CMAKE_MATCH_1}")
+  if(NOT METIS_MAJOR_VERSION)
+    message(WARNING "Could not determine Metis version. Assuming version 4.0.0")
+    set(METIS_VERSION 4.0.0)
+  else()
+    set(METIS_VERSION ${METIS_MAJOR_VERSION}.${METIS_MINOR_VERSION}.${METIS_SUBMINOR_VERSION})
+  endif()
+  if(${METIS_VERSION} VERSION_LESS ${Metis_FIND_VERSION})
+    set(METIS_VERSION_OK FALSE)
+  else()
+    set(METIS_VERSION_OK TRUE)
+  endif()
+
+  if(NOT METIS_VERSION_OK)
+    message(STATUS "Metis version ${METIS_VERSION} found in ${METIS_INCLUDES}, "
+                   "but at least version ${Metis_FIND_VERSION} is required")
+  endif(NOT METIS_VERSION_OK)
+endmacro(_metis_check_version)
+
+  if(METIS_INCLUDES AND Metis_FIND_VERSION)
+    _metis_check_version()
+  else()
+    set(METIS_VERSION_OK TRUE)
+  endif()
+

 find_library(METIS_LIBRARIES metis PATHS $ENV{METISDIR} ${LIB_INSTALL_DIR} PATH_SUFFIXES lib)

 include(FindPackageHandleStandardArgs)
 find_package_handle_standard_args(METIS DEFAULT_MSG
-                                  METIS_INCLUDES METIS_LIBRARIES)
+                                  METIS_INCLUDES METIS_LIBRARIES METIS_VERSION_OK)

 mark_as_advanced(METIS_INCLUDES METIS_LIBRARIES)
--- a/gtsam/3rdparty/Eigen/cmake/language_support.cmake
+++ b/gtsam/3rdparty/Eigen/cmake/language_support.cmake
@ -33,7 +33,7 @@ function(workaround_9220 language language_works)
  file(WRITE ${CMAKE_BINARY_DIR}/language_tests/${language}/CMakeLists.txt
    ${text})
  execute_process(
-    COMMAND ${CMAKE_COMMAND} .
+    COMMAND ${CMAKE_COMMAND} . -G "${CMAKE_GENERATOR}"
    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language}
    RESULT_VARIABLE return_code
    OUTPUT_QUIET
--- a/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt
+++ b/gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt
@ -67,10 +67,10 @@ P.rightCols<cols>()                // P(:, end-cols+1:end)
 P.rightCols(cols)                  // P(:, end-cols+1:end)
 P.topRows<rows>()                  // P(1:rows, :)
 P.topRows(rows)                    // P(1:rows, :)
-P.middleRows<rows>(i)              // P(:, i+1:i+rows)
-P.middleRows(i, rows)              // P(:, i+1:i+rows)
-P.bottomRows<rows>()               // P(:, end-rows+1:end)
-P.bottomRows(rows)                 // P(:, end-rows+1:end)
+P.middleRows<rows>(i)              // P(i+1:i+rows, :)
+P.middleRows(i, rows)              // P(i+1:i+rows, :)
+P.bottomRows<rows>()               // P(end-rows+1:end, :)
+P.bottomRows(rows)                 // P(end-rows+1:end, :)
 P.topLeftCorner(rows, cols)        // P(1:rows, 1:cols)
 P.topRightCorner(rows, cols)       // P(1:rows, end-cols+1:end)
 P.bottomLeftCorner(rows, cols)     // P(end-rows+1:end, 1:cols)
--- a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox
+++ b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox
@ -71,11 +71,10 @@ i.e either row major or column major. The default is column major. Most arithmet
 <td> Constant or Random Insertion</td>
 <td>
 \code
-sm1.setZero(); // Set the matrix with zero elements
-sm1.setConstant(val); //Replace all the nonzero values with val
+sm1.setZero();
 \endcode
 </td>
-<td> The matrix sm1 should have been created before ???</td>
+<td>Remove all non-zero coefficients</td>
 </tr>
 </table>

--- a/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/doc/examples/CMakeLists.txt
@ -6,12 +6,10 @@ foreach(example_src ${examples_SRCS})
  if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
    target_link_libraries(${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO})
  endif()
-  get_target_property(example_executable
-                      ${example} LOCATION)
  add_custom_command(
    TARGET ${example}
    POST_BUILD
-    COMMAND ${example_executable}
+    COMMAND ${example}
    ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out
  )
  add_dependencies(all_examples ${example})
--- a/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/doc/snippets/CMakeLists.txt
@ -14,12 +14,10 @@ foreach(snippet_src ${snippets_SRCS})
  if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
    target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO})
  endif()
-  get_target_property(compile_snippet_executable
-                      ${compile_snippet_target} LOCATION)
  add_custom_command(
    TARGET ${compile_snippet_target}
    POST_BUILD
-    COMMAND ${compile_snippet_executable}
+    COMMAND ${compile_snippet_target}
    ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out
  )
  add_dependencies(all_snippets ${compile_snippet_target})
--- a/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/doc/special_examples/CMakeLists.txt
@ -1,4 +1,3 @@
-
 if(NOT EIGEN_TEST_NOQT)
  find_package(Qt4)
  if(QT4_FOUND)
@ -6,7 +5,6 @@ if(NOT EIGEN_TEST_NOQT)
  endif()
 endif(NOT EIGEN_TEST_NOQT)

-
 if(QT4_FOUND)
  add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp)
  target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY})
@ -19,3 +17,4 @@ if(QT4_FOUND)

  add_dependencies(all_examples Tutorial_sparse_example)
 endif(QT4_FOUND)
+
--- a/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/failtest/CMakeLists.txt
@ -26,6 +26,12 @@ ei_add_failtest("block_on_const_type_actually_const_1")
 ei_add_failtest("transpose_on_const_type_actually_const")
 ei_add_failtest("diagonal_on_const_type_actually_const")

+ei_add_failtest("ref_1")
+ei_add_failtest("ref_2")
+ei_add_failtest("ref_3")
+ei_add_failtest("ref_4")
+ei_add_failtest("ref_5")
+
 if (EIGEN_FAILTEST_FAILURE_COUNT)
  message(FATAL_ERROR
          "${EIGEN_FAILTEST_FAILURE_COUNT} out of ${EIGEN_FAILTEST_COUNT} failtests FAILED. "
--- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt
@ -66,7 +66,7 @@ endif()

 find_package(Pastix)
 find_package(Scotch)
-find_package(Metis)
+find_package(Metis 5.0 REQUIRED)
 if(PASTIX_FOUND AND BLAS_FOUND)
  add_definitions("-DEIGEN_PASTIX_SUPPORT")
  include_directories(${PASTIX_INCLUDES})
@ -279,6 +279,7 @@ ei_add_property(EIGEN_TESTING_SUMMARY "CXX_FLAGS:         ${CMAKE_CXX_FLAGS}\n")
 ei_add_property(EIGEN_TESTING_SUMMARY "Sparse lib flags:  ${SPARSE_LIBS}\n")

 option(EIGEN_TEST_EIGEN2 "Run whole Eigen2 test suite against EIGEN2_SUPPORT" OFF)
+mark_as_advanced(EIGEN_TEST_EIGEN2)
 if(EIGEN_TEST_EIGEN2)
  add_subdirectory(eigen2)
 endif()
--- a/gtsam/3rdparty/Eigen/test/cholesky.cpp
+++ b/gtsam/3rdparty/Eigen/test/cholesky.cpp
@ -320,33 +320,35 @@ template<typename MatrixType> void cholesky_definiteness(const MatrixType& m)
 {
  eigen_assert(m.rows() == 2 && m.cols() == 2);
  MatrixType mat;
+  LDLT<MatrixType> ldlt(2);
+  
  {
    mat << 1, 0, 0, -1;
-    LDLT<MatrixType> ldlt(mat);
+    ldlt.compute(mat);
    VERIFY(!ldlt.isNegative());
    VERIFY(!ldlt.isPositive());
  }
  {
    mat << 1, 2, 2, 1;
-    LDLT<MatrixType> ldlt(mat);
+    ldlt.compute(mat);
    VERIFY(!ldlt.isNegative());
    VERIFY(!ldlt.isPositive());
  }
  {
    mat << 0, 0, 0, 0;
-    LDLT<MatrixType> ldlt(mat);
+    ldlt.compute(mat);
    VERIFY(ldlt.isNegative());
    VERIFY(ldlt.isPositive());
  }
  {
    mat << 0, 0, 0, 1;
-    LDLT<MatrixType> ldlt(mat);
+    ldlt.compute(mat);
    VERIFY(!ldlt.isNegative());
    VERIFY(ldlt.isPositive());
  }
  {
    mat << -1, 0, 0, 0;
-    LDLT<MatrixType> ldlt(mat);
+    ldlt.compute(mat);
    VERIFY(ldlt.isNegative());
    VERIFY(!ldlt.isPositive());
  }
--- a/gtsam/3rdparty/Eigen/test/cwiseop.cpp
+++ b/gtsam/3rdparty/Eigen/test/cwiseop.cpp
@ -9,6 +9,8 @@
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

 #define EIGEN2_SUPPORT
+#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING
+
 #define EIGEN_NO_STATIC_ASSERT
 #include "main.h"
 #include <functional>
--- a/gtsam/3rdparty/Eigen/test/dynalloc.cpp
+++ b/gtsam/3rdparty/Eigen/test/dynalloc.cpp
@ -87,32 +87,6 @@ template<typename T> void check_dynaligned()
  delete obj;
 }

-template<typename T> void check_custom_new_delete()
-{
-  {
-    T* t = new T;
-    delete t;
-  }
-  
-  {
-    std::size_t N = internal::random<std::size_t>(1,10);
-    T* t = new T[N];
-    delete[] t;
-  }
-  
-#ifdef EIGEN_ALIGN
-  {
-    T* t = static_cast<T *>((T::operator new)(sizeof(T)));
-    (T::operator delete)(t, sizeof(T));
-  }
-  
-  {
-    T* t = static_cast<T *>((T::operator new)(sizeof(T)));
-    (T::operator delete)(t);
-  }
-#endif
-}
-
 void test_dynalloc()
 {
  // low level dynamic memory allocation
@ -128,12 +102,6 @@ void test_dynalloc()
    CALL_SUBTEST(check_dynaligned<Matrix4f>() );
    CALL_SUBTEST(check_dynaligned<Vector4d>() );
    CALL_SUBTEST(check_dynaligned<Vector4i>() );
-    CALL_SUBTEST(check_dynaligned<Vector8f>() );
-
-    CALL_SUBTEST( check_custom_new_delete<Vector4f>() );
-    CALL_SUBTEST( check_custom_new_delete<Vector2f>() );
-    CALL_SUBTEST( check_custom_new_delete<Matrix4f>() );
-    CALL_SUBTEST( check_custom_new_delete<MatrixXi>() );
  }
  
  // check static allocation, who knows ?
--- a/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt
+++ b/gtsam/3rdparty/Eigen/test/eigen2/CMakeLists.txt
@ -4,6 +4,7 @@ add_dependencies(eigen2_check eigen2_buildtests)
 add_dependencies(buildtests eigen2_buildtests)

 add_definitions("-DEIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API")
+add_definitions("-DEIGEN_NO_EIGEN2_DEPRECATED_WARNING")

 ei_add_test(eigen2_meta)
 ei_add_test(eigen2_sizeof)
--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp
@ -29,8 +29,6 @@ template<typename MatrixType> void adjoint(const MatrixType& m)
  MatrixType m1 = MatrixType::Random(rows, cols),
             m2 = MatrixType::Random(rows, cols),
             m3(rows, cols),
-             mzero = MatrixType::Zero(rows, cols),
-             identity = SquareMatrixType::Identity(rows, rows),
             square = SquareMatrixType::Random(rows, rows);
  VectorType v1 = VectorType::Random(rows),
             v2 = VectorType::Random(rows),
--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp
@ -23,11 +23,8 @@ template<typename MatrixType> void basicStuff(const MatrixType& m)
             m2 = MatrixType::Random(rows, cols),
             m3(rows, cols),
             mzero = MatrixType::Zero(rows, cols),
-             identity = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
-                              ::Identity(rows, rows),
             square = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>::Random(rows, rows);
  VectorType v1 = VectorType::Random(rows),
-             v2 = VectorType::Random(rows),
             vzero = VectorType::Zero(rows);

  Scalar x = ei_random<Scalar>();
--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp
@ -35,11 +35,8 @@ template<typename MatrixType> void cwiseops(const MatrixType& m)
             mzero = MatrixType::Zero(rows, cols),
             mones = MatrixType::Ones(rows, cols),
             identity = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
-                              ::Identity(rows, rows),
-             square = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>::Random(rows, rows);
-  VectorType v1 = VectorType::Random(rows),
-             v2 = VectorType::Random(rows),
-             vzero = VectorType::Zero(rows),
+                              ::Identity(rows, rows);
+  VectorType vzero = VectorType::Zero(rows),
             vones = VectorType::Ones(rows),
             v3(rows);

--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp
@ -392,6 +392,7 @@ template<typename Scalar> void geometry(void)
  #define VERIFY_EULER(I,J,K, X,Y,Z) { \
    Vector3 ea = m.eulerAngles(I,J,K); \
    Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
+    VERIFY_IS_APPROX(m, m1); \
    VERIFY_IS_APPROX(m,  Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
  }
  VERIFY_EULER(0,1,2, X,Y,Z);
--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp
@ -394,6 +394,7 @@ template<typename Scalar> void geometry(void)
  #define VERIFY_EULER(I,J,K, X,Y,Z) { \
    Vector3 ea = m.eulerAngles(I,J,K); \
    Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
+    VERIFY_IS_APPROX(m, m1); \
    VERIFY_IS_APPROX(m,  Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
  }
  VERIFY_EULER(0,1,2, X,Y,Z);
--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp
@ -25,7 +25,6 @@ template<typename MatrixType> void inverse(const MatrixType& m)

  MatrixType m1 = MatrixType::Random(rows, cols),
             m2(rows, cols),
-             mzero = MatrixType::Zero(rows, cols),
             identity = MatrixType::Identity(rows, rows);

  while(ei_abs(m1.determinant()) < RealScalar(0.1) && rows <= 8)
--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp
@ -25,8 +25,7 @@ template<typename MatrixType> void linearStructure(const MatrixType& m)
  // to test it, hence I consider that we will have tested Random.h
  MatrixType m1 = MatrixType::Random(rows, cols),
             m2 = MatrixType::Random(rows, cols),
-             m3(rows, cols),
-             mzero = MatrixType::Zero(rows, cols);
+             m3(rows, cols);

  Scalar s1 = ei_random<Scalar>();
  while (ei_abs(s1)<1e-3) s1 = ei_random<Scalar>();
--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_nomalloc.cpp
@ -25,22 +25,12 @@ template<typename MatrixType> void nomalloc(const MatrixType& m)
  */

  typedef typename MatrixType::Scalar Scalar;
-  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;

  int rows = m.rows();
  int cols = m.cols();

  MatrixType m1 = MatrixType::Random(rows, cols),
-             m2 = MatrixType::Random(rows, cols),
-             m3(rows, cols),
-             mzero = MatrixType::Zero(rows, cols),
-             identity = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
-                              ::Identity(rows, rows),
-             square = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
-                              ::Random(rows, rows);
-  VectorType v1 = VectorType::Random(rows),
-             v2 = VectorType::Random(rows),
-             vzero = VectorType::Zero(rows);
+             m2 = MatrixType::Random(rows, cols);

  Scalar s1 = ei_random<Scalar>();

--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_submatrices.cpp
@ -51,16 +51,10 @@ template<typename MatrixType> void submatrices(const MatrixType& m)
  MatrixType m1 = MatrixType::Random(rows, cols),
             m2 = MatrixType::Random(rows, cols),
             m3(rows, cols),
-             mzero = MatrixType::Zero(rows, cols),
             ones = MatrixType::Ones(rows, cols),
-             identity = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
-                              ::Identity(rows, rows),
             square = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
                              ::Random(rows, rows);
-  VectorType v1 = VectorType::Random(rows),
-             v2 = VectorType::Random(rows),
-             v3 = VectorType::Random(rows),
-             vzero = VectorType::Zero(rows);
+  VectorType v1 = VectorType::Random(rows);

  Scalar s1 = ei_random<Scalar>();

--- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_triangular.cpp
@ -13,7 +13,6 @@ template<typename MatrixType> void triangular(const MatrixType& m)
 {
  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
-  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;

  RealScalar largerEps = 10*test_precision<RealScalar>();

@ -25,16 +24,7 @@ template<typename MatrixType> void triangular(const MatrixType& m)
             m3(rows, cols),
             m4(rows, cols),
             r1(rows, cols),
-             r2(rows, cols),
-             mzero = MatrixType::Zero(rows, cols),
-             mones = MatrixType::Ones(rows, cols),
-             identity = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
-                              ::Identity(rows, rows),
-             square = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
-                              ::Random(rows, rows);
-  VectorType v1 = VectorType::Random(rows),
-             v2 = VectorType::Random(rows),
-             vzero = VectorType::Zero(rows);
+             r2(rows, cols);

  MatrixType m1up = m1.template part<Eigen::UpperTriangular>();
  MatrixType m2up = m2.template part<Eigen::UpperTriangular>();
--- a/gtsam/3rdparty/Eigen/test/eigen2/product.h
+++ b/gtsam/3rdparty/Eigen/test/eigen2/product.h
@ -40,8 +40,7 @@ template<typename MatrixType> void product(const MatrixType& m)
  // to test it, hence I consider that we will have tested Random.h
  MatrixType m1 = MatrixType::Random(rows, cols),
             m2 = MatrixType::Random(rows, cols),
-             m3(rows, cols),
-             mzero = MatrixType::Zero(rows, cols);
+             m3(rows, cols);
  RowSquareMatrixType
             identity = RowSquareMatrixType::Identity(rows, rows),
             square = RowSquareMatrixType::Random(rows, rows),
@ -49,9 +48,7 @@ template<typename MatrixType> void product(const MatrixType& m)
  ColSquareMatrixType
             square2 = ColSquareMatrixType::Random(cols, cols),
             res2 = ColSquareMatrixType::Random(cols, cols);
-  RowVectorType v1 = RowVectorType::Random(rows),
-             v2 = RowVectorType::Random(rows),
-             vzero = RowVectorType::Zero(rows);
+  RowVectorType v1 = RowVectorType::Random(rows);
  ColVectorType vc2 = ColVectorType::Random(cols), vcres(cols);
  OtherMajorMatrixType tm1 = m1;

--- a/gtsam/3rdparty/Eigen/test/eigen2support.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigen2support.cpp
@ -8,6 +8,7 @@
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

 #define EIGEN2_SUPPORT
+#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING

 #include "main.h"

--- a/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp
+++ b/gtsam/3rdparty/Eigen/test/eigensolver_selfadjoint.cpp
@ -29,7 +29,21 @@ template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m)
  MatrixType a = MatrixType::Random(rows,cols);
  MatrixType a1 = MatrixType::Random(rows,cols);
  MatrixType symmA =  a.adjoint() * a + a1.adjoint() * a1;
+  MatrixType symmC = symmA;
+  
+  // randomly nullify some rows/columns
+  {
+    Index count = 1;//internal::random<Index>(-cols,cols);
+    for(Index k=0; k<count; ++k)
+    {
+      Index i = internal::random<Index>(0,cols-1);
+      symmA.row(i).setZero();
+      symmA.col(i).setZero();
+    }
+  }
+  
  symmA.template triangularView<StrictlyUpper>().setZero();
+  symmC.template triangularView<StrictlyUpper>().setZero();

  MatrixType b = MatrixType::Random(rows,cols);
  MatrixType b1 = MatrixType::Random(rows,cols);
@ -40,7 +54,7 @@ template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m)
  SelfAdjointEigenSolver<MatrixType> eiDirect;
  eiDirect.computeDirect(symmA);
  // generalized eigen pb
-  GeneralizedSelfAdjointEigenSolver<MatrixType> eiSymmGen(symmA, symmB);
+  GeneralizedSelfAdjointEigenSolver<MatrixType> eiSymmGen(symmC, symmB);

  VERIFY_IS_EQUAL(eiSymm.info(), Success);
  VERIFY((symmA.template selfadjointView<Lower>() * eiSymm.eigenvectors()).isApprox(
@ -57,27 +71,28 @@ template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m)
  VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues());
  
  // generalized eigen problem Ax = lBx
-  eiSymmGen.compute(symmA, symmB,Ax_lBx);
+  eiSymmGen.compute(symmC, symmB,Ax_lBx);
  VERIFY_IS_EQUAL(eiSymmGen.info(), Success);
-  VERIFY((symmA.template selfadjointView<Lower>() * eiSymmGen.eigenvectors()).isApprox(
+  VERIFY((symmC.template selfadjointView<Lower>() * eiSymmGen.eigenvectors()).isApprox(
          symmB.template selfadjointView<Lower>() * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps));

  // generalized eigen problem BAx = lx
-  eiSymmGen.compute(symmA, symmB,BAx_lx);
+  eiSymmGen.compute(symmC, symmB,BAx_lx);
  VERIFY_IS_EQUAL(eiSymmGen.info(), Success);
-  VERIFY((symmB.template selfadjointView<Lower>() * (symmA.template selfadjointView<Lower>() * eiSymmGen.eigenvectors())).isApprox(
+  VERIFY((symmB.template selfadjointView<Lower>() * (symmC.template selfadjointView<Lower>() * eiSymmGen.eigenvectors())).isApprox(
         (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps));

  // generalized eigen problem ABx = lx
-  eiSymmGen.compute(symmA, symmB,ABx_lx);
+  eiSymmGen.compute(symmC, symmB,ABx_lx);
  VERIFY_IS_EQUAL(eiSymmGen.info(), Success);
-  VERIFY((symmA.template selfadjointView<Lower>() * (symmB.template selfadjointView<Lower>() * eiSymmGen.eigenvectors())).isApprox(
+  VERIFY((symmC.template selfadjointView<Lower>() * (symmB.template selfadjointView<Lower>() * eiSymmGen.eigenvectors())).isApprox(
         (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps));


+  eiSymm.compute(symmC);
  MatrixType sqrtSymmA = eiSymm.operatorSqrt();
-  VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView<Lower>()), sqrtSymmA*sqrtSymmA);
-  VERIFY_IS_APPROX(sqrtSymmA, symmA.template selfadjointView<Lower>()*eiSymm.operatorInverseSqrt());
+  VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView<Lower>()), sqrtSymmA*sqrtSymmA);
+  VERIFY_IS_APPROX(sqrtSymmA, symmC.template selfadjointView<Lower>()*eiSymm.operatorInverseSqrt());

  MatrixType id = MatrixType::Identity(rows, cols);
  VERIFY_IS_APPROX(id.template selfadjointView<Lower>().operatorNorm(), RealScalar(1));
@ -95,15 +110,15 @@ template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m)
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt());

  // test Tridiagonalization's methods
-  Tridiagonalization<MatrixType> tridiag(symmA);
+  Tridiagonalization<MatrixType> tridiag(symmC);
  // FIXME tridiag.matrixQ().adjoint() does not work
-  VERIFY_IS_APPROX(MatrixType(symmA.template selfadjointView<Lower>()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint());
+  VERIFY_IS_APPROX(MatrixType(symmC.template selfadjointView<Lower>()), tridiag.matrixQ() * tridiag.matrixT().eval() * MatrixType(tridiag.matrixQ()).adjoint());
  
  if (rows > 1)
  {
    // Test matrix with NaN
-    symmA(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN();
-    SelfAdjointEigenSolver<MatrixType> eiSymmNaN(symmA);
+    symmC(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN();
+    SelfAdjointEigenSolver<MatrixType> eiSymmNaN(symmC);
    VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence);
  }
 }
@ -113,8 +128,10 @@ void test_eigensolver_selfadjoint()
  int s = 0;
  for(int i = 0; i < g_repeat; i++) {
    // very important to test 3x3 and 2x2 matrices since we provide special paths for them
+    CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) );
    CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) );
    CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) );
+    CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) );
    CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) );
    s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
    CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) );
--- a/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp
+++ b/gtsam/3rdparty/Eigen/test/geo_hyperplane.cpp
@ -114,6 +114,32 @@ template<typename Scalar> void lines()
  }
 }

+template<typename Scalar> void planes()
+{
+  using std::abs;
+  typedef Hyperplane<Scalar, 3> Plane;
+  typedef Matrix<Scalar,3,1> Vector;
+
+  for(int i = 0; i < 10; i++)
+  {
+    Vector v0 = Vector::Random();
+    Vector v1(v0), v2(v0);
+    if(internal::random<double>(0,1)>0.25)
+      v1 += Vector::Random();
+    if(internal::random<double>(0,1)>0.25)
+      v2 += v1 * std::pow(internal::random<Scalar>(0,1),internal::random<int>(1,16));
+    if(internal::random<double>(0,1)>0.25)
+      v2 += Vector::Random() * std::pow(internal::random<Scalar>(0,1),internal::random<int>(1,16));
+
+    Plane p0 = Plane::Through(v0, v1, v2);
+
+    VERIFY_IS_APPROX(p0.normal().norm(), Scalar(1));
+    VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v0), Scalar(1));
+    VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v1), Scalar(1));
+    VERIFY_IS_MUCH_SMALLER_THAN(p0.absDistance(v2), Scalar(1));
+  }
+}
+
 template<typename Scalar> void hyperplane_alignment()
 {
  typedef Hyperplane<Scalar,3,AutoAlign> Plane3a;
@ -153,5 +179,7 @@ void test_geo_hyperplane()
    CALL_SUBTEST_4( hyperplane(Hyperplane<std::complex<double>,5>()) );
    CALL_SUBTEST_1( lines<float>() );
    CALL_SUBTEST_3( lines<double>() );
+    CALL_SUBTEST_2( planes<float>() );
+    CALL_SUBTEST_5( planes<double>() );
  }
 }
--- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp
+++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp
@ -98,11 +98,19 @@ template<typename Scalar, int Mode, int Options> void transformations()
  Matrix3 matrot1, m;

  Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI));
-  Scalar s0 = internal::random<Scalar>();
+  Scalar s0 = internal::random<Scalar>(),
+         s1 = internal::random<Scalar>();
+  
+  while(v0.norm() < test_precision<Scalar>()) v0 = Vector3::Random();
+  while(v1.norm() < test_precision<Scalar>()) v1 = Vector3::Random();
+    

  VERIFY_IS_APPROX(v0, AngleAxisx(a, v0.normalized()) * v0);
  VERIFY_IS_APPROX(-v0, AngleAxisx(Scalar(M_PI), v0.unitOrthogonal()) * v0);
+  if(abs(cos(a)) > test_precision<Scalar>())
+  {
    VERIFY_IS_APPROX(cos(a)*v0.squaredNorm(), v0.dot(AngleAxisx(a, v0.unitOrthogonal()) * v0));
+  }
  m = AngleAxisx(a, v0.normalized()).toRotationMatrix().adjoint();
  VERIFY_IS_APPROX(Matrix3::Identity(), m * AngleAxisx(a, v0.normalized()));
  VERIFY_IS_APPROX(Matrix3::Identity(), AngleAxisx(a, v0.normalized()) * m);
@ -123,11 +131,18 @@ template<typename Scalar, int Mode, int Options> void transformations()
  // angle-axis conversion
  AngleAxisx aa = AngleAxisx(q1);
  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
-  VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
+  
+  if(abs(aa.angle()) > NumTraits<Scalar>::dummy_precision())
+  {
+    VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
+  }

  aa.fromRotationMatrix(aa.toRotationMatrix());
  VERIFY_IS_APPROX(q1 * v1, Quaternionx(aa) * v1);
-  VERIFY_IS_NOT_APPROX(q1 * v1, Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1);
+  if(abs(aa.angle()) > NumTraits<Scalar>::dummy_precision())
+  {
+    VERIFY( !(q1 * v1).isApprox(Quaternionx(AngleAxisx(aa.angle()*2,aa.axis())) * v1) );
+  }

  // AngleAxis
  VERIFY_IS_APPROX(AngleAxisx(a,v1.normalized()).toRotationMatrix(),
@ -347,7 +362,9 @@ template<typename Scalar, int Mode, int Options> void transformations()
  // test transform inversion
  t0.setIdentity();
  t0.translate(v0);
+  do {
    t0.linear().setRandom();
+  } while(t0.linear().jacobiSvd().singularValues()(2)<test_precision<Scalar>());
  Matrix4 t044 = Matrix4::Zero();
  t044(3,3) = 1;
  t044.block(0,0,t0.matrix().rows(),4) = t0.matrix();
@ -397,6 +414,29 @@ template<typename Scalar, int Mode, int Options> void transformations()
  t20 = Translation2(v20) * (Rotation2D<Scalar>(s0) * Eigen::Scaling(s0));
  t21 = Translation2(v20) * Rotation2D<Scalar>(s0) * Eigen::Scaling(s0);
  VERIFY_IS_APPROX(t20,t21);
+  
+  Rotation2D<Scalar> R0(s0), R1(s1);
+  t20 = Translation2(v20) * (R0 * Eigen::Scaling(s0));
+  t21 = Translation2(v20) * R0 * Eigen::Scaling(s0);
+  VERIFY_IS_APPROX(t20,t21);
+  
+  t20 = Translation2(v20) * (R0 * R0.inverse() * Eigen::Scaling(s0));
+  t21 = Translation2(v20) * Eigen::Scaling(s0);
+  VERIFY_IS_APPROX(t20,t21);
+  
+  VERIFY_IS_APPROX(s0, (R0.slerp(0, R1)).angle());
+  VERIFY_IS_APPROX(s1, (R0.slerp(1, R1)).angle());
+  VERIFY_IS_APPROX(s0, (R0.slerp(0.5, R0)).angle());
+  VERIFY_IS_APPROX(Scalar(0), (R0.slerp(0.5, R0.inverse())).angle());
+  
+  // check basic features
+  {
+    Rotation2D<Scalar> r1;           // default ctor
+    r1 = Rotation2D<Scalar>(s0);     // copy assignment
+    VERIFY_IS_APPROX(r1.angle(),s0);
+    Rotation2D<Scalar> r2(r1);       // copy ctor
+    VERIFY_IS_APPROX(r2.angle(),s0);
+  }
 }

 template<typename Scalar> void transform_alignment()
--- a/gtsam/3rdparty/Eigen/test/jacobisvd.cpp
+++ b/gtsam/3rdparty/Eigen/test/jacobisvd.cpp
@ -321,16 +321,23 @@ void jacobisvd_inf_nan()
  VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf));
  svd.compute(MatrixType::Constant(10,10,some_inf), ComputeFullU | ComputeFullV);

-  Scalar some_nan = zero<Scalar>() / zero<Scalar>();
-  VERIFY(some_nan != some_nan);
-  svd.compute(MatrixType::Constant(10,10,some_nan), ComputeFullU | ComputeFullV);
+  Scalar nan = std::numeric_limits<Scalar>::quiet_NaN();
+  VERIFY(nan != nan);
+  svd.compute(MatrixType::Constant(10,10,nan), ComputeFullU | ComputeFullV);

  MatrixType m = MatrixType::Zero(10,10);
  m(internal::random<int>(0,9), internal::random<int>(0,9)) = some_inf;
  svd.compute(m, ComputeFullU | ComputeFullV);

  m = MatrixType::Zero(10,10);
-  m(internal::random<int>(0,9), internal::random<int>(0,9)) = some_nan;
+  m(internal::random<int>(0,9), internal::random<int>(0,9)) = nan;
+  svd.compute(m, ComputeFullU | ComputeFullV);
+  
+  // regression test for bug 791
+  m.resize(3,3);
+  m << 0,    2*NumTraits<Scalar>::epsilon(),  0.5,
+       0,   -0.5,                             0,
+       nan,  0,                               0;
  svd.compute(m, ComputeFullU | ComputeFullV);
 }

@ -434,6 +441,7 @@ void test_jacobisvd()

    // Test on inf/nan matrix
    CALL_SUBTEST_7( jacobisvd_inf_nan<MatrixXf>() );
+    CALL_SUBTEST_10( jacobisvd_inf_nan<MatrixXd>() );
  }

  CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) ));
--- a/gtsam/3rdparty/Eigen/test/main.h
+++ b/gtsam/3rdparty/Eigen/test/main.h
@ -17,13 +17,36 @@
 #include <sstream>
 #include <vector>
 #include <typeinfo>
+
+// The following includes of STL headers have to be done _before_ the
+// definition of macros min() and max().  The reason is that many STL
+// implementations will not work properly as the min and max symbols collide
+// with the STL functions std:min() and std::max().  The STL headers may check
+// for the macro definition of min/max and issue a warning or undefine the
+// macros.
+//
+// Still, Windows defines min() and max() in windef.h as part of the regular
+// Windows system interfaces and many other Windows APIs depend on these
+// macros being available.  To prevent the macro expansion of min/max and to
+// make Eigen compatible with the Windows environment all function calls of
+// std::min() and std::max() have to be written with parenthesis around the
+// function name.
+//
+// All STL headers used by Eigen should be included here.  Because main.h is
+// included before any Eigen header and because the STL headers are guarded
+// against multiple inclusions, no STL header will see our own min/max macro
+// definitions.
 #include <limits>
 #include <algorithm>
-#include <sstream>
 #include <complex>
 #include <deque>
 #include <queue>
+#include <list>

+// To test that all calls from Eigen code to std::min() and std::max() are
+// protected by parenthesis against macro expansion, the min()/max() macros
+// are defined here and any not-parenthesized min/max call will cause a
+// compiler error.
 #define min(A,B) please_protect_your_min_with_parentheses
 #define max(A,B) please_protect_your_max_with_parentheses

@ -383,6 +406,26 @@ void randomPermutationVector(PermutationVectorType& v, typename PermutationVecto
  }
 }

+template<typename T> bool isNotNaN(const T& x)
+{
+  return x==x;
+}
+
+template<typename T> bool isNaN(const T& x)
+{
+  return x!=x;
+}
+
+template<typename T> bool isInf(const T& x)
+{
+  return x > NumTraits<T>::highest();
+}
+
+template<typename T> bool isMinusInf(const T& x)
+{
+  return x < NumTraits<T>::lowest();
+}
+
 } // end namespace Eigen

 template<typename T> struct GetDifferentType;
--- a/gtsam/3rdparty/Eigen/test/nomalloc.cpp
+++ b/gtsam/3rdparty/Eigen/test/nomalloc.cpp
@ -165,6 +165,38 @@ void ctms_decompositions()
  Eigen::JacobiSVD<Matrix> jSVD; jSVD.compute(A, ComputeFullU | ComputeFullV);
 }

+void test_zerosized() {
+  // default constructors:
+  Eigen::MatrixXd A;
+  Eigen::VectorXd v;
+  // explicit zero-sized:
+  Eigen::ArrayXXd A0(0,0);
+  Eigen::ArrayXd v0(std::ptrdiff_t(0)); // FIXME ArrayXd(0) is ambiguous
+
+  // assigning empty objects to each other:
+  A=A0;
+  v=v0;
+}
+
+template<typename MatrixType> void test_reference(const MatrixType& m) {
+  typedef typename MatrixType::Scalar Scalar;
+  enum { Flag          =  MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor};
+  enum { TransposeFlag = !MatrixType::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor};
+  typename MatrixType::Index rows = m.rows(), cols=m.cols();
+  // Dynamic reference:
+  typedef Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Flag         > > Ref;
+  typedef Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, TransposeFlag> > RefT;
+
+  Ref r1(m);
+  Ref r2(m.block(rows/3, cols/4, rows/2, cols/2));
+  RefT r3(m.transpose());
+  RefT r4(m.topLeftCorner(rows/2, cols/2).transpose());
+
+  VERIFY_RAISES_ASSERT(RefT r5(m));
+  VERIFY_RAISES_ASSERT(Ref r6(m.transpose()));
+  VERIFY_RAISES_ASSERT(Ref r7(Scalar(2) * m));
+}
+
 void test_nomalloc()
 {
  // check that our operator new is indeed called:
@ -175,5 +207,6 @@ void test_nomalloc()
  
  // Check decomposition modules with dynamic matrices that have a known compile-time max size (ctms)
  CALL_SUBTEST_4(ctms_decompositions<float>());
-
+  CALL_SUBTEST_5(test_zerosized());
+  CALL_SUBTEST_6(test_reference(Matrix<float,32,32>()));
 }
--- a/gtsam/3rdparty/Eigen/test/nullary.cpp
+++ b/gtsam/3rdparty/Eigen/test/nullary.cpp
@ -80,7 +80,9 @@ void testVectorType(const VectorType& base)
  Matrix<Scalar,1,Dynamic> col_vector(size);
  row_vector.setLinSpaced(size,low,high);
  col_vector.setLinSpaced(size,low,high);
-  VERIFY( row_vector.isApprox(col_vector.transpose(), NumTraits<Scalar>::epsilon()));
+  // when using the extended precision (e.g., FPU) the relative error might exceed 1 bit
+  // when computing the squared sum in isApprox, thus the 2x factor.
+  VERIFY( row_vector.isApprox(col_vector.transpose(), Scalar(2)*NumTraits<Scalar>::epsilon()));

  Matrix<Scalar,Dynamic,1> size_changer(size+50);
  size_changer.setLinSpaced(size,low,high);
--- a/gtsam/3rdparty/Eigen/test/packetmath.cpp
+++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp
@ -239,6 +239,12 @@ template<typename Scalar> void packetmath_real()
    data2[i] = internal::random<Scalar>(-87,88);
  }
  CHECK_CWISE1_IF(internal::packet_traits<Scalar>::HasExp, std::exp, internal::pexp);
+  {
+    data1[0] = std::numeric_limits<Scalar>::quiet_NaN();
+    packet_helper<internal::packet_traits<Scalar>::HasExp,Packet> h;
+    h.store(data2, internal::pexp(h.load(data1))); 
+    VERIFY(isNaN(data2[0]));
+  }

  for (int i=0; i<size; ++i)
  {
@ -247,8 +253,22 @@ template<typename Scalar> void packetmath_real()
  }
  if(internal::random<float>(0,1)<0.1)
    data1[internal::random<int>(0, PacketSize)] = 0;
-  CHECK_CWISE1_IF(internal::packet_traits<Scalar>::HasLog, std::log, internal::plog);
  CHECK_CWISE1_IF(internal::packet_traits<Scalar>::HasSqrt, std::sqrt, internal::psqrt);
+  CHECK_CWISE1_IF(internal::packet_traits<Scalar>::HasLog, std::log, internal::plog);
+  {
+    data1[0] = std::numeric_limits<Scalar>::quiet_NaN();
+    packet_helper<internal::packet_traits<Scalar>::HasLog,Packet> h;
+    h.store(data2, internal::plog(h.load(data1)));
+    VERIFY(isNaN(data2[0]));
+    data1[0] = -1.0f;
+    h.store(data2, internal::plog(h.load(data1)));
+    VERIFY(isNaN(data2[0]));
+#if !EIGEN_FAST_MATH
+    h.store(data2, internal::psqrt(h.load(data1)));
+    VERIFY(isNaN(data2[0]));
+    VERIFY(isNaN(data2[1]));
+#endif
+  }
 }

 template<typename Scalar> void packetmath_notcomplex()
--- a/gtsam/3rdparty/Eigen/test/product.h
+++ b/gtsam/3rdparty/Eigen/test/product.h
@ -139,4 +139,12 @@ template<typename MatrixType> void product(const MatrixType& m)
  // inner product
  Scalar x = square2.row(c) * square2.col(c2);
  VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum());
+  
+  // outer product
+  VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
+  VERIFY_IS_APPROX(m1.row(r).transpose() * m1.col(c).transpose(), m1.block(r,0,1,cols).transpose() * m1.block(0,c,rows,1).transpose());
+  VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
+  VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
+  VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols));
+  VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols));  
 }
--- a/Show More
+++ b/Show More