Merged in develop (pull request #71). Resolve conflicts.

.gitignore

@@ -4,3 +4,5 @@
 /examples/Data/dubrovnik-3-7-pre-rewritten.txt
 /examples/Data/pose2example-rewritten.txt
 /examples/Data/pose3example-rewritten.txt
+*.txt.user
+*.txt.user.6d59f0c

.project

@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>-j5</value>
+					<value>-j4</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>
@@ -63,7 +63,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
-					<value>false</value>
+					<value>true</value>
 				</dictionary>
 			</arguments>
 		</buildCommand>

CMakeLists.txt

@@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
 endif()
 
 # Set the version number for the library
-set (GTSAM_VERSION_MAJOR 3)
+set (GTSAM_VERSION_MAJOR 4)
-set (GTSAM_VERSION_MINOR 2)
+set (GTSAM_VERSION_MINOR 0)
 set (GTSAM_VERSION_PATCH 0)
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@@ -59,7 +59,7 @@ option(GTSAM_BUILD_STATIC_LIBRARY        "Build a static gtsam library, instead
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
 option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
-option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF) 
+option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
 option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
 option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" ON)
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
@@ -129,7 +129,7 @@ else()
 endif()
 
 
-if(${Boost_VERSION} EQUAL 105600)
+if(NOT (${Boost_VERSION} LESS 105600))
 	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
 	add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
 endif()
@@ -189,6 +189,7 @@ if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
     endif()
 endif()
 
+
 ###############################################################################
 # Option for using system Eigen or GTSAM-bundled Eigen
 ### Disabled until our patches are included in Eigen ###
@@ -202,13 +203,13 @@ set(GTSAM_USE_SYSTEM_EIGEN OFF)
 if(GTSAM_USE_SYSTEM_EIGEN)
 	# Use generic Eigen include paths e.g. <Eigen/Core>
 	set(GTSAM_EIGEN_INCLUDE_PREFIX "")
-	
+
 	find_package(Eigen3 REQUIRED)
 	include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
 else()
 	# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
 	set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
-	
+
 	# Clear any variables set by FindEigen3
 	if(EIGEN3_INCLUDE_DIR)
 		set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
@@ -221,7 +222,6 @@ configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eig
 # Install the configuration file for Eigen
 install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
 
-
 ###############################################################################
 # Global compile options
 
@@ -268,18 +268,18 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR})
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.
 include_directories(BEFORE
-  gtsam/3rdparty/UFconfig 
+  gtsam/3rdparty/UFconfig
   gtsam/3rdparty/CCOLAMD/Include
-  gtsam/3rdparty/metis-5.1.0/include
+  gtsam/3rdparty/metis/include
-  gtsam/3rdparty/metis-5.1.0/libmetis
+  gtsam/3rdparty/metis/libmetis
-  gtsam/3rdparty/metis-5.1.0/GKlib
+  gtsam/3rdparty/metis/GKlib
   ${PROJECT_SOURCE_DIR}
   ${PROJECT_BINARY_DIR} # So we can include generated config header files
   CppUnitLite)
 
 if(MSVC)
 	add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
-	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
+	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
 endif()
 
 # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
@@ -330,6 +330,7 @@ endif(GTSAM_BUILD_UNSTABLE)
 GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
 export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
 
+
 # Check for doxygen availability - optional dependency
 find_package(Doxygen)
 

README.md

