Merge branch 'develop' into feature/SmartFactors3

Conflicts: gtsam/geometry/CalibratedCamera.h gtsam/geometry/tests/testCalibratedCamera.cpp gtsam/geometry/triangulation.h gtsam/nonlinear/ExpressionFactor.h gtsam/slam/SmartProjectionFactor.h gtsam_unstable/slam/SmartStereoProjectionFactor.h tests/testExpressionFactor.cpp
2015-06-19 16:55:30 -04:00 · 2015-06-19 16:55:30 -04:00 · f8205bfe02
parent 2d9fddbcaa cfa656c2b9
commit f8205bfe02
250 changed files with 7354 additions and 5076 deletions
--- a/.cproject
+++ b/.cproject
@ -532,14 +532,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j4</buildArguments>
-				<buildTarget>testSimilarity3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@ -755,6 +747,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testSimilarity3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -1035,6 +1035,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testCyclic.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testCyclic.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@ -1301,7 +1309,6 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1341,7 +1348,6 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1349,7 +1355,6 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1453,6 +1458,7 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1763,6 +1769,7 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1770,6 +1777,7 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1777,6 +1785,7 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1784,6 +1793,7 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -1975,7 +1985,6 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -2037,22 +2046,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testExpressionMeta.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j4</buildArguments>
-				<buildTarget>testCustomChartExpression.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -2127,6 +2120,7 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -2134,6 +2128,7 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -2181,6 +2176,7 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -2188,6 +2184,7 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -2195,6 +2192,7 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -2210,6 +2208,7 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -2783,6 +2782,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testExecutionTrace.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testExecutionTrace.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testIMUSystem.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -3191,6 +3198,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testGroup.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testGroup.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -3329,6 +3344,7 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -3336,6 +3352,7 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -3343,6 +3360,7 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@ -3412,6 +3430,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testLie.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testLie.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
--- 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
@ -192,36 +198,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}")
+	
+	# 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 paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
-	set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
-
+	# 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 +399,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,11 +53,12 @@ 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")
-		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
-	endif()
+    # 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()

 # Set up build type library postfixes
@ -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
@ -367,6 +367,11 @@ 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()
@ -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/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@ -31,28 +31,29 @@
 *
 */

-#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/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 <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 +73,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 +253,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 +287,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;
--- a/gtsam.h
+++ b/gtsam.h
@ -288,6 +288,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 +330,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;

@ -550,6 +574,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
@ -788,56 +822,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(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;
@ -865,6 +859,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();
@ -893,6 +931,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
 //*************************************************************************
@ -2176,9 +2224,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>
--- 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/ceres/eigen.h
+++ b/gtsam/3rdparty/ceres/eigen.h
@ -31,7 +31,7 @@
 #ifndef CERES_INTERNAL_EIGEN_H_
 #define CERES_INTERNAL_EIGEN_H_

-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <Eigen/Core>

 namespace ceres {

--- a/gtsam/3rdparty/ceres/fixed_array.h
+++ b/gtsam/3rdparty/ceres/fixed_array.h
@ -33,7 +33,7 @@
 #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_

 #include <cstddef>
-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <Eigen/Core>
 #include <gtsam/3rdparty/ceres/macros.h>
 #include <gtsam/3rdparty/ceres/manual_constructor.h>

--- a/gtsam/3rdparty/ceres/jet.h
+++ b/gtsam/3rdparty/ceres/jet.h
@ -162,7 +162,7 @@
 #include <limits>
 #include <string>

-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <Eigen/Core>
 #include <gtsam/3rdparty/ceres/fpclassify.h>

 namespace ceres {
--- a/gtsam/3rdparty/gtsam_eigen_includes.h.in
+++ b/gtsam/3rdparty/gtsam_eigen_includes.h.in
@ -1,33 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file    gtsam_eigen_includes.h
- * @brief   File to include the Eigen headers that we use - generated by CMake
- * @author  Richard Roberts
- */
-
-#pragma once
-
-#ifndef MKL_BLAS
-#define MKL_BLAS MKL_DOMAIN_BLAS
-#endif
-
-#cmakedefine EIGEN_USE_MKL_ALL // This is also defined in config.h
-#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense>
-#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR>
-#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU>
-#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD>
-#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry>
-
-
-
-
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@ -131,12 +131,6 @@ else()
    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 endif()

-# Set dataset paths
-set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp"
-    APPEND PROPERTY COMPILE_DEFINITIONS
-	"SOURCE_TREE_DATASET_DIR=\"${PROJECT_SOURCE_DIR}/examples/Data\""
-	"INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"")
-
 # Special cases
 if(MSVC)
 	set_property(SOURCE
--- a/gtsam/base/ConcurrentMap.h
+++ b/gtsam/base/ConcurrentMap.h
@ -95,7 +95,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void save(Archive& ar, const unsigned int version) const
+  void save(Archive& ar, const unsigned int /*version*/) const
  {
    // Copy to an STL container and serialize that
    FastVector<std::pair<KEY, VALUE> > map(this->size());
@ -103,7 +103,7 @@ private:
    ar & BOOST_SERIALIZATION_NVP(map);
  }
  template<class Archive>
-  void load(Archive& ar, const unsigned int version)
+  void load(Archive& ar, const unsigned int /*version*/)
  {
    // Load into STL container and then fill our map
    FastVector<std::pair<KEY, VALUE> > map;
--- a/gtsam/base/DerivedValue.h
+++ b/gtsam/base/DerivedValue.h
@ -129,7 +129,7 @@ public:
 protected:
  /// Assignment operator, protected because only the Value or DERIVED
  /// assignment operators should be used.
-  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
+  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& /*rhs*/) {
    // Nothing to do, do not call base class assignment operator
    return *this;
  }
--- a/gtsam/base/FastDefaultAllocator.h
+++ b/gtsam/base/FastDefaultAllocator.h
@ -17,8 +17,7 @@
 */

 #pragma once
-
-#include <gtsam/global_includes.h>
+#include <gtsam/config.h>      // Configuration from CMake

 #if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL
 #  ifdef GTSAM_USE_TBB
@ -85,4 +84,4 @@ namespace gtsam
    };
  }

-}
+}
--- a/gtsam/base/FastList.h
+++ b/gtsam/base/FastList.h
@ -74,7 +74,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
  }

--- a/gtsam/base/FastMap.h
+++ b/gtsam/base/FastMap.h
@ -19,9 +19,9 @@
 #pragma once

 #include <gtsam/base/FastDefaultAllocator.h>
-#include <map>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/map.hpp>
+#include <map>

 namespace gtsam {

@ -70,7 +70,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
  }
 };
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@ -21,6 +21,7 @@
 #include <gtsam/base/FastDefaultAllocator.h>
 #include <boost/mpl/has_xxx.hpp>
 #include <boost/utility/enable_if.hpp>
+#include <boost/serialization/serialization.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/set.hpp>
 #include <set>
@ -111,7 +112,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
  }
 };
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@ -95,7 +95,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
  }

--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@ -183,7 +183,7 @@ public:
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & boost::serialization::make_nvp("GenericValue",
              boost::serialization::base_object<Value>(*this));
      ar & boost::serialization::make_nvp("value", value_);
--- a/gtsam/base/Group.h
+++ b/gtsam/base/Group.h
@ -13,16 +13,20 @@
 * @file Group.h
 *
 * @brief Concept check class for variable types with Group properties
- * @date Nov 5, 2011
+ * @date November, 2011
 * @author Alex Cunningham
+ * @author Frank Dellaert
 */

 #pragma once

+#include <gtsam/base/Testable.h>
+
 #include <boost/concept_check.hpp>
 #include <boost/concept/requires.hpp>
 #include <boost/type_traits/is_base_of.hpp>
 #include <boost/static_assert.hpp>
+#include <utility>

 namespace gtsam {

@ -83,21 +87,119 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
      && traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
 }

-/// Macro to add group traits, additive flavor
-#define GTSAM_ADDITIVE_GROUP(GROUP) \
-    typedef additive_group_tag group_flavor; \
-    static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \
-    static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \
-    static GROUP Inverse(const GROUP &g) { return -g;}
+namespace internal {

-/// Macro to add group traits, multiplicative flavor
-#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \
-    typedef additive_group_tag group_flavor; \
-    static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \
-    static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \
-    static GROUP Inverse(const GROUP &g) { return g.inverse();}
+/// A helper class that implements the traits interface for multiplicative groups.
+/// Assumes existence of identity, operator* and inverse method
+template<class Class>
+struct MultiplicativeGroupTraits {
+  typedef group_tag structure_category;
+  typedef multiplicative_group_tag group_flavor;
+  static Class Identity() { return Class::identity(); }
+  static Class Compose(const Class &g, const Class & h) { return g * h;}
+  static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
+  static Class Inverse(const Class &g) { return g.inverse();}
+};

-} // \ namespace gtsam
+/// Both multiplicative group traits and Testable
+template<class Class>
+struct MultiplicativeGroup : MultiplicativeGroupTraits<Class>, Testable<Class> {};
+
+/// A helper class that implements the traits interface for additive groups.
+/// Assumes existence of identity and three additive operators
+template<class Class>
+struct AdditiveGroupTraits {
+  typedef group_tag structure_category;
+  typedef additive_group_tag group_flavor;
+  static Class Identity() { return Class::identity(); }
+  static Class Compose(const Class &g, const Class & h) { return g + h;}
+  static Class Between(const Class &g, const Class & h) { return h - g;}
+  static Class Inverse(const Class &g) { return -g;}
+};
+
+/// Both additive group traits and Testable
+template<class Class>
+struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
+
+}  // namespace internal
+
+/// compose multiple times
+template<typename G>
+BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
+compose_pow(const G& g, size_t n) {
+  if (n == 0) return traits<G>::Identity();
+  else if (n == 1) return g;
+  else return traits<G>::Compose(compose_pow(g, n - 1), g);
+}
+
+/// Template to construct the direct product of two arbitrary groups
+/// Assumes nothing except group structure and Testable from G and H
+template<typename G, typename H>
+class DirectProduct: public std::pair<G, H> {
+  BOOST_CONCEPT_ASSERT((IsGroup<G>));
+  BOOST_CONCEPT_ASSERT((IsGroup<H>));
+
+public:
+  /// Default constructor yields identity
+  DirectProduct():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
+
+  // Construct from two subgroup elements
+  DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
+
+  // identity
+  static DirectProduct identity() { return DirectProduct(); }
+
+  DirectProduct operator*(const DirectProduct& other) const {
+    return DirectProduct(traits<G>::Compose(this->first, other.first),
+        traits<H>::Compose(this->second, other.second));
+  }
+  DirectProduct inverse() const {
+    return DirectProduct(this->first.inverse(), this->second.inverse());
+  }
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<typename G, typename H>
+struct traits<DirectProduct<G, H> > :
+  internal::MultiplicativeGroupTraits<DirectProduct<G, H> > {};
+
+/// Template to construct the direct sum of two additive groups
+/// Assumes existence of three additive operators for both groups
+template<typename G, typename H>
+class DirectSum: public std::pair<G, H> {
+  BOOST_CONCEPT_ASSERT((IsGroup<G>));  // TODO(frank): check additive
+  BOOST_CONCEPT_ASSERT((IsGroup<H>));  // TODO(frank): check additive
+
+  const G& g() const { return this->first; }
+  const H& h() const { return this->second;}
+
+public:
+  /// Default constructor yields identity
+  DirectSum():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
+
+  // Construct from two subgroup elements
+  DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
+
+  // identity
+  static DirectSum identity() { return DirectSum(); }
+
+  DirectSum operator+(const DirectSum& other) const {
+    return DirectSum(g()+other.g(), h()+other.h());
+  }
+  DirectSum operator-(const DirectSum& other) const {
+    return DirectSum(g()-other.g(), h()-other.h());
+  }
+  DirectSum operator-() const {
+    return DirectSum(- g(), - h());
+  }
+};
+
+// Define direct sums to be a model of the Additive Group concept
+template<typename G, typename H>
+struct traits<DirectSum<G, H> > :
+  internal::AdditiveGroupTraits<DirectSum<G, H> > {};
+
+}  // namespace gtsam

 /**
 * Macros for using the IsGroup
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@ -136,8 +136,10 @@ namespace internal {
 /// A helper class that implements the traits interface for GTSAM lie groups.
 /// To use this for your gtsam type, define:
 /// template<> struct traits<Class> : public internal::LieGroupTraits<Class> {};
+/// Assumes existence of: identity, dimension, localCoordinates, retract,
+/// and additionally Logmap, Expmap, compose, between, and inverse
 template<class Class>
-struct LieGroupTraits : Testable<Class> {
+struct LieGroupTraits {
  typedef lie_group_tag structure_category;

  /// @name Group
@ -167,12 +169,10 @@ struct LieGroupTraits : Testable<Class> {
      ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
    return origin.retract(v, Horigin, Hv);
  }
-
  /// @}

  /// @name Lie Group
  /// @{
-
  static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
    return Class::Logmap(m, Hm);
  }
@ -195,11 +195,12 @@ struct LieGroupTraits : Testable<Class> {
      ChartJacobian H = boost::none) {
    return m.inverse(H);
  }
-
  /// @}
-
 };

+/// Both LieGroupTraits and Testable
+template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
+
 } // \ namepsace internal

 /**
@ -248,7 +249,7 @@ public:
    // log and exp map without Jacobians
    g = traits<T>::Expmap(v);
    v = traits<T>::Logmap(g);
-    // log and expnential map with Jacobians
+    // log and exponential map with Jacobians
    g = traits<T>::Expmap(v, Hg);
    v = traits<T>::Logmap(g, Hg);
  }
--- a/gtsam/base/LieMatrix_Deprecated.h
+++ b/gtsam/base/LieMatrix_Deprecated.h
@ -124,7 +124,7 @@ private:
  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("Matrix",
       boost::serialization::base_object<Matrix>(*this));

--- a/gtsam/base/LieVector_Deprecated.h
+++ b/gtsam/base/LieVector_Deprecated.h
@ -103,7 +103,7 @@ private:
  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("Vector",
       boost::serialization::base_object<Vector>(*this));
  }
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@ -46,7 +46,7 @@ struct manifold_tag {};
 * There may be multiple possible retractions for a given manifold, which can be chosen
 * between depending on the computational complexity.  The important criteria for
 * the creation for the retract and localCoordinates functions is that they be
- * inverse operations. The new notion of a Chart guarantees that.
+ * inverse operations.
 *
 */

@ -90,9 +90,9 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {

 /// A helper that implements the traits interface for GTSAM manifolds.
 /// To use this for your class type, define:
-/// template<> struct traits<Class> : public Manifold<Class> { };
+/// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
 template<class Class>
-struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
+struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {

  // Check that Class has the necessary machinery
  BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
@ -116,6 +116,9 @@ struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
  }
 };

+/// Both ManifoldTraits and Testable
+template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
+
 } // \ namespace internal

 /// Check invariants for Manifold type
@ -135,7 +138,7 @@ class IsManifold {
 public:

  typedef typename traits<T>::structure_category structure_category_tag;
-  static const size_t dim = traits<T>::dimension;
+  static const int dim = traits<T>::dimension;
  typedef typename traits<T>::ManifoldType ManifoldType;
  typedef typename traits<T>::TangentVector TangentVector;

@ -165,6 +168,53 @@ struct FixedDimension {
      "FixedDimension instantiated for dymanically-sized type.");
 };

+/// Helper class to construct the product manifold of two other manifolds, M1 and M2
+/// Assumes nothing except manifold structure from M1 and M2
+template<typename M1, typename M2>
+class ProductManifold: public std::pair<M1, M2> {
+  BOOST_CONCEPT_ASSERT((IsManifold<M1>));
+  BOOST_CONCEPT_ASSERT((IsManifold<M2>));
+
+protected:
+  enum { dimension1 = traits<M1>::dimension };
+  enum { dimension2 = traits<M2>::dimension };
+
+public:
+  enum { dimension = dimension1 + dimension2 };
+  inline static size_t Dim() { return dimension;}
+  inline size_t dim() const { return dimension;}
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  /// Default constructor yields identity
+  ProductManifold():std::pair<M1,M2>(traits<M1>::Identity(),traits<M2>::Identity()) {}
+
+  // Construct from two original manifold values
+  ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
+
+  /// Retract delta to manifold
+  ProductManifold retract(const TangentVector& xi) const {
+    M1 m1 = traits<M1>::Retract(this->first,  xi.template head<dimension1>());
+    M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
+    return ProductManifold(m1,m2);
+  }
+
+  /// Compute the coordinates in the tangent space
+  TangentVector localCoordinates(const ProductManifold& other) const {
+    typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first,  other.first);
+    typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
+    TangentVector v;
+    v << v1, v2;
+    return v;
+  }
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<typename M1, typename M2>
+struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
+};
+
 } // \ namespace gtsam

 ///**
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@ -20,8 +20,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/FastList.h>
-#include <gtsam/3rdparty/Eigen/Eigen/SVD>  
-#include <gtsam/3rdparty/Eigen/Eigen/LU>
+#include <Eigen/SVD>
+#include <Eigen/LU>

 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
@ -706,6 +706,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
  return ss.str();
 }