@@ -1,48 +1,49 @@
-README - Georgia Tech Smoothing and Mapping library
+README - Georgia Tech Smoothing and Mapping library
-===================================================
+===================================================
-Version: Pre-Release 3.2.0
+
-
+What is GTSAM?
-What is GTSAM?
+--------------
---------------
+
-
+GTSAM is a library of C++ classes that implement smoothing and
-GTSAM is a library of C++ classes that implement smoothing and
+mapping (SAM) in robotics and vision, using factor graphs and Bayes
-mapping (SAM) in robotics and vision, using factor graphs and Bayes
+networks as the underlying computing paradigm rather than sparse
-networks as the underlying computing paradigm rather than sparse
+matrices.
-matrices.
+
-
+On top of the C++ library, GTSAM includes a MATLAB interface (enable
-On top of the C++ library, GTSAM includes a MATLAB interface (enable
+GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
-GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
+is under development.
-is under development.
+
-
+Quickstart
-Quickstart
+----------
-----------
+
-
+In the root library folder execute:
-In the root library folder execute:
+
-
+```
-```
+#!bash
-#!bash
+$ mkdir build
-$ mkdir build
+$ cd build
-$ cd build
+$ cmake ..
-$ cmake ..
+$ make check (optional, runs unit tests)
-$ make check (optional, runs unit tests)
+$ make install
-$ make install
+```
-```
+
-
+Prerequisites:
-Prerequisites:
+
-
+- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
-- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
+- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
-- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
+
-
+Optional prerequisites - used automatically if findable by CMake:
-Optional prerequisites - used automatically if findable by CMake:
+
-
+- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
-- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
+- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
-- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
+
-
+Additional Information
-Additional Information
+----------------------
-----------------------
+
-
+Read about important [`GTSAM-Concepts`] here.
-See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
+
-
+See the [`INSTALL`] file for more detailed installation instructions.
-GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
+
-
+GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
-Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM.
+
+Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM.

cmake/GtsamBuildTypes.cmake

@@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build typ
 # Add debugging flags but only on the first pass
 if(NOT FIRST_PASS_DONE)
   if(MSVC)
-    set(CMAKE_C_FLAGS_DEBUG            "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_C_FLAGS_DEBUG            "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
-    set(CMAKE_CXX_FLAGS_DEBUG          "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_CXX_FLAGS_DEBUG          "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
     set(CMAKE_C_FLAGS_RELWITHDEBINFO   "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_C_FLAGS_RELEASE          "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
@@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE)
     set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
     mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING)
   else()
-    set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
-    set(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
     set(CMAKE_C_FLAGS_RELWITHDEBINFO   "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_C_FLAGS_RELEASE          "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)

examples/CameraResectioning.cpp

@@ -48,7 +48,7 @@ public:
   virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
       boost::none) const {
     SimpleCamera camera(pose, *K_);
-    Point2 reprojectionError(camera.project(P_, H) - p_);
+    Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
     return reprojectionError.vector();
   }
 };
@@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
 
   /* 2. add factors to the graph */
   // add measurement factors
-  SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
+  SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
   boost::shared_ptr<ResectioningFactor> factor;
   graph.push_back(
       boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

examples/LocalizationExample.cpp

@@ -93,8 +93,8 @@ public:
     // Consequently, the Jacobians are:
     // [ derror_x/dx  derror_x/dy  derror_x/dtheta ] = [1 0 0]
     // [ derror_y/dx  derror_y/dy  derror_y/dtheta ] = [0 1 0]
-    if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0);
+    if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
-    return (Vector(2) << q.x() - mx_, q.y() - my_);
+    return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
   }
 
   // The second is a 'clone' function that allows the factor to be copied. Under most
@@ -118,14 +118,14 @@ int main(int argc, char** argv) {
 
   // 2a. Add odometry factors
   // For simplicity, we will use the same noise model for each odometry factor
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
   graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
 
   // 2b. Add "GPS-like" measurements
   // We will use our custom UnaryFactor for this.
-  noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
+  noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
   graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
   graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
   graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

examples/METISOrderingExample.cpp

@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 METISOrdering.cpp
+ * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
+ * @author Frank Dellaert
+ * @author Andrew Melim
+ */
+
+/**
+ * Example of a simple 2D localization example optimized using METIS ordering
+ * - For more details on the full optimization pipeline, see OdometryExample.cpp
+ */
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/Values.h>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv) {
+  NonlinearFactorGraph graph;
+
+  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
+
+  Pose2 odometry(2.0, 0.0, 0.0);
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
+  graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
+  graph.print("\nFactor Graph:\n"); // print
+
+  Values initial;
+  initial.insert(1, Pose2(0.5, 0.0, 0.2));
+  initial.insert(2, Pose2(2.3, 0.1, -0.2));
+  initial.insert(3, Pose2(4.1, 0.1, 0.1));
+  initial.print("\nInitial Estimate:\n"); // print
+
+  // optimize using Levenberg-Marquardt optimization
+  LevenbergMarquardtParams params;
+  // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
+  // By default this parameter is set to OrderingType::COLAMD
+  params.orderingType = Ordering::METIS;
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+
+  return 0;
+}

examples/OdometryExample.cpp

@@ -64,13 +64,13 @@ int main(int argc, char** argv) {
   // Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
 
   // Add odometry factors
   Pose2 odometry(2.0, 0.0, 0.0);
   // For simplicity, we will use the same noise model for each odometry factor
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
   graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

examples/PlanarSLAMExample.cpp

@@ -80,18 +80,18 @@ int main(int argc, char** argv) {
 
   // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
   graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
 
   // Add two odometry factors
   Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
   graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
   graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
 
   // Add Range-Bearing measurements to two different landmarks
   // create a noise model for the landmark measurements
-  noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
+  noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
   // create the measurement values - indices are (pose id, landmark id)
   Rot2 bearing11 = Rot2::fromDegrees(45),
        bearing21 = Rot2::fromDegrees(90),

examples/Pose2SLAMExample.cpp

@@ -71,11 +71,11 @@ int main(int argc, char** argv) {
 
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
 
   // For simplicity, we will use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses

examples/Pose2SLAMExample_g2o.cpp

@@ -64,7 +64,7 @@ int main(const int argc, const char *argv[]) {
   // Add prior on the pose having index (key) = 0
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
+      noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
   std::cout << "Adding prior on pose 0 " << std::endl;
 

examples/Pose2SLAMExample_graph.cpp

@@ -31,14 +31,14 @@ int main (int argc, char** argv) {
   // we are in build/examples, data is in examples/Data
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
-  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
   string graph_file = findExampleDataFile("w100.graph");
   boost::tie(graph, initial) = load2D(graph_file, model);
   initial->print("Initial estimate:\n");
 
   // Add a Gaussian prior on first poses
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
-  SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01));
+  SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
   graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
 
   // Single Step Optimization using Levenberg-Marquardt

examples/Pose2SLAMExample_graphviz.cpp

@@ -32,11 +32,11 @@ int main (int argc, char** argv) {
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
 
   // For simplicity, we will use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));

examples/Pose2SLAMExample_lago.cpp

@@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
   // Add prior on the pose having index (key) = 0
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
+      noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
   graphWithPrior.print();
 

examples/Pose2SLAMwSPCG.cpp

@@ -68,12 +68,12 @@ int main(int argc, char** argv) {
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 prior(0.0, 0.0, 0.0); // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
 
   // 2b. Add odometry factors
   // For simplicity, we will use the same noise model for each odometry factor
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2),    odometryNoise));
   graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
@@ -85,7 +85,7 @@ int main(int argc, char** argv) {
   // these constraints may be identified in many ways, such as appearance-based techniques
   // with camera images.
   // We will use another Between Factor to enforce this constraint, with the distance set to zero,
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
   graph.print("\nFactor Graph:\n"); // print
 

examples/Pose3SLAMExample_g2o.cpp

@@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
   // Add prior on the first key
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
   // Add prior on the first key
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
   // Add prior on the first key
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;

examples/SFMExample.cpp

@@ -80,13 +80,13 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t i = 0; i < poses.size(); ++i) {
+    SimpleCamera camera(poses[i], *K);
     for (size_t j = 0; j < points.size(); ++j) {
-      SimpleCamera camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
       graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
     }
@@ -111,6 +111,8 @@ int main(int argc, char* argv[]) {
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();
   result.print("Final results:\n");
+  cout << "initial error = " << graph.error(initialEstimate) << endl;
+  cout << "final error = " << graph.error(result) << endl;
 
   return 0;
 }

examples/SFMExample_SmartFactor.cpp

@@ -61,8 +61,7 @@ using namespace std;
 using namespace gtsam;
 
 // Make the typename short so it looks much cleaner
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor;
-    gtsam::Cal3_S2> SmartFactor;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
@@ -108,7 +107,7 @@ int main(int argc, char* argv[]) {
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
-      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
+      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
   graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
 
   // Because the structure-from-motion problem has a scale ambiguity, the problem is

examples/SelfCalibrationExample.cpp

@@ -53,7 +53,7 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
   // Simulated measurements from each camera pose, adding them to the factor graph
@@ -74,7 +74,7 @@ int main(int argc, char* argv[]) {
   graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
 
   // Add a prior on the calibration.
-  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100));
+  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
   graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
 
   // Create the initial estimate to the solution

examples/SolverComparer.cpp

@@ -589,7 +589,7 @@ void runStats()
 {
   cout << "Gathering statistics..." << endl;
   GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
-  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear)));
+  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
   treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
 
   ofstream file;

examples/StereoVOExample_large.cpp

@@ -103,7 +103,9 @@ int main(int argc, char** argv){
 
   cout << "Optimizing" << endl;
   //create Levenberg-Marquardt optimizer to optimize the factor graph
-  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
+  LevenbergMarquardtParams params;
+  params.orderingType = Ordering::METIS;
+  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;

examples/VisualISAM2Example.cpp

@@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
     // adding it to iSAM.
     if( i == 0) {
       // Add a prior on pose x0
-      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
       graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
       // Add a prior on landmark l0

examples/VisualISAMExample.cpp

@@ -108,7 +108,7 @@ int main(int argc, char* argv[]) {
     if (i == 0) {
       // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
       noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
-          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
+          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
       graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
       // Add a prior on landmark l0

examples/easyPoint2KalmanFilter.cpp

@@ -37,7 +37,7 @@ int main() {
 
   // Create the Kalman Filter initialization point
   Point2 x_initial(0.0, 0.0);
-  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
+  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
 
   // Create Key for initial pose
   Symbol x0('x',0);
@@ -57,8 +57,8 @@ int main() {
   // For the purposes of this example, let us assume we are using a constant-position model and
   // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1]
   // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1].
-  Vector u = (Vector(2) << 1.0, 0.0);
+  Vector u = Vector2(1.0, 0.0);
-  SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true);
+  SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);
 
   // This simple motion can be modeled with a BetweenFactor
   // Create Key for next pose
@@ -83,7 +83,7 @@ int main() {
   // For the purposes of this example, let us assume we have something like a GPS that returns
   // the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise
   // R = [0.25 0 ; 0 0.25].
-  SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true);
+  SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);
 
   // This simple measurement can be modeled with a PriorFactor
   Point2 z1(1.0, 0.0);

gtsam.h

@@ -157,7 +157,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/LieScalar.h>
-virtual class LieScalar : gtsam::Value {
+class LieScalar {
   // Standard constructors
   LieScalar();
   LieScalar(double d);
@@ -186,7 +186,7 @@ virtual class LieScalar : gtsam::Value {
 };
 
 #include <gtsam/base/LieVector.h>
-virtual class LieVector : gtsam::Value {
+class LieVector {
   // Standard constructors
   LieVector();
   LieVector(Vector v);
@@ -218,7 +218,7 @@ virtual class LieVector : gtsam::Value {
 };
 
 #include <gtsam/base/LieMatrix.h>
-virtual class LieMatrix : gtsam::Value {
+class LieMatrix {
   // Standard constructors
   LieMatrix();
   LieMatrix(Matrix v);
@@ -253,7 +253,7 @@ virtual class LieMatrix : gtsam::Value {
 // geometry
 //*************************************************************************
 
-virtual class Point2 : gtsam::Value {
+class Point2 {
   // Standard Constructors
   Point2();
   Point2(double x, double y);
@@ -290,7 +290,7 @@ virtual class Point2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class StereoPoint2 : gtsam::Value {
+class StereoPoint2 {
   // Standard Constructors
   StereoPoint2();
   StereoPoint2(double uL, double uR, double v);
@@ -325,7 +325,7 @@ virtual class StereoPoint2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Point3 : gtsam::Value {
+class Point3 {
   // Standard Constructors
   Point3();
   Point3(double x, double y, double z);
@@ -361,7 +361,7 @@ virtual class Point3 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Rot2 : gtsam::Value {
+class Rot2 {
   // Standard Constructors and Named Constructors
   Rot2();
   Rot2(double theta);
@@ -406,7 +406,7 @@ virtual class Rot2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Rot3 : gtsam::Value {
+class Rot3 {
   // Standard Constructors and Named Constructors
   Rot3();
   Rot3(Matrix R);
@@ -462,7 +462,7 @@ virtual class Rot3 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Pose2 : gtsam::Value {
+class Pose2 {
   // Standard Constructor
   Pose2();
   Pose2(const gtsam::Pose2& pose);
@@ -512,7 +512,7 @@ virtual class Pose2 : gtsam::Value {
   void serialize() const;
 };
 
-virtual class Pose3 : gtsam::Value {
+class Pose3 {
   // Standard Constructors
   Pose3();
   Pose3(const gtsam::Pose3& pose);
@@ -564,7 +564,7 @@ virtual class Pose3 : gtsam::Value {
 };
 
 #include <gtsam/geometry/Unit3.h>
-virtual class Unit3 : gtsam::Value {
+class Unit3 {
   // Standard Constructors
   Unit3();
   Unit3(const gtsam::Point3& pose);
@@ -585,7 +585,7 @@ virtual class Unit3 : gtsam::Value {
 };
 
 #include <gtsam/geometry/EssentialMatrix.h>
-virtual class EssentialMatrix : gtsam::Value {
+class EssentialMatrix {
   // Standard Constructors
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
@@ -606,7 +606,7 @@ virtual class EssentialMatrix : gtsam::Value {
   double error(Vector vA, Vector vB);
 };
 
-virtual class Cal3_S2 : gtsam::Value {
+class Cal3_S2 {
   // Standard Constructors
   Cal3_S2();
   Cal3_S2(double fx, double fy, double s, double u0, double v0);
@@ -643,7 +643,7 @@ virtual class Cal3_S2 : gtsam::Value {
 };
 
 #include <gtsam/geometry/Cal3DS2.h>
-virtual class Cal3DS2 : gtsam::Value {
+class Cal3DS2 {
   // Standard Constructors
   Cal3DS2();
   Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
@@ -699,7 +699,43 @@ class Cal3_S2Stereo {
   double baseline() const;
 };
 
-virtual class CalibratedCamera : gtsam::Value {
+#include <gtsam/geometry/Cal3Bundler.h>
+class Cal3Bundler {
+  // Standard Constructors
+  Cal3Bundler();
+  Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
+
+  // Manifold
+  static size_t Dim();
+  size_t dim() const;
+  gtsam::Cal3Bundler retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
+
+  // Action on Point2
+  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+
+  // Standard Interface
+  double fx() const;
+  double fy() const;
+  double k1() const;
+  double k2() const;
+  double u0() const;
+  double v0() const;
+  Vector vector() const;
+  Vector k() const;
+  //Matrix K() const; //FIXME: Uppercase
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+class CalibratedCamera {
   // Standard Constructors and Named Constructors
   CalibratedCamera();
   CalibratedCamera(const gtsam::Pose3& pose);
@@ -716,10 +752,6 @@ virtual class CalibratedCamera : gtsam::Value {
   gtsam::CalibratedCamera retract(const Vector& d) const;
   Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
 
-  // Group
-  gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
-  gtsam::CalibratedCamera inverse() const;
-
   // Action on Point3
   gtsam::Point2 project(const gtsam::Point3& point) const;
   static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@@ -732,7 +764,7 @@ virtual class CalibratedCamera : gtsam::Value {
   void serialize() const;
 };
 
-virtual class SimpleCamera : gtsam::Value {
+class SimpleCamera {
   // Standard Constructors and Named Constructors
   SimpleCamera();
   SimpleCamera(const gtsam::Pose3& pose);
@@ -771,7 +803,7 @@ virtual class SimpleCamera : gtsam::Value {
 };
 
 template<CALIBRATION = {gtsam::Cal3DS2}>
-virtual class PinholeCamera : gtsam::Value {
+class PinholeCamera {
   // Standard Constructors and Named Constructors
   PinholeCamera();
   PinholeCamera(const gtsam::Pose3& pose);
@@ -809,7 +841,7 @@ virtual class PinholeCamera : gtsam::Value {
   void serialize() const;
 };
 
-virtual class StereoCamera : gtsam::Value {
+class StereoCamera {
   // Standard Constructors and Named Constructors
   StereoCamera();
   StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@@ -862,7 +894,7 @@ virtual class SymbolicFactor {
 };
 
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
-class SymbolicFactorGraph {
+virtual class SymbolicFactorGraph {
   SymbolicFactorGraph();
   SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
   SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@@ -1676,15 +1708,12 @@ class Values {
   void print(string s) const;
   bool equals(const gtsam::Values& other, double tol) const;
 
-  void insert(size_t j, const gtsam::Value& value);
   void insert(const gtsam::Values& values);
-  void update(size_t j, const gtsam::Value& val);
   void update(const gtsam::Values& values);
   void erase(size_t j);
   void swap(gtsam::Values& values);
 
   bool exists(size_t j) const;
-  gtsam::Value at(size_t j) const;
   gtsam::KeyList keys() const;
 
   gtsam::VectorValues zeroVectors() const;
@@ -1694,6 +1723,46 @@ class Values {
 
   // enabling serialization functionality
   void serialize() const;
+
+  // New in 4.0, we have to specialize every insert/update/at to generate wrappers
+  // Instead of the old:
+  // void insert(size_t j, const gtsam::Value& value);
+  // void update(size_t j, const gtsam::Value& val);
+  // gtsam::Value at(size_t j) const;
+
+  void insert(size_t j, const gtsam::Point2& t);
+  void insert(size_t j, const gtsam::Point3& t);
+  void insert(size_t j, const gtsam::Rot2& t);
+  void insert(size_t j, const gtsam::Pose2& t);
+  void insert(size_t j, const gtsam::Rot3& t);
+  void insert(size_t j, const gtsam::Pose3& t);
+  void insert(size_t j, const gtsam::Cal3_S2& t);
+  void insert(size_t j, const gtsam::Cal3DS2& t);
+  void insert(size_t j, const gtsam::Cal3Bundler& t);
+  void insert(size_t j, const gtsam::EssentialMatrix& t);
+  void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
+  void insert(size_t j, Vector t);
+  void insert(size_t j, Matrix t);
+
+  // Fixed size version
+  void insertFixed(size_t j, Vector t, size_t n);
+
+  void update(size_t j, const gtsam::Point2& t);
+  void update(size_t j, const gtsam::Point3& t);
+  void update(size_t j, const gtsam::Rot2& t);
+  void update(size_t j, const gtsam::Pose2& t);
+  void update(size_t j, const gtsam::Rot3& t);
+  void update(size_t j, const gtsam::Pose3& t);
+  void update(size_t j, const gtsam::Cal3_S2& t);
+  void update(size_t j, const gtsam::Cal3DS2& t);
+  void update(size_t j, const gtsam::Cal3Bundler& t);
+  void update(size_t j, const gtsam::EssentialMatrix& t);
+  void update(size_t j, const gtsam::imuBias::ConstantBias& t);
+  void update(size_t j, Vector t);
+  void update(size_t j, Matrix t);
+
+  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
+  T at(size_t j);
 };
 
 // Actually a FastList<Key>
@@ -1791,6 +1860,7 @@ class KeyGroupMap {
 class Marginals {
   Marginals(const gtsam::NonlinearFactorGraph& graph,
       const gtsam::Values& solution);
+
   void print(string s) const;
   Matrix marginalCovariance(size_t variable) const;
   Matrix marginalInformation(size_t variable) const;
@@ -2089,7 +2159,7 @@ class NonlinearISAM {
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/slam/PriorFactor.h>
-template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = { gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2100,7 +2170,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
 
 
 #include <gtsam/slam/BetweenFactor.h>
-template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
+template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
 virtual class BetweenFactor : gtsam::NoiseModelFactor {
   BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
   T measured() const;
@@ -2110,8 +2180,9 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 };
 
 
+
 #include <gtsam/nonlinear/NonlinearEquality.h>
-template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
   NonlinearEquality(size_t j, const T& feasible);
@@ -2133,10 +2204,14 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
-typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
+
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+// Commented out by Frank Dec 2014: not poses!
-typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
+// If needed, we need a RangeFactor that takes a camera, extracts the pose
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
+// Should be easy with Expressions
+//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
+//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
+//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
 
 
 #include <gtsam/slam/BearingFactor.h>
@@ -2210,7 +2285,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
-template<POSE, LANDMARK, CALIBRATION>
+template<POSE, CALIBRATION>
 virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
 
   SmartProjectionPoseFactor(double rankTol, double linThreshold,
@@ -2226,7 +2301,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
   //void serialize() const;
 };
 
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
 
 
 #include <gtsam/slam/StereoFactor.h>
@@ -2292,7 +2367,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph,
 namespace imuBias {
 #include <gtsam/navigation/ImuBias.h>
 
-virtual class ConstantBias : gtsam::Value {
+class ConstantBias {
   // Standard Constructor
   ConstantBias();
   ConstantBias(Vector biasAcc, Vector biasGyro);
@@ -2328,6 +2403,9 @@ virtual class ConstantBias : gtsam::Value {
 }///\namespace imuBias
 
 #include <gtsam/navigation/ImuFactor.h>
+class PoseVelocity{
+    PoseVelocity(const gtsam::Pose3& pose, Vector velocity);
+  };
 class ImuFactorPreintegratedMeasurements {
   // Standard Constructor
   ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
@@ -2338,6 +2416,20 @@ class ImuFactorPreintegratedMeasurements {
   void print(string s) const;
   bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
 
+  Matrix measurementCovariance() const;
+  Matrix deltaRij() const;
+  double deltaTij() const;
+  Vector deltaPij() const;
+  Vector deltaVij() const;
+  Vector biasHat() const;
+  Matrix delPdelBiasAcc() const;
+  Matrix delPdelBiasOmega() const;
+  Matrix delVdelBiasAcc() const;
+  Matrix delVdelBiasOmega() const;
+  Matrix delRdelBiasOmega() const;
+  Matrix preintMeasCov() const;
+
+
   // Standard Interface
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
@@ -2349,16 +2441,56 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
   ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
       const gtsam::Pose3& body_P_sensor);
-
   // Standard Interface
   gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+  gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
-      const gtsam::imuBias::ConstantBias& bias,
       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis) const;
 };
 
+#include <gtsam/navigation/AHRSFactor.h>
+class AHRSFactorPreintegratedMeasurements {
+  // Standard Constructor
+  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
+
+  // get Data
+  Matrix measurementCovariance() const;
+  Matrix deltaRij() const;
+  double deltaTij() const;
+  Vector biasHat() const;
+
+  // Standard Interface
+  void integrateMeasurement(Vector measuredOmega, double deltaT);
+  void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+  void resetIntegration() ;
+};
+
+virtual class AHRSFactor : gtsam::NonlinearFactor {
+  AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
+      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
+  AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
+      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
+      const gtsam::Pose3& body_P_sensor);
+
+  // Standard Interface
+  gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
+      Vector bias) const;
+  gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
+      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
+      Vector omegaCoriolis) const;
+};
+
 #include <gtsam/navigation/CombinedImuFactor.h>
+class PoseVelocityBias{
+    PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
+  };
 class CombinedImuFactorPreintegratedMeasurements {
   // Standard Constructor
   CombinedImuFactorPreintegratedMeasurements(
@@ -2387,6 +2519,19 @@ class CombinedImuFactorPreintegratedMeasurements {
   // Standard Interface
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+
+  Matrix measurementCovariance() const;
+  Matrix deltaRij() const;
+  double deltaTij() const;
+  Vector deltaPij() const;
+  Vector deltaVij() const;
+  Vector biasHat() const;
+  Matrix delPdelBiasAcc() const;
+  Matrix delPdelBiasOmega() const;
+  Matrix delVdelBiasAcc() const;
+  Matrix delVdelBiasOmega() const;
+  Matrix delRdelBiasOmega() const;
+  Matrix PreintMeasCov() const;
 };
 
 virtual class CombinedImuFactor : gtsam::NonlinearFactor {
@@ -2395,10 +2540,36 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+  gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i,
-      const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
-      Vector gravity, Vector omegaCoriolis) const;
+      Vector gravity, Vector omegaCoriolis);
+};
+
+#include <gtsam/navigation/AttitudeFactor.h>
+//virtual class AttitudeFactor : gtsam::NonlinearFactor {
+//  AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
+//  AttitudeFactor();
+//};
+virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
+  Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
+      const gtsam::Unit3& bRef);
+  Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
+  Rot3AttitudeFactor();
+  void print(string s) const;
+  bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
+  gtsam::Unit3 nZ() const;
+  gtsam::Unit3 bRef() const;
+};
+
+virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
+      const gtsam::Unit3& bRef);
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
+  Pose3AttitudeFactor();
+  void print(string s) const;
+  bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
+  gtsam::Unit3 nZ() const;
+  gtsam::Unit3 bRef() const;
 };
 
 //*************************************************************************

gtsam/3rdparty/CMakeLists.txt

@@ -18,7 +18,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 
 	# do the same for the unsupported eigen folder
 	file(GLOB_RECURSE  unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
-	
+
 	file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
 	foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
 		get_filename_component(filename ${unsupported_eigen_dir} NAME)
@@ -36,17 +36,17 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 	install(DIRECTORY Eigen/Eigen
 		DESTINATION include/gtsam/3rdparty/Eigen
 		FILES_MATCHING PATTERN "*.h")
-	
+
 	install(DIRECTORY Eigen/unsupported/Eigen
 		DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
 		FILES_MATCHING PATTERN "*.h")
 endif()
 
-option(GTSAM_BUILD_METIS "Build metis library" ON)
 option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
-if(GTSAM_BUILD_METIS)
+add_subdirectory(metis)
-	add_subdirectory(metis-5.1.0)
+
-endif(GTSAM_BUILD_METIS)
+add_subdirectory(ceres)
+
 ############ NOTE:  When updating GeographicLib be sure to disable building their examples
 ############        and unit tests by commenting out their lines:
 # add_subdirectory (examples)
@@ -73,3 +73,5 @@ endif()
 if(GTSAM_INSTALL_GEOGRAPHICLIB)
   add_subdirectory(GeographicLib)
 endif()
+
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

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

@@ -25,14 +25,10 @@ namespace Eigen {
   * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
   */
 template<typename XprType>
-struct CommaInitializer :
+struct CommaInitializer
-  public internal::dense_xpr_base<CommaInitializer<XprType> >::type
 {
-  typedef typename internal::dense_xpr_base<CommaInitializer<XprType> >::type Base;
+  typedef typename XprType::Scalar Scalar;
-  EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer)
+  typedef typename XprType::Index Index;
-  typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
-    XprType, const XprType&>::type ExpressionTypeNested;
-  typedef typename XprType::InnerIterator InnerIterator;
 
   inline CommaInitializer(XprType& xpr, const Scalar& s)
     : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
@@ -108,82 +104,12 @@ struct CommaInitializer :
     */
   inline XprType& finished() { return m_xpr; }
 
-  // The following implement the DenseBase interface
-
-  inline Index rows() const { return m_xpr.rows(); }
-  inline Index cols() const { return m_xpr.cols(); }
-  inline Index outerStride() const { return m_xpr.outerStride(); }
-  inline Index innerStride() const { return m_xpr.innerStride(); }
-
-  inline CoeffReturnType coeff(Index row, Index col) const
-  {
-    return m_xpr.coeff(row, col);
-  }
-
-  inline CoeffReturnType coeff(Index index) const
-  {
-    return m_xpr.coeff(index);
-  }
-
-  inline const Scalar& coeffRef(Index row, Index col) const
-  {
-    return m_xpr.const_cast_derived().coeffRef(row, col);
-  }
-
-  inline const Scalar& coeffRef(Index index) const
-  {
-    return m_xpr.const_cast_derived().coeffRef(index);
-  }
-
-  inline Scalar& coeffRef(Index row, Index col)
-  {
-    return m_xpr.const_cast_derived().coeffRef(row, col);
-  }
-
-  inline Scalar& coeffRef(Index index)
-  {
-    return m_xpr.const_cast_derived().coeffRef(index);
-  }
-
-  template<int LoadMode>
-  inline const PacketScalar packet(Index row, Index col) const
-  {
-    return m_xpr.template packet<LoadMode>(row, col);
-  }
-
-  template<int LoadMode>
-  inline void writePacket(Index row, Index col, const PacketScalar& x)
-  {
-    m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
-  }
-
-  template<int LoadMode>
-  inline const PacketScalar packet(Index index) const
-  {
-    return m_xpr.template packet<LoadMode>(index);
-  }
-
-  template<int LoadMode>
-  inline void writePacket(Index index, const PacketScalar& x)
-  {
-    m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x);
-  }
-
-  const XprType& _expression() const { return m_xpr; }
-
   XprType& m_xpr;   // target expression
   Index m_row;              // current row id
   Index m_col;              // current col id
   Index m_currentBlockRows; // current block height
 };
 
-namespace internal {
-  template<typename XprType>
-  struct traits<CommaInitializer<XprType> > : traits<XprType>
-  {
-  };
-}
-
 /** \anchor MatrixBaseCommaInitRef
   * Convenient operator to set the coefficients of a matrix.
   *

gtsam/3rdparty/ceres/CMakeLists.txt

@@ -0,0 +1,2 @@
+file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h")
+install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres)

gtsam/3rdparty/ceres/autodiff.h

@@ -0,0 +1,314 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+//
+// Computation of the Jacobian matrix for vector-valued functions of multiple
+// variables, using automatic differentiation based on the implementation of
+// dual numbers in jet.h. Before reading the rest of this file, it is adivsable
+// to read jet.h's header comment in detail.
+//
+// The helper wrapper AutoDiff::Differentiate() computes the jacobian of
+// functors with templated operator() taking this form:
+//
+//   struct F {
+//     template<typename T>
+//     bool operator()(const T *x, const T *y, ..., T *z) {
+//       // Compute z[] based on x[], y[], ...
+//       // return true if computation succeeded, false otherwise.
+//     }
+//   };
+//
+// All inputs and outputs may be vector-valued.
+//
+// To understand how jets are used to compute the jacobian, a
+// picture may help. Consider a vector-valued function, F, returning 3
+// dimensions and taking a vector-valued parameter of 4 dimensions:
+//
+//     y            x
+//   [ * ]    F   [ * ]
+//   [ * ]  <---  [ * ]
+//   [ * ]        [ * ]
+//                [ * ]
+//
+// Similar to the 2-parameter example for f described in jet.h, computing the
+// jacobian dy/dx is done by substutiting a suitable jet object for x and all
+// intermediate steps of the computation of F. Since x is has 4 dimensions, use
+// a Jet<double, 4>.
+//
+// Before substituting a jet object for x, the dual components are set
+// appropriately for each dimension of x:
+//
+//          y                       x
+//   [ * | * * * * ]    f   [ * | 1 0 0 0 ]   x0
+//   [ * | * * * * ]  <---  [ * | 0 1 0 0 ]   x1
+//   [ * | * * * * ]        [ * | 0 0 1 0 ]   x2
+//         ---+---          [ * | 0 0 0 1 ]   x3
+//            |                   ^ ^ ^ ^
+//          dy/dx                 | | | +----- infinitesimal for x3
+//                                | | +------- infinitesimal for x2
+//                                | +--------- infinitesimal for x1
+//                                +----------- infinitesimal for x0
+//
+// The reason to set the internal 4x4 submatrix to the identity is that we wish
+// to take the derivative of y separately with respect to each dimension of x.
+// Each column of the 4x4 identity is therefore for a single component of the
+// independent variable x.
+//
+// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the
+// extended y vector, indicated in the above diagram.
+//
+// Functors with multiple parameters
+// ---------------------------------
+// In practice, it is often convenient to use a function f of two or more
+// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet
+// framework is designed for a single-parameter vector-valued input. The wrapper
+// in this file addresses this issue adding support for functions with one or
+// more parameter vectors.
+//
+// To support multiple parameters, all the parameter vectors are concatenated
+// into one and treated as a single parameter vector, except that since the
+// functor expects different inputs, we need to construct the jets as if they
+// were part of a single parameter vector. The extended jets are passed
+// separately for each parameter.
+//
+// For example, consider a functor F taking two vector parameters, p[2] and
+// q[3], and producing an output y[4]:
+//
+//   struct F {
+//     template<typename T>
+//     bool operator()(const T *p, const T *q, T *z) {
+//       // ...
+//     }
+//   };
+//
+// In this case, the necessary jet type is Jet<double, 5>. Here is a
+// visualization of the jet objects in this case:
+//
+//          Dual components for p ----+
+//                                    |
+//                                   -+-
+//           y                 [ * | 1 0 | 0 0 0 ]    --- p[0]
+//                             [ * | 0 1 | 0 0 0 ]    --- p[1]
+//   [ * | . . | + + + ]         |
+//   [ * | . . | + + + ]         v
+//   [ * | . . | + + + ]  <--- F(p, q)
+//   [ * | . . | + + + ]            ^
+//         ^^^   ^^^^^              |
+//        dy/dp  dy/dq            [ * | 0 0 | 1 0 0 ] --- q[0]
+//                                [ * | 0 0 | 0 1 0 ] --- q[1]
+//                                [ * | 0 0 | 0 0 1 ] --- q[2]
+//                                            --+--
+//                                              |
+//          Dual components for q --------------+
+//
+// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+"
+// of y in the above diagram are the derivatives of y with respect to p and q
+// respectively. This is how autodiff works for functors taking multiple vector
+// valued arguments (up to 6).
+//
+// Jacobian NULL pointers
+// ----------------------
+// In general, the functions below will accept NULL pointers for all or some of
+// the Jacobian parameters, meaning that those Jacobians will not be computed.
+
+#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_
+#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_
+
+#include <stddef.h>
+
+#include <gtsam/3rdparty/ceres/jet.h>
+#include <gtsam/3rdparty/ceres/eigen.h>
+#include <gtsam/3rdparty/ceres/fixed_array.h>
+#include <gtsam/3rdparty/ceres/variadic_evaluate.h>
+#define DCHECK assert
+#define DCHECK_GT(a,b) assert((a)>(b))
+
+namespace ceres {
+namespace internal {
+
+// Extends src by a 1st order pertubation for every dimension and puts it in
+// dst. The size of src is N. Since this is also used for perturbations in
+// blocked arrays, offset is used to shift which part of the jet the
+// perturbation occurs. This is used to set up the extended x augmented by an
+// identity matrix. The JetT type should be a Jet type, and T should be a
+// numeric type (e.g. double). For example,
+//
+//             0   1 2   3 4 5   6 7 8
+//   dst[0]  [ * | . . | 1 0 0 | . . . ]
+//   dst[1]  [ * | . . | 0 1 0 | . . . ]
+//   dst[2]  [ * | . . | 0 0 1 | . . . ]
+//
+// is what would get put in dst if N was 3, offset was 3, and the jet type JetT
+// was 8-dimensional.
+template <typename JetT, typename T, int N>
+inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) {
+  DCHECK(src);
+  DCHECK(dst);
+  for (int j = 0; j < N; ++j) {
+    dst[j].a = src[j];
+    dst[j].v.setZero();
+    dst[j].v[offset + j] = T(1.0);
+  }
+}
+
+// Takes the 0th order part of src, assumed to be a Jet type, and puts it in
+// dst. This is used to pick out the "vector" part of the extended y.
+template <typename JetT, typename T>
+inline void Take0thOrderPart(int M, const JetT *src, T dst) {
+  DCHECK(src);
+  for (int i = 0; i < M; ++i) {
+    dst[i] = src[i].a;
+  }
+}
+
+// Takes N 1st order parts, starting at index N0, and puts them in the M x N
+// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
+template <typename JetT, typename T, int N0, int N>
+inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
+  DCHECK(src);
+  DCHECK(dst);
+  for (int i = 0; i < M; ++i) {
+    Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) =
+        src[i].v.template segment<N>(N0);
+  }
+}
+
+// This is in a struct because default template parameters on a
+// function are not supported in C++03 (though it is available in
+// C++0x). N0 through N5 are the dimension of the input arguments to
+// the user supplied functor.
+template <typename Functor, typename T,
+          int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
+          int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
+struct AutoDiff {
+  static bool Differentiate(const Functor& functor,
+                            T const *const *parameters,
+                            int num_outputs,
+                            T *function_value,
+                            T **jacobians) {
+    // This block breaks the 80 column rule to keep it somewhat readable.
+    DCHECK_GT(num_outputs, 0);
+    DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)));
+
+    typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT;
+    FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
+        N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
+
+    // These are the positions of the respective jets in the fixed array x.
+    const int jet0  = 0;
+    const int jet1  = N0;
+    const int jet2  = N0 + N1;
+    const int jet3  = N0 + N1 + N2;
+    const int jet4  = N0 + N1 + N2 + N3;
+    const int jet5  = N0 + N1 + N2 + N3 + N4;
+    const int jet6  = N0 + N1 + N2 + N3 + N4 + N5;
+    const int jet7  = N0 + N1 + N2 + N3 + N4 + N5 + N6;
+    const int jet8  = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7;
+    const int jet9  = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8;
+
+    const JetT *unpacked_parameters[10] = {
+        x.get() + jet0,
+        x.get() + jet1,
+        x.get() + jet2,
+        x.get() + jet3,
+        x.get() + jet4,
+        x.get() + jet5,
+        x.get() + jet6,
+        x.get() + jet7,
+        x.get() + jet8,
+        x.get() + jet9,
+    };
+
+    JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
+
+#define CERES_MAKE_1ST_ORDER_PERTURBATION(i)                            \
+    if (N ## i) {                                                       \
+      internal::Make1stOrderPerturbation<JetT, T, N ## i>(              \
+          jet ## i,                                                     \
+          parameters[i],                                                \
+          x.get() + jet ## i);                                          \
+    }
+    CERES_MAKE_1ST_ORDER_PERTURBATION(0);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(1);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(2);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(3);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(4);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(5);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(6);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(7);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(8);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(9);
+#undef CERES_MAKE_1ST_ORDER_PERTURBATION
+
+    if (!VariadicEvaluate<Functor, JetT,
+                          N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
+        functor, unpacked_parameters, output)) {
+      return false;
+    }
+
+    internal::Take0thOrderPart(num_outputs, output, function_value);
+
+#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
+    if (N ## i) { \
+      if (jacobians[i]) { \
+        internal::Take1stOrderPart<JetT, T, \
+                                   jet ## i, \
+                                   N ## i>(num_outputs, \
+                                           output, \
+                                           jacobians[i]); \
+      } \
+    }
+    CERES_TAKE_1ST_ORDER_PERTURBATION(0);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(1);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(2);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(3);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(4);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(5);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(6);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(7);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(8);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(9);
+#undef CERES_TAKE_1ST_ORDER_PERTURBATION
+    return true;
+  }
+};
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_AUTODIFF_H_

gtsam/3rdparty/ceres/eigen.h

@@ -0,0 +1,93 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_INTERNAL_EIGEN_H_
+#define CERES_INTERNAL_EIGEN_H_
+
+#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+
+namespace ceres {
+
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
+typedef Eigen::Matrix<double,
+                      Eigen::Dynamic,
+                      Eigen::Dynamic,
+                      Eigen::RowMajor> Matrix;
+typedef Eigen::Map<Vector> VectorRef;
+typedef Eigen::Map<Matrix> MatrixRef;
+typedef Eigen::Map<const Vector> ConstVectorRef;
+typedef Eigen::Map<const Matrix> ConstMatrixRef;
+
+// Column major matrices for DenseSparseMatrix/DenseQRSolver
+typedef Eigen::Matrix<double,
+                      Eigen::Dynamic,
+                      Eigen::Dynamic,
+                      Eigen::ColMajor> ColMajorMatrix;
+
+typedef Eigen::Map<ColMajorMatrix, 0,
+                   Eigen::Stride<Eigen::Dynamic, 1> > ColMajorMatrixRef;
+
+typedef Eigen::Map<const ColMajorMatrix,
+                   0,
+                   Eigen::Stride<Eigen::Dynamic, 1> > ConstColMajorMatrixRef;
+
+
+
+// C++ does not support templated typdefs, thus the need for this
+// struct so that we can support statically sized Matrix and Maps.
+template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic>
+struct EigenTypes {
+  typedef Eigen::Matrix <double, num_rows, num_cols, Eigen::RowMajor>
+  Matrix;
+
+  typedef Eigen::Map<
+    Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
+  MatrixRef;
+
+  typedef Eigen::Matrix <double, num_rows, 1>
+  Vector;
+
+  typedef Eigen::Map <
+    Eigen::Matrix<double, num_rows, 1> >
+  VectorRef;
+
+
+  typedef Eigen::Map<
+    const Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
+  ConstMatrixRef;
+
+  typedef Eigen::Map <
+    const Eigen::Matrix<double, num_rows, 1> >
+  ConstVectorRef;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_INTERNAL_EIGEN_H_

gtsam/3rdparty/ceres/example.h

@@ -0,0 +1,78 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+//         sameeragarwal@google.com (Sameer Agarwal)
+//
+// Some Ceres Snippets copied for testing
+
+#pragma once
+
+#include <gtsam/3rdparty/ceres/rotation.h>
+
+// Templated pinhole camera model for used with Ceres. The camera is
+// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
+// focal length and 2 for radial distortion. The principal point is not modeled
+// (i.e. it is assumed be located at the image center).
+struct SnavelyProjection {
+
+  template<typename T>
+  bool operator()(const T* const camera, const T* const point,
+      T* predicted) const {
+    // camera[0,1,2] are the angle-axis rotation.
+    T p[3];
+    ceres::AngleAxisRotatePoint(camera, point, p);
+
+    // camera[3,4,5] are the translation.
+    p[0] += camera[3];
+    p[1] += camera[4];
+    p[2] += camera[5];
+
+    // Compute the center of distortion. The sign change comes from
+    // the camera model that Noah Snavely's Bundler assumes, whereby
+    // the camera coordinate system has a negative z axis.
+    T xp = -p[0] / p[2];
+    T yp = -p[1] / p[2];
+
+    // Apply second and fourth order radial distortion.
+    const T& l1 = camera[7];
+    const T& l2 = camera[8];
+    T r2 = xp * xp + yp * yp;
+    T distortion = T(1.0) + r2 * (l1 + l2 * r2);
+
+    // Compute final projected point position.
+    const T& focal = camera[6];
+    predicted[0] = focal * distortion * xp;
+    predicted[1] = focal * distortion * yp;
+
+    return true;
+  }
+
+};
+
+

gtsam/3rdparty/ceres/fixed_array.h

@@ -0,0 +1,190 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: rennie@google.com (Jeffrey Rennie)
+// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray
+
+#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
+
+#include <cstddef>
+#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <gtsam/3rdparty/ceres/macros.h>
+#include <gtsam/3rdparty/ceres/manual_constructor.h>
+
+namespace ceres {
+namespace internal {
+
+// A FixedArray<T> represents a non-resizable array of T where the
+// length of the array does not need to be a compile time constant.
+//
+// FixedArray allocates small arrays inline, and large arrays on
+// the heap.  It is a good replacement for non-standard and deprecated
+// uses of alloca() and variable length arrays (a GCC extension).
+//
+// FixedArray keeps performance fast for small arrays, because it
+// avoids heap operations.  It also helps reduce the chances of
+// accidentally overflowing your stack if large input is passed to
+// your function.
+//
+// Also, FixedArray is useful for writing portable code.  Not all
+// compilers support arrays of dynamic size.
+
+// Most users should not specify an inline_elements argument and let
+// FixedArray<> automatically determine the number of elements
+// to store inline based on sizeof(T).
+//
+// If inline_elements is specified, the FixedArray<> implementation
+// will store arrays of length <= inline_elements inline.
+//
+// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize
+// simple types like int, double, bool, etc.
+//
+// Non-POD types will be default-initialized just like regular vectors or
+// arrays.
+
+#if defined(_WIN64)
+   typedef __int64      ssize_t;
+#elif defined(_WIN32)
+   typedef __int32      ssize_t;
+#endif
+
+template <typename T, ssize_t inline_elements = -1>
+class FixedArray {
+ public:
+  // For playing nicely with stl:
+  typedef T value_type;
+  typedef T* iterator;
+  typedef T const* const_iterator;
+  typedef T& reference;
+  typedef T const& const_reference;
+  typedef T* pointer;
+  typedef std::ptrdiff_t difference_type;
+  typedef size_t size_type;
+
+  // REQUIRES: n >= 0
+  // Creates an array object that can store "n" elements.
+  //
+  // FixedArray<T> will not zero-initialiaze POD (simple) types like int,
+  // double, bool, etc.
+  // Non-POD types will be default-initialized just like regular vectors or
+  // arrays.
+  explicit FixedArray(size_type n);
+
+  // Releases any resources.
+  ~FixedArray();
+
+  // Returns the length of the array.
+  inline size_type size() const { return size_; }
+
+  // Returns the memory size of the array in bytes.
+  inline size_t memsize() const { return size_ * sizeof(T); }
+
+  // Returns a pointer to the underlying element array.
+  inline const T* get() const { return &array_[0].element; }
+  inline T* get() { return &array_[0].element; }
+
+  // REQUIRES: 0 <= i < size()
+  // Returns a reference to the "i"th element.
+  inline T& operator[](size_type i) {
+    assert(i < size_);
+    return array_[i].element;
+  }
+
+  // REQUIRES: 0 <= i < size()
+  // Returns a reference to the "i"th element.
+  inline const T& operator[](size_type i) const {
+    assert(i < size_);
+    return array_[i].element;
+  }
+
+  inline iterator begin() { return &array_[0].element; }
+  inline iterator end() { return &array_[size_].element; }
+
+  inline const_iterator begin() const { return &array_[0].element; }
+  inline const_iterator end() const { return &array_[size_].element; }
+
+ private:
+  // Container to hold elements of type T.  This is necessary to handle
+  // the case where T is a a (C-style) array.  The size of InnerContainer
+  // and T must be the same, otherwise callers' assumptions about use
+  // of this code will be broken.
+  struct InnerContainer {
+    T element;
+  };
+
+  // How many elements should we store inline?
+  //   a. If not specified, use a default of 256 bytes (256 bytes
+  //      seems small enough to not cause stack overflow or unnecessary
+  //      stack pollution, while still allowing stack allocation for
+  //      reasonably long character arrays.
+  //   b. Never use 0 length arrays (not ISO C++)
+  static const size_type S1 = ((inline_elements < 0)
+                               ? (256/sizeof(T)) : inline_elements);
+  static const size_type S2 = (S1 <= 0) ? 1 : S1;
+  static const size_type kInlineElements = S2;
+
+  size_type const       size_;
+  InnerContainer* const array_;
+
+  // Allocate some space, not an array of elements of type T, so that we can
+  // skip calling the T constructors and destructors for space we never use.
+  ManualConstructor<InnerContainer> inline_space_[kInlineElements];
+};
+
+// Implementation details follow
+
+template <class T, ssize_t S>
+inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n)
+    : size_(n),
+      array_((n <= kInlineElements
+              ? reinterpret_cast<InnerContainer*>(inline_space_)
+              : new InnerContainer[n])) {
+  // Construct only the elements actually used.
+  if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) {
+    for (size_t i = 0; i != size_; ++i) {
+      inline_space_[i].Init();
+    }
+  }
+}
+
+template <class T, ssize_t S>
+inline FixedArray<T, S>::~FixedArray() {
+  if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) {
+    delete[] array_;
+  } else {
+    for (size_t i = 0; i != size_; ++i) {
+      inline_space_[i].Destroy();
+    }
+  }
+}
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_

gtsam/3rdparty/ceres/fpclassify.h

@@ -0,0 +1,87 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+//
+// Portable floating point classification. The names are picked such that they
+// do not collide with macros. For example, "isnan" in C99 is a macro and hence
+// does not respect namespaces.
+//
+// TODO(keir): Finish porting!
+
+#ifndef CERES_PUBLIC_FPCLASSIFY_H_
+#define CERES_PUBLIC_FPCLASSIFY_H_
+
+#if defined(_MSC_VER)
+#include <float.h>
+#endif
+
+#include <limits>
+
+namespace ceres {
+
+#if defined(_MSC_VER)
+
+inline bool IsFinite  (double x) { return _finite(x) != 0;                   }
+inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; }
+inline bool IsNaN     (double x) { return _isnan(x) != 0;                    }
+inline bool IsNormal  (double x) {
+  int classification = _fpclass(x);
+  return classification == _FPCLASS_NN ||
+         classification == _FPCLASS_PN;
+}
+
+#elif defined(ANDROID) && defined(_STLPORT_VERSION)
+
+// On Android, when using the STLPort, the C++ isnan and isnormal functions
+// are defined as macros.
+inline bool IsNaN     (double x) { return isnan(x);    }
+inline bool IsNormal  (double x) { return isnormal(x); }
+// On Android NDK r6, when using STLPort, the isinf and isfinite functions are
+// not available, so reimplement them.
+inline bool IsInfinite(double x) {
+  return x ==  std::numeric_limits<double>::infinity() ||
+         x == -std::numeric_limits<double>::infinity();
+}
+inline bool IsFinite(double x) {
+  return !isnan(x) && !IsInfinite(x);
+}
+
+# else
+
+// These definitions are for the normal Unix suspects.
+inline bool IsFinite  (double x) { return std::isfinite(x); }
+inline bool IsInfinite(double x) { return std::isinf(x);    }
+inline bool IsNaN     (double x) { return std::isnan(x);    }
+inline bool IsNormal  (double x) { return std::isnormal(x); }
+
+#endif
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_FPCLASSIFY_H_

gtsam/3rdparty/ceres/jet.h

@@ -0,0 +1,670 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+//
+// A simple implementation of N-dimensional dual numbers, for automatically
+// computing exact derivatives of functions.
+//
+// While a complete treatment of the mechanics of automatic differentation is
+// beyond the scope of this header (see
+// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the
+// basic idea is to extend normal arithmetic with an extra element, "e," often
+// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual
+// numbers are extensions of the real numbers analogous to complex numbers:
+// whereas complex numbers augment the reals by introducing an imaginary unit i
+// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such
+// that e^2 = 0. Dual numbers have two components: the "real" component and the
+// "infinitesimal" component, generally written as x + y*e. Surprisingly, this
+// leads to a convenient method for computing exact derivatives without needing
+// to manipulate complicated symbolic expressions.
+//
+// For example, consider the function
+//
+//   f(x) = x^2 ,
+//
+// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
+// Next, augument 10 with an infinitesimal to get:
+//
+//   f(10 + e) = (10 + e)^2
+//             = 100 + 2 * 10 * e + e^2
+//             = 100 + 20 * e       -+-
+//                     --            |
+//                     |             +--- This is zero, since e^2 = 0
+//                     |
+//                     +----------------- This is df/dx!
+//
+// Note that the derivative of f with respect to x is simply the infinitesimal
+// component of the value of f(x + e). So, in order to take the derivative of
+// any function, it is only necessary to replace the numeric "object" used in
+// the function with one extended with infinitesimals. The class Jet, defined in
+// this header, is one such example of this, where substitution is done with
+// templates.
+//
+// To handle derivatives of functions taking multiple arguments, different
+// infinitesimals are used, one for each variable to take the derivative of. For
+// example, consider a scalar function of two scalar parameters x and y:
+//
+//   f(x, y) = x^2 + x * y
+//
+// Following the technique above, to compute the derivatives df/dx and df/dy for
+// f(1, 3) involves doing two evaluations of f, the first time replacing x with
+// x + e, the second time replacing y with y + e.
+//
+// For df/dx:
+//
+//   f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
+//               = 1 + 2 * e + 3 + 3 * e
+//               = 4 + 5 * e
+//
+//               --> df/dx = 5
+//
+// For df/dy:
+//
+//   f(1, 3 + e) = 1^2 + 1 * (3 + e)
+//               = 1 + 3 + e
+//               = 4 + e
+//
+//               --> df/dy = 1
+//
+// To take the gradient of f with the implementation of dual numbers ("jets") in
+// this file, it is necessary to create a single jet type which has components
+// for the derivative in x and y, and passing them to a templated version of f:
+//
+//   template<typename T>
+//   T f(const T &x, const T &y) {
+//     return x * x + x * y;
+//   }
+//
+//   // The "2" means there should be 2 dual number components.
+//   Jet<double, 2> x(0);  // Pick the 0th dual number for x.
+//   Jet<double, 2> y(1);  // Pick the 1st dual number for y.
+//   Jet<double, 2> z = f(x, y);
+//
+//   LOG(INFO) << "df/dx = " << z.a[0]
+//             << "df/dy = " << z.a[1];
+//
+// Most users should not use Jet objects directly; a wrapper around Jet objects,
+// which makes computing the derivative, gradient, or jacobian of templated
+// functors simple, is in autodiff.h. Even autodiff.h should not be used
+// directly; instead autodiff_cost_function.h is typically the file of interest.
+//
+// For the more mathematically inclined, this file implements first-order
+// "jets". A 1st order jet is an element of the ring
+//
+//   T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
+//
+// which essentially means that each jet consists of a "scalar" value 'a' from T
+// and a 1st order perturbation vector 'v' of length N:
+//
+//   x = a + \sum_i v[i] t_i
+//
+// A shorthand is to write an element as x = a + u, where u is the pertubation.
+// Then, the main point about the arithmetic of jets is that the product of
+// perturbations is zero:
+//
+//   (a + u) * (b + v) = ab + av + bu + uv
+//                     = ab + (av + bu) + 0
+//
+// which is what operator* implements below. Addition is simpler:
+//
+//   (a + u) + (b + v) = (a + b) + (u + v).
+//
+// The only remaining question is how to evaluate the function of a jet, for
+// which we use the chain rule:
+//
+//   f(a + u) = f(a) + f'(a) u
+//
+// where f'(a) is the (scalar) derivative of f at a.
+//
+// By pushing these things through sufficiently and suitably templated
+// functions, we can do automatic differentiation. Just be sure to turn on
+// function inlining and common-subexpression elimination, or it will be very
+// slow!
+//
+// WARNING: Most Ceres users should not directly include this file or know the
+// details of how jets work. Instead the suggested method for automatic
+// derivatives is to use autodiff_cost_function.h, which is a wrapper around
+// both jets.h and autodiff.h to make taking derivatives of cost functions for
+// use in Ceres easier.
+
+#ifndef CERES_PUBLIC_JET_H_
+#define CERES_PUBLIC_JET_H_
+
+#include <cmath>
+#include <iosfwd>
+#include <iostream>  // NOLINT
+#include <limits>
+#include <string>
+
+#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <gtsam/3rdparty/ceres/fpclassify.h>
+
+namespace ceres {
+
+template <typename T, int N>
+struct Jet {
+  enum { DIMENSION = N };
+
+  // Default-construct "a" because otherwise this can lead to false errors about
+  // uninitialized uses when other classes relying on default constructed T
+  // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that
+  // the C++ standard mandates that e.g. default constructed doubles are
+  // initialized to 0.0; see sections 8.5 of the C++03 standard.
+  Jet() : a() {
+    v.setZero();
+  }
+
+  // Constructor from scalar: a + 0.
+  explicit Jet(const T& value) {
+    a = value;
+    v.setZero();
+  }
+
+  // Constructor from scalar plus variable: a + t_i.
+  Jet(const T& value, int k) {
+    a = value;
+    v.setZero();
+    v[k] = T(1.0);
+  }
+
+  // Constructor from scalar and vector part
+  // The use of Eigen::DenseBase allows Eigen expressions
+  // to be passed in without being fully evaluated until
+  // they are assigned to v
+  template<typename Derived>
+  EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v)
+      : a(a), v(v) {
+  }
+
+  // Compound operators
+  Jet<T, N>& operator+=(const Jet<T, N> &y) {
+    *this = *this + y;
+    return *this;
+  }
+
+  Jet<T, N>& operator-=(const Jet<T, N> &y) {
+    *this = *this - y;
+    return *this;
+  }
+
+  Jet<T, N>& operator*=(const Jet<T, N> &y) {
+    *this = *this * y;
+    return *this;
+  }
+
+  Jet<T, N>& operator/=(const Jet<T, N> &y) {
+    *this = *this / y;
+    return *this;
+  }
+
+  // The scalar part.
+  T a;
+
+  // The infinitesimal part.
+  //
+  // Note the Eigen::DontAlign bit is needed here because this object
+  // gets allocated on the stack and as part of other arrays and
+  // structs. Forcing the right alignment there is the source of much
+  // pain and suffering. Even if that works, passing Jets around to
+  // functions by value has problems because the C++ ABI does not
+  // guarantee alignment for function arguments.
+  //
+  // Setting the DontAlign bit prevents Eigen from using SSE for the
+  // various operations on Jets. This is a small performance penalty
+  // since the AutoDiff code will still expose much of the code as
+  // statically sized loops to the compiler. But given the subtle
+  // issues that arise due to alignment, especially when dealing with
+  // multiple platforms, it seems to be a trade off worth making.
+  Eigen::Matrix<T, N, 1, Eigen::DontAlign> v;
+};
+
+// Unary +
+template<typename T, int N> inline
+Jet<T, N> const& operator+(const Jet<T, N>& f) {
+  return f;
+}
+
+// TODO(keir): Try adding __attribute__((always_inline)) to these functions to
+// see if it causes a performance increase.
+
+// Unary -
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>&f) {
+  return Jet<T, N>(-f.a, -f.v);
+}
+
+// Binary +
+template<typename T, int N> inline
+Jet<T, N> operator+(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  return Jet<T, N>(f.a + g.a, f.v + g.v);
+}
+
+// Binary + with a scalar: x + s
+template<typename T, int N> inline
+Jet<T, N> operator+(const Jet<T, N>& f, T s) {
+  return Jet<T, N>(f.a + s, f.v);
+}
+
+// Binary + with a scalar: s + x
+template<typename T, int N> inline
+Jet<T, N> operator+(T s, const Jet<T, N>& f) {
+  return Jet<T, N>(f.a + s, f.v);
+}
+
+// Binary -
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  return Jet<T, N>(f.a - g.a, f.v - g.v);
+}
+
+// Binary - with a scalar: x - s
+template<typename T, int N> inline
+Jet<T, N> operator-(const Jet<T, N>& f, T s) {
+  return Jet<T, N>(f.a - s, f.v);
+}
+
+// Binary - with a scalar: s - x
+template<typename T, int N> inline
+Jet<T, N> operator-(T s, const Jet<T, N>& f) {
+  return Jet<T, N>(s - f.a, -f.v);
+}
+
+// Binary *
+template<typename T, int N> inline
+Jet<T, N> operator*(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a);
+}
+
+// Binary * with a scalar: x * s
+template<typename T, int N> inline
+Jet<T, N> operator*(const Jet<T, N>& f, T s) {
+  return Jet<T, N>(f.a * s, f.v * s);
+}
+
+// Binary * with a scalar: s * x
+template<typename T, int N> inline
+Jet<T, N> operator*(T s, const Jet<T, N>& f) {
+  return Jet<T, N>(f.a * s, f.v * s);
+}
+
+// Binary /
+template<typename T, int N> inline
+Jet<T, N> operator/(const Jet<T, N>& f,
+                    const Jet<T, N>& g) {
+  // This uses:
+  //
+  //   a + u   (a + u)(b - v)   (a + u)(b - v)
+  //   ----- = -------------- = --------------
+  //   b + v   (b + v)(b - v)        b^2
+  //
+  // which holds because v*v = 0.
+  const T g_a_inverse = T(1.0) / g.a;
+  const T f_a_by_g_a = f.a * g_a_inverse;
+  return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse);
+}
+
+// Binary / with a scalar: s / x
+template<typename T, int N> inline
+Jet<T, N> operator/(T s, const Jet<T, N>& g) {
+  const T minus_s_g_a_inverse2 = -s / (g.a * g.a);
+  return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2);
+}
+
+// Binary / with a scalar: x / s
+template<typename T, int N> inline
+Jet<T, N> operator/(const Jet<T, N>& f, T s) {
+  const T s_inverse = 1.0 / s;
+  return Jet<T, N>(f.a * s_inverse, f.v * s_inverse);
+}
+
+// Binary comparison operators for both scalars and jets.
+#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
+template<typename T, int N> inline \
+bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \
+  return f.a op g.a; \
+} \
+template<typename T, int N> inline \
+bool operator op(const T& s, const Jet<T, N>& g) { \
+  return s op g.a; \
+} \
+template<typename T, int N> inline \
+bool operator op(const Jet<T, N>& f, const T& s) { \
+  return f.a op s; \
+}
+CERES_DEFINE_JET_COMPARISON_OPERATOR( <  )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( <= )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( >  )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( >= )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( == )  // NOLINT
+CERES_DEFINE_JET_COMPARISON_OPERATOR( != )  // NOLINT
+#undef CERES_DEFINE_JET_COMPARISON_OPERATOR
+
+// Pull some functions from namespace std.
+//
+// This is necessary because we want to use the same name (e.g. 'sqrt') for
+// double-valued and Jet-valued functions, but we are not allowed to put
+// Jet-valued functions inside namespace std.
+//
+// TODO(keir): Switch to "using".
+inline double abs     (double x) { return std::abs(x);      }
+inline double log     (double x) { return std::log(x);      }
+inline double exp     (double x) { return std::exp(x);      }
+inline double sqrt    (double x) { return std::sqrt(x);     }
+inline double cos     (double x) { return std::cos(x);      }
+inline double acos    (double x) { return std::acos(x);     }
+inline double sin     (double x) { return std::sin(x);      }
+inline double asin    (double x) { return std::asin(x);     }
+inline double tan     (double x) { return std::tan(x);      }
+inline double atan    (double x) { return std::atan(x);     }
+inline double sinh    (double x) { return std::sinh(x);     }
+inline double cosh    (double x) { return std::cosh(x);     }
+inline double tanh    (double x) { return std::tanh(x);     }
+inline double pow  (double x, double y) { return std::pow(x, y);   }
+inline double atan2(double y, double x) { return std::atan2(y, x); }
+
+// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
+
+// abs(x + h) ~= x + h or -(x + h)
+template <typename T, int N> inline
+Jet<T, N> abs(const Jet<T, N>& f) {
+  return f.a < T(0.0) ? -f : f;
+}
+
+// log(a + h) ~= log(a) + h / a
+template <typename T, int N> inline
+Jet<T, N> log(const Jet<T, N>& f) {
+  const T a_inverse = T(1.0) / f.a;
+  return Jet<T, N>(log(f.a), f.v * a_inverse);
+}
+
+// exp(a + h) ~= exp(a) + exp(a) h
+template <typename T, int N> inline
+Jet<T, N> exp(const Jet<T, N>& f) {
+  const T tmp = exp(f.a);
+  return Jet<T, N>(tmp, tmp * f.v);
+}
+
+// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
+template <typename T, int N> inline
+Jet<T, N> sqrt(const Jet<T, N>& f) {
+  const T tmp = sqrt(f.a);
+  const T two_a_inverse = T(1.0) / (T(2.0) * tmp);
+  return Jet<T, N>(tmp, f.v * two_a_inverse);
+}
+
+// cos(a + h) ~= cos(a) - sin(a) h
+template <typename T, int N> inline
+Jet<T, N> cos(const Jet<T, N>& f) {
+  return Jet<T, N>(cos(f.a), - sin(f.a) * f.v);
+}
+
+// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
+template <typename T, int N> inline
+Jet<T, N> acos(const Jet<T, N>& f) {
+  const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a);
+  return Jet<T, N>(acos(f.a), tmp * f.v);
+}
+
+// sin(a + h) ~= sin(a) + cos(a) h
+template <typename T, int N> inline
+Jet<T, N> sin(const Jet<T, N>& f) {
+  return Jet<T, N>(sin(f.a), cos(f.a) * f.v);
+}
+
+// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
+template <typename T, int N> inline
+Jet<T, N> asin(const Jet<T, N>& f) {
+  const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a);
+  return Jet<T, N>(asin(f.a), tmp * f.v);
+}
+
+// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
+template <typename T, int N> inline
+Jet<T, N> tan(const Jet<T, N>& f) {
+  const T tan_a = tan(f.a);
+  const T tmp = T(1.0) + tan_a * tan_a;
+  return Jet<T, N>(tan_a, tmp * f.v);
+}
+
+// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
+template <typename T, int N> inline
+Jet<T, N> atan(const Jet<T, N>& f) {
+  const T tmp = T(1.0) / (T(1.0) + f.a * f.a);
+  return Jet<T, N>(atan(f.a), tmp * f.v);
+}
+
+// sinh(a + h) ~= sinh(a) + cosh(a) h
+template <typename T, int N> inline
+Jet<T, N> sinh(const Jet<T, N>& f) {
+  return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v);
+}
+
+// cosh(a + h) ~= cosh(a) + sinh(a) h
+template <typename T, int N> inline
+Jet<T, N> cosh(const Jet<T, N>& f) {
+  return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v);
+}
+
+// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
+template <typename T, int N> inline
+Jet<T, N> tanh(const Jet<T, N>& f) {
+  const T tanh_a = tanh(f.a);
+  const T tmp = T(1.0) - tanh_a * tanh_a;
+  return Jet<T, N>(tanh_a, tmp * f.v);
+}
+
+// Jet Classification. It is not clear what the appropriate semantics are for
+// these classifications. This picks that IsFinite and isnormal are "all"
+// operations, i.e. all elements of the jet must be finite for the jet itself
+// to be finite (or normal). For IsNaN and IsInfinite, the answer is less
+// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any
+// part of a jet is nan or inf, then the entire jet is nan or inf. This leads
+// to strange situations like a jet can be both IsInfinite and IsNaN, but in
+// practice the "any" semantics are the most useful for e.g. checking that
+// derivatives are sane.
+
+// The jet is finite if all parts of the jet are finite.
+template <typename T, int N> inline
+bool IsFinite(const Jet<T, N>& f) {
+  if (!IsFinite(f.a)) {
+    return false;
+  }
+  for (int i = 0; i < N; ++i) {
+    if (!IsFinite(f.v[i])) {
+      return false;
+    }
+  }
+  return true;
+}
+
+// The jet is infinite if any part of the jet is infinite.
+template <typename T, int N> inline
+bool IsInfinite(const Jet<T, N>& f) {
+  if (IsInfinite(f.a)) {
+    return true;
+  }
+  for (int i = 0; i < N; i++) {
+    if (IsInfinite(f.v[i])) {
+      return true;
+    }
+  }
+  return false;
+}
+
+// The jet is NaN if any part of the jet is NaN.
+template <typename T, int N> inline
+bool IsNaN(const Jet<T, N>& f) {
+  if (IsNaN(f.a)) {
+    return true;
+  }
+  for (int i = 0; i < N; ++i) {
+    if (IsNaN(f.v[i])) {
+      return true;
+    }
+  }
+  return false;
+}
+
+// The jet is normal if all parts of the jet are normal.
+template <typename T, int N> inline
+bool IsNormal(const Jet<T, N>& f) {
+  if (!IsNormal(f.a)) {
+    return false;
+  }
+  for (int i = 0; i < N; ++i) {
+    if (!IsNormal(f.v[i])) {
+      return false;
+    }
+  }
+  return true;
+}
+
+// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
+//
+// In words: the rate of change of theta is 1/r times the rate of
+// change of (x, y) in the positive angular direction.
+template <typename T, int N> inline
+Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) {
+  // Note order of arguments:
+  //
+  //   f = a + da
+  //   g = b + db
+
+  T const tmp = T(1.0) / (f.a * f.a + g.a * g.a);
+  return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
+}
+
+
+// pow -- base is a differentiable function, exponent is a constant.
+// (a+da)^p ~= a^p + p*a^(p-1) da
+template <typename T, int N> inline
+Jet<T, N> pow(const Jet<T, N>& f, double g) {
+  T const tmp = g * pow(f.a, g - T(1.0));
+  return Jet<T, N>(pow(f.a, g), tmp * f.v);
+}
+
+// pow -- base is a constant, exponent is a differentiable function.
+// (a)^(p+dp) ~= a^p + a^p log(a) dp
+template <typename T, int N> inline
+Jet<T, N> pow(double f, const Jet<T, N>& g) {
+  T const tmp = pow(f, g.a);
+  return Jet<T, N>(tmp, log(f) * tmp * g.v);
+}
+
+
+// pow -- both base and exponent are differentiable functions.
+// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db
+template <typename T, int N> inline
+Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
+  T const tmp1 = pow(f.a, g.a);
+  T const tmp2 = g.a * pow(f.a, g.a - T(1.0));
+  T const tmp3 = tmp1 * log(f.a);
+
+  return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
+}
+
+// Define the helper functions Eigen needs to embed Jet types.
+//
+// NOTE(keir): machine_epsilon() and precision() are missing, because they don't
+// work with nested template types (e.g. where the scalar is itself templated).
+// Among other things, this means that decompositions of Jet's does not work,
+// for example
+//
+//   Matrix<Jet<T, N> ... > A, x, b;
+//   ...
+//   A.solve(b, &x)
+//
+// does not work and will fail with a strange compiler error.
+//
+// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we
+// switch to 3.0, also add the rest of the specialization functionality.
+template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x;              }  // NOLINT
+template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x;              }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_imag(const Jet<T, N>&  ) { return Jet<T, N>(0.0); }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_abs (const Jet<T, N>& x) { return fabs(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_abs2(const Jet<T, N>& x) { return x * x;          }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_sqrt(const Jet<T, N>& x) { return sqrt(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_exp (const Jet<T, N>& x) { return exp(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_log (const Jet<T, N>& x) { return log(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_sin (const Jet<T, N>& x) { return sin(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_cos (const Jet<T, N>& x) { return cos(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_tan (const Jet<T, N>& x) { return tan(x);         }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_atan(const Jet<T, N>& x) { return atan(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_sinh(const Jet<T, N>& x) { return sinh(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_cosh(const Jet<T, N>& x) { return cosh(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_tanh(const Jet<T, N>& x) { return tanh(x);        }  // NOLINT
+template<typename T, int N> inline       Jet<T, N>  ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); }  // NOLINT
+
+// Note: This has to be in the ceres namespace for argument dependent lookup to
+// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with
+// strange compile errors.
+template <typename T, int N>
+inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) {
+  return s << "[" << z.a << " ; " << z.v.transpose() << "]";
+}
+
+}  // namespace ceres
+
+namespace Eigen {
+
+// Creating a specialization of NumTraits enables placing Jet objects inside
+// Eigen arrays, getting all the goodness of Eigen combined with autodiff.
+template<typename T, int N>
+struct NumTraits<ceres::Jet<T, N> > {
+  typedef ceres::Jet<T, N> Real;
+  typedef ceres::Jet<T, N> NonInteger;
+  typedef ceres::Jet<T, N> Nested;
+
+  static typename ceres::Jet<T, N> dummy_precision() {
+    return ceres::Jet<T, N>(1e-12);
+  }
+
+  static inline Real epsilon() {
+    return Real(std::numeric_limits<T>::epsilon());
+  }
+
+  enum {
+    IsComplex = 0,
+    IsInteger = 0,
+    IsSigned,
+    ReadCost = 1,
+    AddCost = 1,
+    // For Jet types, multiplication is more expensive than addition.
+    MulCost = 3,
+    HasFloatingPoint = 1,
+    RequireInitialization = 1
+  };
+};
+
+}  // namespace Eigen
+
+#endif  // CERES_PUBLIC_JET_H_

gtsam/3rdparty/ceres/macros.h

@@ -0,0 +1,170 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+//
+// Various Google-specific macros.
+//
+// This code is compiled directly on many platforms, including client
+// platforms like Windows, Mac, and embedded systems.  Before making
+// any changes here, make sure that you're not breaking any platforms.
+
+#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_
+#define CERES_PUBLIC_INTERNAL_MACROS_H_
+
+#include <cstddef>  // For size_t.
+
+// A macro to disallow the copy constructor and operator= functions
+// This should be used in the private: declarations for a class
+//
+// For disallowing only assign or copy, write the code directly, but declare
+// the intend in a comment, for example:
+//
+//   void operator=(const TypeName&);  // _DISALLOW_ASSIGN
+
+// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY
+// are broken semantically, one should either use disallow both or
+// neither. Try to avoid these in new code.
+#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \
+  TypeName(const TypeName&);               \
+  void operator=(const TypeName&)
+
+// A macro to disallow all the implicit constructors, namely the
+// default constructor, copy constructor and operator= functions.
+//
+// This should be used in the private: declarations for a class
+// that wants to prevent anyone from instantiating it. This is
+// especially useful for classes containing only static methods.
+#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \
+  TypeName();                                    \
+  CERES_DISALLOW_COPY_AND_ASSIGN(TypeName)
+
+// The arraysize(arr) macro returns the # of elements in an array arr.
+// The expression is a compile-time constant, and therefore can be
+// used in defining new arrays, for example.  If you use arraysize on
+// a pointer by mistake, you will get a compile-time error.
+//
+// One caveat is that arraysize() doesn't accept any array of an
+// anonymous type or a type defined inside a function.  In these rare
+// cases, you have to use the unsafe ARRAYSIZE() macro below.  This is
+// due to a limitation in C++'s template system.  The limitation might
+// eventually be removed, but it hasn't happened yet.
+
+// This template function declaration is used in defining arraysize.
+// Note that the function doesn't need an implementation, as we only
+// use its type.
+template <typename T, size_t N>
+char (&ArraySizeHelper(T (&array)[N]))[N];
+
+// That gcc wants both of these prototypes seems mysterious. VC, for
+// its part, can't decide which to use (another mystery). Matching of
+// template overloads: the final frontier.
+#ifndef _WIN32
+template <typename T, size_t N>
+char (&ArraySizeHelper(const T (&array)[N]))[N];
+#endif
+
+#define arraysize(array) (sizeof(ArraySizeHelper(array)))
+
+// ARRAYSIZE performs essentially the same calculation as arraysize,
+// but can be used on anonymous types or types defined inside
+// functions.  It's less safe than arraysize as it accepts some
+// (although not all) pointers.  Therefore, you should use arraysize
+// whenever possible.
+//
+// The expression ARRAYSIZE(a) is a compile-time constant of type
+// size_t.
+//
+// ARRAYSIZE catches a few type errors.  If you see a compiler error
+//
+//   "warning: division by zero in ..."
+//
+// when using ARRAYSIZE, you are (wrongfully) giving it a pointer.
+// You should only use ARRAYSIZE on statically allocated arrays.
+//
+// The following comments are on the implementation details, and can
+// be ignored by the users.
+//
+// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in
+// the array) and sizeof(*(arr)) (the # of bytes in one array
+// element).  If the former is divisible by the latter, perhaps arr is
+// indeed an array, in which case the division result is the # of
+// elements in the array.  Otherwise, arr cannot possibly be an array,
+// and we generate a compiler error to prevent the code from
+// compiling.
+//
+// Since the size of bool is implementation-defined, we need to cast
+// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final
+// result has type size_t.
+//
+// This macro is not perfect as it wrongfully accepts certain
+// pointers, namely where the pointer size is divisible by the pointee
+// size.  Since all our code has to go through a 32-bit compiler,
+// where a pointer is 4 bytes, this means all pointers to a type whose
+// size is 3 or greater than 4 will be (righteously) rejected.
+//
+// Kudos to Jorg Brown for this simple and elegant implementation.
+//
+// - wan 2005-11-16
+//
+// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However,
+// the definition comes from the over-broad windows.h header that
+// introduces a macro, ERROR, that conflicts with the logging framework
+// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE.
+#define CERES_ARRAYSIZE(a)                              \
+  ((sizeof(a) / sizeof(*(a))) /                         \
+   static_cast<size_t>(!(sizeof(a) % sizeof(*(a)))))
+
+// Tell the compiler to warn about unused return values for functions
+// declared with this macro.  The macro should be used on function
+// declarations following the argument list:
+//
+//   Sprocket* AllocateSprocket() MUST_USE_RESULT;
+//
+#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \
+  && !defined(COMPILER_ICC)
+#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result))
+#else
+#define CERES_MUST_USE_RESULT
+#endif
+
+// Platform independent macros to get aligned memory allocations.
+// For example
+//
+//   MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16);
+//
+// Gives us an instance of MyFoo which is aligned at a 16 byte
+// boundary.
+#if defined(_MSC_VER)
+#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n))
+#define CERES_ALIGN_OF(T) __alignof(T)
+#elif defined(__GNUC__)
+#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n)))
+#define CERES_ALIGN_OF(T) __alignof(T)
+#endif
+
+#endif  // CERES_PUBLIC_INTERNAL_MACROS_H_

gtsam/3rdparty/ceres/manual_constructor.h

@@ -0,0 +1,208 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: kenton@google.com (Kenton Varda)
+//
+// ManualConstructor statically-allocates space in which to store some
+// object, but does not initialize it.  You can then call the constructor
+// and destructor for the object yourself as you see fit.  This is useful
+// for memory management optimizations, where you want to initialize and
+// destroy an object multiple times but only allocate it once.
+//
+// (When I say ManualConstructor statically allocates space, I mean that
+// the ManualConstructor object itself is forced to be the right size.)
+
+#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
+#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
+
+#include <new>
+
+namespace ceres {
+namespace internal {
+
+// ------- Define CERES_ALIGNED_CHAR_ARRAY --------------------------------
+
+#ifndef CERES_ALIGNED_CHAR_ARRAY
+
+// Because MSVC and older GCCs require that the argument to their alignment
+// construct to be a literal constant integer, we use a template instantiated
+// at all the possible powers of two.
+template<int alignment, int size> struct AlignType { };
+template<int size> struct AlignType<0, size> { typedef char result[size]; };
+
+#if !defined(CERES_ALIGN_ATTRIBUTE)
+#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler
+#else  // !defined(CERES_ALIGN_ATTRIBUTE)
+
+#define CERES_ALIGN_TYPE_TEMPLATE(X) \
+  template<int size> struct AlignType<X, size> { \
+    typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \
+  }
+
+CERES_ALIGN_TYPE_TEMPLATE(1);
+CERES_ALIGN_TYPE_TEMPLATE(2);
+CERES_ALIGN_TYPE_TEMPLATE(4);
+CERES_ALIGN_TYPE_TEMPLATE(8);
+CERES_ALIGN_TYPE_TEMPLATE(16);
+CERES_ALIGN_TYPE_TEMPLATE(32);
+CERES_ALIGN_TYPE_TEMPLATE(64);
+CERES_ALIGN_TYPE_TEMPLATE(128);
+CERES_ALIGN_TYPE_TEMPLATE(256);
+CERES_ALIGN_TYPE_TEMPLATE(512);
+CERES_ALIGN_TYPE_TEMPLATE(1024);
+CERES_ALIGN_TYPE_TEMPLATE(2048);
+CERES_ALIGN_TYPE_TEMPLATE(4096);
+CERES_ALIGN_TYPE_TEMPLATE(8192);
+// Any larger and MSVC++ will complain.
+
+#undef CERES_ALIGN_TYPE_TEMPLATE
+
+#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \
+  typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result
+
+#endif  // !defined(CERES_ALIGN_ATTRIBUTE)
+
+#endif  // CERES_ALIGNED_CHAR_ARRAY
+
+template <typename Type>
+class ManualConstructor {
+ public:
+  // No constructor or destructor because one of the most useful uses of
+  // this class is as part of a union, and members of a union cannot have
+  // constructors or destructors.  And, anyway, the whole point of this
+  // class is to bypass these.
+
+  inline Type* get() {
+    return reinterpret_cast<Type*>(space_);
+  }
+  inline const Type* get() const  {
+    return reinterpret_cast<const Type*>(space_);
+  }
+
+  inline Type* operator->() { return get(); }
+  inline const Type* operator->() const { return get(); }
+
+  inline Type& operator*() { return *get(); }
+  inline const Type& operator*() const { return *get(); }
+
+  // This is needed to get around the strict aliasing warning GCC generates.
+  inline void* space() {
+    return reinterpret_cast<void*>(space_);
+  }
+
+  // You can pass up to four constructor arguments as arguments of Init().
+  inline void Init() {
+    new(space()) Type;
+  }
+
+  template <typename T1>
+  inline void Init(const T1& p1) {
+    new(space()) Type(p1);
+  }
+
+  template <typename T1, typename T2>
+  inline void Init(const T1& p1, const T2& p2) {
+    new(space()) Type(p1, p2);
+  }
+
+  template <typename T1, typename T2, typename T3>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3) {
+    new(space()) Type(p1, p2, p3);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) {
+    new(space()) Type(p1, p2, p3, p4);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5) {
+    new(space()) Type(p1, p2, p3, p4, p5);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8, typename T9>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8,
+                   const T9& p9) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8, typename T9, typename T10>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8,
+                   const T9& p9, const T10& p10) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10);
+  }
+
+  template <typename T1, typename T2, typename T3, typename T4, typename T5,
+            typename T6, typename T7, typename T8, typename T9, typename T10,
+            typename T11>
+  inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
+                   const T5& p5, const T6& p6, const T7& p7, const T8& p8,
+                   const T9& p9, const T10& p10, const T11& p11) {
+    new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11);
+  }
+
+  inline void Destroy() {
+    get()->~Type();
+  }
+
+ private:
+  CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_;
+};
+
+#undef CERES_ALIGNED_CHAR_ARRAY
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_

gtsam/3rdparty/ceres/rotation.h

@@ -0,0 +1,647 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+//         sameeragarwal@google.com (Sameer Agarwal)
+//
+// Templated functions for manipulating rotations. The templated
+// functions are useful when implementing functors for automatic
+// differentiation.
+//
+// In the following, the Quaternions are laid out as 4-vectors, thus:
+//
+//   q[0]  scalar part.
+//   q[1]  coefficient of i.
+//   q[2]  coefficient of j.
+//   q[3]  coefficient of k.
+//
+// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
+
+#ifndef CERES_PUBLIC_ROTATION_H_
+#define CERES_PUBLIC_ROTATION_H_
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <assert.h>
+#define DCHECK assert
+
+namespace ceres {
+
+// Trivial wrapper to index linear arrays as matrices, given a fixed
+// column and row stride. When an array "T* array" is wrapped by a
+//
+//   (const) MatrixAdapter<T, row_stride, col_stride> M"
+//
+// the expression  M(i, j) is equivalent to
+//
+//   arrary[i * row_stride + j * col_stride]
+//
+// Conversion functions to and from rotation matrices accept
+// MatrixAdapters to permit using row-major and column-major layouts,
+// and rotation matrices embedded in larger matrices (such as a 3x4
+// projection matrix).
+template <typename T, int row_stride, int col_stride>
+struct MatrixAdapter;
+
+// Convenience functions to create a MatrixAdapter that treats the
+// array pointed to by "pointer" as a 3x3 (contiguous) column-major or
+// row-major matrix.
+template <typename T>
+MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer);
+
+template <typename T>
+MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer);
+
+// Convert a value in combined axis-angle representation to a quaternion.
+// The value angle_axis is a triple whose norm is an angle in radians,
+// and whose direction is aligned with the axis of rotation,
+// and quaternion is a 4-tuple that will contain the resulting quaternion.
+// The implementation may be used with auto-differentiation up to the first
+// derivative, higher derivatives may have unexpected results near the origin.
+template<typename T>
+void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
+
+// Convert a quaternion to the equivalent combined axis-angle representation.
+// The value quaternion must be a unit quaternion - it is not normalized first,
+// and angle_axis will be filled with a value whose norm is the angle of
+// rotation in radians, and whose direction is the axis of rotation.
+// The implemention may be used with auto-differentiation up to the first
+// derivative, higher derivatives may have unexpected results near the origin.
+template<typename T>
+void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
+
+// Conversions between 3x3 rotation matrix (in column major order) and
+// axis-angle rotation representations.  Templated for use with
+// autodifferentiation.
+template <typename T>
+void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
+
+template <typename T, int row_stride, int col_stride>
+void RotationMatrixToAngleAxis(
+    const MatrixAdapter<const T, row_stride, col_stride>& R,
+    T* angle_axis);
+
+template <typename T>
+void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
+
+template <typename T, int row_stride, int col_stride>
+void AngleAxisToRotationMatrix(
+    const T* angle_axis,
+    const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Conversions between 3x3 rotation matrix (in row major order) and
+// Euler angle (in degrees) rotation representations.
+//
+// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
+// axes, respectively.  They are applied in that same order, so the
+// total rotation R is Rz * Ry * Rx.
+template <typename T>
+void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
+
+template <typename T, int row_stride, int col_stride>
+void EulerAnglesToRotationMatrix(
+    const T* euler,
+    const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Convert a 4-vector to a 3x3 scaled rotation matrix.
+//
+// The choice of rotation is such that the quaternion [1 0 0 0] goes to an
+// identity matrix and for small a, b, c the quaternion [1 a b c] goes to
+// the matrix
+//
+//         [  0 -c  b ]
+//   I + 2 [  c  0 -a ] + higher order terms
+//         [ -b  a  0 ]
+//
+// which corresponds to a Rodrigues approximation, the last matrix being
+// the cross-product matrix of [a b c]. Together with the property that
+// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
+//
+// The rotation matrix is row-major.
+//
+// No normalization of the quaternion is performed, i.e.
+// R = ||q||^2 * Q, where Q is an orthonormal matrix
+// such that det(Q) = 1 and Q*Q' = I
+template <typename T> inline
+void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToScaledRotation(
+    const T q[4],
+    const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Same as above except that the rotation matrix is normalized by the
+// Frobenius norm, so that R * R' = I (and det(R) = 1).
+template <typename T> inline
+void QuaternionToRotation(const T q[4], T R[3 * 3]);
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToRotation(
+    const T q[4],
+    const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Rotates a point pt by a quaternion q:
+//
+//   result = R(q) * pt
+//
+// Assumes the quaternion is unit norm. This assumption allows us to
+// write the transform as (something)*pt + pt, as is clear from the
+// formula below. If you pass in a quaternion with |q|^2 = 2 then you
+// WILL NOT get back 2 times the result you get for a unit quaternion.
+template <typename T> inline
+void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
+
+// With this function you do not need to assume that q has unit norm.
+// It does assume that the norm is non-zero.
+template <typename T> inline
+void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
+
+// zw = z * w, where * is the Quaternion product between 4 vectors.
+template<typename T> inline
+void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
+
+// xy = x cross y;
+template<typename T> inline
+void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
+
+template<typename T> inline
+T DotProduct(const T x[3], const T y[3]);
+
+// y = R(angle_axis) * x;
+template<typename T> inline
+void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
+
+// --- IMPLEMENTATION
+
+template<typename T, int row_stride, int col_stride>
+struct MatrixAdapter {
+  T* pointer_;
+  explicit MatrixAdapter(T* pointer)
+    : pointer_(pointer)
+  {}
+
+  T& operator()(int r, int c) const {
+    return pointer_[r * row_stride + c * col_stride];
+  }
+};
+
+template <typename T>
+MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) {
+  return MatrixAdapter<T, 1, 3>(pointer);
+}
+
+template <typename T>
+MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) {
+  return MatrixAdapter<T, 3, 1>(pointer);
+}
+
+template<typename T>
+inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
+  const T& a0 = angle_axis[0];
+  const T& a1 = angle_axis[1];
+  const T& a2 = angle_axis[2];
+  const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
+
+  // For points not at the origin, the full conversion is numerically stable.
+  if (theta_squared > T(0.0)) {
+    const T theta = sqrt(theta_squared);
+    const T half_theta = theta * T(0.5);
+    const T k = sin(half_theta) / theta;
+    quaternion[0] = cos(half_theta);
+    quaternion[1] = a0 * k;
+    quaternion[2] = a1 * k;
+    quaternion[3] = a2 * k;
+  } else {
+    // At the origin, sqrt() will produce NaN in the derivative since
+    // the argument is zero.  By approximating with a Taylor series,
+    // and truncating at one term, the value and first derivatives will be
+    // computed correctly when Jets are used.
+    const T k(0.5);
+    quaternion[0] = T(1.0);
+    quaternion[1] = a0 * k;
+    quaternion[2] = a1 * k;
+    quaternion[3] = a2 * k;
+  }
+}
+
+template<typename T>
+inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
+  const T& q1 = quaternion[1];
+  const T& q2 = quaternion[2];
+  const T& q3 = quaternion[3];
+  const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
+
+  // For quaternions representing non-zero rotation, the conversion
+  // is numerically stable.
+  if (sin_squared_theta > T(0.0)) {
+    const T sin_theta = sqrt(sin_squared_theta);
+    const T& cos_theta = quaternion[0];
+
+    // If cos_theta is negative, theta is greater than pi/2, which
+    // means that angle for the angle_axis vector which is 2 * theta
+    // would be greater than pi.
+    //
+    // While this will result in the correct rotation, it does not
+    // result in a normalized angle-axis vector.
+    //
+    // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
+    // which is equivalent saying
+    //
+    //   theta - pi = atan(sin(theta - pi), cos(theta - pi))
+    //              = atan(-sin(theta), -cos(theta))
+    //
+    const T two_theta =
+        T(2.0) * ((cos_theta < 0.0)
+                  ? atan2(-sin_theta, -cos_theta)
+                  : atan2(sin_theta, cos_theta));
+    const T k = two_theta / sin_theta;
+    angle_axis[0] = q1 * k;
+    angle_axis[1] = q2 * k;
+    angle_axis[2] = q3 * k;
+  } else {
+    // For zero rotation, sqrt() will produce NaN in the derivative since
+    // the argument is zero.  By approximating with a Taylor series,
+    // and truncating at one term, the value and first derivatives will be
+    // computed correctly when Jets are used.
+    const T k(2.0);
+    angle_axis[0] = q1 * k;
+    angle_axis[1] = q2 * k;
+    angle_axis[2] = q3 * k;
+  }
+}
+
+// The conversion of a rotation matrix to the angle-axis form is
+// numerically problematic when then rotation angle is close to zero
+// or to Pi. The following implementation detects when these two cases
+// occurs and deals with them by taking code paths that are guaranteed
+// to not perform division by a small number.
+template <typename T>
+inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
+  RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis);
+}
+
+template <typename T, int row_stride, int col_stride>
+void RotationMatrixToAngleAxis(
+    const MatrixAdapter<const T, row_stride, col_stride>& R,
+    T* angle_axis) {
+  // x = k * 2 * sin(theta), where k is the axis of rotation.
+  angle_axis[0] = R(2, 1) - R(1, 2);
+  angle_axis[1] = R(0, 2) - R(2, 0);
+  angle_axis[2] = R(1, 0) - R(0, 1);
+
+  static const T kOne = T(1.0);
+  static const T kTwo = T(2.0);
+
+  // Since the right hand side may give numbers just above 1.0 or
+  // below -1.0 leading to atan misbehaving, we threshold.
+  T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo,
+                                 T(-1.0)),
+                        kOne);
+
+  // sqrt is guaranteed to give non-negative results, so we only
+  // threshold above.
+  T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] +
+                             angle_axis[1] * angle_axis[1] +
+                             angle_axis[2] * angle_axis[2]) / kTwo,
+                        kOne);
+
+  // Use the arctan2 to get the right sign on theta
+  const T theta = atan2(sintheta, costheta);
+
+  // Case 1: sin(theta) is large enough, so dividing by it is not a
+  // problem. We do not use abs here, because while jets.h imports
+  // std::abs into the namespace, here in this file, abs resolves to
+  // the int version of the function, which returns zero always.
+  //
+  // We use a threshold much larger then the machine epsilon, because
+  // if sin(theta) is small, not only do we risk overflow but even if
+  // that does not occur, just dividing by a small number will result
+  // in numerical garbage. So we play it safe.
+  static const double kThreshold = 1e-12;
+  if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
+    const T r = theta / (kTwo * sintheta);
+    for (int i = 0; i < 3; ++i) {
+      angle_axis[i] *= r;
+    }
+    return;
+  }
+
+  // Case 2: theta ~ 0, means sin(theta) ~ theta to a good
+  // approximation.
+  if (costheta > 0.0) {
+    const T kHalf = T(0.5);
+    for (int i = 0; i < 3; ++i) {
+      angle_axis[i] *= kHalf;
+    }
+    return;
+  }
+
+  // Case 3: theta ~ pi, this is the hard case. Since theta is large,
+  // and sin(theta) is small. Dividing by theta by sin(theta) will
+  // either give an overflow or worse still numerically meaningless
+  // results. Thus we use an alternate more complicated formula
+  // here.
+
+  // Since cos(theta) is negative, division by (1-cos(theta)) cannot
+  // overflow.
+  const T inv_one_minus_costheta = kOne / (kOne - costheta);
+
+  // We now compute the absolute value of coordinates of the axis
+  // vector using the diagonal entries of R. To resolve the sign of
+  // these entries, we compare the sign of angle_axis[i]*sin(theta)
+  // with the sign of sin(theta). If they are the same, then
+  // angle_axis[i] should be positive, otherwise negative.
+  for (int i = 0; i < 3; ++i) {
+    angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta);
+    if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) ||
+        ((sintheta > 0.0) && (angle_axis[i] < 0.0))) {
+      angle_axis[i] = -angle_axis[i];
+    }
+  }
+}
+
+template <typename T>
+inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
+  AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride>
+void AngleAxisToRotationMatrix(
+    const T* angle_axis,
+    const MatrixAdapter<T, row_stride, col_stride>& R) {
+  static const T kOne = T(1.0);
+  const T theta2 = DotProduct(angle_axis, angle_axis);
+  if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+    // We want to be careful to only evaluate the square root if the
+    // norm of the angle_axis vector is greater than zero. Otherwise
+    // we get a division by zero.
+    const T theta = sqrt(theta2);
+    const T wx = angle_axis[0] / theta;
+    const T wy = angle_axis[1] / theta;
+    const T wz = angle_axis[2] / theta;
+
+    const T costheta = cos(theta);
+    const T sintheta = sin(theta);
+
+    R(0, 0) =     costheta   + wx*wx*(kOne -    costheta);
+    R(1, 0) =  wz*sintheta   + wx*wy*(kOne -    costheta);
+    R(2, 0) = -wy*sintheta   + wx*wz*(kOne -    costheta);
+    R(0, 1) =  wx*wy*(kOne - costheta)     - wz*sintheta;
+    R(1, 1) =     costheta   + wy*wy*(kOne -    costheta);
+    R(2, 1) =  wx*sintheta   + wy*wz*(kOne -    costheta);
+    R(0, 2) =  wy*sintheta   + wx*wz*(kOne -    costheta);
+    R(1, 2) = -wx*sintheta   + wy*wz*(kOne -    costheta);
+    R(2, 2) =     costheta   + wz*wz*(kOne -    costheta);
+  } else {
+    // Near zero, we switch to using the first order Taylor expansion.
+    R(0, 0) =  kOne;
+    R(1, 0) =  angle_axis[2];
+    R(2, 0) = -angle_axis[1];
+    R(0, 1) = -angle_axis[2];
+    R(1, 1) =  kOne;
+    R(2, 1) =  angle_axis[0];
+    R(0, 2) =  angle_axis[1];
+    R(1, 2) = -angle_axis[0];
+    R(2, 2) = kOne;
+  }
+}
+
+template <typename T>
+inline void EulerAnglesToRotationMatrix(const T* euler,
+                                        const int row_stride_parameter,
+                                        T* R) {
+  DCHECK(row_stride_parameter==3);
+  EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride>
+void EulerAnglesToRotationMatrix(
+    const T* euler,
+    const MatrixAdapter<T, row_stride, col_stride>& R) {
+  const double kPi = 3.14159265358979323846;
+  const T degrees_to_radians(kPi / 180.0);
+
+  const T pitch(euler[0] * degrees_to_radians);
+  const T roll(euler[1] * degrees_to_radians);
+  const T yaw(euler[2] * degrees_to_radians);
+
+  const T c1 = cos(yaw);
+  const T s1 = sin(yaw);
+  const T c2 = cos(roll);
+  const T s2 = sin(roll);
+  const T c3 = cos(pitch);
+  const T s3 = sin(pitch);
+
+  R(0, 0) = c1*c2;
+  R(0, 1) = -s1*c3 + c1*s2*s3;
+  R(0, 2) = s1*s3 + c1*s2*c3;
+
+  R(1, 0) = s1*c2;
+  R(1, 1) = c1*c3 + s1*s2*s3;
+  R(1, 2) = -c1*s3 + s1*s2*c3;
+
+  R(2, 0) = -s2;
+  R(2, 1) = c2*s3;
+  R(2, 2) = c2*c3;
+}
+
+template <typename T> inline
+void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
+  QuaternionToScaledRotation(q, RowMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToScaledRotation(
+    const T q[4],
+    const MatrixAdapter<T, row_stride, col_stride>& R) {
+  // Make convenient names for elements of q.
+  T a = q[0];
+  T b = q[1];
+  T c = q[2];
+  T d = q[3];
+  // This is not to eliminate common sub-expression, but to
+  // make the lines shorter so that they fit in 80 columns!
+  T aa = a * a;
+  T ab = a * b;
+  T ac = a * c;
+  T ad = a * d;
+  T bb = b * b;
+  T bc = b * c;
+  T bd = b * d;
+  T cc = c * c;
+  T cd = c * d;
+  T dd = d * d;
+
+  R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad);  R(0, 2) = T(2) * (ac + bd);  // NOLINT
+  R(1, 0) = T(2) * (ad + bc);  R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab);  // NOLINT
+  R(2, 0) = T(2) * (bd - ac);  R(2, 1) = T(2) * (ab + cd);  R(2, 2) = aa - bb - cc + dd; // NOLINT
+}
+
+template <typename T> inline
+void QuaternionToRotation(const T q[4], T R[3 * 3]) {
+  QuaternionToRotation(q, RowMajorAdapter3x3(R));
+}
+
+template <typename T, int row_stride, int col_stride> inline
+void QuaternionToRotation(const T q[4],
+                          const MatrixAdapter<T, row_stride, col_stride>& R) {
+  QuaternionToScaledRotation(q, R);
+
+  T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
+  CHECK_NE(normalizer, T(0));
+  normalizer = T(1) / normalizer;
+
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      R(i, j) *= normalizer;
+    }
+  }
+}
+
+template <typename T> inline
+void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
+  const T t2 =  q[0] * q[1];
+  const T t3 =  q[0] * q[2];
+  const T t4 =  q[0] * q[3];
+  const T t5 = -q[1] * q[1];
+  const T t6 =  q[1] * q[2];
+  const T t7 =  q[1] * q[3];
+  const T t8 = -q[2] * q[2];
+  const T t9 =  q[2] * q[3];
+  const T t1 = -q[3] * q[3];
+  result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0];  // NOLINT
+  result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1];  // NOLINT
+  result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2];  // NOLINT
+}
+
+template <typename T> inline
+void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
+  // 'scale' is 1 / norm(q).
+  const T scale = T(1) / sqrt(q[0] * q[0] +
+                              q[1] * q[1] +
+                              q[2] * q[2] +
+                              q[3] * q[3]);
+
+  // Make unit-norm version of q.
+  const T unit[4] = {
+    scale * q[0],
+    scale * q[1],
+    scale * q[2],
+    scale * q[3],
+  };
+
+  UnitQuaternionRotatePoint(unit, pt, result);
+}
+
+template<typename T> inline
+void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
+  zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
+  zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
+  zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
+  zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
+}
+
+// xy = x cross y;
+template<typename T> inline
+void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
+  x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
+  x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
+  x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
+}
+
+template<typename T> inline
+T DotProduct(const T x[3], const T y[3]) {
+  return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
+}
+
+template<typename T> inline
+void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
+  const T theta2 = DotProduct(angle_axis, angle_axis);
+  if (theta2 > T(std::numeric_limits<double>::epsilon())) {
+    // Away from zero, use the rodriguez formula
+    //
+    //   result = pt costheta +
+    //            (w x pt) * sintheta +
+    //            w (w . pt) (1 - costheta)
+    //
+    // We want to be careful to only evaluate the square root if the
+    // norm of the angle_axis vector is greater than zero. Otherwise
+    // we get a division by zero.
+    //
+    const T theta = sqrt(theta2);
+    const T costheta = cos(theta);
+    const T sintheta = sin(theta);
+    const T theta_inverse = 1.0 / theta;
+
+    const T w[3] = { angle_axis[0] * theta_inverse,
+                     angle_axis[1] * theta_inverse,
+                     angle_axis[2] * theta_inverse };
+
+    // Explicitly inlined evaluation of the cross product for
+    // performance reasons.
+    const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
+                              w[2] * pt[0] - w[0] * pt[2],
+                              w[0] * pt[1] - w[1] * pt[0] };
+    const T tmp =
+        (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
+
+    result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
+    result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
+    result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
+  } else {
+    // Near zero, the first order Taylor approximation of the rotation
+    // matrix R corresponding to a vector w and angle w is
+    //
+    //   R = I + hat(w) * sin(theta)
+    //
+    // But sintheta ~ theta and theta * w = angle_axis, which gives us
+    //
+    //  R = I + hat(w)
+    //
+    // and actually performing multiplication with the point pt, gives us
+    // R * pt = pt + w x pt.
+    //
+    // Switching to the Taylor expansion near zero provides meaningful
+    // derivatives when evaluated using Jets.
+    //
+    // Explicitly inlined evaluation of the cross product for
+    // performance reasons.
+    const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
+                              angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
+                              angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };
+
+    result[0] = pt[0] + w_cross_pt[0];
+    result[1] = pt[1] + w_cross_pt[1];
+    result[2] = pt[2] + w_cross_pt[2];
+  }
+}
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_ROTATION_H_

gtsam/3rdparty/ceres/variadic_evaluate.h

@@ -0,0 +1,181 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2013 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//         mierle@gmail.com (Keir Mierle)
+
+#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
+#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
+
+#include <stddef.h>
+
+#include <gtsam/3rdparty/ceres/jet.h>
+#include <gtsam/3rdparty/ceres/eigen.h>
+#include <gtsam/3rdparty/ceres/fixed_array.h>
+
+namespace ceres {
+namespace internal {
+
+// This block of quasi-repeated code calls the user-supplied functor, which may
+// take a variable number of arguments. This is accomplished by specializing the
+// struct based on the size of the trailing parameters; parameters with 0 size
+// are assumed missing.
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7, int N8, int N9>
+struct VariadicEvaluate {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   input[8],
+                   input[9],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7, int N8>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   input[8],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1>
+struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0>
+struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   output);
+  }
+};
+
+}  // namespace internal
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_

gtsam/3rdparty/gtsam_eigen_includes.h.in

@@ -27,3 +27,7 @@
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU>
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD>
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry>
+
+
+
+

gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt

@@ -1,16 +0,0 @@
-# Add this directory for internal users.
-include_directories(.)
-# Find sources.
-file(GLOB metis_sources *.c)
-# Build libmetis.
-add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
-if(UNIX)
-  target_link_libraries(metis m)
-endif()
-
-
-install(TARGETS metis
-  LIBRARY DESTINATION include/gtsam/3rdparty/metis/lib
-  RUNTIME DESTINATION include/gtsam/3rdparty/metis/lib
-  ARCHIVE DESTINATION include/gtsam/3rdparty/metis/lib)
-

gtsam/3rdparty/metis/CMakeLists.txt

@@ -37,3 +37,6 @@ add_subdirectory("libmetis")
 if(GTSAM_BUILD_METIS_EXECUTABLES)
   add_subdirectory("programs")
 endif()
+
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+

gtsam/3rdparty/metis/GKlib/ms_stdint.h

@@ -1,7 +1,7 @@
 // ISO C9x  compliant stdint.h for Microsoft Visual Studio
 // Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 
 // 
-//  Copyright (c) 2006 Alexander Chemeris
+//  Copyright (c) 2006-2013 Alexander Chemeris
 // 
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
@@ -13,8 +13,9 @@
 //      notice, this list of conditions and the following disclaimer in the
 //      documentation and/or other materials provided with the distribution.
 // 
-//   3. The name of the author may be used to endorse or promote products
+//   3. Neither the name of the product nor the names of its contributors may
-//      derived from this software without specific prior written permission.
+//      be used to endorse or promote products derived from this software
+//      without specific prior written permission.
 // 
 // THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
 // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
@@ -40,30 +41,59 @@
 #pragma once
 #endif
 
+#if _MSC_VER >= 1600 // [
+#include <stdint.h>
+#else // ] _MSC_VER >= 1600 [
+
 #include <limits.h>
 
-// For Visual Studio 6 in C++ mode wrap <wchar.h> include with 'extern "C++" {}'
+// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
+// compiling for ARM we should wrap <wchar.h> include with 'extern "C++" {}'
 // or compiler give many errors like this:
 //   error C2733: second C linkage of overloaded function 'wmemchr' not allowed
-#if (_MSC_VER < 1300) && defined(__cplusplus)
+#ifdef __cplusplus
-   extern "C++" {
+extern "C" {
-#endif 
-#     include <wchar.h>
-#if (_MSC_VER < 1300) && defined(__cplusplus)
-   }
 #endif
+#  include <wchar.h>
+#ifdef __cplusplus
+}
+#endif
+
+// Define _W64 macros to mark types changing their size, like intptr_t.
+#ifndef _W64
+#  if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
+#     define _W64 __w64
+#  else
+#     define _W64
+#  endif
+#endif
+
 
 // 7.18.1 Integer types
 
 // 7.18.1.1 Exact-width integer types
-typedef __int8            int8_t;
+
-typedef __int16           int16_t;
+// Visual Studio 6 and Embedded Visual C++ 4 doesn't
-typedef __int32           int32_t;
+// realize that, e.g. char has the same size as __int8
-typedef __int64           int64_t;
+// so we give up on __intX for them.
+#if (_MSC_VER < 1300)
+typedef signed char       int8_t;
+typedef signed short      int16_t;
+typedef signed int        int32_t;
+typedef unsigned char     uint8_t;
+typedef unsigned short    uint16_t;
+typedef unsigned int      uint32_t;
+#else
+typedef signed __int8     int8_t;
+typedef signed __int16    int16_t;
+typedef signed __int32    int32_t;
 typedef unsigned __int8   uint8_t;
 typedef unsigned __int16  uint16_t;
 typedef unsigned __int32  uint32_t;
-typedef unsigned __int64  uint64_t;
+#endif
+typedef signed __int64       int64_t;
+typedef unsigned __int64     uint64_t;
+
 
 // 7.18.1.2 Minimum-width integer types
 typedef int8_t    int_least8_t;
@@ -87,11 +117,11 @@ typedef uint64_t  uint_fast64_t;
 
 // 7.18.1.4 Integer types capable of holding object pointers
 #ifdef _WIN64 // [
-   typedef __int64           intptr_t;
+typedef signed __int64    intptr_t;
-   typedef unsigned __int64  uintptr_t;
+typedef unsigned __int64  uintptr_t;
 #else // _WIN64 ][
-   typedef int               intptr_t;
+typedef _W64 signed int   intptr_t;
-   typedef unsigned int      uintptr_t;
+typedef _W64 unsigned int uintptr_t;
 #endif // _WIN64 ]
 
 // 7.18.1.5 Greatest-width integer types
@@ -213,10 +243,17 @@ typedef uint64_t  uintmax_t;
 #define UINT64_C(val) val##ui64
 
 // 7.18.4.2 Macros for greatest-width integer constants
-#define INTMAX_C   INT64_C
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
-#define UINTMAX_C  UINT64_C
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C //   [
+#  define INTMAX_C   INT64_C
+#endif // INTMAX_C    ]
+#ifndef UINTMAX_C //  [
+#  define UINTMAX_C  UINT64_C
+#endif // UINTMAX_C   ]
 
 #endif // __STDC_CONSTANT_MACROS ]
 
+#endif // _MSC_VER >= 1600 ]
 
-#endif // _MSC_STDINT_H_ ]
+#endif // _MSC_STDINT_H_ ]