+/* ************************************************************************* */
 void inplace_QR(Matrix& A){
  size_t rows = A.rows();
  size_t cols = A.cols();
@ -716,7 +717,13 @@ void inplace_QR(Matrix& A){
  HCoeffsType hCoeffs(size);
  RowVectorType temp(cols);

+#ifdef GTSAM_USE_SYSTEM_EIGEN
+  // System-Eigen is used, and MKL is off
+  Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>(A, hCoeffs, 48, temp.data());
+#else
+  // Patched Eigen is used, and MKL is either on or off
  Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
+#endif

  zeroBelowDiagonal(A);
 }
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@ -23,9 +23,9 @@
 #pragma once
 #include <gtsam/base/Vector.h>
 #include <boost/math/special_functions/fpclassify.hpp>
-#include <gtsam/3rdparty/Eigen/Eigen/Core>
-#include <gtsam/3rdparty/Eigen/Eigen/Cholesky>
-#include <gtsam/3rdparty/Eigen/Eigen/LU>
+#include <Eigen/Core>
+#include <Eigen/Cholesky>
+#include <Eigen/LU>
 #include <boost/format.hpp>
 #include <boost/tuple/tuple.hpp>

@ -535,7 +535,7 @@ namespace boost {

    // split version - sends sizes ahead
    template<class Archive>
-    void save(Archive & ar, const gtsam::Matrix & m, unsigned int version) {
+    void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) {
      const size_t rows = m.rows(), cols = m.cols();
      ar << BOOST_SERIALIZATION_NVP(rows);
      ar << BOOST_SERIALIZATION_NVP(cols);
@ -543,7 +543,7 @@ namespace boost {
    }

    template<class Archive>
-    void load(Archive & ar, gtsam::Matrix & m, unsigned int version) {
+    void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) {
      size_t rows, cols;
      ar >> BOOST_SERIALIZATION_NVP(rows);
      ar >> BOOST_SERIALIZATION_NVP(cols);
--- a/gtsam/base/OptionalJacobian.h
+++ b/gtsam/base/OptionalJacobian.h
@ -19,7 +19,7 @@

 #pragma once

-#include <gtsam/3rdparty/Eigen/Eigen/Dense>
+#include <Eigen/Dense>

 #ifndef OPTIONALJACOBIAN_NOBOOST
 #include <boost/optional.hpp>
@ -83,7 +83,7 @@ public:
 #ifndef OPTIONALJACOBIAN_NOBOOST

  /// Constructor with boost::none just makes empty
-  OptionalJacobian(boost::none_t none) :
+  OptionalJacobian(boost::none_t /*none*/) :
      map_(NULL) {
  }

@ -142,7 +142,7 @@ public:
 #ifndef OPTIONALJACOBIAN_NOBOOST

  /// Constructor with boost::none just makes empty
-  OptionalJacobian(boost::none_t none) :
+  OptionalJacobian(boost::none_t /*none*/) :
    pointer_(NULL) {
  }

@ -168,5 +168,20 @@ public:
  Jacobian* operator->(){ return pointer_; }
 };

+// forward declare
+template <typename T> struct traits;
+
+/**
+ * @brief: meta-function to generate JacobianTA optional reference
+ * Used mainly by Expressions
+ * @param T return type
+ * @param A argument type
+ */
+template<class T, class A>
+struct MakeOptionalJacobian {
+  typedef OptionalJacobian<traits<T>::dimension,
+      traits<A>::dimension> type;
+};
+
 } // namespace gtsam

--- a/gtsam/base/ProductLieGroup.h
+++ b/gtsam/base/ProductLieGroup.h
@ -0,0 +1,197 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------1------------------------------------------- */
+
+/**
+ * @file ProductLieGroup.h
+ * @date May, 2015
+ * @author Frank Dellaert
+ * @brief Group product of two Lie Groups
+ */
+
+#pragma once
+
+#include <gtsam/base/Lie.h>
+#include <utility>  // pair
+
+namespace gtsam {
+
+/// Template to construct the product Lie group of two other Lie groups
+/// Assumes Lie group structure for G and H
+template<typename G, typename H>
+class ProductLieGroup: public std::pair<G, H> {
+  BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
+  typedef std::pair<G, H> Base;
+
+protected:
+  enum {dimension1 = traits<G>::dimension};
+  enum {dimension2 = traits<H>::dimension};
+
+public:
+  /// Default constructor yields identity
+  ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
+
+  // Construct from two subgroup elements
+  ProductLieGroup(const G& g, const H& h):Base(g,h) {}
+
+  // Construct from base
+  ProductLieGroup(const Base& base):Base(base) {}
+
+  /// @name Group
+  /// @{
+  typedef multiplicative_group_tag group_flavor;
+  static ProductLieGroup identity() {return ProductLieGroup();}
+
+  ProductLieGroup operator*(const ProductLieGroup& other) const {
+    return ProductLieGroup(traits<G>::Compose(this->first,other.first),
+        traits<H>::Compose(this->second,other.second));
+  }
+  ProductLieGroup inverse() const {
+    return ProductLieGroup(this->first.inverse(), this->second.inverse());
+  }
+  ProductLieGroup compose(const ProductLieGroup& g) const {
+    return (*this) * g;
+  }
+  ProductLieGroup between(const ProductLieGroup& g) const {
+    return this->inverse() * g;
+  }
+  /// @}
+
+  /// @name Manifold
+  /// @{
+  enum {dimension = dimension1 + dimension2};
+  inline static size_t Dim() {return dimension;}
+  inline size_t dim() const {return dimension;}
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  static ProductLieGroup Retract(const TangentVector& v) {
+    return ProductLieGroup::ChartAtOrigin::Retract(v);
+  }
+  static TangentVector LocalCoordinates(const ProductLieGroup& g) {
+    return ProductLieGroup::ChartAtOrigin::Local(g);
+  }
+  ProductLieGroup retract(const TangentVector& v) const {
+    return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
+  }
+  TangentVector localCoordinates(const ProductLieGroup& g) const {
+    return ProductLieGroup::ChartAtOrigin::Local(between(g));
+  }
+  /// @}
+
+  /// @name Lie Group
+  /// @{
+protected:
+  typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
+  typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
+  typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
+
+public:
+  ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
+    H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
+    if (H1) {
+      H1->setZero();
+      H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    if (H2) *H2 = Jacobian::Identity();
+    return ProductLieGroup(g,h);
+  }
+  ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
+    H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
+    if (H1) {
+      H1->setZero();
+      H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    if (H2) *H2 = Jacobian::Identity();
+    return ProductLieGroup(g,h);
+  }
+  ProductLieGroup inverse(ChartJacobian D) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
+    H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
+    if (D) {
+      D->setZero();
+      D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    return ProductLieGroup(g,h);
+  }
+  static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
+    if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet");
+    G g = traits<G>::Expmap(v.template head<dimension1>());
+    H h = traits<H>::Expmap(v.template tail<dimension2>());
+    return ProductLieGroup(g,h);
+  }
+  static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
+    if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet");
+    typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first);
+    typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second);
+    TangentVector v;
+    v << v1, v2;
+    return v;
+  }
+  struct ChartAtOrigin {
+    static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
+      return Logmap(m, Hm);
+    }
+    static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
+      return Expmap(v, Hv);
+    }
+  };
+  ProductLieGroup expmap(const TangentVector& v) const {
+    return compose(ProductLieGroup::Expmap(v));
+  }
+  TangentVector logmap(const ProductLieGroup& g) const {
+    return ProductLieGroup::Logmap(between(g));
+  }
+  static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
+    return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
+  }
+  static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
+    return ProductLieGroup::ChartAtOrigin::Local(g,H1);
+  }
+  ProductLieGroup retract(const TangentVector& v, //
+      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+    Jacobian D_g_v;
+    ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
+    ProductLieGroup h = compose(g,H1,H2);
+    if (H2) *H2 = (*H2) * D_g_v;
+    return h;
+  }
+  TangentVector localCoordinates(const ProductLieGroup& g, //
+      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+    ProductLieGroup h = between(g,H1,H2);
+    Jacobian D_v_h;
+    TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
+    if (H1) *H1 = D_v_h * (*H1);
+    if (H2) *H2 = D_v_h * (*H2);
+    return v;
+  }
+  /// @}
+
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<typename G, typename H>
+struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
+    ProductLieGroup<G, H> > {
+};
+} // namespace gtsam
+
--- a/gtsam/base/SymmetricBlockMatrix.cpp
+++ b/gtsam/base/SymmetricBlockMatrix.cpp
@ -61,13 +61,13 @@ VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(

  // Split conditional

-    // Create one big conditionals with many frontal variables.
-    gttic(Construct_conditional);
-    const size_t varDim = offset(nFrontals);
-    VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
-    Ab.full() = matrix_.topRows(varDim);
-    Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
-    gttoc(Construct_conditional);
+  // Create one big conditionals with many frontal variables.
+  gttic(Construct_conditional);
+  const size_t varDim = offset(nFrontals);
+  VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
+  Ab.full() = matrix_.topRows(varDim);
+  Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
+  gttoc(Construct_conditional);

  gttic(Remaining_factor);
  // Take lower-right block of Ab_ to get the remaining factor
--- a/gtsam/base/SymmetricBlockMatrix.h
+++ b/gtsam/base/SymmetricBlockMatrix.h
@ -199,6 +199,7 @@ namespace gtsam {

    void checkBlock(DenseIndex block) const
    {
+      static_cast<void>(block); //Disable unused varibale warnings.
      assert(matrix_.rows() == matrix_.cols());
      assert(matrix_.cols() == variableColOffsets_.back());
      assert(block >= 0);
@ -235,7 +236,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      // Fill in the lower triangle part of the matrix, so boost::serialization won't
      // complain about uninitialized data with an input_stream_error exception
      // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error
--- a/gtsam/base/SymmetricBlockMatrixBlockExpr.h
+++ b/gtsam/base/SymmetricBlockMatrixBlockExpr.h
@ -93,7 +93,7 @@ namespace gtsam

    /// Create a SymmetricBlockMatrixBlockExpr from the specified range of blocks of a
    /// SymmetricBlockMatrix.
-    SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char dummy) :
+    SymmetricBlockMatrixBlockExpr(SymmetricBlockMatrixType& blockMatrix, Index firstBlock, Index blocks, char /*dummy*/) :
      xpr_(blockMatrix), myBlock_(blockMatrix.matrix_.block(0, 0, 0, 0))
    {
      initIndices(firstBlock, firstBlock, blocks, blocks);
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@ -121,7 +121,7 @@ namespace gtsam {
    virtual Vector localCoordinates_(const Value& value) const = 0;

    /** Assignment operator */
-    virtual Value& operator=(const Value& rhs) {
+    virtual Value& operator=(const Value& /*rhs*/) {
      //needs a empty definition so recursion in implicit derived assignment operators work
     return *this;
    }
@ -166,7 +166,7 @@ namespace gtsam {
     * */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) {
    }

  };
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@ -222,11 +222,7 @@ double norm_2(const Vector& v) {

 /* ************************************************************************* */
 Vector reciprocal(const Vector &a) {
-  size_t n = a.size();
-  Vector b(n);
-  for( size_t i = 0; i < n; i++ )
-    b(i) = 1.0/a(i);
-  return b;
+  return a.array().inverse();
 }

 /* ************************************************************************* */
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@ -26,7 +26,7 @@
 #endif

 #include <gtsam/global_includes.h>
-#include <gtsam/3rdparty/Eigen/Eigen/Core>
+#include <Eigen/Core>
 #include <iosfwd>
 #include <list>

@ -355,14 +355,14 @@ namespace boost {

    // split version - copies into an STL vector for serialization
    template<class Archive>
-    void save(Archive & ar, const gtsam::Vector & v, unsigned int version) {
+    void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) {
      const size_t size = v.size();
      ar << BOOST_SERIALIZATION_NVP(size);
      ar << make_nvp("data", make_array(v.data(), v.size()));
    }

    template<class Archive>
-    void load(Archive & ar, gtsam::Vector & v, unsigned int version) {
+    void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) {
      size_t size;
      ar >> BOOST_SERIALIZATION_NVP(size);
      v.resize(size);
@ -371,12 +371,12 @@ namespace boost {

    // split version - copies into an STL vector for serialization
    template<class Archive, int D>
-    void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int version) {
+    void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
      ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
    }

    template<class Archive, int D>
-    void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int version) {
+    void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
      ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
    }

--- a/gtsam/base/VectorSpace.h
+++ b/gtsam/base/VectorSpace.h
@ -20,17 +20,10 @@ template<typename T> struct traits;

 namespace internal {

-/// VectorSpace Implementation for Fixed sizes
+/// VectorSpaceTraits Implementation for Fixed sizes
 template<class Class, int N>
 struct VectorSpaceImpl {

-  /// @name Group
-  /// @{
-  static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
-  static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
-  static Class Inverse(const Class& m) { return -m;}
-  /// @}
-
  /// @name Manifold
  /// @{
  typedef Eigen::Matrix<double, N, 1> TangentVector;
@ -68,21 +61,21 @@ struct VectorSpaceImpl {
    return Class(v);
  }

-  static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+  static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
+      ChartJacobian H2 = boost::none) {
    if (H1) *H1 = Jacobian::Identity();
    if (H2) *H2 = Jacobian::Identity();
    return v1 + v2;
  }

-  static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
-      ChartJacobian H2) {
+  static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
+      ChartJacobian H2 = boost::none) {
    if (H1) *H1 = - Jacobian::Identity();
    if (H2) *H2 =   Jacobian::Identity();
    return v2 - v1;
  }

-  static Class Inverse(const Class& v, ChartJacobian H) {
+  static Class Inverse(const Class& v, ChartJacobian H = boost::none) {
    if (H) *H = - Jacobian::Identity();
    return -v;
  }
@ -90,7 +83,7 @@ struct VectorSpaceImpl {
  /// @}
 };

-/// VectorSpace implementation for dynamic types.
+/// VectorSpaceTraits implementation for dynamic types.
 template<class Class>
 struct VectorSpaceImpl<Class,Eigen::Dynamic> {

@ -166,11 +159,11 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
  /// @}
 };

-/// A helper that implements the traits interface for GTSAM lie groups.
+/// A helper that implements the traits interface for GTSAM vector space types.
 /// To use this for your gtsam type, define:
-/// template<> struct traits<Type> : public VectorSpace<Type> { };
+/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { };
 template<class Class>
-struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
+struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {

  typedef vector_space_tag structure_category;

@ -185,9 +178,12 @@ struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
  enum { dimension = Class::dimension};
  typedef Class ManifoldType;
  /// @}
-
 };

+/// Both VectorSpaceTRaits and Testable
+template<class Class>
+struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
+
 /// A helper that implements the traits interface for GTSAM lie groups.
 /// To use this for your gtsam type, define:
 /// template<> struct traits<Type> : public ScalarTraits<Type> { };
@ -401,7 +397,8 @@ struct DynamicTraits {
    return result;
  }

-  static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
+  static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) {
+    static_cast<void>(H);
    throw std::runtime_error("Expmap not defined for dynamic types");
  }

--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@ -199,6 +199,7 @@ namespace gtsam {
    }

    void checkBlock(DenseIndex block) const {
+      static_cast<void>(block); //Disable unused varibale warnings.
      assert(matrix_.cols() == variableColOffsets_.back());
      assert(block < (DenseIndex)variableColOffsets_.size() - 1);
      assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
@ -220,7 +221,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(matrix_);
      ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
      ar & BOOST_SERIALIZATION_NVP(rowStart_);
--- a/gtsam/base/tests/testGroup.cpp
+++ b/gtsam/base/tests/testGroup.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   testGroup.cpp
+ * @brief  Unit tests for groups
+ * @author Frank Dellaert
+ **/
+
+#include <gtsam/base/Group.h>
+#include <gtsam/base/Testable.h>
+#include <Eigen/Core>
+#include <iostream>
+#include <boost/foreach.hpp>
+
+namespace gtsam {
+
+/// Symmetric group
+template<int N>
+class Symmetric: private Eigen::PermutationMatrix<N> {
+  Symmetric(const Eigen::PermutationMatrix<N>& P) :
+      Eigen::PermutationMatrix<N>(P) {
+  }
+public:
+  static Symmetric identity() { return Symmetric(); }
+  Symmetric() {
+    Eigen::PermutationMatrix<N>::setIdentity();
+  }
+  static Symmetric Transposition(int i, int j) {
+    Symmetric g;
+    return g.applyTranspositionOnTheRight(i, j);
+  }
+  Symmetric operator*(const Symmetric& other) const {
+    return Eigen::PermutationMatrix<N>::operator*(other);
+  }
+  bool operator==(const Symmetric& other) const {
+    for (size_t i = 0; i < N; i++)
+      if (this->indices()[i] != other.indices()[i])
+        return false;
+    return true;
+  }
+  Symmetric inverse() const {
+    return Eigen::PermutationMatrix<N>(Eigen::PermutationMatrix<N>::inverse());
+  }
+  friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) {
+    for (size_t i = 0; i < N; i++)
+      os << m.indices()[i] << " ";
+    return os;
+  }
+  void print(const std::string& s = "") const {
+    std::cout << s << *this << std::endl;
+  }
+  bool equals(const Symmetric<N>& other, double tol = 0) const {
+    return this->indices() == other.indices();
+  }
+};
+
+/// Define permutation group traits to be a model of the Multiplicative Group concept
+template<int N>
+struct traits<Symmetric<N> > : internal::MultiplicativeGroupTraits<Symmetric<N> >,
+    Testable<Symmetric<N> > {
+};
+
+} // namespace gtsam
+
+#include <gtsam/base/Testable.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+typedef Symmetric<2> S2;
+TEST(Group, S2) {
+  S2 e, s1 = S2::Transposition(0, 1);
+  BOOST_CONCEPT_ASSERT((IsGroup<S2>));
+  EXPECT(check_group_invariants(e, s1));
+}
+
+//******************************************************************************
+typedef Symmetric<3> S3;
+TEST(Group, S3) {
+  S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
+  BOOST_CONCEPT_ASSERT((IsGroup<S3>));
+  EXPECT(check_group_invariants(e, s1));
+  EXPECT(assert_equal(s1, s1 * e));
+  EXPECT(assert_equal(s1, e * s1));
+  EXPECT(assert_equal(e, s1 * s1));
+  S3 g = s1 * s2; // 1 2 0
+  EXPECT(assert_equal(s1, g * s2));
+  EXPECT(assert_equal(e, compose_pow(g, 0)));
+  EXPECT(assert_equal(g, compose_pow(g, 1)));
+  EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup
+}
+
+//******************************************************************************
+// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon,
+// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon)
+namespace gtsam {
+typedef DirectProduct<S2, S3> Dih6;
+
+std::ostream &operator<<(std::ostream &os, const Dih6& m) {
+  os << "( " << m.first << ", " << m.second << ")";
+  return os;
+}
+
+// Provide traits with Testable
+
+template<>
+struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
+  static void Print(const Dih6& m, const string& s = "") {
+    cout << s << m << endl;
+  }
+  static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) {
+    return m1 == m2;
+  }
+};
+} // namespace gtsam
+
+TEST(Group, Dih6) {
+  Dih6 e, g(S2::Transposition(0, 1),
+      S3::Transposition(0, 1) * S3::Transposition(1, 2));
+  BOOST_CONCEPT_ASSERT((IsGroup<Dih6>));
+  EXPECT(check_group_invariants(e, g));
+  EXPECT(assert_equal(e, compose_pow(g, 0)));
+  EXPECT(assert_equal(g, compose_pow(g, 1)));
+  EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+
--- a/gtsam/base/tests/testLieScalar.cpp
+++ b/gtsam/base/tests/testLieScalar.cpp
@ -37,8 +37,8 @@ TEST(LieScalar , Concept) {
 //******************************************************************************
 TEST(LieScalar , Invariants) {
  LieScalar lie1(2), lie2(3);
-  check_group_invariants(lie1, lie2);
-  check_manifold_invariants(lie1, lie2);
+  CHECK(check_group_invariants(lie1, lie2));
+  CHECK(check_manifold_invariants(lie1, lie2));
 }

 /* ************************************************************************* */
--- a/gtsam/base/tests/testTreeTraversal.cpp
+++ b/gtsam/base/tests/testTreeTraversal.cpp
@ -45,11 +45,11 @@ struct TestForest {
 };

 TestForest makeTestForest() {
-  //    0     1
-  //   / \
-  //  2   3
-  //      |
-  //      4
+  //     0     1
+  //   / |
+  //  2  3
+  //     |
+  //     4
  TestForest forest;
  forest.roots_.push_back(boost::make_shared<TestNode>(0));
  forest.roots_.push_back(boost::make_shared<TestNode>(1));
--- a/gtsam/base/timing.cpp
+++ b/gtsam/base/timing.cpp
@ -16,24 +16,28 @@
 * @date     Oct 5, 2010
 */

-#include <math.h>
-#include <stdio.h>
-#include <iostream>
-#include <iomanip>
-#include <stdlib.h>
-#include <boost/foreach.hpp>
-#include <boost/format.hpp>
-#include <boost/algorithm/string.hpp>
-
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>

+#include <boost/algorithm/string/replace.hpp>
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
+
+#include <cmath>
+#include <cstddef>
+#include <cassert>
+#include <iomanip>
+#include <iostream>
+#include <map>
+#include <stdexcept>
+#include <utility>
+
 namespace gtsam {
 namespace internal {

-GTSAM_EXPORT boost::shared_ptr<TimingOutline> timingRoot(
+GTSAM_EXPORT boost::shared_ptr<TimingOutline> gTimingRoot(
    new TimingOutline("Total", getTicTocID("Total")));
-GTSAM_EXPORT boost::weak_ptr<TimingOutline> timingCurrent(timingRoot);
+GTSAM_EXPORT boost::weak_ptr<TimingOutline> gCurrentTimer(gTimingRoot);

 /* ************************************************************************* */
 // Implementation of TimingOutline
@ -50,8 +54,8 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) {
 }

 /* ************************************************************************* */
-TimingOutline::TimingOutline(const std::string& label, size_t myId) :
-    myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
+TimingOutline::TimingOutline(const std::string& label, size_t id) :
+    id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
        0), lastChildOrder_(0), label_(label) {
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
  timer_.stop();
@ -153,7 +157,7 @@ const boost::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
 }

 /* ************************************************************************* */
-void TimingOutline::ticInternal() {
+void TimingOutline::tic() {
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
  assert(timer_.is_stopped());
  timer_.start();
@ -169,7 +173,7 @@ void TimingOutline::ticInternal() {
 }

 /* ************************************************************************* */
-void TimingOutline::tocInternal() {
+void TimingOutline::toc() {
 #ifdef GTSAM_USING_NEW_BOOST_TIMERS

  assert(!timer_.is_stopped());
@ -212,7 +216,6 @@ void TimingOutline::finishedIteration() {
 }

 /* ************************************************************************* */
-// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements
 size_t getTicTocID(const char *descriptionC) {
  const std::string description(descriptionC);
  // Global (static) map from strings to ID numbers and current next ID number
@ -232,37 +235,33 @@ size_t getTicTocID(const char *descriptionC) {
 }

 /* ************************************************************************* */
-void ticInternal(size_t id, const char *labelC) {
+void tic(size_t id, const char *labelC) {
  const std::string label(labelC);
-  if (ISDEBUG("timing-verbose"))
-    std::cout << "gttic_(" << id << ", " << label << ")" << std::endl;
  boost::shared_ptr<TimingOutline> node = //
-      timingCurrent.lock()->child(id, label, timingCurrent);
-  timingCurrent = node;
-  node->ticInternal();
+      gCurrentTimer.lock()->child(id, label, gCurrentTimer);
+  gCurrentTimer = node;
+  node->tic();
 }

 /* ************************************************************************* */
-void tocInternal(size_t id, const char *label) {
-  if (ISDEBUG("timing-verbose"))
-    std::cout << "gttoc(" << id << ", " << label << ")" << std::endl;
-  boost::shared_ptr<TimingOutline> current(timingCurrent.lock());
-  if (id != current->myId_) {
-    timingRoot->print();
+void toc(size_t id, const char *label) {
+  boost::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
+  if (id != current->id_) {
+    gTimingRoot->print();
    throw std::invalid_argument(
        (boost::format(
            "gtsam timing:  Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".")
            % label % current->label_).str());
  }
  if (!current->parent_.lock()) {
-    timingRoot->print();
+    gTimingRoot->print();
    throw std::invalid_argument(
        (boost::format(
            "gtsam timing:  Mismatched tic/toc: extra gttoc(\"%s\"), already at the root")
            % label).str());
  }
-  current->tocInternal();
-  timingCurrent = current->parent_;
+  current->toc();
+  gCurrentTimer = current->parent_;
 }

 } // namespace internal
--- a/gtsam/base/timing.h
+++ b/gtsam/base/timing.h
@ -17,12 +17,15 @@
 */
 #pragma once

-#include <string>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <boost/version.hpp>
-#include <gtsam/global_includes.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/dllexport.h>
+
+#include <boost/smart_ptr/shared_ptr.hpp>
+#include <boost/smart_ptr/weak_ptr.hpp>
+#include <boost/version.hpp>
+
+#include <cstddef>
+#include <string>

 // This file contains the GTSAM timing instrumentation library, a low-overhead method for
 // learning at a medium-fine level how much time various components of an algorithm take
@ -125,16 +128,21 @@
 namespace gtsam {

  namespace internal {
+    // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements
    GTSAM_EXPORT size_t getTicTocID(const char *description);
-    GTSAM_EXPORT void ticInternal(size_t id, const char *label);
-    GTSAM_EXPORT void tocInternal(size_t id, const char *label);
+
+    // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method
+    GTSAM_EXPORT void tic(size_t id, const char *label);
+
+    // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer
+    GTSAM_EXPORT void toc(size_t id, const char *label);

    /**
     * Timing Entry, arranged in a tree
     */
    class GTSAM_EXPORT TimingOutline {
    protected:
-      size_t myId_;
+      size_t id_;
      size_t t_;
      size_t tWall_;
      double t2_ ; ///< cache the \sum t_i^2
@ -176,29 +184,38 @@ namespace gtsam {
      void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
      const boost::shared_ptr<TimingOutline>&
        child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
-      void ticInternal();
-      void tocInternal();
+      void tic();
+      void toc();
      void finishedIteration();

-      GTSAM_EXPORT friend void tocInternal(size_t id, const char *label);
+      GTSAM_EXPORT friend void toc(size_t id, const char *label);
    }; // \TimingOutline

    /**
-     * No documentation
+     * Small class that calls internal::tic at construction, and internol::toc when destroyed
     */
    class AutoTicToc {
-    private:
+     private:
      size_t id_;
-      const char *label_;
+      const char* label_;
      bool isSet_;
-    public:
-      AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); }
-      void stop() { tocInternal(id_, label_); isSet_ = false; }
-      ~AutoTicToc() { if(isSet_) stop(); }
+
+     public:
+      AutoTicToc(size_t id, const char* label)
+          : id_(id), label_(label), isSet_(true) {
+        tic(id_, label_);
+      }
+      void stop() {
+        toc(id_, label_);
+        isSet_ = false;
+      }
+      ~AutoTicToc() {
+        if (isSet_) stop();
+      }
    };

-    GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> timingRoot;
-    GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> timingCurrent;
+    GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> gTimingRoot;
+    GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> gCurrentTimer;
  }

 // Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined)
@ -210,7 +227,7 @@ namespace gtsam {
 // tic
 #define gttic_(label) \
  static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
-  ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label)
+  ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label)

 // toc
 #define gttoc_(label) \
@ -228,26 +245,26 @@ namespace gtsam {

 // indicate iteration is finished
 inline void tictoc_finishedIteration_() {
-  ::gtsam::internal::timingRoot->finishedIteration(); }
+  ::gtsam::internal::gTimingRoot->finishedIteration(); }

 // print
 inline void tictoc_print_() {
-  ::gtsam::internal::timingRoot->print(); }
+  ::gtsam::internal::gTimingRoot->print(); }

 // print mean and standard deviation
 inline void tictoc_print2_() {
-  ::gtsam::internal::timingRoot->print2(); }
+  ::gtsam::internal::gTimingRoot->print2(); }

 // get a node by label and assign it to variable
 #define tictoc_getNode(variable, label) \
  static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \
  const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \
-  ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent);
+  ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer);

 // reset
 inline void tictoc_reset_() {
-  ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total")));
-  ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; }
+  ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total")));
+  ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; }

 #ifdef ENABLE_TIMING
 #define gttic(label) gttic_(label)
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@ -25,7 +25,7 @@
 #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@"

 // Paths to example datasets distributed with GTSAM
-#define GTSAM_SOURCE_TREE_DATASET_DIR "@CMAKE_SOURCE_DIR@/examples/Data"
+#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data"
 #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data"

 // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices)
@ -42,6 +42,9 @@
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB

+// Whether we are using system-Eigen or our own patched version
+#cmakedefine GTSAM_USE_SYSTEM_EIGEN
+
 // Whether Eigen will use MKL (if MKL was found and GTSAM_WITH_EIGEN_MKL is enabled in CMake)
 #cmakedefine GTSAM_USE_EIGEN_MKL
 #cmakedefine EIGEN_USE_MKL_ALL // This is also defined in gtsam_eigen_includes.h
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@ -467,7 +467,7 @@ namespace gtsam {

    // find highest label among branches
    boost::optional<L> highestLabel;
-    boost::optional<size_t> nrChoices;
+    size_t nrChoices = 0;
    for (Iterator it = begin; it != end; it++) {
      if (it->root_->isLeaf())
        continue;
@ -475,22 +475,21 @@ namespace gtsam {
          boost::dynamic_pointer_cast<const Choice>(it->root_);
      if (!highestLabel || c->label() > *highestLabel) {
        highestLabel.reset(c->label());
-        nrChoices.reset(c->nrChoices());
+        nrChoices = c->nrChoices();
      }
    }

    // if label is already in correct order, just put together a choice on label
-    if (!highestLabel || !nrChoices || label > *highestLabel) {
+    if (!nrChoices || !highestLabel || label > *highestLabel) {
      boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
      for (Iterator it = begin; it != end; it++)
        choiceOnLabel->push_back(it->root_);
      return Choice::Unique(choiceOnLabel);
    } else {
      // Set up a new choice on the highest label
-      boost::shared_ptr<Choice> choiceOnHighestLabel(
-          new Choice(*highestLabel, *nrChoices));
+      boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, nrChoices));
      // now, for all possible values of highestLabel
-      for (size_t index = 0; index < *nrChoices; index++) {
+      for (size_t index = 0; index < nrChoices; index++) {
        // make a new set of functions for composing by iterating over the given
        // functions, and selecting the appropriate branch.
        std::vector<DecisionTree> functions;
--- a/gtsam/discrete/DiscreteBayesNet.h
+++ b/gtsam/discrete/DiscreteBayesNet.h
@ -90,7 +90,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    }
  };
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@ -159,7 +159,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(f_);
    ar & BOOST_SERIALIZATION_NVP(k1_);
    ar & BOOST_SERIALIZATION_NVP(k2_);
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@ -109,7 +109,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version)
+  void serialize(Archive & ar, const unsigned int /*version*/)
  {
    ar & boost::serialization::make_nvp("Cal3DS2",
        boost::serialization::base_object<Cal3DS2_Base>(*this));
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@ -153,7 +153,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version)
+  void serialize(Archive & ar, const unsigned int /*version*/)
  {
    ar & BOOST_SERIALIZATION_NVP(fx_);
    ar & BOOST_SERIALIZATION_NVP(fy_);
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@ -134,7 +134,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version)
+  void serialize(Archive & ar, const unsigned int /*version*/)
  {
    ar & boost::serialization::make_nvp("Cal3Unified",
        boost::serialization::base_object<Cal3DS2_Base>(*this));
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@ -21,7 +21,6 @@

 #pragma once

-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>

 namespace gtsam {
@ -211,7 +210,7 @@ private:
  /// Serialization function
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(fx_);
    ar & BOOST_SERIALIZATION_NVP(fy_);
    ar & BOOST_SERIALIZATION_NVP(s_);
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@ -144,7 +144,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class Archive>
-    void serialize(Archive & ar, const unsigned int version)
+    void serialize(Archive & ar, const unsigned int /*version*/)
    {
      ar & BOOST_SERIALIZATION_NVP(K_);
      ar & BOOST_SERIALIZATION_NVP(b_);
--- a/gtsam/geometry/CalibratedCamera.cpp
+++ b/gtsam/geometry/CalibratedCamera.cpp
@ -93,12 +93,6 @@ Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) {
  return Point2(u, v);
 }

-/* ************************************************************************* */
-Point2 PinholeBase::project_to_camera_old(const Point3& pc,
-    OptionalJacobian<2, 3> Dpoint) {
-  return Project(pc);
-}
-
 /* ************************************************************************* */
 Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) {
  if (Dpoint) {
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@ -167,10 +167,6 @@ public:
  static Point2 Project(const Point3& pc, //
      OptionalJacobian<2, 3> Dpoint = boost::none);

-  /// @deprecated not correct naming for static function, use Project above
-  static Point2 project_to_camera_old(const Point3& pc, //
-      OptionalJacobian<2, 3> Dpoint = boost::none);
-
  /**
   * Project from 3D point at infinity in camera coordinates into image
   * Does *not* throw a CheiralityException, even if pc behind image plane
@ -222,7 +218,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(pose_);
  }

@ -374,7 +370,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar
        & boost::serialization::make_nvp("PinholeBase",
            boost::serialization::base_object<PinholeBase>(*this));
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@ -319,7 +319,7 @@ private:
  /// Serialization function
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & (*this);
  }
 };
--- a/gtsam/geometry/Cyclic.h
+++ b/gtsam/geometry/Cyclic.h
@ -16,10 +16,8 @@
 **/

 #include <gtsam/base/Group.h>
-#include <cstddef>
-#include <string>
-#include <iostream>
-#include <assert.h>
+#include <gtsam/base/Testable.h>
+#include <iostream> // for cout :-(

 namespace gtsam {

@ -33,10 +31,11 @@ public:
      i_(i) {
    assert(i < N);
  }
-  /// Idenity element
-  static Cyclic Identity() {
-    return Cyclic(0);
+  /// Default constructor yields identity
+  Cyclic():i_(0) {
  }
+  static Cyclic identity() { return Cyclic();}
+
  /// Cast to size_t
  operator size_t() const {
    return i_;
@ -63,20 +62,10 @@ public:
  }
 };

-/// Define cyclic group traits to be a model of the Group concept
+/// Define cyclic group to be a model of the Additive Group concept
 template<size_t N>
-struct traits<Cyclic<N> > {
-  typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic<N>)
-  static Cyclic<N> Identity() {
-    return Cyclic<N>::Identity();
-  }
-  static bool Equals(const Cyclic<N>& a, const Cyclic<N>& b,
-      double tol = 1e-9) {
-    return a.equals(b, tol);
-  }
-  static void Print(const Cyclic<N>& c, const std::string &s = "") {
-    c.print(s);
-  }
+struct traits<Cyclic<N> > : internal::AdditiveGroupTraits<Cyclic<N> >, //
+    Testable<Cyclic<N> > {
 };

 } // \namespace gtsam
--- a/gtsam/geometry/EssentialMatrix.cpp
+++ b/gtsam/geometry/EssentialMatrix.cpp
@ -13,64 +13,46 @@ using namespace std;
 namespace gtsam {

 /* ************************************************************************* */
-EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
+EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
    OptionalJacobian<5, 6> H) {
-  const Rot3& _1R2_ = _1P2_.rotation();
-  const Point3& _1T2_ = _1P2_.translation();
+  const Rot3& aRb = aPb.rotation();
+  const Point3& aTb = aPb.translation();
  if (!H) {
    // just make a direction out of translation and create E
-    Unit3 direction(_1T2_);
-    return EssentialMatrix(_1R2_, direction);
+    Unit3 direction(aTb);
+    return EssentialMatrix(aRb, direction);
  } else {
    // Calculate the 5*6 Jacobian H = D_E_1P2
    // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
    // First get 2*3 derivative from Unit3::FromPoint3
    Matrix23 D_direction_1T2;
-    Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
+    Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2);
    *H << I_3x3, Z_3x3, //
-    Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
-    return EssentialMatrix(_1R2_, direction);
+    Matrix23::Zero(), D_direction_1T2 * aRb.matrix();
+    return EssentialMatrix(aRb, direction);
  }
 }

 /* ************************************************************************* */
 void EssentialMatrix::print(const string& s) const {
  cout << s;
-  aRb_.print("R:\n");
-  aTb_.print("d: ");
-}
-
-/* ************************************************************************* */
-EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
-  assert(xi.size() == 5);
-  Vector3 omega(sub(xi, 0, 3));
-  Vector2 z(sub(xi, 3, 5));
-  Rot3 R = aRb_.retract(omega);
-  Unit3 t = aTb_.retract(z);
-  return EssentialMatrix(R, t);
-}
-
-/* ************************************************************************* */
-Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
-  Vector5 v;
-  v << aRb_.localCoordinates(other.aRb_),
-      aTb_.localCoordinates(other.aTb_);
-  return v;
+  rotation().print("R:\n");
+  direction().print("d: ");
 }

 /* ************************************************************************* */
 Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
    OptionalJacobian<3, 3> Dpoint) const {
-  Pose3 pose(aRb_, aTb_.point3());
+  Pose3 pose(rotation(), direction().point3());
  Matrix36 DE_;
  Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
  if (DE) {
    // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
    // The last 3 columns are derivative with respect to change in translation
-    // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
+    // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
    // Duy made an educated guess that this needs to be rotated to the local frame
    Matrix35 H;
-    H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
+    H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis();
    *DE = H;
  }
  return q;
@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
    OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {

  // The rotation must be conjugated to act in the camera frame
-  Rot3 c1Rc2 = aRb_.conjugate(cRb);
+  Rot3 c1Rc2 = rotation().conjugate(cRb);

  if (!HE && !HR) {
    // Rotate translation direction and return
-    Unit3 c1Tc2 = cRb * aTb_;
+    Unit3 c1Tc2 = cRb * direction();
    return EssentialMatrix(c1Rc2, c1Tc2);
  } else {
    // Calculate derivatives
    Matrix23 D_c1Tc2_cRb; // 2*3
    Matrix2 D_c1Tc2_aTb; // 2*2
-    Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
+    Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb);
    if (HE)
      *HE << cRb.matrix(), Matrix32::Zero(), //
      Matrix23::Zero(), D_c1Tc2_aTb;
@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
  if (H) {
    // See math.lyx
    Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
-    Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
-        * aTb_.basis();
+    Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
+        * direction().basis();
    *H << HR, HD;
  }
  return dot(vA, E_ * vB);
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@ -10,6 +10,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Manifold.h>
 #include <iosfwd>

 namespace gtsam {
@ -20,17 +21,18 @@ namespace gtsam {
 * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
 * We can then non-linearly optimize immediately on this 5-dimensional manifold.
 */
-class GTSAM_EXPORT EssentialMatrix {
+class GTSAM_EXPORT EssentialMatrix : private ProductManifold<Rot3, Unit3> {

 private:
-
-  Rot3 aRb_; ///< Rotation between a and b
-  Unit3 aTb_; ///< translation direction from a to b
+  typedef ProductManifold<Rot3, Unit3> Base;
  Matrix3 E_; ///< Essential matrix

-public:
+  /// Construct from Base
+  EssentialMatrix(const Base& base) :
+      Base(base), E_(direction().skew() * rotation().matrix()) {
+  }

-  enum { dimension = 5 };
+public:

  /// Static function to convert Point2 to homogeneous coordinates
  static Vector3 Homogeneous(const Point2& p) {
@ -42,12 +44,12 @@ public:

  /// Default constructor
  EssentialMatrix() :
-      aTb_(1, 0, 0), E_(aTb_.skew()) {
+      Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
  }

  /// Construct from rotation and translation
  EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
-      aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
+      Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
  }

  /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
@ -72,7 +74,8 @@ public:

  /// assert equality up to a tolerance
  bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
-    return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
+    return rotation().equals(other.rotation(), tol)
+        && direction().equals(other.direction(), tol);
  }

  /// @}
@ -80,22 +83,19 @@ public:
  /// @name Manifold
  /// @{

-  /// Dimensionality of tangent space = 5 DOF
-  inline static size_t Dim() {
-    return 5;
-  }
-
-  /// Return the dimensionality of the tangent space
-  size_t dim() const {
-    return 5;
-  }
+  using Base::dimension;
+  using Base::dim;
+  using Base::Dim;

  /// Retract delta to manifold
-  EssentialMatrix retract(const Vector& xi) const;
+  EssentialMatrix retract(const TangentVector& v) const {
+    return Base::retract(v);
+  }

  /// Compute the coordinates in the tangent space
-  Vector5 localCoordinates(const EssentialMatrix& other) const;
-
+  TangentVector localCoordinates(const EssentialMatrix& other) const {
+    return Base::localCoordinates(other);
+  }
  /// @}

  /// @name Essential matrix methods
@ -103,12 +103,12 @@ public:

  /// Rotation
  inline const Rot3& rotation() const {
-    return aRb_;
+    return this->first;
  }

  /// Direction
  inline const Unit3& direction() const {
-    return aTb_;
+    return this->second;
  }

  /// Return 3*3 matrix representation
@ -118,12 +118,12 @@ public:

  /// Return epipole in image_a , as Unit3 to allow for infinity
  inline const Unit3& epipole_a() const {
-    return aTb_; // == direction()
+    return direction();
  }

  /// Return epipole in image_b, as Unit3 to allow for infinity
  inline Unit3 epipole_b() const {
-    return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
+    return rotation().unrotate(direction());
  }

  /**
@ -179,9 +179,9 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & BOOST_SERIALIZATION_NVP(aRb_);
-      ar & BOOST_SERIALIZATION_NVP(aTb_);
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & BOOST_SERIALIZATION_NVP(first);
+      ar & BOOST_SERIALIZATION_NVP(second);

      ar & boost::serialization::make_nvp("E11", E_(0,0));
      ar & boost::serialization::make_nvp("E12", E_(0,1));
@ -195,7 +195,6 @@ private:
    }

  /// @}
-
 };

 template<>
--- a/gtsam/geometry/OrientedPlane3.cpp
+++ b/gtsam/geometry/OrientedPlane3.cpp
@ -73,7 +73,7 @@ Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
 }

 /* ************************************************************************* */
-OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {
+OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
  // Retract the Unit3
  Vector2 n_v(v(0), v(1));
  Unit3 n_retracted = n_.retract(n_v);
@ -83,7 +83,7 @@ OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {

 /* ************************************************************************* */
 Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
-  Vector n_local = n_.localCoordinates(y.n_);
+  Vector2 n_local = n_.localCoordinates(y.n_);
  double d_local = d_ - y.d_;
  Vector3 e;
  e << n_local(0), n_local(1), -d_local;
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@ -51,7 +51,7 @@ public:
      n_(s), d_(d) {
  }

-  OrientedPlane3(Vector vec) :
+  OrientedPlane3(const Vector4& vec) :
      n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
  }

@ -62,7 +62,7 @@ public:
    d_ = d;
  }

-  /// The print fuction
+  /// The print function
  void print(const std::string& s = std::string()) const;

  /// The equals function with tolerance
@ -89,7 +89,7 @@ public:
  }

  /// The retract function
-  OrientedPlane3 retract(const Vector& v) const;
+  OrientedPlane3 retract(const Vector3& v) const;

  /// The local coordinates function
  Vector3 localCoordinates(const OrientedPlane3& s) const;
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@ -295,7 +295,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar
        & boost::serialization::make_nvp("PinholeBaseK",
            boost::serialization::base_object<Base>(*this));
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@ -42,6 +42,8 @@ private:

 public:

+  typedef CALIBRATION CalibrationType;
+
  /// @name Standard Constructors
  /// @{

@ -203,7 +205,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar
    & boost::serialization::make_nvp("PinholeBase",
        boost::serialization::base_object<PinholeBase>(*this));
@ -313,7 +315,10 @@ public:
  /// print
  void print(const std::string& s = "PinholePose") const {
    Base::print(s);
-    K_->print(s + ".calibration");
+    if (!K_)
+      std::cout << "s No calibration given" << std::endl;
+    else
+      K_->print(s + ".calibration");
  }

  /// @}
@ -380,7 +385,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar
        & boost::serialization::make_nvp("PinholeBaseK",
            boost::serialization::base_object<Base>(*this));
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@ -148,7 +148,7 @@ public:
  /// @{

  /// equality
-  inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
+  inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;}

  /// get x
  double x() const {return x_;}
@ -185,16 +185,18 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version)
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/)
  {
    ar & BOOST_SERIALIZATION_NVP(x_);
    ar & BOOST_SERIALIZATION_NVP(y_);
  }

  /// @}
-
 };

+// For MATLAB wrapper
+typedef std::vector<Point2> Point2Vector;
+
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {return p*s;}

--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@ -181,7 +181,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-      void serialize(ARCHIVE & ar, const unsigned int version)
+      void serialize(ARCHIVE & ar, const unsigned int /*version*/)
    {
      ar & BOOST_SERIALIZATION_NVP(x_);
      ar & BOOST_SERIALIZATION_NVP(y_);
@ -189,15 +189,14 @@ namespace gtsam {
    }

    /// @}
-
  };

-  /// Syntactic sugar for multiplying coordinates by a scalar s*p
-  inline Point3 operator*(double s, const Point3& p) { return p*s;}
+/// Syntactic sugar for multiplying coordinates by a scalar s*p
+inline Point3 operator*(double s, const Point3& p) { return p*s;}

-  template<>
-  struct traits<Point3> : public internal::VectorSpace<Point3> {};
+template<>
+struct traits<Point3> : public internal::VectorSpace<Point3> {};

- template<>
-  struct traits<const Point3> : public internal::VectorSpace<Point3> {};
+template<>
+struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 }
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@ -59,7 +59,7 @@ bool Pose2::equals(const Pose2& q, double tol) const {
 }

 /* ************************************************************************* */
-Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) {
+Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) {
  if (H) *H = Pose2::ExpmapDerivative(xi);
  assert(xi.size() == 3);
  Point2 v(xi(0),xi(1));
@ -130,7 +130,7 @@ Matrix3 Pose2::AdjointMap() const {
 }

 /* ************************************************************************* */
-Matrix3 Pose2::adjointMap(const Vector& v) {
+Matrix3 Pose2::adjointMap(const Vector3& v) {
  // See Chirikjian12book2, vol.2, pg. 36
  Matrix3 ad = zeros(3,3);
  ad(0,1) = -v[2];
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@ -118,7 +118,7 @@ public:
  /// @{

  ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
-  static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none);
+  static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);

  ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
  static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
@ -128,15 +128,14 @@ public:
   * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
   */
  Matrix3 AdjointMap() const;
-  inline Vector Adjoint(const Vector& xi) const {
-    assert(xi.size() == 3);
+  inline Vector3 Adjoint(const Vector3& xi) const {
    return AdjointMap()*xi;
  }

  /**
   * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
   */
-  static Matrix3 adjointMap(const Vector& v);
+  static Matrix3 adjointMap(const Vector3& v);

  /**
   * wedge for SE(2):
@ -271,7 +270,7 @@ private:
  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(t_);
    ar & BOOST_SERIALIZATION_NVP(r_);
  }
@ -291,10 +290,10 @@ typedef std::pair<Point2,Point2> Point2Pair;
 GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);

 template<>
-struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
+struct traits<Pose2> : public internal::LieGroup<Pose2> {};

 template<>
-struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
+struct traits<const Pose2> : public internal::LieGroup<Pose2> {};

 } // namespace gtsam

--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@ -111,8 +111,10 @@ bool Pose3::equals(const Pose3& pose, double tol) const {

 /* ************************************************************************* */
 /** Modified from Murray94book version (which assumes w and v normalized?) */
-Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) {
-  if (H) *H = ExpmapDerivative(xi);
+Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
+  if (H) {
+    *H = ExpmapDerivative(xi);
+  }

  // get angular velocity omega and translational velocity v from twist xi
  Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
@ -254,6 +256,14 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
  return J;
 }

+/* ************************************************************************* */
+const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
+  if (H) {
+    *H << Z_3x3, rotation().matrix();
+  }
+  return t_;
+}
+
 /* ************************************************************************* */
 Matrix4 Pose3::matrix() const {
  const Matrix3 R = R_.matrix();
@ -280,8 +290,9 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
    Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    (*Dpose) << DR, R;
  }
-  if (Dpoint)
+  if (Dpoint) {
    *Dpoint = R_.matrix();
+  }
  return R_ * p + t_;
 }

@ -299,17 +310,18 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
        +wz, 0.0, -wx, 0.0,-1.0, 0.0,
        -wy, +wx, 0.0, 0.0, 0.0,-1.0;
  }
-  if (Dpoint)
+  if (Dpoint) {
    *Dpoint = Rt;
+  }
  return q;
 }

 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
    OptionalJacobian<1, 3> H2) const {
-  if (!H1 && !H2)
+  if (!H1 && !H2) {
    return transform_to(point).norm();
-  else {
+  } else {
    Matrix36 D1;
    Matrix3 D2;
    Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
@ -342,25 +354,25 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
    return boost::none; // we need at least three pairs

  // calculate centroids
-  Vector cp = zero(3), cq = zero(3);
-  BOOST_FOREACH(const Point3Pair& pair, pairs){
-  cp += pair.first.vector();
-  cq += pair.second.vector();
-}
+  Vector3 cp = Vector3::Zero(), cq = Vector3::Zero();
+  BOOST_FOREACH(const Point3Pair& pair, pairs) {
+    cp += pair.first.vector();
+    cq += pair.second.vector();
+  }
  double f = 1.0 / n;
  cp *= f;
  cq *= f;

  // Add to form H matrix
-  Matrix3 H = Eigen::Matrix3d::Zero();
-  BOOST_FOREACH(const Point3Pair& pair, pairs){
-  Vector dp = pair.first.vector() - cp;
-  Vector dq = pair.second.vector() - cq;
-  H += dp * dq.transpose();
-}
+  Matrix3 H = Z_3x3;
+  BOOST_FOREACH(const Point3Pair& pair, pairs) {
+    Vector3 dp = pair.first.vector() - cp;
+    Vector3 dq = pair.second.vector() - cq;
+    H += dp * dq.transpose();
+  }

 // Compute SVD
-  Matrix U,V;
+  Matrix U, V;
  Vector S;
  svd(H, U, S, V);

--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@ -110,7 +110,7 @@ public:
  /// @{

  /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
-  static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none);
+  static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none);

  /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
  static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
@ -125,7 +125,7 @@ public:
   * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
   * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
   */
-  Vector Adjoint(const Vector& xi_b) const {
+  Vector6 Adjoint(const Vector6& xi_b) const {
    return AdjointMap() * xi_b;
  } /// FIXME Not tested - marked as incorrect

@ -223,9 +223,7 @@ public:
  }

  /// get translation
-  const Point3& translation() const {
-    return t_;
-  }
+  const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;

  /// get x
  double x() const {
@ -294,7 +292,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(R_);
    ar & BOOST_SERIALIZATION_NVP(t_);
  }
@ -322,11 +320,14 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
 typedef std::pair<Point3, Point3> Point3Pair;
 GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);

-template<>
-struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
+// For MATLAB wrapper
+typedef std::vector<Pose3> Pose3Vector;

 template<>
-struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
+struct traits<Pose3> : public internal::LieGroup<Pose3> {};
+
+template<>
+struct traits<const Pose3> : public internal::LieGroup<Pose3> {};


 } // namespace gtsam
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@ -118,12 +118,12 @@ namespace gtsam {
    Matrix1 AdjointMap() const { return I_1x1; }

    /// Left-trivialized derivative of the exponential map
-    static Matrix ExpmapDerivative(const Vector& v) {
+    static Matrix ExpmapDerivative(const Vector& /*v*/) {
      return ones(1);
    }

    /// Left-trivialized derivative inverse of the exponential map
-    static Matrix LogmapDerivative(const Vector& v) {
+    static Matrix LogmapDerivative(const Vector& /*v*/) {
      return ones(1);
    }

@ -200,7 +200,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(c_);
      ar & BOOST_SERIALIZATION_NVP(s_);
    }
@ -208,9 +208,9 @@ namespace gtsam {
  };

  template<>
-  struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
+  struct traits<Rot2> : public internal::LieGroup<Rot2> {};

  template<>
-  struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
+  struct traits<const Rot2> : public internal::LieGroup<Rot2> {};

 } // gtsam
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@ -428,7 +428,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version)
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/)
    {
 #ifndef GTSAM_USE_QUATERNIONS
       ar & boost::serialization::make_nvp("rot11", rot_(0,0));
@ -463,9 +463,9 @@ namespace gtsam {
  GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);

  template<>
-  struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
+  struct traits<Rot3> : public internal::LieGroup<Rot3> {};
  
  template<>
-  struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
+  struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
 }

--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@ -129,11 +129,11 @@ public:
 };

 template<>
-struct traits<SO3> : public internal::LieGroupTraits<SO3> {
+struct traits<SO3> : public internal::LieGroup<SO3> {
 };

 template<>
-struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
+struct traits<const SO3> : public internal::LieGroup<SO3> {
 };
 } // end namespace gtsam

--- a/gtsam/geometry/SimpleCamera.h
+++ b/gtsam/geometry/SimpleCamera.h
@ -24,7 +24,114 @@
 namespace gtsam {

  /// A simple camera class with a Cal3_S2 calibration
-  typedef PinholeCamera<Cal3_S2> SimpleCamera;
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+
+/**
+ * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
+ * Use PinholeCameraCal3_S2 instead
+ */
+class SimpleCamera : public PinholeCameraCal3_S2 {
+
+  typedef PinholeCamera<Cal3_S2> Base;
+  typedef boost::shared_ptr<SimpleCamera> shared_ptr;
+
+public:
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  SimpleCamera() :
+      Base() {
+  }
+
+  /** constructor with pose */
+  explicit SimpleCamera(const Pose3& pose) :
+      Base(pose) {
+  }
+
+  /** constructor with pose and calibration */
+  SimpleCamera(const Pose3& pose, const Cal3_S2& K) :
+      Base(pose, K) {
+  }
+
+  /// @}
+   /// @name Named Constructors
+   /// @{
+
+   /**
+    * Create a level camera at the given 2D pose and height
+    * @param K the calibration
+    * @param pose2 specifies the location and viewing direction
+    * (theta 0 = looking in direction of positive X axis)
+    * @param height camera height
+    */
+   static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2,
+       double height) {
+     return SimpleCamera(Base::LevelPose(pose2, height), K);
+   }
+
+   /// PinholeCamera::level with default calibration
+   static SimpleCamera Level(const Pose2& pose2, double height) {
+    return SimpleCamera::Level(Cal3_S2(), pose2, height);
+   }
+
+   /**
+    * Create a camera at the given eye position looking at a target point in the scene
+    * with the specified up direction vector.
+    * @param eye specifies the camera position
+    * @param target the point to look at
+    * @param upVector specifies the camera up direction vector,
+    *        doesn't need to be on the image plane nor orthogonal to the viewing axis
+    * @param K optional calibration parameter
+    */
+   static SimpleCamera Lookat(const Point3& eye, const Point3& target,
+       const Point3& upVector, const Cal3_S2& K = Cal3_S2()) {
+    return SimpleCamera(Base::LookatPose(eye, target, upVector), K);
+   }
+
+   /// @}
+   /// @name Advanced Constructors
+   /// @{
+
+   /// Init from vector, can be 6D (default calibration) or dim
+   explicit SimpleCamera(const Vector &v) :
+       Base(v) {
+   }
+
+   /// Init from Vector and calibration
+   SimpleCamera(const Vector &v, const Vector &K) :
+       Base(v, K) {
+   }
+
+   /// Copy this object as its actual derived type.
+   SimpleCamera::shared_ptr clone() const { return boost::make_shared<SimpleCamera>(*this); }
+
+
+   /// @}
+   /// @name Manifold
+   /// @{
+
+   /// move a cameras according to d
+   SimpleCamera retract(const Vector& d) const {
+     if ((size_t) d.size() == 6)
+       return SimpleCamera(this->pose().retract(d), calibration());
+     else
+       return SimpleCamera(this->pose().retract(d.head(6)),
+           calibration().retract(d.tail(calibration().dim())));
+   }
+
+   /// @}
+
+};
+
+template<>
+struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {
+};
+
+template<>
+struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {
+};

  /// Recover camera from 3*4 camera matrix
  GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
--- a/gtsam/geometry/StereoCamera.cpp
+++ b/gtsam/geometry/StereoCamera.cpp
@ -91,8 +91,42 @@ namespace gtsam {
    double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
    double X = Z * (measured[0] - K_->px()) / K_->fx();
    double Y = Z * (measured[2] - K_->py()) / K_->fy();
-    Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
-    return world_point;
+    Point3 point = leftCamPose_.transform_from(Point3(X, Y, Z));
+    return point;
+  }
+
+  /* ************************************************************************* */
+  Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1,
+                                    OptionalJacobian<3, 3> H2)  const {
+    const Cal3_S2Stereo& K = *K_;
+    const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline();
+
+    double uL = z.uL(), uR = z.uR(), v = z.v();
+    double disparity = uL - uR;
+
+    double local_z = b * fx / disparity;
+    const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z);
+
+    if(H1 || H2) {
+      double z_partial_uR = local_z/disparity;
+      double x_partial_uR = local.x()/disparity;
+      double y_partial_uR = local.y()/disparity;
+      Matrix3 D_local_z;
+      D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0,
+          -y_partial_uR, y_partial_uR, local.z() / fy,
+          -z_partial_uR, z_partial_uR, 0;
+
+      Matrix3 D_point_local;
+      const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local);
+
+      if(H2) {
+        *H2 = D_point_local * D_local_z;
+      }
+
+      return point;
+    }
+
+    return leftCamPose_.transform_from(local);
  }

 }
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@ -137,6 +137,14 @@ public:
  /// back-project a measurement
  Point3 backproject(const StereoPoint2& z) const;

+  /** Back-project the 2D point and compute optional derivatives
+   * @param H1 derivative with respect to pose
+   * @param H2 derivative with respect to point
+   */
+  Point3 backproject2(const StereoPoint2& z,
+                      OptionalJacobian<3, 6> H1 = boost::none,
+                      OptionalJacobian<3, 3> H2 = boost::none) const;
+
  /// @}
  /// @name Deprecated
  /// @{
@ -157,7 +165,7 @@ private:

  friend class boost::serialization::access;
  template<class Archive>
-  void serialize(Archive & ar, const unsigned int version) {
+  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
    ar & BOOST_SERIALIZATION_NVP(K_);
  }
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@ -18,165 +18,145 @@

 #pragma once

-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Point2.h>
+#include <gtsam/base/VectorSpace.h>
+#include <boost/serialization/nvp.hpp>

 namespace gtsam {

-  /**
-   * A 2D stereo point, v will be same for rectified images
-   * @addtogroup geometry
-   * \nosubgrouping
-   */
-  class GTSAM_EXPORT StereoPoint2 {
-  public:
-    static const size_t dimension = 3;
-  private:
-    double uL_, uR_, v_;
+/**
+ * A 2D stereo point, v will be same for rectified images
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT StereoPoint2 {
+private:

-  public:
+  double uL_, uR_, v_;

-    /// @name Standard Constructors
-    /// @{
+public:
+  enum { dimension = 3 };
+  /// @name Standard Constructors
+  /// @{

-    /** default constructor */
-    StereoPoint2() :
+  /** default constructor */
+  StereoPoint2() :
      uL_(0), uR_(0), v_(0) {
-    }
+  }

-    /** constructor */
-    StereoPoint2(double uL, double uR, double v) :
+  /** constructor */
+  StereoPoint2(double uL, double uR, double v) :
      uL_(uL), uR_(uR), v_(v) {
-    }
+  }

-    /// @}
-    /// @name Testable
-    /// @{
+  /// construct from 3D vector
+  StereoPoint2(const Vector3& v) :
+    uL_(v(0)), uR_(v(1)), v_(v(2)) {}

-    /** print */
-    void print(const std::string& s="") const;
+  /// @}
+  /// @name Testable
+  /// @{

-    /** equals */
-    bool equals(const StereoPoint2& q, double tol=1e-9) const {
-      return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_
-          - q.v_) < tol);
-    }
+  /** print */
+  void print(const std::string& s = "") const;

-    /// @}
-    /// @name Group
-    /// @{
+  /** equals */
+  bool equals(const StereoPoint2& q, double tol = 1e-9) const {
+    return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol
+        && fabs(v_ - q.v_) < tol);
+  }

-    /// identity
-    inline static StereoPoint2 identity() { return StereoPoint2(); }
+  /// @}
+  /// @name Group
+  /// @{

-    /// inverse
-    inline StereoPoint2 inverse() const {
-      return StereoPoint2()- (*this);
-    }
+  /// identity
+  inline static StereoPoint2 identity() {
+    return StereoPoint2();
+  }

-    /// "Compose", just adds the coordinates of two points.
-    inline StereoPoint2 compose(const StereoPoint2& p1) const {
-      return *this + p1;
-    }
+  /// inverse
+  StereoPoint2 operator-() const {
+    return StereoPoint2(-uL_, -uR_, -v_);
+  }

-    /// add two stereo points
-    StereoPoint2 operator+(const StereoPoint2& b) const {
-      return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
-    }
+  /// add
+  inline StereoPoint2 operator +(const StereoPoint2& b) const {
+    return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
+  }

-    /// subtract two stereo points
-    StereoPoint2 operator-(const StereoPoint2& b) const {
-      return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
-    }
-    
-    /// @}
-    /// @name Manifold
-    /// @{
+  /// subtract
+  inline StereoPoint2 operator -(const StereoPoint2& b) const {
+    return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
+  }

-    /// dimension of the variable - used to autodetect sizes */
-    inline static size_t Dim() { return dimension; }
+  /// @}
+  /// @name Standard Interface
+  /// @{

-    /// return dimensionality of tangent space, DOF = 3
-    inline size_t dim() const { return dimension; }
+  /// equality
+  inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;}

-    /// Updates a with tangent space delta
-    inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
+  /// get uL
+  inline double uL() const {return uL_;}

-    /// Returns inverse retraction
-    inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
+  /// get uR
+  inline double uR() const {return uR_;}

-    /// @}
-    /// @name Lie Group
-    /// @{
+  /// get v
+  inline double v() const {return v_;}

-    /** Exponential map around identity - just create a Point2 from a vector */
-    static inline StereoPoint2 Expmap(const Vector& d) {
-      return StereoPoint2(d(0), d(1), d(2));
-    }
+  /** convert to vector */
+  Vector3 vector() const {
+    return Vector3(uL_, uR_, v_);
+  }

-    /** Log map around identity - just return the Point2 as a vector */
-    static inline Vector Logmap(const StereoPoint2& p) {
-      return p.vector();
-    }
+  /** convenient function to get a Point2 from the left image */
+  Point2 point2() const {
+    return Point2(uL_, v_);
+  }

-    /** The difference between another point and this point */
-    inline StereoPoint2 between(const StereoPoint2& p2) const {
-      return p2 - *this;
-    }
+  /** convenient function to get a Point2 from the right image */
+  Point2 right() const {
+    return Point2(uR_, v_);
+  }

-    /// @}
-    /// @name Standard Interface
-    /// @{
+  /// @name Deprecated
+  /// @{
+  inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);}
+  inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;}
+  inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; }
+  inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
+  inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
+  static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); }
+  static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); }
+  /// @}

-    /// get uL
-    inline double uL() const {return uL_;}
+  /// Streaming
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);

-    /// get uR
-    inline double uR() const {return uR_;}
+private:

-    /// get v
-    inline double v() const {return v_;}
+  /// @}
+  /// @name Advanced Interface
+  /// @{

-    /** convert to vector */
-    Vector3 vector() const {
-      return Vector3(uL_, uR_, v_);
-    }
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(uL_);
+    ar & BOOST_SERIALIZATION_NVP(uR_);
+    ar & BOOST_SERIALIZATION_NVP(v_);
+  }

-    /** convenient function to get a Point2 from the left image */
-    Point2 point2() const {
-      return Point2(uL_, v_);
-    }
+  /// @}

-    /** convenient function to get a Point2 from the right image */
-    Point2 right() const {
-      return Point2(uR_, v_);
-    }
+};

-    /// Streaming
-    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
+template<>
+struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};

-  private:
-
-    /// @}
-    /// @name Advanced Interface
-    /// @{
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & BOOST_SERIALIZATION_NVP(uL_);
-      ar & BOOST_SERIALIZATION_NVP(uR_);
-      ar & BOOST_SERIALIZATION_NVP(v_);
-    }
-
-    /// @}
-
-  };
-
-  template<>
-  struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
-
-  template<>
-  struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
+template<>
+struct traits<const StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
 }
--- a/gtsam/geometry/Unit3.cpp
+++ b/gtsam/geometry/Unit3.cpp
@ -119,7 +119,7 @@ Matrix3 Unit3::skew() const {
 }

 /* ************************************************************************* */
-Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
+Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
  Matrix23 Bt = basis().transpose();
  Vector2 xi = Bt * q.p_.vector();
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@ -108,7 +108,7 @@ public:
  }

  /// Return unit-norm Vector
-  Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
+  Vector3 unitVector(boost::optional<Matrix&> H = boost::none) const {
    if (H)
      *H = basis();
    return (p_.vector());
@ -120,7 +120,7 @@ public:
  }

  /// Signed, vector-valued error between two directions
-  Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
+  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;

  /// Distance between two directions
  double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
@ -160,7 +160,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(p_);
    // homebrew serialize Eigen Matrix
    ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
--- a/gtsam/geometry/tests/testCyclic.cpp
+++ b/gtsam/geometry/tests/testCyclic.cpp
@ -22,52 +22,114 @@
 using namespace std;
 using namespace gtsam;

-typedef Cyclic<3> G; // Let's use the cyclic group of order 3
+typedef Cyclic<3> Z3; // Let's use the cyclic group of order 3
+typedef Cyclic<2> Z2;

 //******************************************************************************
 TEST(Cyclic, Concept) {
-  BOOST_CONCEPT_ASSERT((IsGroup<G>));
-  EXPECT_LONGS_EQUAL(0, traits<G>::Identity());
+  BOOST_CONCEPT_ASSERT((IsGroup<Z3>));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Identity());
 }

 //******************************************************************************
 TEST(Cyclic, Constructor) {
-  G g(0);
+  Z3 g(0);
 }

 //******************************************************************************
 TEST(Cyclic, Compose) {
-  EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(0),G(0)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(0),G(1)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(0),G(2)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Compose(Z3(0),Z3(0)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Compose(Z3(0),Z3(1)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Compose(Z3(0),Z3(2)));

-  EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(2),G(0)));
-  EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(2),G(1)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(2),G(2)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Compose(Z3(2),Z3(0)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Compose(Z3(2),Z3(1)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Compose(Z3(2),Z3(2)));
 }

 //******************************************************************************
 TEST(Cyclic, Between) {
-  EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(0),G(0)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(0),G(1)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(0),G(2)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Between(Z3(0),Z3(0)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Between(Z3(0),Z3(1)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Between(Z3(0),Z3(2)));

-  EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(2),G(0)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(2),G(1)));
-  EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(2),G(2)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Between(Z3(2),Z3(0)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Between(Z3(2),Z3(1)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Between(Z3(2),Z3(2)));
 }

 //******************************************************************************
-TEST(Cyclic, Ivnverse) {
-  EXPECT_LONGS_EQUAL(0, traits<G>::Inverse(G(0)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Inverse(G(1)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Inverse(G(2)));
+TEST(Cyclic, Inverse) {
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Inverse(Z3(0)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Inverse(Z3(1)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Inverse(Z3(2)));
+}
+
+//******************************************************************************
+TEST(Cyclic, Negation) {
+  EXPECT_LONGS_EQUAL(0, -Z3(0));
+  EXPECT_LONGS_EQUAL(2, -Z3(1));
+  EXPECT_LONGS_EQUAL(1, -Z3(2));
+}
+
+//******************************************************************************
+TEST(Cyclic, Negation2) {
+  EXPECT_LONGS_EQUAL(0, -Z2(0));
+  EXPECT_LONGS_EQUAL(1, -Z2(1));
 }

 //******************************************************************************
 TEST(Cyclic , Invariants) {
-  G g(2), h(1);
- check_group_invariants(g,h);
+  Z3 g(2), h(1);
+  EXPECT(check_group_invariants(g,h));
+}
+
+//******************************************************************************
+// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
+// smallest non-cyclic group called the Klein four-group:
+typedef DirectSum<Z2, Z2> K4;
+
+namespace gtsam {
+
+/// Define K4 to be a model of the Additive Group concept, and provide Testable
+template<>
+struct traits<K4> : internal::AdditiveGroupTraits<K4> {
+  static void Print(const K4& m, const string& s = "") {
+    cout << s << "(" << m.first << "," << m.second << ")" << endl;
+  }
+  static bool Equals(const K4& m1, const K4& m2, double tol = 1e-8) {
+    return m1 == m2;
+  }
+};
+
+}  // namespace gtsam
+
+TEST(Cyclic , DirectSum) {
+  // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
+  // smallest non-cyclic group called the Klein four-group:
+  BOOST_CONCEPT_ASSERT((IsGroup<K4>));
+  BOOST_CONCEPT_ASSERT((IsTestable<K4>));
+
+  // Refer to http://en.wikipedia.org/wiki/Klein_four-group
+  K4 e(0,0), a(0, 1), b(1, 0), c(1, 1);
+  EXPECT(assert_equal(a, - a));
+  EXPECT(assert_equal(b, - b));
+  EXPECT(assert_equal(c, - c));
+  EXPECT(assert_equal(a, a + e));
+  EXPECT(assert_equal(b, b + e));
+  EXPECT(assert_equal(c, c + e));
+  EXPECT(assert_equal(e, a + a));
+  EXPECT(assert_equal(e, b + b));
+  EXPECT(assert_equal(e, c + c));
+  EXPECT(assert_equal(c, a + b));
+  EXPECT(assert_equal(b, a + c));
+  EXPECT(assert_equal(a, b + c));
+  EXPECT(assert_equal(c, a - b));
+  EXPECT(assert_equal(a, b - c));
+  EXPECT(assert_equal(b, c - a));
+  EXPECT(check_group_invariants(a, b));
+  EXPECT(check_group_invariants(b, c));
+  EXPECT(check_group_invariants(c, a));
 }

 //******************************************************************************
--- a/gtsam/geometry/tests/testEssentialMatrix.cpp
+++ b/gtsam/geometry/tests/testEssentialMatrix.cpp
@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) {
  EXPECT(assert_equal(expected, actual));
 }

+//*************************************************************************
+TEST (EssentialMatrix, FromPose3) {
+  EssentialMatrix expected(c1Rc2, Unit3(c1Tc2));
+  Pose3 pose(c1Rc2, c1Tc2);
+  EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
+  EXPECT(assert_equal(expected, actual));
+}
+
 //*******************************************************************************
 TEST(EssentialMatrix, localCoordinates0) {
  EssentialMatrix E;
@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) {
  Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
  EXPECT(assert_equal(zero(5), actual, 1e-8));

-  Vector d = zero(6);
-  d(4) += 1e-5;
+  Vector6 d;
+  d << 0.1, 0.2, 0.3, 0, 0, 0;
  Vector actual2 = hx.localCoordinates(
      EssentialMatrix::FromPose3(pose.retract(d)));
-  EXPECT(assert_equal(zero(5), actual2, 1e-8));
+  EXPECT(assert_equal(d.head(5), actual2, 1e-8));
 }

 //*************************************************************************
@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) {
  EXPECT(assert_equal(expected, actual));
 }

+//*************************************************************************
+TEST (EssentialMatrix, RoundTrip) {
+  Vector5 d;
+  d << 0.1, 0.2, 0.3, 0.4, 0.5;
+  EssentialMatrix e, hx = e.retract(d);
+  Vector actual = e.localCoordinates(hx);
+  EXPECT(assert_equal(d, actual, 1e-8));
+}
+
 //*************************************************************************
 Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
  return E.transform_to(point);
--- a/gtsam/geometry/tests/testPoint2.cpp
+++ b/gtsam/geometry/tests/testPoint2.cpp
@ -37,8 +37,8 @@ TEST(Double , Concept) {
 //******************************************************************************
 TEST(Double , Invariants) {
  double p1(2), p2(5);
-  check_group_invariants(p1, p2);
-  check_manifold_invariants(p1, p2);
+  EXPECT(check_group_invariants(p1, p2));
+  EXPECT(check_manifold_invariants(p1, p2));
 }

 //******************************************************************************
@ -51,8 +51,8 @@ TEST(Point2 , Concept) {
 //******************************************************************************
 TEST(Point2 , Invariants) {
  Point2 p1(1, 2), p2(4, 5);
-  check_group_invariants(p1, p2);
-  check_manifold_invariants(p1, p2);
+  EXPECT(check_group_invariants(p1, p2));
+  EXPECT(check_manifold_invariants(p1, p2));
 }

 /* ************************************************************************* */
@ -61,6 +61,12 @@ TEST(Point2, constructor) {
  EXPECT(assert_equal(p1, p2));
 }

+/* ************************************************************************* */
+TEST(Point2, equality) {
+  Point2 p1(1, 2), p2(1,3);
+  EXPECT(!(p1 == p2));
+}
+
 /* ************************************************************************* */
 TEST(Point2, Lie) {
  Point2 p1(1, 2), p2(4, 5);
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@ -36,8 +36,8 @@ TEST(Point3 , Concept) {
 //******************************************************************************
 TEST(Point3 , Invariants) {
  Point3 p1(1, 2, 3), p2(4, 5, 6);
-  check_group_invariants(p1, p2);
-  check_manifold_invariants(p1, p2);
+  EXPECT(check_group_invariants(p1, p2));
+  EXPECT(check_manifold_invariants(p1, p2));
 }

 /* ************************************************************************* */
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@ -775,15 +775,15 @@ Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
 TEST(Pose2 , Invariants) {
  Pose2 id;

-  check_group_invariants(id,id);
-  check_group_invariants(id,T1);
-  check_group_invariants(T2,id);
-  check_group_invariants(T2,T1);
+  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_group_invariants(T2,id));
+  EXPECT(check_group_invariants(T2,T1));

-  check_manifold_invariants(id,id);
-  check_manifold_invariants(id,T1);
-  check_manifold_invariants(T2,id);
-  check_manifold_invariants(T2,T1);
+  EXPECT(check_manifold_invariants(id,id));
+  EXPECT(check_manifold_invariants(id,T1));
+  EXPECT(check_manifold_invariants(T2,id));
+  EXPECT(check_manifold_invariants(T2,T1));

 }

--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@ -186,6 +186,17 @@ TEST(Pose3, expmaps_galore_full)
  EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
 }

+/* ************************************************************************* */
+// Check translation and its pushforward
+TEST(Pose3, translation) {
+  Matrix actualH;
+  EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8));
+
+  Matrix numericalH = numericalDerivative11<Point3, Pose3>(
+      boost::bind(&Pose3::translation, _1, boost::none), T);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
 /* ************************************************************************* */
 TEST(Pose3, Adjoint_compose_full)
 {
@ -749,15 +760,15 @@ TEST( Pose3, stream)
 TEST(Pose3 , Invariants) {
  Pose3 id;

-  check_group_invariants(id,id);
-  check_group_invariants(id,T3);
-  check_group_invariants(T2,id);
-  check_group_invariants(T2,T3);
+  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(id,T3));
+  EXPECT(check_group_invariants(T2,id));
+  EXPECT(check_group_invariants(T2,T3));

-  check_manifold_invariants(id,id);
-  check_manifold_invariants(id,T3);
-  check_manifold_invariants(T2,id);
-  check_manifold_invariants(T2,T3);
+  EXPECT(check_manifold_invariants(id,id));
+  EXPECT(check_manifold_invariants(id,T3));
+  EXPECT(check_manifold_invariants(T2,id));
+  EXPECT(check_manifold_invariants(T2,T3));
 }

 //******************************************************************************
--- a/gtsam/geometry/tests/testQuaternion.cpp
+++ b/gtsam/geometry/tests/testQuaternion.cpp
@ -107,15 +107,15 @@ TEST(Quaternion , Inverse) {

 //******************************************************************************
 TEST(Quaternion , Invariants) {
-  check_group_invariants(id, id);
-  check_group_invariants(id, R1);
-  check_group_invariants(R2, id);
-  check_group_invariants(R2, R1);
+  EXPECT(check_group_invariants(id, id));
+  EXPECT(check_group_invariants(id, R1));
+  EXPECT(check_group_invariants(R2, id));
+  EXPECT(check_group_invariants(R2, R1));

-  check_manifold_invariants(id, id);
-  check_manifold_invariants(id, R1);
-  check_manifold_invariants(R2, id);
-  check_manifold_invariants(R2, R1);
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, R1));
+  EXPECT(check_manifold_invariants(R2, id));
+  EXPECT(check_manifold_invariants(R2, R1));
 }

 //******************************************************************************
--- a/gtsam/geometry/tests/testRot2.cpp
+++ b/gtsam/geometry/tests/testRot2.cpp
@ -163,15 +163,15 @@ Rot2 T2(0.2);
 TEST(Rot2 , Invariants) {
  Rot2 id;

-  check_group_invariants(id,id);
-  check_group_invariants(id,T1);
-  check_group_invariants(T2,id);
-  check_group_invariants(T2,T1);
+  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_group_invariants(T2,id));
+  EXPECT(check_group_invariants(T2,T1));

-  check_manifold_invariants(id,id);
-  check_manifold_invariants(id,T1);
-  check_manifold_invariants(T2,id);
-  check_manifold_invariants(T2,T1);
+  EXPECT(check_manifold_invariants(id,id));
+  EXPECT(check_manifold_invariants(id,T1));
+  EXPECT(check_manifold_invariants(T2,id));
+  EXPECT(check_manifold_invariants(T2,T1));

 }

--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@ -659,17 +659,17 @@ Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
 TEST(Rot3 , Invariants) {
  Rot3 id;

-  check_group_invariants(id,id);
-  check_group_invariants(id,T1);
-  check_group_invariants(T2,id);
-  check_group_invariants(T2,T1);
-  check_group_invariants(T1,T2);
+  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_group_invariants(T2,id));
+  EXPECT(check_group_invariants(T2,T1));
+  EXPECT(check_group_invariants(T1,T2));

-  check_manifold_invariants(id,id);
-  check_manifold_invariants(id,T1);
-  check_manifold_invariants(T2,id);
-  check_manifold_invariants(T2,T1);
-  check_manifold_invariants(T1,T2);
+  EXPECT(check_manifold_invariants(id,id));
+  EXPECT(check_manifold_invariants(id,T1));
+  EXPECT(check_manifold_invariants(T2,id));
+  EXPECT(check_manifold_invariants(T2,T1));
+  EXPECT(check_manifold_invariants(T1,T2));
 }

 //******************************************************************************
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@ -57,15 +57,15 @@ TEST(SO3 , Retract) {

 //******************************************************************************
 TEST(SO3 , Invariants) {
-  check_group_invariants(id,id);
-  check_group_invariants(id,R1);
-  check_group_invariants(R2,id);
-  check_group_invariants(R2,R1);
+  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(id,R1));
+  EXPECT(check_group_invariants(R2,id));
+  EXPECT(check_group_invariants(R2,R1));

-  check_manifold_invariants(id,id);
-  check_manifold_invariants(id,R1);
-  check_manifold_invariants(R2,id);
-  check_manifold_invariants(R2,R1);
+  EXPECT(check_manifold_invariants(id,id));
+  EXPECT(check_manifold_invariants(id,R1));
+  EXPECT(check_manifold_invariants(R2,id));
+  EXPECT(check_manifold_invariants(R2,R1));
 }

 //******************************************************************************
--- a/gtsam/geometry/tests/testStereoCamera.cpp
+++ b/gtsam/geometry/tests/testStereoCamera.cpp
@ -99,13 +99,13 @@ TEST( StereoCamera, Dproject)
  Matrix actual1; stereoCam.project2(landmark, actual1, boost::none);
  CHECK(assert_equal(expected1,actual1,1e-7));

-  Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
+  Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K);
  Matrix actual2; stereoCam.project2(landmark, boost::none, actual2);
  CHECK(assert_equal(expected2,actual2,1e-7));
 }

 /* ************************************************************************* */
-TEST( StereoCamera, backproject)
+TEST( StereoCamera, backproject_case1)
 {
  Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
  StereoCamera stereoCam2(Pose3(), K2);
@ -117,7 +117,7 @@ TEST( StereoCamera, backproject)
 }

 /* ************************************************************************* */
-TEST( StereoCamera, backproject2)
+TEST( StereoCamera, backproject_case2)
 {
  Rot3 R(0.589511291,  -0.804859792,  0.0683931805,
         -0.804435942,  -0.592650676,  -0.0405925523,
@ -132,6 +132,53 @@ TEST( StereoCamera, backproject2)
  CHECK(assert_equal(z, actual, 1e-3));
 }

+static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) {
+  return StereoCamera(pose, boost::make_shared<Cal3_S2Stereo>(K)).backproject(point);
+}
+
+/* ************************************************************************* */
+TEST( StereoCamera, backproject2_case1)
+{
+  Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
+  StereoCamera stereoCam2(Pose3(), K2);
+
+  Point3 expected_point(1.2, 2.3, 4.5);
+  StereoPoint2 stereo_point = stereoCam2.project(expected_point);
+
+  Matrix actual_jacobian_1, actual_jacobian_2;
+  Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2);
+  CHECK(assert_equal(expected_point, actual_point, 1e-8));
+
+  Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2);
+  CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6));
+
+  Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2);
+  CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6));
+}
+
+TEST( StereoCamera, backproject2_case2)
+{
+  Rot3 R(0.589511291,  -0.804859792,  0.0683931805,
+         -0.804435942,  -0.592650676,  -0.0405925523,
+         0.0732045588,  -0.0310882277,  -0.996832359);
+  Point3 t(53.5239823, 23.7866016, -4.42379876);
+  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
+  StereoCamera camera(Pose3(R,t), K);
+  StereoPoint2 z(184.812, 129.068, 714.768);
+
+  Matrix actual_jacobian_1, actual_jacobian_2;
+  Point3 l = camera.backproject2(z, actual_jacobian_1, actual_jacobian_2);
+
+  StereoPoint2 actual = camera.project(l);
+  CHECK(assert_equal(z, actual, 1e-3));
+
+  Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(R,t), z, *K);
+  CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6));
+
+  Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(R,t), z, *K);
+  CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6));
+}
+
 /* ************************************************************************* */
  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */
--- a/gtsam/geometry/tests/testStereoPoint2.cpp
+++ b/gtsam/geometry/tests/testStereoPoint2.cpp
@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)

 //******************************************************************************
 TEST(StereoPoint2 , Concept) {
+  BOOST_CONCEPT_ASSERT((IsGroup<StereoPoint2>));
  BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >));
+  BOOST_CONCEPT_ASSERT((IsVectorSpace<StereoPoint2>));
 }

 /* ************************************************************************* */
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@ -23,14 +23,8 @@

 namespace gtsam {

-/**
- * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
- * @param projection_matrices Projection matrices (K*P^-1)
- * @param measurements 2D measurements
- * @param rank_tol SVD rank tolerance
- * @return Triangulated Point3
- */
-Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
+Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34>& projection_matrices,
    const std::vector<Point2>& measurements, double rank_tol) {

  // number of cameras
@ -56,7 +50,16 @@ Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
  if (rank < 3)
    throw(TriangulationUnderconstrainedException());

-  // Create 3D point from eigenvector
+  return v;
+}
+
+Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
+    const std::vector<Point2>& measurements, double rank_tol) {
+
+  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
+      rank_tol);
+
+  // Create 3D point from homogeneous coordinates
  return Point3(sub((v / v(3)), 0, 3));
 }

@ -88,6 +91,5 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
  return result.at<Point3>(landmarkKey);
 }

-
 } // \namespace gtsam

--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@ -43,6 +43,17 @@ public:
  }
 };

+/**
+ * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
+ * @param projection_matrices Projection matrices (K*P^-1)
+ * @param measurements 2D measurements
+ * @param rank_tol SVD rank tolerance
+ * @return Triangulated point, in homogeneous coordinates
+ */
+GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34>& projection_matrices,
+    const std::vector<Point2>& measurements, double rank_tol = 1e-9);
+
 /**
 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
 * @param projection_matrices Projection matrices (K*P^-1)
@ -52,7 +63,7 @@ public:
 */
 GTSAM_EXPORT Point3 triangulateDLT(
    const std::vector<Matrix34>& projection_matrices,
-    const std::vector<Point2>& measurements, double rank_tol);
+    const std::vector<Point2>& measurements, double rank_tol = 1e-9);

 /**
 * Create a factor graph with projection factors from poses and one calibration
@ -94,8 +105,9 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
 */
 template<class CAMERA>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
-    const std::vector<CAMERA>& cameras, const std::vector<Point2>& measurements,
-    Key landmarkKey, const Point3& initialEstimate) {
+    const std::vector<CAMERA>& cameras,
+    const std::vector<Point2>& measurements, Key landmarkKey,
+    const Point3& initialEstimate) {
  Values values;
  values.insert(landmarkKey, initialEstimate); // Initial landmark value
  NonlinearFactorGraph graph;
@ -159,7 +171,8 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
 * @return refined Point3
 */
 template<class CAMERA>
-Point3 triangulateNonlinear(const std::vector<CAMERA>& cameras,
+Point3 triangulateNonlinear(
+    const std::vector<CAMERA>& cameras,
    const std::vector<Point2>& measurements, const Point3& initialEstimate) {

  // Create a factor graph and initial values
@ -180,6 +193,25 @@ Point3 triangulateNonlinear(
  (cameras, measurements, initialEstimate);
 }

+/**
+ * Create a 3*4 camera projection matrix from calibration and pose.
+ * Functor for partial application on calibration
+ * @param pose The camera pose
+ * @param cal  The calibration
+ * @return Returns a Matrix34
+ */
+template<class CALIBRATION>
+struct CameraProjectionMatrix {
+  CameraProjectionMatrix(const CALIBRATION& calibration) :
+      K_(calibration.K()) {
+  }
+  Matrix34 operator()(const Pose3& pose) const {
+    return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
+  }
+private:
+  const Matrix3 K_;
+};
+
 /**
 * Function to triangulate 3D landmark point from an arbitrary number
 * of poses (at least 2) using the DLT. The function checks that the
@ -204,11 +236,11 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,

  // construct projection matrices from poses & calibration
  std::vector<Matrix34> projection_matrices;
-  BOOST_FOREACH(const Pose3& pose, poses) {
-    projection_matrices.push_back(
-        sharedCal->K() * (pose.inverse().matrix()).block<3, 4>(0, 0));
-  }
-  // Do DLT: throws TriangulationUnderconstrainedException if rank<3
+  CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
+  BOOST_FOREACH(const Pose3& pose, poses)
+    projection_matrices.push_back(createP(pose));
+
+  // Triangulate linearly
  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);

  // Then refine using non-linear optimization
@ -241,24 +273,23 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 * @return Returns a Point3
 */
 template<class CAMERA>
-Point3 triangulatePoint3(const std::vector<CAMERA>& cameras,
+Point3 triangulatePoint3(
+    const std::vector<CAMERA>& cameras,
    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
    bool optimize = false) {

  size_t m = cameras.size();
-  assert(measurements.size()==m);
+  assert(measurements.size() == m);

  if (m < 2)
    throw(TriangulationUnderconstrainedException());

  // construct projection matrices from poses & calibration
  std::vector<Matrix34> projection_matrices;
-  BOOST_FOREACH(const CAMERA& camera, cameras) {
-    Matrix P_ = (camera.pose().inverse().matrix());
+  BOOST_FOREACH(const CAMERA& camera, cameras)
    projection_matrices.push_back(
-        camera.calibration().K() * (P_.block<3, 4>(0, 0)));
-  }
-  // Do DLT: throws TriangulationUnderconstrainedException if rank<3
+        CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
+            camera.pose()));
  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);

  // The n refine using non-linear optimization
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@ -253,7 +253,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(nodes_);
      ar & BOOST_SERIALIZATION_NVP(roots_);
    }
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@ -160,7 +160,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(conditional_);
      ar & BOOST_SERIALIZATION_NVP(parent_);
      ar & BOOST_SERIALIZATION_NVP(children);
--- a/gtsam/inference/ClusterTree-inst.h
+++ b/gtsam/inference/ClusterTree-inst.h
@ -99,7 +99,7 @@ namespace gtsam

        // Do dense elimination step
        std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
-          eliminationFunction(gatheredFactors, Ordering(node->keys));
+          eliminationFunction(gatheredFactors, node->orderedFrontalKeys);

        // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
        myData.bayesTreeNode->setEliminationResult(eliminationResult);
@ -123,7 +123,7 @@ namespace gtsam
    const std::string& s, const KeyFormatter& keyFormatter) const
  {
    std::cout << s;
-    BOOST_FOREACH(Key j, keys)
+    BOOST_FOREACH(Key j, orderedFrontalKeys)
      std::cout << j << "  ";
    std::cout << "problemSize = " << problemSize_ << std::endl;
  }
--- a/gtsam/inference/ClusterTree.h
+++ b/gtsam/inference/ClusterTree.h
@ -11,7 +11,7 @@

 #include <gtsam/base/Testable.h>
 #include <gtsam/base/FastVector.h>
-#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Ordering.h>

 namespace gtsam
 {
@ -37,11 +37,11 @@ namespace gtsam
    typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine

    struct Cluster {
-      typedef FastVector<Key> Keys;
+      typedef Ordering Keys;
      typedef FastVector<sharedFactor> Factors;
      typedef FastVector<boost::shared_ptr<Cluster> > Children;

-      Keys keys; ///< Frontal keys of this node
+      Keys orderedFrontalKeys; ///< Frontal keys of this node
      Factors factors; ///< Factors associated with this node
      Children children; ///< sub-trees
      int problemSize_;
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@ -141,7 +141,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
    }

--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@ -160,7 +160,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class Archive>
-    void serialize(Archive & ar, const unsigned int version) {
+    void serialize(Archive & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(keys_);
    }

--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@ -342,7 +342,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(factors_);
    }

--- a/gtsam/inference/JunctionTree-inst.h
+++ b/gtsam/inference/JunctionTree-inst.h
@ -49,7 +49,7 @@ namespace gtsam {
      // structure with its own JT node, and create a child pointer in its parent.
      ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(&parentData);
      myData.myJTNode = boost::make_shared<typename JunctionTree<BAYESTREE,GRAPH>::Node>();
-      myData.myJTNode->keys.push_back(node->key);
+      myData.myJTNode->orderedFrontalKeys.push_back(node->key);
      myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
      parentData.myJTNode->children.push_back(myData.myJTNode);
      return myData;
@ -101,13 +101,20 @@ namespace gtsam {
          const typename JunctionTree<BAYESTREE, GRAPH>::Node& childToMerge =
            *myData.myJTNode->children[child - nrMergedChildren];
          // Merge keys, factors, and children.
-          myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end());
-          myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end());
-          myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end());
+          myData.myJTNode->orderedFrontalKeys.insert(
+              myData.myJTNode->orderedFrontalKeys.begin(),
+              childToMerge.orderedFrontalKeys.begin(),
+              childToMerge.orderedFrontalKeys.end());
+          myData.myJTNode->factors.insert(myData.myJTNode->factors.end(),
+                                          childToMerge.factors.begin(),
+                                          childToMerge.factors.end());
+          myData.myJTNode->children.insert(myData.myJTNode->children.end(),
+                                           childToMerge.children.begin(),
+                                           childToMerge.children.end());
          // Increment problem size
          combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_);
          // Increment number of frontal variables
-          myNrFrontals += childToMerge.keys.size();
+          myNrFrontals += childToMerge.orderedFrontalKeys.size();
          // Remove child from list.
          myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren));
          // Increment number of merged children
--- a/gtsam/inference/LabeledSymbol.h
+++ b/gtsam/inference/LabeledSymbol.h
@ -109,7 +109,7 @@ private:
  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(c_);
    ar & BOOST_SERIALIZATION_NVP(label_);
    ar & BOOST_SERIALIZATION_NVP(j_);
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@ -18,15 +18,15 @@

 #pragma once

-#include <algorithm>
-#include <vector>
-#include <boost/assign/list_inserter.hpp>
-
-#include <gtsam/base/FastSet.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/MetisIndex.h>
 #include <gtsam/inference/FactorGraph.h>
+#include <gtsam/base/FastSet.h>
+
+#include <boost/assign/list_inserter.hpp>
+#include <algorithm>
+#include <vector>

 namespace gtsam {

@ -104,9 +104,9 @@ namespace gtsam {
    /// VariableIndex, it is faster to use COLAMD(const VariableIndex&).  This function constrains
    /// the variables in \c constrainLast to the end of the ordering, and orders all other variables
    /// before in a fill-reducing ordering.  If \c forceOrder is true, the variables in \c
-    /// constrainLast will be ordered in the same order specified in the vector<Key> \c
-    /// constrainLast.   If \c forceOrder is false, the variables in \c constrainFirst will be
-    /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
+    /// constrainFirst will be ordered in the same order specified in the vector<Key> \c
+    /// constrainFirst.   If \c forceOrder is false, the variables in \c constrainFirst will be
+    /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
    template<class FACTOR>
    static Ordering colamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
      const std::vector<Key>& constrainFirst, bool forceOrder = false) {
@ -117,7 +117,7 @@ namespace gtsam {
    /// orders all other variables after in a fill-reducing ordering.  If \c forceOrder is true, the
    /// variables in \c constrainFirst will be ordered in the same order specified in the
    /// vector<Key> \c constrainFirst.   If \c forceOrder is false, the variables in \c
-    /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to
+    /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
    /// reduce fill-in as well.
    static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex,
      const std::vector<Key>& constrainFirst, bool forceOrder = false);
@ -187,7 +187,7 @@ namespace gtsam {
    /** Serialization function */
    friend class boost::serialization::access;
    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    }
  };
--- a/Show More
+++ b/Show More