Merge branch 'develop'

Conflicts:
	.cproject
	examples/Pose3SLAMExample_g2o.cpp
	examples/Pose3SLAMExample_initializePose3Chordal.cpp
	examples/Pose3SLAMExample_initializePose3Gradient.cpp
	gtsam/slam/InitializePose3.cpp
	gtsam/slam/InitializePose3.h
	gtsam/slam/tests/testInitializePose3.cpp

.cproject

@@ -582,7 +582,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -590,7 +589,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -638,7 +636,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -646,7 +643,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -654,7 +650,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -670,7 +665,6 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1006,7 +1000,6 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1236,46 +1229,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testFixedVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1358,6 +1311,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1397,6 +1351,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1404,6 +1359,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1417,6 +1373,46 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testFixedVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1674,7 +1670,6 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1682,7 +1677,6 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1690,7 +1684,6 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1698,7 +1691,6 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2425,7 +2417,6 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2433,7 +2424,6 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2441,7 +2431,6 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2647,14 +2636,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testInitializePose3.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testInitializePose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2929,6 +2910,7 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

CMakeLists.txt

@@ -123,6 +123,11 @@ else()
 endif()
 
 
+if(${Boost_VERSION} EQUAL 105600)
+	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
+	add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
+endif()
+
 ###############################################################################
 # Find TBB
 find_package(TBB)
@@ -169,9 +174,9 @@ endif()
 
 ###############################################################################
 # Find OpenMP (if we're also using MKL)
-if(GTSAM_WITH_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
+find_package(OpenMP)  # do this here to generate correct message if disabled
-    find_package(OpenMP)
 
+if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
     if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
         set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
         set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")

cmake/FindMKL.cmake

@@ -58,6 +58,7 @@ FIND_PATH(MKL_ROOT_DIR
     /opt/intel/mkl/*/
     /opt/intel/cmkl/
     /opt/intel/cmkl/*/
+    /opt/intel/*/mkl/
     /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal
         "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl"
         "C:/Program Files (x86)/Intel/Composer XE 2013/mkl"

examples/Data/pose3example-offdiagonal.txt

@@ -1,3 +1,3 @@
 VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
 VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
-EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000   10000.000000 2.000000 2.000000 2.000000 2.000000   10000.000000 3.000000 3.000000 3.000000   10000.000000 4.000000 4.000000   10000.000000 5.000000   10000.000000
+EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000   10000.000000 2.000000 2.000000 2.000000 2.000000   10000.000000 3.000000 3.000000 3.000000   10000.000000 4.000000 4.000000   10000.000000 5.000000   10000.0000

examples/LocalizationExample.cpp

@@ -120,15 +120,15 @@ int main(int argc, char** argv) {
   // 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));
   // Create odometry (Between) factors between consecutive poses
-  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
+  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
-  graph.push_back(BetweenFactor<Pose2>(2, 3, 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
-  graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
+  graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
-  graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
+  graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
-  graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
+  graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
   graph.print("\nFactor Graph:\n"); // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution

examples/OdometryExample.cpp

@@ -65,15 +65,15 @@ int main(int argc, char** argv) {
   // 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));
-  graph.push_back(PriorFactor<Pose2>(1, priorMean, priorNoise));
+  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));
   // Create odometry (Between) factors between consecutive poses
-  graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
+  graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
-  graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
+  graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
   graph.print("\nFactor Graph:\n"); // print
 
   // Create the data structure to hold the initialEstimate estimate to the solution

examples/PlanarSLAMExample.cpp

@@ -81,13 +81,13 @@ 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
-  graph.push_back(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
+  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
-  graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
+  graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
-  graph.push_back(BetweenFactor<Pose2>(x2, x3, 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
@@ -101,9 +101,9 @@ int main(int argc, char** argv) {
          range32 = 2.0;
 
   // Add Bearing-Range factors
-  graph.push_back(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
+  graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
-  graph.push_back(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
+  graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
-  graph.push_back(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
+  graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
 
   // Print
   graph.print("Factor Graph:\n");

examples/Pose2SLAMExample.cpp

@@ -72,23 +72,23 @@ 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));
-  graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
+  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));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses
-  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
+  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
-  graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
+  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
-  graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
+  graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
-  graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
+  graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
 
   // 2c. Add the loop closure constraint
   // This factor encodes the fact that we have returned to the same pose. In real systems,
   // 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:
-  graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
+  graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
   graph.print("\nFactor Graph:\n"); // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution

examples/Pose3SLAMExample_changeKeys.cpp

@@ -17,7 +17,6 @@
  * @author Luca Carlone
  */
 
-#include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>

examples/Pose3SLAMExample_g2o.cpp

@@ -17,7 +17,6 @@
  * @author Luca Carlone
  */
 
-#include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>

examples/Pose3SLAMExample_rotOnlyGN.cpp

@@ -1,95 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file Pose3SLAMExample_initializePose3.cpp
- * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
- * @date Aug 25, 2014
- * @author Luca Carlone
- */
-
-#include <gtsam/slam/InitializePose3.h>
-#include <gtsam/slam/dataset.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <fstream>
-
-using namespace std;
-using namespace gtsam;
-
-int main(const int argc, const char *argv[]) {
-
-  // Read graph from file
-  string g2oFile;
-  if (argc < 2)
-    g2oFile = findExampleDataFile("pose3example.txt");
-  else
-    g2oFile = argv[1];
-
-  NonlinearFactorGraph::shared_ptr graph;
-  Values::shared_ptr initial;
-  bool is3D = true;
-  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
-
-  Values initialRot;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
-    Key key = key_value.key;
-    Pose3 pose = initial->at<Pose3>(key);
-    Rot3 R = pose.rotation();
-    initialRot.insert(key,R);
-  }
-
-  noiseModel::Unit::shared_ptr identityModel = noiseModel::Unit::Create(3);
-  NonlinearFactorGraph graphRot;
-  BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *graph) {
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-    if(pose3Between){
-      Key key1 = pose3Between->key1();
-      Key key2 = pose3Between->key2();
-      Pose3 Pij = pose3Between->measured();
-      Rot3 Rij = Pij.rotation();
-      NonlinearFactor::shared_ptr factorRot(new BetweenFactor<Rot3>(key1, key2, Rij, identityModel));
-      graphRot.add(factorRot);
-    }else{
-      std::cout << "Found a factor that is not a Between<Pose3>: not admitted" << std::endl;
-      return 1;
-    }
-  }
-  // Add prior on the first key
-  graphRot.add(PriorFactor<Rot3>(0, Rot3(), identityModel));
-
-  std::cout << "Optimizing Rot3 via GN" << std::endl;
-  // GaussNewtonParams params;
-  GaussNewtonOptimizer optimizer(graphRot, initialRot);
-  Values GNrot = optimizer.optimize();
-  std::cout << "done!" << std::endl;
-
-  // Wrap estimate as poses to write in g2o format
-  Values GNposes;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNrot) {
-    Key key = key_value.key;
-    Rot3 R = GNrot.at<Rot3>(key);
-    GNposes.insert(key,Pose3(R,Point3()));
-  }
-
-  if (argc < 3) {
-    GNrot.print("initialization");
-  } else {
-    const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, GNposes, outputFile);
-    std::cout << "done! " << std::endl;
-  }
-  return 0;
-}

examples/SolverComparer.cpp

@@ -13,6 +13,22 @@
 * @brief   Incremental and batch solving, timing, and accuracy comparisons
 * @author  Richard Roberts
 * @date    August, 2013
+*
+* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
+*
+* Solve in incremental and write to file w_inc:
+* ./SolverComparer --incremental -d w10000 -o w_inc
+*
+* You can then perturb that initialization to get batch something to optimize.
+* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
+* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
+*
+* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
+* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
+*
+* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
+* ./SolverComparer --compare w_inc w_batch
+*
 */
 
 #include <gtsam/base/timing.h>

examples/VisualISAMExample.cpp

@@ -14,6 +14,7 @@
  * @brief   A visualSLAM example for the structure-from-motion problem on a simulated dataset
  * This version uses iSAM to solve the problem incrementally
  * @author  Duy-Nguyen Ta
+ * @author  Frank Dellaert
  */
 
 /**
@@ -61,7 +62,8 @@ int main(int argc, char* argv[]) {
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  noiseModel::Isotropic::shared_ptr noise = //
+      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -69,7 +71,8 @@ int main(int argc, char* argv[]) {
   // Create the set of ground-truth poses
   vector<Pose3> poses = createPoses();
 
-  // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates
+  // Create a NonlinearISAM object which will relinearize and reorder the variables
+  // every "relinearizeInterval" updates
   int relinearizeInterval = 3;
   NonlinearISAM isam(relinearizeInterval);
 
@@ -82,32 +85,44 @@ int main(int argc, char* argv[]) {
 
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
+      // Create ground truth measurement
       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));
+      // Add measurement
+      graph.add(
+          GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
+              Symbol('x', i), Symbol('l', j), K));
     }
 
-    // Add an initial guess for the current pose
     // Intentionally initialize the variables off from the ground truth
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+    Pose3 initial_xi = poses[i].compose(noise);
+
+    // Add an initial guess for the current pose
+    initialEstimate.insert(Symbol('x', i), initial_xi);
 
     // If this is the first iteration, add a prior on the first pose to set the coordinate frame
     // and a prior on the first landmark to set the scale
     // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
     // adding it to iSAM.
-    if( i == 0) {
+    if (i == 0) {
-      // Add a prior on pose x0
+      // 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))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
-      graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
+          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
+      graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
       // Add a prior on landmark l0
-      noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+      noiseModel::Isotropic::shared_ptr pointNoise =
-      graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+          noiseModel::Isotropic::Sigma(3, 0.1);
+      graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
 
       // Add initial guesses to all observed landmarks
-      // Intentionally initialize the variables off from the ground truth
+      Point3 noise(-0.25, 0.20, 0.15);
-      for (size_t j = 0; j < points.size(); ++j)
+      for (size_t j = 0; j < points.size(); ++j) {
-        initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+        // Intentionally initialize the variables off from the ground truth
+        Point3 initial_lj = points[j].compose(noise);
+        initialEstimate.insert(Symbol('l', j), initial_lj);
+      }
 
     } else {
       // Update iSAM with the new factors

gtsam/3rdparty/Eigen/CTestConfig.cmake

@@ -4,14 +4,10 @@
 ## # The following are required to uses Dart and the Cdash dashboard
 ##   ENABLE_TESTING()
 ##   INCLUDE(CTest)
-set(CTEST_PROJECT_NAME "Eigen")
+set(CTEST_PROJECT_NAME "Eigen3.2")
 set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
 
 set(CTEST_DROP_METHOD "http")
 set(CTEST_DROP_SITE "manao.inria.fr")
-set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen")
+set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.2")
 set(CTEST_DROP_SITE_CDASH TRUE)
-set(CTEST_PROJECT_SUBPROJECTS
-Official
-Unsupported
-)

gtsam/3rdparty/Eigen/Eigen/Core

@@ -95,7 +95,7 @@
     extern "C" {
       // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
       // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
-      #ifdef __INTEL_COMPILER
+      #if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110
         #include <immintrin.h>
       #else
         #include <emmintrin.h>
@@ -165,7 +165,7 @@
 #endif
 
 // required for __cpuid, needs to be included after cmath
-#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64))
+#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE))
   #include <intrin.h>
 #endif
 

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h

@@ -274,30 +274,13 @@ template<> struct ldlt_inplace<Lower>
       return true;
     }
 
-    RealScalar cutoff(0), biggest_in_corner;
-
     for (Index k = 0; k < size; ++k)
     {
       // Find largest diagonal element
       Index index_of_biggest_in_corner;
-      biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
+      mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
       index_of_biggest_in_corner += k;
 
-      if(k == 0)
-      {
-        // The biggest overall is the point of reference to which further diagonals
-        // are compared; if any diagonal is negligible compared
-        // to the largest overall, the algorithm bails.
-        cutoff = abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
-      }
-
-      // Finish early if the matrix is not full rank.
-      if(biggest_in_corner < cutoff)
-      {
-        for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
-        break;
-      }
-
       transpositions.coeffRef(k) = index_of_biggest_in_corner;
       if(k != index_of_biggest_in_corner)
       {
@@ -328,15 +311,20 @@ template<> struct ldlt_inplace<Lower>
 
       if(k>0)
       {
-        temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
+        temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint();
         mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
         if(rs>0)
           A21.noalias() -= A20 * temp.head(k);
       }
-      if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
+      
-        A21 /= mat.coeffRef(k,k);
+      // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot
-
+      // was smaller than the cutoff value. However, soince LDLT is not rank-revealing
+      // we should only make sure we do not introduce INF or NaN values.
+      // LAPACK also uses 0 as the cutoff value.
       RealScalar realAkk = numext::real(mat.coeffRef(k,k));
+      if((rs>0) && (abs(realAkk) > RealScalar(0)))
+        A21 /= realAkk;
+
       if (sign == PositiveSemiDef) {
         if (realAkk < 0) sign = Indefinite;
       } else if (sign == NegativeSemiDef) {
@@ -516,14 +504,20 @@ struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
     typedef typename LDLTType::MatrixType MatrixType;
     typedef typename LDLTType::Scalar Scalar;
     typedef typename LDLTType::RealScalar RealScalar;
-    const Diagonal<const MatrixType> vectorD = dec().vectorD();
+    const typename Diagonal<const MatrixType>::RealReturnType vectorD(dec().vectorD());
-    RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits<Scalar>::epsilon(),
+    // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
-				 RealScalar(1) / NumTraits<RealScalar>::highest()); // motivated by LAPACK's xGELSS
+    // as motivated by LAPACK's xGELSS:
+    // RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
+    // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
+    // diagonal element is not well justified and to numerical issues in some cases.
+    // Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
+    RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
+    
     for (Index i = 0; i < vectorD.size(); ++i) {
       if(abs(vectorD(i)) > tolerance)
-	dst.row(i) /= vectorD(i);
+        dst.row(i) /= vectorD(i);
       else
-	dst.row(i).setZero();
+        dst.row(i).setZero();
     }
 
     // dst = L^-T (D^-1 L^-1 P b)
@@ -576,7 +570,7 @@ MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
   // L^* P
   res = matrixU() * res;
   // D(L^*P)
-  res = vectorD().asDiagonal() * res;
+  res = vectorD().real().asDiagonal() * res;
   // L(DL^*P)
   res = matrixL() * res;
   // P^T (LDL^*P)

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

@@ -81,7 +81,7 @@ struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprTyp
                        && (InnerStrideAtCompileTime == 1)
                         ? PacketAccessBit : 0,
     MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0,
-    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (traits<XprType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,
     FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
     FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
     Flags0 = traits<XprType>::Flags & ( (HereditaryBits & ~RowMajorBit) |

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

@@ -47,6 +47,17 @@ struct CommaInitializer :
     m_xpr.block(0, 0, other.rows(), other.cols()) = other;
   }
 
+  /* Copy/Move constructor which transfers ownership. This is crucial in 
+   * absence of return value optimization to avoid assertions during destruction. */
+  // FIXME in C++11 mode this could be replaced by a proper RValue constructor
+  inline CommaInitializer(const CommaInitializer& o)
+  : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
+    // Mark original object as finished. In absence of R-value references we need to const_cast:
+    const_cast<CommaInitializer&>(o).m_row = m_xpr.rows();
+    const_cast<CommaInitializer&>(o).m_col = m_xpr.cols();
+    const_cast<CommaInitializer&>(o).m_currentBlockRows = 0;
+  }
+
   /* inserts a scalar value in the target matrix */
   CommaInitializer& operator,(const Scalar& s)
   {

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

@@ -0,0 +1,154 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_COMMAINITIALIZER_H
+#define EIGEN_COMMAINITIALIZER_H
+
+namespace Eigen { 
+
+/** \class CommaInitializer
+  * \ingroup Core_Module
+  *
+  * \brief Helper class used by the comma initializer operator
+  *
+  * This class is internally used to implement the comma initializer feature. It is
+  * the return type of MatrixBase::operator<<, and most of the time this is the only
+  * way it is used.
+  *
+  * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
+  */
+template<typename XprType>
+struct CommaInitializer
+{
+  typedef typename XprType::Scalar Scalar;
+  typedef typename XprType::Index Index;
+
+  inline CommaInitializer(XprType& xpr, const Scalar& s)
+    : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
+  {
+    m_xpr.coeffRef(0,0) = s;
+  }
+
+  template<typename OtherDerived>
+  inline CommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other)
+    : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
+  {
+    m_xpr.block(0, 0, other.rows(), other.cols()) = other;
+  }
+
+  /* Copy/Move constructor which transfers ownership. This is crucial in 
+   * absence of return value optimization to avoid assertions during destruction. */
+  // FIXME in C++11 mode this could be replaced by a proper RValue constructor
+  inline CommaInitializer(const CommaInitializer& o)
+  : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
+    // Mark original object as finished. In absence of R-value references we need to const_cast:
+    const_cast<CommaInitializer&>(o).m_row = m_xpr.rows();
+    const_cast<CommaInitializer&>(o).m_col = m_xpr.cols();
+    const_cast<CommaInitializer&>(o).m_currentBlockRows = 0;
+  }
+
+  /* inserts a scalar value in the target matrix */
+  CommaInitializer& operator,(const Scalar& s)
+  {
+    if (m_col==m_xpr.cols())
+    {
+      m_row+=m_currentBlockRows;
+      m_col = 0;
+      m_currentBlockRows = 1;
+      eigen_assert(m_row<m_xpr.rows()
+        && "Too many rows passed to comma initializer (operator<<)");
+    }
+    eigen_assert(m_col<m_xpr.cols()
+      && "Too many coefficients passed to comma initializer (operator<<)");
+    eigen_assert(m_currentBlockRows==1);
+    m_xpr.coeffRef(m_row, m_col++) = s;
+    return *this;
+  }
+
+  /* inserts a matrix expression in the target matrix */
+  template<typename OtherDerived>
+  CommaInitializer& operator,(const DenseBase<OtherDerived>& other)
+  {
+    if(other.cols()==0 || other.rows()==0)
+      return *this;
+    if (m_col==m_xpr.cols())
+    {
+      m_row+=m_currentBlockRows;
+      m_col = 0;
+      m_currentBlockRows = other.rows();
+      eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows()
+        && "Too many rows passed to comma initializer (operator<<)");
+    }
+    eigen_assert(m_col<m_xpr.cols()
+      && "Too many coefficients passed to comma initializer (operator<<)");
+    eigen_assert(m_currentBlockRows==other.rows());
+    if (OtherDerived::SizeAtCompileTime != Dynamic)
+      m_xpr.template block<OtherDerived::RowsAtCompileTime != Dynamic ? OtherDerived::RowsAtCompileTime : 1,
+                              OtherDerived::ColsAtCompileTime != Dynamic ? OtherDerived::ColsAtCompileTime : 1>
+                    (m_row, m_col) = other;
+    else
+      m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other;
+    m_col += other.cols();
+    return *this;
+  }
+
+  inline ~CommaInitializer()
+  {
+    eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows()
+         && m_col == m_xpr.cols()
+         && "Too few coefficients passed to comma initializer (operator<<)");
+  }
+
+  /** \returns the built matrix once all its coefficients have been set.
+    * Calling finished is 100% optional. Its purpose is to write expressions
+    * like this:
+    * \code
+    * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
+    * \endcode
+    */
+  inline XprType& finished() { 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
+};
+
+/** \anchor MatrixBaseCommaInitRef
+  * Convenient operator to set the coefficients of a matrix.
+  *
+  * The coefficients must be provided in a row major order and exactly match
+  * the size of the matrix. Otherwise an assertion is raised.
+  *
+  * Example: \include MatrixBase_set.cpp
+  * Output: \verbinclude MatrixBase_set.out
+  * 
+  * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order.
+  *
+  * \sa CommaInitializer::finished(), class CommaInitializer
+  */
+template<typename Derived>
+inline CommaInitializer<Derived> DenseBase<Derived>::operator<< (const Scalar& s)
+{
+  return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
+}
+
+/** \sa operator<<(const Scalar&) */
+template<typename Derived>
+template<typename OtherDerived>
+inline CommaInitializer<Derived>
+DenseBase<Derived>::operator<<(const DenseBase<OtherDerived>& other)
+{
+  return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_COMMAINITIALIZER_H

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

@@ -24,6 +24,14 @@ namespace internal {
 
 struct constructor_without_unaligned_array_assert {};
 
+template<typename T, int Size> void check_static_allocation_size()
+{
+  // if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit
+  #if EIGEN_STACK_ALLOCATION_LIMIT
+  EIGEN_STATIC_ASSERT(Size * sizeof(T) <= EIGEN_STACK_ALLOCATION_LIMIT, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+  #endif
+}
+
 /** \internal
   * Static array. If the MatrixOrArrayOptions require auto-alignment, the array will be automatically aligned:
   * to 16 bytes boundary if the total size is a multiple of 16 bytes.
@@ -38,12 +46,12 @@ struct plain_array
 
   plain_array() 
   { 
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 
   plain_array(constructor_without_unaligned_array_assert) 
   { 
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 };
 
@@ -76,12 +84,12 @@ struct plain_array<T, Size, MatrixOrArrayOptions, 16>
   plain_array() 
   { 
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(0xf);
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 
   plain_array(constructor_without_unaligned_array_assert) 
   { 
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 };
 

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

@@ -589,7 +589,7 @@ struct linspaced_op_impl<Scalar,true>
 
   template<typename Index>
   EIGEN_STRONG_INLINE const Packet packetOp(Index i) const
-  { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1<Packet>(i),m_interPacket))); }
+  { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1<Packet>(Scalar(i)),m_interPacket))); }
 
   const Scalar m_low;
   const Scalar m_step;
@@ -609,7 +609,7 @@ template <typename Scalar, bool RandomAccess> struct functor_traits< linspaced_o
 template <typename Scalar, bool RandomAccess> struct linspaced_op
 {
   typedef typename packet_traits<Scalar>::type Packet;
-  linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/(num_steps-1))) {}
+  linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/Scalar(num_steps-1))) {}
 
   template<typename Index>
   EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return impl(i); }

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

@@ -237,6 +237,8 @@ template<typename Derived> class MapBase<Derived, WriteAccessors>
     using Base::Base::operator=;
 };
 
+#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS
+
 } // end namespace Eigen
 
 #endif // EIGEN_MAPBASE_H

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

@@ -101,7 +101,7 @@ struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
   template<typename Derived> struct match {
     enum {
       HasDirectAccess = internal::has_direct_access<Derived>::ret,
-      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
+      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
       InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic)
                       || int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime)
                       || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1),
@@ -172,8 +172,12 @@ protected:
     }
     else
       ::new (static_cast<Base*>(this)) Base(expr.data(), expr.rows(), expr.cols());
-    ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(),
+    
-                                 StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride());    
+    if(Expression::IsVectorAtCompileTime && (!PlainObjectType::IsVectorAtCompileTime) && ((Expression::Flags&RowMajorBit)!=(PlainObjectType::Flags&RowMajorBit)))
+      ::new (&m_stride) StrideBase(expr.innerStride(), StrideType::InnerStrideAtCompileTime==0?0:1);
+    else
+      ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(),
+                                   StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride());    
   }
 
   StrideBase m_stride;

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

@@ -278,21 +278,21 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
 
     /** Efficient triangular matrix times vector/matrix product */
     template<typename OtherDerived>
-    TriangularProduct<Mode,true,MatrixType,false,OtherDerived, OtherDerived::IsVectorAtCompileTime>
+    TriangularProduct<Mode, true, MatrixType, false, OtherDerived, OtherDerived::ColsAtCompileTime==1>
     operator*(const MatrixBase<OtherDerived>& rhs) const
     {
       return TriangularProduct
-              <Mode,true,MatrixType,false,OtherDerived,OtherDerived::IsVectorAtCompileTime>
+              <Mode, true, MatrixType, false, OtherDerived, OtherDerived::ColsAtCompileTime==1>
               (m_matrix, rhs.derived());
     }
 
     /** Efficient vector/matrix times triangular matrix product */
     template<typename OtherDerived> friend
-    TriangularProduct<Mode,false,OtherDerived,OtherDerived::IsVectorAtCompileTime,MatrixType,false>
+    TriangularProduct<Mode, false, OtherDerived, OtherDerived::RowsAtCompileTime==1, MatrixType, false>
     operator*(const MatrixBase<OtherDerived>& lhs, const TriangularView& rhs)
     {
       return TriangularProduct
-              <Mode,false,OtherDerived,OtherDerived::IsVectorAtCompileTime,MatrixType,false>
+              <Mode, false, OtherDerived, OtherDerived::RowsAtCompileTime==1, MatrixType, false>
               (lhs.derived(),rhs.m_matrix);
     }
 

gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h

@@ -54,8 +54,25 @@
 #endif
 
 #if defined EIGEN_USE_MKL
+#   include <mkl.h> 
+/*Check IMKL version for compatibility: < 10.3 is not usable with Eigen*/
+#   ifndef INTEL_MKL_VERSION
+#       undef EIGEN_USE_MKL /* INTEL_MKL_VERSION is not even defined on older versions */
+#   elif INTEL_MKL_VERSION < 100305    /* the intel-mkl-103-release-notes say this was when the lapacke.h interface was added*/
+#       undef EIGEN_USE_MKL
+#   endif
+#   ifndef EIGEN_USE_MKL
+    /*If the MKL version is too old, undef everything*/
+#       undef   EIGEN_USE_MKL_ALL
+#       undef   EIGEN_USE_BLAS
+#       undef   EIGEN_USE_LAPACKE
+#       undef   EIGEN_USE_MKL_VML
+#       undef   EIGEN_USE_LAPACKE_STRICT
+#       undef   EIGEN_USE_LAPACKE
+#   endif
+#endif
 
-#include <mkl.h>
+#if defined EIGEN_USE_MKL
 #include <mkl_lapacke.h>
 #define EIGEN_MKL_VML_THRESHOLD 128
 

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h

@@ -13,7 +13,7 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 2
-#define EIGEN_MINOR_VERSION 1
+#define EIGEN_MINOR_VERSION 2
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@@ -289,7 +289,8 @@ namespace Eigen {
 #endif
 
 #ifndef EIGEN_STACK_ALLOCATION_LIMIT
-#define EIGEN_STACK_ALLOCATION_LIMIT 20000
+// 131072 == 128 KB
+#define EIGEN_STACK_ALLOCATION_LIMIT 131072
 #endif
 
 #ifndef EIGEN_DEFAULT_IO_FORMAT

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h

@@ -272,12 +272,12 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size)
   // The defined(_mm_free) is just here to verify that this MSVC version
   // implements _mm_malloc/_mm_free based on the corresponding _aligned_
   // functions. This may not always be the case and we just try to be safe.
-  #if defined(_MSC_VER) && defined(_mm_free)
+  #if defined(_MSC_VER) && (!defined(_WIN32_WCE)) && defined(_mm_free)
     result = _aligned_realloc(ptr,new_size,16);
   #else
     result = generic_aligned_realloc(ptr,new_size,old_size);
   #endif
-#elif defined(_MSC_VER)
+#elif defined(_MSC_VER) && (!defined(_WIN32_WCE))
   result = _aligned_realloc(ptr,new_size,16);
 #else
   result = handmade_aligned_realloc(ptr,new_size,old_size);
@@ -630,6 +630,8 @@ template<typename T> class aligned_stack_memory_handler
       } \
       void operator delete(void * ptr) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
       void operator delete[](void * ptr) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
+      void operator delete(void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
+      void operator delete[](void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
       /* in-place new and delete. since (at least afaik) there is no actual   */ \
       /* memory allocated we can safely let the default implementation handle */ \
       /* this particular case. */ \
@@ -777,9 +779,9 @@ namespace internal {
 
 #ifdef EIGEN_CPUID
 
-inline bool cpuid_is_vendor(int abcd[4], const char* vendor)
+inline bool cpuid_is_vendor(int abcd[4], const int vendor[3])
 {
-  return abcd[1]==(reinterpret_cast<const int*>(vendor))[0] && abcd[3]==(reinterpret_cast<const int*>(vendor))[1] && abcd[2]==(reinterpret_cast<const int*>(vendor))[2];
+  return abcd[1]==vendor[0] && abcd[3]==vendor[1] && abcd[2]==vendor[2];
 }
 
 inline void queryCacheSizes_intel_direct(int& l1, int& l2, int& l3)
@@ -921,13 +923,16 @@ inline void queryCacheSizes(int& l1, int& l2, int& l3)
 {
   #ifdef EIGEN_CPUID
   int abcd[4];
+  const int GenuineIntel[] = {0x756e6547, 0x49656e69, 0x6c65746e};
+  const int AuthenticAMD[] = {0x68747541, 0x69746e65, 0x444d4163};
+  const int AMDisbetter_[] = {0x69444d41, 0x74656273, 0x21726574}; // "AMDisbetter!"
 
   // identify the CPU vendor
   EIGEN_CPUID(abcd,0x0,0);
   int max_std_funcs = abcd[1];
-  if(cpuid_is_vendor(abcd,"GenuineIntel"))
+  if(cpuid_is_vendor(abcd,GenuineIntel))
     queryCacheSizes_intel(l1,l2,l3,max_std_funcs);
-  else if(cpuid_is_vendor(abcd,"AuthenticAMD") || cpuid_is_vendor(abcd,"AMDisbetter!"))
+  else if(cpuid_is_vendor(abcd,AuthenticAMD) || cpuid_is_vendor(abcd,AMDisbetter_))
     queryCacheSizes_amd(l1,l2,l3);
   else
     // by default let's use Intel's API

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h

@@ -203,6 +203,8 @@ public:
   * \li \c Quaternionf for \c float
   * \li \c Quaterniond for \c double
   *
+  * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized.
+  *
   * \sa  class AngleAxis, class Transform
   */
 
@@ -344,7 +346,7 @@ class Map<const Quaternion<_Scalar>, _Options >
 
     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
       *
-      * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order:
+      * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
       * \code *coeffs == {x, y, z, w} \endcode
       *
       * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
@@ -464,7 +466,7 @@ QuaternionBase<Derived>::_transformVector(Vector3 v) const
     // Note that this algorithm comes from the optimization by hand
     // of the conversion to a Matrix followed by a Matrix/Vector product.
     // It appears to be much faster than the common algorithm found
-    // in the litterature (30 versus 39 flops). It also requires two
+    // in the literature (30 versus 39 flops). It also requires two
     // Vector3 as temporaries.
     Vector3 uv = this->vec().cross(v);
     uv += uv;
@@ -584,7 +586,7 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri
   //    which yields a singular value problem
   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
   {
-    c = max<Scalar>(c,-1);
+    c = (max)(c,Scalar(-1));
     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
     Vector3 axis = svd.matrixV().col(2);
@@ -667,10 +669,10 @@ QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& oth
 {
   using std::acos;
   using std::abs;
-  double d = abs(this->dot(other));
+  Scalar d = abs(this->dot(other));
-  if (d>=1.0)
+  if (d>=Scalar(1))
     return Scalar(0);
-  return static_cast<Scalar>(2 * acos(d));
+  return Scalar(2) * acos(d);
 }
 
  

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h

@@ -194,9 +194,9 @@ public:
   /** type of the matrix used to represent the linear part of the transformation */
   typedef Matrix<Scalar,Dim,Dim,Options> LinearMatrixType;
   /** type of read/write reference to the linear part of the transformation */
-  typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact)> LinearPart;
+  typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> LinearPart;
   /** type of read reference to the linear part of the transformation */
-  typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact)> ConstLinearPart;
+  typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> ConstLinearPart;
   /** type of read/write reference to the affine part of the transformation */
   typedef typename internal::conditional<int(Mode)==int(AffineCompact),
                               MatrixType&,

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h

@@ -113,7 +113,7 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
   const Index n = src.cols(); // number of measurements
 
   // required for demeaning ...
-  const RealScalar one_over_n = 1 / static_cast<RealScalar>(n);
+  const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n);
 
   // computation of mean
   const VectorType src_mean = src.rowwise().sum() * one_over_n;
@@ -136,16 +136,16 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
 
   // Eq. (39)
   VectorType S = VectorType::Ones(m);
-  if (sigma.determinant()<0) S(m-1) = -1;
+  if (sigma.determinant()<Scalar(0)) S(m-1) = Scalar(-1);
 
   // Eq. (40) and (43)
   const VectorType& d = svd.singularValues();
   Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank;
   if (rank == m-1) {
-    if ( svd.matrixU().determinant() * svd.matrixV().determinant() > 0 ) {
+    if ( svd.matrixU().determinant() * svd.matrixV().determinant() > Scalar(0) ) {
       Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose();
     } else {
-      const Scalar s = S(m-1); S(m-1) = -1;
+      const Scalar s = S(m-1); S(m-1) = Scalar(-1);
       Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
       S(m-1) = s;
     }
@@ -156,7 +156,7 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
   if (with_scaling)
   {
     // Eq. (42)
-    const Scalar c = 1/src_var * svd.singularValues().dot(S);
+    const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);
 
     // Eq. (41)
     Rt.col(m).head(m) = dst_mean;

gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h

@@ -48,7 +48,7 @@ void apply_block_householder_on_the_left(MatrixType& mat, const VectorsType& vec
   typedef typename MatrixType::Index Index;
   enum { TFactorSize = MatrixType::ColsAtCompileTime };
   Index nbVecs = vectors.cols();
-  Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize> T(nbVecs,nbVecs);
+  Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize, ColMajor> T(nbVecs,nbVecs);
   make_block_householder_triangular_factor(T, vectors, hCoeffs);
 
   const TriangularView<const VectorsType, UnitLower>& V(vectors);

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h

@@ -61,6 +61,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
   VectorType s(n), t(n);
 
   RealScalar tol2 = tol*tol;
+  RealScalar eps2 = NumTraits<Scalar>::epsilon()*NumTraits<Scalar>::epsilon();
   int i = 0;
   int restarts = 0;
 
@@ -69,7 +70,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
     Scalar rho_old = rho;
 
     rho = r0.dot(r);
-    if (internal::isMuchSmallerThan(rho,r0_sqnorm))
+    if (abs(rho) < eps2*r0_sqnorm)
     {
       // The new residual vector became too orthogonal to the arbitrarily choosen direction r0
       // Let's restart with a new r0:

gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h

@@ -20,10 +20,11 @@ namespace Eigen {
   *
   * \param MatrixType the type of the matrix of which we are computing the LU decomposition
   *
-  * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A
+  * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A is
-  * is decomposed as A = PLUQ where L is unit-lower-triangular, U is upper-triangular, and P and Q
+  * decomposed as \f$ A = P^{-1} L U Q^{-1} \f$ where L is unit-lower-triangular, U is
-  * are permutation matrices. This is a rank-revealing LU decomposition. The eigenvalues (diagonal
+  * upper-triangular, and P and Q are permutation matrices. This is a rank-revealing LU
-  * coefficients) of U are sorted in such a way that any zeros are at the end.
+  * decomposition. The eigenvalues (diagonal coefficients) of U are sorted in such a way that any
+  * zeros are at the end.
   *
   * This decomposition provides the generic approach to solving systems of linear equations, computing
   * the rank, invertibility, inverse, kernel, and determinant.
@@ -511,8 +512,8 @@ typename internal::traits<MatrixType>::Scalar FullPivLU<MatrixType>::determinant
 }
 
 /** \returns the matrix represented by the decomposition,
- * i.e., it returns the product: P^{-1} L U Q^{-1}.
+ * i.e., it returns the product: \f$ P^{-1} L U Q^{-1} \f$.
- * This function is provided for debug purpose. */
+ * This function is provided for debug purposes. */
 template<typename MatrixType>
 MatrixType FullPivLU<MatrixType>::reconstructedMatrix() const
 {

gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h

@@ -109,7 +109,7 @@ class NaturalOrdering
   * \class COLAMDOrdering
   *
   * Functor computing the \em column \em approximate \em minimum \em degree ordering 
-  * The matrix should be in column-major format
+  * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()).
   */
 template<typename Index>
 class COLAMDOrdering
@@ -118,10 +118,14 @@ class COLAMDOrdering
     typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType; 
     typedef Matrix<Index, Dynamic, 1> IndexVector;
     
-    /** Compute the permutation vector form a sparse matrix */
+    /** Compute the permutation vector \a perm form the sparse matrix \a mat
+      * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+      */
     template <typename MatrixType>
     void operator() (const MatrixType& mat, PermutationType& perm)
     {
+      eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering");
+      
       Index m = mat.rows();
       Index n = mat.cols();
       Index nnz = mat.nonZeros();
@@ -132,12 +136,12 @@ class COLAMDOrdering
       Index stats [COLAMD_STATS];
       internal::colamd_set_defaults(knobs);
       
-      Index info;
       IndexVector p(n+1), A(Alen); 
       for(Index i=0; i <= n; i++)   p(i) = mat.outerIndexPtr()[i];
       for(Index i=0; i < nnz; i++)  A(i) = mat.innerIndexPtr()[i];
       // Call Colamd routine to compute the ordering 
-      info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats); 
+      Index info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats); 
+      EIGEN_UNUSED_VARIABLE(info);
       eigen_assert( info && "COLAMD failed " );
       
       perm.resize(n);

gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h

@@ -76,7 +76,8 @@ template<typename _MatrixType> class ColPivHouseholderQR
         m_colsTranspositions(),
         m_temp(),
         m_colSqNorms(),
-        m_isInitialized(false) {}
+        m_isInitialized(false),
+        m_usePrescribedThreshold(false) {}
 
     /** \brief Default Constructor with memory preallocation
       *

gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h

@@ -375,17 +375,19 @@ struct svd_precondition_2x2_block_to_be_real<MatrixType, QRPreconditioner, true>
     Scalar z;
     JacobiRotation<Scalar> rot;
     RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p,p)) + numext::abs2(work_matrix.coeff(q,p)));
+    
     if(n==0)
     {
       z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q);
       work_matrix.row(p) *= z;
       if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z);
       if(work_matrix.coeff(q,q)!=Scalar(0))
+      {
         z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
-      else
+        work_matrix.row(q) *= z;
-        z = Scalar(0);
+        if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
-      work_matrix.row(q) *= z;
+      }
-      if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
+      // otherwise the second row is already zero, so we have nothing to do.
     }
     else
     {
@@ -415,6 +417,7 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q,
                             JacobiRotation<RealScalar> *j_right)
 {
   using std::sqrt;
+  using std::abs;
   Matrix<RealScalar,2,2> m;
   m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)),
        numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q));
@@ -428,9 +431,11 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q,
   }
   else
   {
-    RealScalar u = d / t;
+    RealScalar t2d2 = numext::hypot(t,d);
-    rot1.c() = RealScalar(1) / sqrt(RealScalar(1) + numext::abs2(u));
+    rot1.c() = abs(t)/t2d2;
-    rot1.s() = rot1.c() * u;
+    rot1.s() = d/t2d2;
+    if(t<RealScalar(0))
+      rot1.s() = -rot1.s();
   }
   m.applyOnTheLeft(0,1,rot1);
   j_right->makeJacobi(m,0,1);
@@ -531,8 +536,9 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD()
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
-        m_rows(-1), m_cols(-1)
+        m_rows(-1), m_cols(-1), m_diagSize(0)
     {}
 
 
@@ -545,6 +551,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD(Index rows, Index cols, unsigned int computationOptions = 0)
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
         m_rows(-1), m_cols(-1)
     {
@@ -564,6 +571,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD(const MatrixType& matrix, unsigned int computationOptions = 0)
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
         m_rows(-1), m_cols(-1)
     {
@@ -665,6 +673,69 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
       eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
       return m_nonzeroSingularValues;
     }
+    
+    /** \returns the rank of the matrix of which \c *this is the SVD.
+      *
+      * \note This method has to determine which singular values should be considered nonzero.
+      *       For that, it uses the threshold value that you can control by calling
+      *       setThreshold(const RealScalar&).
+      */
+    inline Index rank() const
+    {
+      using std::abs;
+      eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
+      if(m_singularValues.size()==0) return 0;
+      RealScalar premultiplied_threshold = m_singularValues.coeff(0) * threshold();
+      Index i = m_nonzeroSingularValues-1;
+      while(i>=0 && m_singularValues.coeff(i) < premultiplied_threshold) --i;
+      return i+1;
+    }
+    
+    /** Allows to prescribe a threshold to be used by certain methods, such as rank() and solve(),
+      * which need to determine when singular values are to be considered nonzero.
+      * This is not used for the SVD decomposition itself.
+      *
+      * When it needs to get the threshold value, Eigen calls threshold().
+      * The default is \c NumTraits<Scalar>::epsilon()
+      *
+      * \param threshold The new value to use as the threshold.
+      *
+      * A singular value will be considered nonzero if its value is strictly greater than
+      *  \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$.
+      *
+      * If you want to come back to the default behavior, call setThreshold(Default_t)
+      */
+    JacobiSVD& setThreshold(const RealScalar& threshold)
+    {
+      m_usePrescribedThreshold = true;
+      m_prescribedThreshold = threshold;
+      return *this;
+    }
+
+    /** Allows to come back to the default behavior, letting Eigen use its default formula for
+      * determining the threshold.
+      *
+      * You should pass the special object Eigen::Default as parameter here.
+      * \code svd.setThreshold(Eigen::Default); \endcode
+      *
+      * See the documentation of setThreshold(const RealScalar&).
+      */
+    JacobiSVD& setThreshold(Default_t)
+    {
+      m_usePrescribedThreshold = false;
+      return *this;
+    }
+
+    /** Returns the threshold that will be used by certain methods such as rank().
+      *
+      * See the documentation of setThreshold(const RealScalar&).
+      */
+    RealScalar threshold() const
+    {
+      eigen_assert(m_isInitialized || m_usePrescribedThreshold);
+      return m_usePrescribedThreshold ? m_prescribedThreshold
+                                      : (std::max<Index>)(1,m_diagSize)*NumTraits<Scalar>::epsilon();
+    }
 
     inline Index rows() const { return m_rows; }
     inline Index cols() const { return m_cols; }
@@ -677,11 +748,12 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     MatrixVType m_matrixV;
     SingularValuesType m_singularValues;
     WorkMatrixType m_workMatrix;
-    bool m_isInitialized, m_isAllocated;
+    bool m_isInitialized, m_isAllocated, m_usePrescribedThreshold;
     bool m_computeFullU, m_computeThinU;
     bool m_computeFullV, m_computeThinV;
     unsigned int m_computationOptions;
     Index m_nonzeroSingularValues, m_rows, m_cols, m_diagSize;
+    RealScalar m_prescribedThreshold;
 
     template<typename __MatrixType, int _QRPreconditioner, bool _IsComplex>
     friend struct internal::svd_precondition_2x2_block_to_be_real;
@@ -764,6 +836,11 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
     if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols);
     if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize);
   }
+  
+  // Scaling factor to reduce over/under-flows
+  RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff();
+  if(scale==RealScalar(0)) scale = RealScalar(1);
+  m_workMatrix /= scale;
 
   /*** step 2. The main Jacobi SVD iteration. ***/
 
@@ -833,6 +910,8 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
       if(computeV()) m_matrixV.col(pos).swap(m_matrixV.col(i));
     }
   }
+  
+  m_singularValues *= scale;
 
   m_isInitialized = true;
   return *this;
@@ -854,11 +933,11 @@ struct solve_retval<JacobiSVD<_MatrixType, QRPreconditioner>, Rhs>
     // So A^{-1} = V S^{-1} U^*
 
     Matrix<Scalar, Dynamic, Rhs::ColsAtCompileTime, 0, _MatrixType::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime> tmp;
-    Index nonzeroSingVals = dec().nonzeroSingularValues();
+    Index rank = dec().rank();
     
-    tmp.noalias() = dec().matrixU().leftCols(nonzeroSingVals).adjoint() * rhs();
+    tmp.noalias() = dec().matrixU().leftCols(rank).adjoint() * rhs();
-    tmp = dec().singularValues().head(nonzeroSingVals).asDiagonal().inverse() * tmp;
+    tmp = dec().singularValues().head(rank).asDiagonal().inverse() * tmp;
-    dst = dec().matrixV().leftCols(nonzeroSingVals) * tmp;
+    dst = dec().matrixV().leftCols(rank) * tmp;
   }
 };
 } // end namespace internal

gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h

@@ -37,6 +37,7 @@ class SimplicialCholeskyBase : internal::noncopyable
 {
   public:
     typedef typename internal::traits<Derived>::MatrixType MatrixType;
+    typedef typename internal::traits<Derived>::OrderingType OrderingType;
     enum { UpLo = internal::traits<Derived>::UpLo };
     typedef typename MatrixType::Scalar Scalar;
     typedef typename MatrixType::RealScalar RealScalar;
@@ -240,15 +241,16 @@ class SimplicialCholeskyBase : internal::noncopyable
     RealScalar m_shiftScale;
 };
 
-template<typename _MatrixType, int _UpLo = Lower> class SimplicialLLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialLLT;
-template<typename _MatrixType, int _UpLo = Lower> class SimplicialLDLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialLDLT;
-template<typename _MatrixType, int _UpLo = Lower> class SimplicialCholesky;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialCholesky;
 
 namespace internal {
 
-template<typename _MatrixType, int _UpLo> struct traits<SimplicialLLT<_MatrixType,_UpLo> >
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
 {
   typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
   enum { UpLo = _UpLo };
   typedef typename MatrixType::Scalar                         Scalar;
   typedef typename MatrixType::Index                          Index;
@@ -259,9 +261,10 @@ template<typename _MatrixType, int _UpLo> struct traits<SimplicialLLT<_MatrixTyp
   static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
 };
 
-template<typename _MatrixType,int _UpLo> struct traits<SimplicialLDLT<_MatrixType,_UpLo> >
+template<typename _MatrixType,int _UpLo, typename _Ordering> struct traits<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
 {
   typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
   enum { UpLo = _UpLo };
   typedef typename MatrixType::Scalar                             Scalar;
   typedef typename MatrixType::Index                              Index;
@@ -272,9 +275,10 @@ template<typename _MatrixType,int _UpLo> struct traits<SimplicialLDLT<_MatrixTyp
   static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
 };
 
-template<typename _MatrixType, int _UpLo> struct traits<SimplicialCholesky<_MatrixType,_UpLo> >
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
 {
   typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
   enum { UpLo = _UpLo };
 };
 
@@ -294,11 +298,12 @@ template<typename _MatrixType, int _UpLo> struct traits<SimplicialCholesky<_Matr
   * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
   * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
   *               or Upper. Default is Lower.
+  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
   *
-  * \sa class SimplicialLDLT
+  * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering
   */
-template<typename _MatrixType, int _UpLo>
+template<typename _MatrixType, int _UpLo, typename _Ordering>
-    class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo> >
+    class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
 {
 public:
     typedef _MatrixType MatrixType;
@@ -382,11 +387,12 @@ public:
   * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
   * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
   *               or Upper. Default is Lower.
+  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
   *
-  * \sa class SimplicialLLT
+  * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering
   */
-template<typename _MatrixType, int _UpLo>
+template<typename _MatrixType, int _UpLo, typename _Ordering>
-    class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo> >
+    class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
 {
 public:
     typedef _MatrixType MatrixType;
@@ -467,8 +473,8 @@ public:
   *
   * \sa class SimplicialLDLT, class SimplicialLLT
   */
-template<typename _MatrixType, int _UpLo>
+template<typename _MatrixType, int _UpLo, typename _Ordering>
-    class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo> >
+    class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
 {
 public:
     typedef _MatrixType MatrixType;
@@ -612,15 +618,13 @@ void SimplicialCholeskyBase<Derived>::ordering(const MatrixType& a, CholMatrixTy
 {
   eigen_assert(a.rows()==a.cols());
   const Index size = a.rows();
-  // TODO allows to configure the permutation
   // Note that amd compute the inverse permutation
   {
     CholMatrixType C;
     C = a.template selfadjointView<UpLo>();
-    // remove diagonal entries:
+    
-    // seems not to be needed
+    OrderingType ordering;
-    // C.prune(keep_diag());
+    ordering(C,m_Pinv);
-    internal::minimum_degree_ordering(C, m_Pinv);
   }
 
   if(m_Pinv.size()>0)

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h

@@ -51,8 +51,8 @@ class CompressedStorage
     CompressedStorage& operator=(const CompressedStorage& other)
     {
       resize(other.size());
-      memcpy(m_values, other.m_values, m_size * sizeof(Scalar));
+      internal::smart_copy(other.m_values,  other.m_values  + m_size, m_values);
-      memcpy(m_indices, other.m_indices, m_size * sizeof(Index));
+      internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices);
       return *this;
     }
 
@@ -83,10 +83,10 @@ class CompressedStorage
         reallocate(m_size);
     }
 
-    void resize(size_t size, float reserveSizeFactor = 0)
+    void resize(size_t size, double reserveSizeFactor = 0)
     {
       if (m_allocatedSize<size)
-        reallocate(size + size_t(reserveSizeFactor*size));
+        reallocate(size + size_t(reserveSizeFactor*double(size)));
       m_size = size;
     }
 

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h

@@ -73,7 +73,8 @@ class CwiseBinaryOpImpl<BinaryOp,Lhs,Rhs,Sparse>::InnerIterator
     typedef internal::sparse_cwise_binary_op_inner_iterator_selector<
       BinaryOp,Lhs,Rhs, InnerIterator> Base;
 
-    EIGEN_STRONG_INLINE InnerIterator(const CwiseBinaryOpImpl& binOp, Index outer)
+    // NOTE: we have to prefix Index by "typename Lhs::" to avoid an ICE with VC11
+    EIGEN_STRONG_INLINE InnerIterator(const CwiseBinaryOpImpl& binOp, typename Lhs::Index outer)
       : Base(binOp.derived(),outer)
     {}
 };

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h

@@ -19,7 +19,10 @@ template<typename Lhs, typename Rhs, int InnerSize> struct SparseDenseProductRet
 
 template<typename Lhs, typename Rhs> struct SparseDenseProductReturnType<Lhs,Rhs,1>
 {
-  typedef SparseDenseOuterProduct<Lhs,Rhs,false> Type;
+  typedef typename internal::conditional<
+    Lhs::IsRowMajor,
+    SparseDenseOuterProduct<Rhs,Lhs,true>,
+    SparseDenseOuterProduct<Lhs,Rhs,false> >::type Type;
 };
 
 template<typename Lhs, typename Rhs, int InnerSize> struct DenseSparseProductReturnType
@@ -29,7 +32,10 @@ template<typename Lhs, typename Rhs, int InnerSize> struct DenseSparseProductRet
 
 template<typename Lhs, typename Rhs> struct DenseSparseProductReturnType<Lhs,Rhs,1>
 {
-  typedef SparseDenseOuterProduct<Rhs,Lhs,true> Type;
+  typedef typename internal::conditional<
+    Rhs::IsRowMajor,
+    SparseDenseOuterProduct<Rhs,Lhs,true>,
+    SparseDenseOuterProduct<Lhs,Rhs,false> >::type Type;
 };
 
 namespace internal {
@@ -114,17 +120,30 @@ class SparseDenseOuterProduct<Lhs,Rhs,Transpose>::InnerIterator : public _LhsNes
     typedef typename SparseDenseOuterProduct::Index Index;
   public:
     EIGEN_STRONG_INLINE InnerIterator(const SparseDenseOuterProduct& prod, Index outer)
-      : Base(prod.lhs(), 0), m_outer(outer), m_factor(prod.rhs().coeff(outer))
+      : Base(prod.lhs(), 0), m_outer(outer), m_factor(get(prod.rhs(), outer, typename internal::traits<Rhs>::StorageKind() ))
-    {
+    { }
-    }
 
     inline Index outer() const { return m_outer; }
-    inline Index row() const { return Transpose ? Base::row() : m_outer; }
+    inline Index row() const { return Transpose ? m_outer : Base::index(); }
-    inline Index col() const { return Transpose ? m_outer : Base::row(); }
+    inline Index col() const { return Transpose ? Base::index() : m_outer; }
 
     inline Scalar value() const { return Base::value() * m_factor; }
 
   protected:
+    static Scalar get(const _RhsNested &rhs, Index outer, Dense = Dense())
+    {
+      return rhs.coeff(outer);
+    }
+    
+    static Scalar get(const _RhsNested &rhs, Index outer, Sparse = Sparse())
+    {
+      typename Traits::_RhsNested::InnerIterator it(rhs, outer);
+      if (it && it.index()==0)
+        return it.value();
+      
+      return Scalar(0);
+    }
+    
     Index m_outer;
     Scalar m_factor;
 };

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h

@@ -940,7 +940,7 @@ void set_from_triplets(const InputIterator& begin, const InputIterator& end, Spa
   enum { IsRowMajor = SparseMatrixType::IsRowMajor };
   typedef typename SparseMatrixType::Scalar Scalar;
   typedef typename SparseMatrixType::Index Index;
-  SparseMatrix<Scalar,IsRowMajor?ColMajor:RowMajor> trMat(mat.rows(),mat.cols());
+  SparseMatrix<Scalar,IsRowMajor?ColMajor:RowMajor,Index> trMat(mat.rows(),mat.cols());
 
   if(begin!=end)
   {
@@ -1178,7 +1178,7 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_Index>::Scalar& Sparse
   size_t p = m_outerIndex[outer+1];
   ++m_outerIndex[outer+1];
 
-  float reallocRatio = 1;
+  double reallocRatio = 1;
   if (m_data.allocatedSize()<=m_data.size())
   {
     // if there is no preallocated memory, let's reserve a minimum of 32 elements
@@ -1190,13 +1190,13 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_Index>::Scalar& Sparse
     {
       // we need to reallocate the data, to reduce multiple reallocations
       // we use a smart resize algorithm based on the current filling ratio
-      // in addition, we use float to avoid integers overflows
+      // in addition, we use double to avoid integers overflows
-      float nnzEstimate = float(m_outerIndex[outer])*float(m_outerSize)/float(outer+1);
+      double nnzEstimate = double(m_outerIndex[outer])*double(m_outerSize)/double(outer+1);
-      reallocRatio = (nnzEstimate-float(m_data.size()))/float(m_data.size());
+      reallocRatio = (nnzEstimate-double(m_data.size()))/double(m_data.size());
       // furthermore we bound the realloc ratio to:
       //   1) reduce multiple minor realloc when the matrix is almost filled
       //   2) avoid to allocate too much memory when the matrix is almost empty
-      reallocRatio = (std::min)((std::max)(reallocRatio,1.5f),8.f);
+      reallocRatio = (std::min)((std::max)(reallocRatio,1.5),8.);
     }
   }
   m_data.resize(m_data.size()+1,reallocRatio);

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h

@@ -26,7 +26,7 @@ template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>
     inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); }
 };
 
-// NOTE: VC10 trigger an ICE if don't put typename TransposeImpl<MatrixType,Sparse>:: in front of Index,
+// NOTE: VC10 and VC11 trigger an ICE if don't put typename TransposeImpl<MatrixType,Sparse>:: in front of Index,
 // a typedef typename TransposeImpl<MatrixType,Sparse>::Index Index;
 // does not fix the issue.
 // An alternative is to define the nested class in the parent class itself.
@@ -40,8 +40,8 @@ template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>::InnerItera
     EIGEN_STRONG_INLINE InnerIterator(const TransposeImpl& trans, typename TransposeImpl<MatrixType,Sparse>::Index outer)
       : Base(trans.derived().nestedExpression(), outer)
     {}
-    Index row() const { return Base::col(); }
+    typename TransposeImpl<MatrixType,Sparse>::Index row() const { return Base::col(); }
-    Index col() const { return Base::row(); }
+    typename TransposeImpl<MatrixType,Sparse>::Index col() const { return Base::row(); }
 };
 
 template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>::ReverseInnerIterator
@@ -54,8 +54,8 @@ template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>::ReverseInn
     EIGEN_STRONG_INLINE ReverseInnerIterator(const TransposeImpl& xpr, typename TransposeImpl<MatrixType,Sparse>::Index outer)
       : Base(xpr.derived().nestedExpression(), outer)
     {}
-    Index row() const { return Base::col(); }
+    typename TransposeImpl<MatrixType,Sparse>::Index row() const { return Base::col(); }
-    Index col() const { return Base::row(); }
+    typename TransposeImpl<MatrixType,Sparse>::Index col() const { return Base::row(); }
 };
 
 } // end namespace Eigen

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h

@@ -84,8 +84,10 @@ template<typename Lhs, typename Rhs>        class DenseTimeSparseProduct;
 template<typename Lhs, typename Rhs, bool Transpose> class SparseDenseOuterProduct;
 
 template<typename Lhs, typename Rhs> struct SparseSparseProductReturnType;
-template<typename Lhs, typename Rhs, int InnerSize = internal::traits<Lhs>::ColsAtCompileTime> struct DenseSparseProductReturnType;
+template<typename Lhs, typename Rhs,
-template<typename Lhs, typename Rhs, int InnerSize = internal::traits<Lhs>::ColsAtCompileTime> struct SparseDenseProductReturnType;
+         int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct DenseSparseProductReturnType;         
+template<typename Lhs, typename Rhs,
+         int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct SparseDenseProductReturnType;
 template<typename MatrixType,int UpLo> class SparseSymmetricPermutationProduct;
 
 namespace internal {

gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h

@@ -2,7 +2,7 @@
 // for linear algebra.
 //
 // Copyright (C) 2012-2013 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
-// Copyright (C) 2012-2013 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2012-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -58,6 +58,7 @@ namespace internal {
   * \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module 
   *  OrderingMethods \endlink module for the list of built-in and external ordering methods.
   * 
+  * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
   * 
   */
 template<typename _MatrixType, typename _OrderingType>
@@ -77,10 +78,23 @@ class SparseQR
     SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
     { }
     
+    /** Construct a QR factorization of the matrix \a mat.
+      * 
+      * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+      * 
+      * \sa compute()
+      */
     SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
     {
       compute(mat);
     }
+    
+    /** Computes the QR factorization of the sparse matrix \a mat.
+      * 
+      * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+      * 
+      * \sa analyzePattern(), factorize()
+      */
     void compute(const MatrixType& mat)
     {
       analyzePattern(mat);
@@ -166,7 +180,7 @@ class SparseQR
       y.bottomRows(y.rows()-rank).setZero();
 
       // Apply the column permutation
-      if (m_perm_c.size())  dest.topRows(cols()) = colsPermutation() * y.topRows(cols());
+      if (m_perm_c.size())  dest = colsPermutation() * y.topRows(cols());
       else                  dest = y.topRows(cols());
       
       m_info = Success;
@@ -206,7 +220,7 @@ class SparseQR
     
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful,
+      * \returns \c Success if computation was successful,
       *          \c NumericalIssue if the QR factorization reports a numerical problem
       *          \c InvalidInput if the input matrix is invalid
       *
@@ -255,20 +269,24 @@ class SparseQR
 };
 
 /** \brief Preprocessing step of a QR factorization 
+  * 
+  * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
   * 
   * In this step, the fill-reducing permutation is computed and applied to the columns of A
-  * and the column elimination tree is computed as well. Only the sparcity pattern of \a mat is exploited.
+  * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited.
   * 
   * \note In this step it is assumed that there is no empty row in the matrix \a mat.
   */
 template <typename MatrixType, typename OrderingType>
 void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
 {
+  eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
   // Compute the column fill reducing ordering
   OrderingType ord; 
   ord(mat, m_perm_c); 
   Index n = mat.cols();
   Index m = mat.rows();
+  Index diagSize = (std::min)(m,n);
   
   if (!m_perm_c.size())
   {
@@ -280,20 +298,20 @@ void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
   m_outputPerm_c = m_perm_c.inverse();
   internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
   
-  m_R.resize(n, n);
+  m_R.resize(m, n);
-  m_Q.resize(m, n);
+  m_Q.resize(m, diagSize);
   
   // Allocate space for nonzero elements : rough estimation
   m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree
   m_Q.reserve(2*mat.nonZeros());
-  m_hcoeffs.resize(n);
+  m_hcoeffs.resize(diagSize);
   m_analysisIsok = true;
 }
 
 /** \brief Performs the numerical QR factorization of the input matrix
   * 
   * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with
-  * a matrix having the same sparcity pattern than \a mat.
+  * a matrix having the same sparsity pattern than \a mat.
   * 
   * \param mat The sparse column-major matrix
   */
@@ -306,11 +324,12 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
   eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step");
   Index m = mat.rows();
   Index n = mat.cols();
-  IndexVector mark(m); mark.setConstant(-1);  // Record the visited nodes
+  Index diagSize = (std::min)(m,n);
-  IndexVector Ridx(n), Qidx(m);               // Store temporarily the row indexes for the current column of R and Q
+  IndexVector mark((std::max)(m,n)); mark.setConstant(-1);  // Record the visited nodes
-  Index nzcolR, nzcolQ;                       // Number of nonzero for the current column of R and Q
+  IndexVector Ridx(n), Qidx(m);                             // Store temporarily the row indexes for the current column of R and Q
-  ScalarVector tval(m);                       // The dense vector used to compute the current column
+  Index nzcolR, nzcolQ;                                     // Number of nonzero for the current column of R and Q
-  bool found_diag;
+  ScalarVector tval(m);                                     // The dense vector used to compute the current column
+  RealScalar pivotThreshold = m_threshold;
     
   m_pmat = mat;
   m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
@@ -322,7 +341,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
     m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i]; 
   }
   
-  /* Compute the default threshold, see : 
+  /* Compute the default threshold as in MatLab, see:
    * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
    * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 
    */
@@ -330,24 +349,24 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
   {
     RealScalar max2Norm = 0.0;
     for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm());
-    m_threshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
+    pivotThreshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
   }
   
   // Initialize the numerical permutation
   m_pivotperm.setIdentity(n);
   
   Index nonzeroCol = 0; // Record the number of valid pivots
+  m_Q.startVec(0);
   
   // Left looking rank-revealing QR factorization: compute a column of R and Q at a time
-  for (Index col = 0; col < (std::min)(n,m); ++col)
+  for (Index col = 0; col < n; ++col)
   {
     mark.setConstant(-1);
     m_R.startVec(col);
-    m_Q.startVec(col);
     mark(nonzeroCol) = col;
     Qidx(0) = nonzeroCol;
     nzcolR = 0; nzcolQ = 1;
-    found_diag = col>=m;
+    bool found_diag = nonzeroCol>=m;
     tval.setZero(); 
     
     // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e.,
@@ -356,7 +375,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
     // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
     for (typename MatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
     {
-      Index curIdx = nonzeroCol ;
+      Index curIdx = nonzeroCol;
       if(itp) curIdx = itp.row();
       if(curIdx == nonzeroCol) found_diag = true;
       
@@ -398,7 +417,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
     // Browse all the indexes of R(:,col) in reverse order
     for (Index i = nzcolR-1; i >= 0; i--)
     {
-      Index curIdx = m_pivotperm.indices()(Ridx(i));
+      Index curIdx = Ridx(i);
       
       // Apply the curIdx-th householder vector to the current column (temporarily stored into tval)
       Scalar tdot(0);
@@ -427,33 +446,37 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
         }
       }
     } // End update current column
-        
+    
-    // Compute the Householder reflection that eliminate the current column
-    // FIXME this step should call the Householder module.
     Scalar tau;
-    RealScalar beta;
+    RealScalar beta = 0;
-    Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
     
-    // First, the squared norm of Q((col+1):m, col)
+    if(nonzeroCol < diagSize)
-    RealScalar sqrNorm = 0.;
-    for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
-    
-    if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
     {
-      tau = RealScalar(0);
+      // Compute the Householder reflection that eliminate the current column
-      beta = numext::real(c0);
+      // FIXME this step should call the Householder module.
-      tval(Qidx(0)) = 1;
+      Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
-     }
+      
-    else
+      // First, the squared norm of Q((col+1):m, col)
-    {
+      RealScalar sqrNorm = 0.;
-      beta = std::sqrt(numext::abs2(c0) + sqrNorm);
+      for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
-      if(numext::real(c0) >= RealScalar(0))
+      if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
-        beta = -beta;
+      {
-      tval(Qidx(0)) = 1;
+        tau = RealScalar(0);
-      for (Index itq = 1; itq < nzcolQ; ++itq)
+        beta = numext::real(c0);
-        tval(Qidx(itq)) /= (c0 - beta);
+        tval(Qidx(0)) = 1;
-      tau = numext::conj((beta-c0) / beta);
+      }
-        
+      else
+      {
+        using std::sqrt;
+        beta = sqrt(numext::abs2(c0) + sqrNorm);
+        if(numext::real(c0) >= RealScalar(0))
+          beta = -beta;
+        tval(Qidx(0)) = 1;
+        for (Index itq = 1; itq < nzcolQ; ++itq)
+          tval(Qidx(itq)) /= (c0 - beta);
+        tau = numext::conj((beta-c0) / beta);
+          
+      }
     }
 
     // Insert values in R
@@ -467,24 +490,25 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
       }
     }
 
-    if(abs(beta) >= m_threshold)
+    if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold)
     {
       m_R.insertBackByOuterInner(col, nonzeroCol) = beta;
-      nonzeroCol++;
       // The householder coefficient
-      m_hcoeffs(col) = tau;
+      m_hcoeffs(nonzeroCol) = tau;
       // Record the householder reflections
       for (Index itq = 0; itq < nzcolQ; ++itq)
       {
         Index iQ = Qidx(itq);
-        m_Q.insertBackByOuterInnerUnordered(col,iQ) = tval(iQ);
+        m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ);
         tval(iQ) = Scalar(0.);
-      }    
+      }
+      nonzeroCol++;
+      if(nonzeroCol<diagSize)
+        m_Q.startVec(nonzeroCol);
     }
     else
     {
       // Zero pivot found: move implicitly this column to the end
-      m_hcoeffs(col) = Scalar(0);
       for (Index j = nonzeroCol; j < n-1; j++) 
         std::swap(m_pivotperm.indices()(j), m_pivotperm.indices()[j+1]);
       
@@ -493,6 +517,8 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
     }
   }
   
+  m_hcoeffs.tail(diagSize-nonzeroCol).setZero();
+  
   // Finalize the column pointers of the sparse matrices R and Q
   m_Q.finalize();
   m_Q.makeCompressed();
@@ -561,14 +587,16 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
   template<typename DesType>
   void evalTo(DesType& res) const
   {
+    Index m = m_qr.rows();
     Index n = m_qr.cols();
+    Index diagSize = (std::min)(m,n);
     res = m_other;
     if (m_transpose)
     {
       eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
       //Compute res = Q' * other column by column
       for(Index j = 0; j < res.cols(); j++){
-        for (Index k = 0; k < n; k++)
+        for (Index k = 0; k < diagSize; k++)
         {
           Scalar tau = Scalar(0);
           tau = m_qr.m_Q.col(k).dot(res.col(j));
@@ -581,10 +609,10 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
     else
     {
       eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
-      // Compute res = Q' * other column by column
+      // Compute res = Q * other column by column
       for(Index j = 0; j < res.cols(); j++)
       {
-        for (Index k = n-1; k >=0; k--)
+        for (Index k = diagSize-1; k >=0; k--)
         {
           Scalar tau = Scalar(0);
           tau = m_qr.m_Q.col(k).dot(res.col(j));
@@ -618,7 +646,7 @@ struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<Sp
     return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
   }
   inline Index rows() const { return m_qr.rows(); }
-  inline Index cols() const { return m_qr.cols(); }
+  inline Index cols() const { return (std::min)(m_qr.rows(),m_qr.cols()); }
   // To use for operations with the transpose of Q
   SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const
   {

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h

@@ -11,7 +11,7 @@
 #ifndef EIGEN_STDDEQUE_H
 #define EIGEN_STDDEQUE_H
 
-#include "Eigen/src/StlSupport/details.h"
+#include "details.h"
 
 // Define the explicit instantiation (e.g. necessary for the Intel compiler)
 #if defined(__INTEL_COMPILER) || defined(__GNUC__)

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h

@@ -10,7 +10,7 @@
 #ifndef EIGEN_STDLIST_H
 #define EIGEN_STDLIST_H
 
-#include "Eigen/src/StlSupport/details.h"
+#include "details.h"
 
 // Define the explicit instantiation (e.g. necessary for the Intel compiler)
 #if defined(__INTEL_COMPILER) || defined(__GNUC__)

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h

@@ -11,7 +11,7 @@
 #ifndef EIGEN_STDVECTOR_H
 #define EIGEN_STDVECTOR_H
 
-#include "Eigen/src/StlSupport/details.h"
+#include "details.h"
 
 /**
  * This section contains a convenience MACRO which allows an easy specialization of

gtsam/3rdparty/Eigen/blas/README.txt

@@ -1,9 +1,6 @@
 
 This directory contains a BLAS library built on top of Eigen.
 
-This is currently a work in progress which is far to be ready for use,
-but feel free to contribute to it if you wish.
-
 This module is not built by default. In order to compile it, you need to
 type 'make blas' from within your build dir.
 

gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake

@@ -41,7 +41,7 @@ endif()
 
 # copy ctest properties, which currently
 # o raise the warning levels
-configure_file(${CMAKE_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl)
+configure_file(${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl)
 
 # restore default CMAKE_MAKE_PROGRAM
 set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE})
@@ -50,7 +50,7 @@ set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE})
 set(CMAKE_MAKE_PROGRAM_SAVE) 
 set(EIGEN_MAKECOMMAND_PLACEHOLDER)
 
-configure_file(${CMAKE_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake)
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake)
 
 # some documentation of this function would be nice
 ei_init_testing()

gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt

@@ -41,8 +41,8 @@ MatrixXd::Ones(rows,cols)           // ones(rows,cols)
 C.setOnes(rows,cols)                // C = ones(rows,cols)
 MatrixXd::Random(rows,cols)         // rand(rows,cols)*2-1        // MatrixXd::Random returns uniform random numbers in (-1, 1).
 C.setRandom(rows,cols)              // C = rand(rows,cols)*2-1
-VectorXd::LinSpace(size,low,high)   // linspace(low,high,size)'
+VectorXd::LinSpaced(size,low,high)   // linspace(low,high,size)'
-v.setLinSpace(size,low,high)        // v = linspace(low,high,size)'
+v.setLinSpaced(size,low,high)        // v = linspace(low,high,size)'
 
 
 // Matrix slicing and blocks. All expressions listed here are read/write.
@@ -91,6 +91,8 @@ R.adjoint()                        // R'
 R.transpose()                      // R.' or conj(R')
 R.diagonal()                       // diag(R)
 x.asDiagonal()                     // diag(x)
+R.transpose().colwise().reverse(); // rot90(R)
+R.conjugate()                      // conj(R)
 
 // All the same as Matlab, but matlab doesn't have *= style operators.
 // Matrix-vector.  Matrix-matrix.   Matrix-scalar.
@@ -167,6 +169,8 @@ x.cross(y)                // cross(x, y) Requires #include <Eigen/Geometry>
 A.cast<double>();                  // double(A)
 A.cast<float>();                   // single(A)
 A.cast<int>();                     // int32(A)
+A.real();                          // real(A)
+A.imag();                          // imag(A)
 // if the original type equals destination type, no work is done
 
 // Note that for most operations Eigen requires all operands to have the same type:

gtsam/3rdparty/Eigen/doc/LinearLeastSquares.dox

@@ -1,27 +0,0 @@
-namespace Eigen {
-
-/** \eigenManualPage LinearLeastSquares Solving linear least squares problems
-
-lede
-
-\eigenAutoToc
-
-\section LinearLeastSquaresCopied Copied
-
-The best way to do least squares solving is with a SVD decomposition. Eigen provides one as the JacobiSVD class, and its solve()
-is doing least-squares solving.
-
-Here is an example:
-<table class="example">
-<tr><th>Example:</th><th>Output:</th></tr>
-<tr>
-  <td>\include TutorialLinAlgSVDSolve.cpp </td>
-  <td>\verbinclude TutorialLinAlgSVDSolve.out </td>
-</tr>
-</table>
-
-For more information, including faster but less reliable methods, read our page concentrating on \ref LinearLeastSquares "linear least squares problems".
-
-*/
-
-}

gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox

@@ -62,6 +62,8 @@ run time. However, these assertions do cost time and can thus be turned off.
    expect that any objects passed to it are aligned. This will turn off vectorization. Not defined by default.
  - \b EIGEN_DONT_ALIGN_STATICALLY - disables alignment of arrays on the stack. Not defined by default, unless
    \c EIGEN_DONT_ALIGN is defined.
+ - \b EIGEN_DONT_PARALLELIZE - if defined, this disables multi-threading. This is only relevant if you enabled OpenMP.
+   See \ref TopicMultiThreading for details.
  - \b EIGEN_DONT_VECTORIZE - disables explicit vectorization when defined. Not defined by default, unless 
    alignment is disabled by %Eigen's platform test or the user defining \c EIGEN_DONT_ALIGN.
  - \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently
@@ -69,7 +71,10 @@ run time. However, these assertions do cost time and can thus be turned off.
    Define it to 0 to disable.
  - \b EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable
    unrolling. The size of a loop here is expressed in %Eigen's own notion of "number of FLOPS", it does not
-   correspond to the number of iterations or the number of instructions. The default is value 100. 
+   correspond to the number of iterations or the number of instructions. The default is value 100.
+ - \b EIGEN_STACK_ALLOCATION_LIMIT - defines the maximum bytes for a buffer to be allocated on the stack. For internal
+   temporary buffers, dynamic memory allocation is employed as a fall back. For fixed-size matrices or arrays, exceeding
+   this threshold raises a compile time assertion. Use 0 to set no limit. Default is 128 KB.
 
 
 \section TopicPreprocessorDirectivesPlugins Plugins

gtsam/3rdparty/Eigen/doc/TutorialSparse.dox

@@ -253,12 +253,15 @@ SparseMatrix<double> A, B;
 B = SparseMatrix<double>(A.transpose()) + A;
 \endcode
 
-Binary coefficient wise operators can also mix sparse and dense expressions:
+Some binary coefficient-wise operators can also mix sparse and dense expressions:
 \code
 sm2 = sm1.cwiseProduct(dm1);
-dm2 = sm1 + dm1;
+dm1 += sm1;
 \endcode
 
+However, it is not yet possible to add a sparse and a dense matrix as in <tt>dm2 = sm1 + dm1</tt>.
+Please write this as the equivalent <tt>dm2 = dm1; dm2 += sm1</tt> (we plan to lift this restriction
+in the next release of %Eigen).
 
 %Sparse expressions also support transposition:
 \code

gtsam/3rdparty/Eigen/test/block.cpp

@@ -10,6 +10,26 @@
 #define EIGEN_NO_STATIC_ASSERT // otherwise we fail at compile time on unused paths
 #include "main.h"
 
+template<typename MatrixType, typename Index, typename Scalar>
+typename Eigen::internal::enable_if<!NumTraits<typename MatrixType::Scalar>::IsComplex,typename MatrixType::Scalar>::type
+block_real_only(const MatrixType &m1, Index r1, Index r2, Index c1, Index c2, const Scalar& s1) {
+  // check cwise-Functions:
+  VERIFY_IS_APPROX(m1.row(r1).cwiseMax(s1), m1.cwiseMax(s1).row(r1));
+  VERIFY_IS_APPROX(m1.col(c1).cwiseMin(s1), m1.cwiseMin(s1).col(c1));
+
+  VERIFY_IS_APPROX(m1.block(r1,c1,r2-r1+1,c2-c1+1).cwiseMin(s1), m1.cwiseMin(s1).block(r1,c1,r2-r1+1,c2-c1+1));
+  VERIFY_IS_APPROX(m1.block(r1,c1,r2-r1+1,c2-c1+1).cwiseMax(s1), m1.cwiseMax(s1).block(r1,c1,r2-r1+1,c2-c1+1));
+  
+  return Scalar(0);
+}
+
+template<typename MatrixType, typename Index, typename Scalar>
+typename Eigen::internal::enable_if<NumTraits<typename MatrixType::Scalar>::IsComplex,typename MatrixType::Scalar>::type
+block_real_only(const MatrixType &, Index, Index, Index, Index, const Scalar&) {
+  return Scalar(0);
+}
+
+
 template<typename MatrixType> void block(const MatrixType& m)
 {
   typedef typename MatrixType::Index Index;
@@ -37,6 +57,8 @@ template<typename MatrixType> void block(const MatrixType& m)
   Index c1 = internal::random<Index>(0,cols-1);
   Index c2 = internal::random<Index>(c1,cols-1);
 
+  block_real_only(m1, r1, r2, c1, c1, s1);
+
   //check row() and col()
   VERIFY_IS_EQUAL(m1.col(c1).transpose(), m1.transpose().row(c1));
   //check operator(), both constant and non-constant, on row() and col()
@@ -51,7 +73,8 @@ template<typename MatrixType> void block(const MatrixType& m)
   VERIFY_IS_APPROX(m1.col(c1), m1_copy.col(c1) + s1 * m1_copy.col(c2));
   m1.col(c1).col(0) += s1 * m1_copy.col(c2);
   VERIFY_IS_APPROX(m1.col(c1), m1_copy.col(c1) + Scalar(2) * s1 * m1_copy.col(c2));
-
+  
+  
   //check block()
   Matrix<Scalar,Dynamic,Dynamic> b1(1,1); b1(0,0) = m1(r1,c1);
 

gtsam/3rdparty/Eigen/test/cholesky.cpp

@@ -68,6 +68,7 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
   Index cols = m.cols();
 
   typedef typename MatrixType::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;
   typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
 
@@ -179,6 +180,57 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
     // restore
     if(sign == -1)
       symm = -symm;
+
+    // check matrices coming from linear constraints with Lagrange multipliers
+    if(rows>=3)
+    {
+      SquareMatrixType A = symm;
+      int c = internal::random<int>(0,rows-2);
+      A.bottomRightCorner(c,c).setZero();
+      // Make sure a solution exists:
+      vecX.setRandom();
+      vecB = A * vecX;
+      vecX.setZero();
+      ldltlo.compute(A);
+      VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix());
+      vecX = ldltlo.solve(vecB);
+      VERIFY_IS_APPROX(A * vecX, vecB);
+    }
+    
+    // check non-full rank matrices
+    if(rows>=3)
+    {
+      int r = internal::random<int>(1,rows-1);
+      Matrix<Scalar,Dynamic,Dynamic> a = Matrix<Scalar,Dynamic,Dynamic>::Random(rows,r);
+      SquareMatrixType A = a * a.adjoint();
+      // Make sure a solution exists:
+      vecX.setRandom();
+      vecB = A * vecX;
+      vecX.setZero();
+      ldltlo.compute(A);
+      VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix());
+      vecX = ldltlo.solve(vecB);
+      VERIFY_IS_APPROX(A * vecX, vecB);
+    }
+    
+    // check matrices with a wide spectrum
+    if(rows>=3)
+    {
+      RealScalar s = (std::min)(16,std::numeric_limits<RealScalar>::max_exponent10/8);
+      Matrix<Scalar,Dynamic,Dynamic> a = Matrix<Scalar,Dynamic,Dynamic>::Random(rows,rows);
+      Matrix<RealScalar,Dynamic,1> d =  Matrix<RealScalar,Dynamic,1>::Random(rows);
+      for(int k=0; k<rows; ++k)
+        d(k) = d(k)*std::pow(RealScalar(10),internal::random<RealScalar>(-s,s));
+      SquareMatrixType A = a * d.asDiagonal() * a.adjoint();
+      // Make sure a solution exists:
+      vecX.setRandom();
+      vecB = A * vecX;
+      vecX.setZero();
+      ldltlo.compute(A);
+      VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix());
+      vecX = ldltlo.solve(vecB);
+      VERIFY_IS_APPROX(A * vecX, vecB);
+    }
   }
 
   // update/downdate

gtsam/3rdparty/Eigen/test/dynalloc.cpp

@@ -53,7 +53,7 @@ void check_aligned_new()
 
 void check_aligned_stack_alloc()
 {
-  for(int i = 1; i < 1000; i++)
+  for(int i = 1; i < 400; i++)
   {
     ei_declare_aligned_stack_constructed_variable(float,p,i,0);
     VERIFY(size_t(p)%ALIGNMENT==0);
@@ -87,6 +87,32 @@ template<typename T> void check_dynaligned()
   delete obj;
 }
 
+template<typename T> void check_custom_new_delete()
+{
+  {
+    T* t = new T;
+    delete t;
+  }
+  
+  {
+    std::size_t N = internal::random<std::size_t>(1,10);
+    T* t = new T[N];
+    delete[] t;
+  }
+  
+#ifdef EIGEN_ALIGN
+  {
+    T* t = static_cast<T *>((T::operator new)(sizeof(T)));
+    (T::operator delete)(t, sizeof(T));
+  }
+  
+  {
+    T* t = static_cast<T *>((T::operator new)(sizeof(T)));
+    (T::operator delete)(t);
+  }
+#endif
+}
+
 void test_dynalloc()
 {
   // low level dynamic memory allocation
@@ -102,6 +128,12 @@ void test_dynalloc()
     CALL_SUBTEST(check_dynaligned<Matrix4f>() );
     CALL_SUBTEST(check_dynaligned<Vector4d>() );
     CALL_SUBTEST(check_dynaligned<Vector4i>() );
+    CALL_SUBTEST(check_dynaligned<Vector8f>() );
+
+    CALL_SUBTEST( check_custom_new_delete<Vector4f>() );
+    CALL_SUBTEST( check_custom_new_delete<Vector2f>() );
+    CALL_SUBTEST( check_custom_new_delete<Matrix4f>() );
+    CALL_SUBTEST( check_custom_new_delete<MatrixXi>() );
   }
   
   // check static allocation, who knows ?

gtsam/3rdparty/Eigen/test/jacobisvd.cpp

@@ -67,6 +67,7 @@ template<typename MatrixType, int QRPreconditioner>
 void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions)
 {
   typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
   typedef typename MatrixType::Index Index;
   Index rows = m.rows();
   Index cols = m.cols();
@@ -81,9 +82,90 @@ void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions)
 
   RhsType rhs = RhsType::Random(rows, internal::random<Index>(1, cols));
   JacobiSVD<MatrixType, QRPreconditioner> svd(m, computationOptions);
+
+       if(internal::is_same<RealScalar,double>::value) svd.setThreshold(1e-8);
+  else if(internal::is_same<RealScalar,float>::value)  svd.setThreshold(1e-4);
+  
   SolutionType x = svd.solve(rhs);
+  
+  RealScalar residual = (m*x-rhs).norm();
+  // Check that there is no significantly better solution in the neighborhood of x
+  if(!test_isMuchSmallerThan(residual,rhs.norm()))
+  {
+    // If the residual is very small, then we have an exact solution, so we are already good.
+    for(int k=0;k<x.rows();++k)
+    {
+      SolutionType y(x);
+      y.row(k).array() += 2*NumTraits<RealScalar>::epsilon();
+      RealScalar residual_y = (m*y-rhs).norm();
+      VERIFY( test_isApprox(residual_y,residual) || residual < residual_y );
+      
+      y.row(k) = x.row(k).array() - 2*NumTraits<RealScalar>::epsilon();
+      residual_y = (m*y-rhs).norm();
+      VERIFY( test_isApprox(residual_y,residual) || residual < residual_y );
+    }
+  }
+  
   // evaluate normal equation which works also for least-squares solutions
-  VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs);
+  if(internal::is_same<RealScalar,double>::value)
+  {
+    // This test is not stable with single precision.
+    // This is probably because squaring m signicantly affects the precision.
+    VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs);
+  }
+  
+  // check minimal norm solutions
+  {
+    // generate a full-rank m x n problem with m<n
+    enum {
+      RankAtCompileTime2 = ColsAtCompileTime==Dynamic ? Dynamic : (ColsAtCompileTime)/2+1,
+      RowsAtCompileTime3 = ColsAtCompileTime==Dynamic ? Dynamic : ColsAtCompileTime+1
+    };
+    typedef Matrix<Scalar, RankAtCompileTime2, ColsAtCompileTime> MatrixType2;
+    typedef Matrix<Scalar, RankAtCompileTime2, 1> RhsType2;
+    typedef Matrix<Scalar, ColsAtCompileTime, RankAtCompileTime2> MatrixType2T;
+    Index rank = RankAtCompileTime2==Dynamic ? internal::random<Index>(1,cols) : Index(RankAtCompileTime2);
+    MatrixType2 m2(rank,cols);
+    int guard = 0;
+    do {
+      m2.setRandom();
+    } while(m2.jacobiSvd().setThreshold(test_precision<Scalar>()).rank()!=rank && (++guard)<10);
+    VERIFY(guard<10);
+    RhsType2 rhs2 = RhsType2::Random(rank);
+    // use QR to find a reference minimal norm solution
+    HouseholderQR<MatrixType2T> qr(m2.adjoint());
+    Matrix<Scalar,Dynamic,1> tmp = qr.matrixQR().topLeftCorner(rank,rank).template triangularView<Upper>().adjoint().solve(rhs2);
+    tmp.conservativeResize(cols);
+    tmp.tail(cols-rank).setZero();
+    SolutionType x21 = qr.householderQ() * tmp;
+    // now check with SVD
+    JacobiSVD<MatrixType2, ColPivHouseholderQRPreconditioner> svd2(m2, computationOptions);
+    SolutionType x22 = svd2.solve(rhs2);
+    VERIFY_IS_APPROX(m2*x21, rhs2);
+    VERIFY_IS_APPROX(m2*x22, rhs2);
+    VERIFY_IS_APPROX(x21, x22);
+    
+    // Now check with a rank deficient matrix
+    typedef Matrix<Scalar, RowsAtCompileTime3, ColsAtCompileTime> MatrixType3;
+    typedef Matrix<Scalar, RowsAtCompileTime3, 1> RhsType3;
+    Index rows3 = RowsAtCompileTime3==Dynamic ? internal::random<Index>(rank+1,2*cols) : Index(RowsAtCompileTime3);
+    Matrix<Scalar,RowsAtCompileTime3,Dynamic> C = Matrix<Scalar,RowsAtCompileTime3,Dynamic>::Random(rows3,rank);
+    MatrixType3 m3 = C * m2;
+    RhsType3 rhs3 = C * rhs2;
+    JacobiSVD<MatrixType3, ColPivHouseholderQRPreconditioner> svd3(m3, computationOptions);
+    SolutionType x3 = svd3.solve(rhs3);
+    if(svd3.rank()!=rank) {
+      std::cout << m3 << "\n\n";
+      std::cout << svd3.singularValues().transpose() << "\n";
+    std::cout << svd3.rank() << " == " << rank << "\n";
+    std::cout << x21.norm() << " == " << x3.norm() << "\n";
+    }
+//     VERIFY_IS_APPROX(m3*x3, rhs3);
+    VERIFY_IS_APPROX(m3*x21, rhs3);
+    VERIFY_IS_APPROX(m2*x3, rhs2);
+    
+    VERIFY_IS_APPROX(x21, x3);
+  }
 }
 
 template<typename MatrixType, int QRPreconditioner>
@@ -92,10 +174,9 @@ void jacobisvd_test_all_computation_options(const MatrixType& m)
   if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols())
     return;
   JacobiSVD<MatrixType, QRPreconditioner> fullSvd(m, ComputeFullU|ComputeFullV);
-
+  CALL_SUBTEST(( jacobisvd_check_full(m, fullSvd) ));
-  jacobisvd_check_full(m, fullSvd);
+  CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeFullV) ));
-  jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeFullV);
+  
-
   #if defined __INTEL_COMPILER
   // remark #111: statement is unreachable
   #pragma warning disable 111
@@ -103,20 +184,20 @@ void jacobisvd_test_all_computation_options(const MatrixType& m)
   if(QRPreconditioner == FullPivHouseholderQRPreconditioner)
     return;
 
-  jacobisvd_compare_to_full(m, ComputeFullU, fullSvd);
+  CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU, fullSvd) ));
-  jacobisvd_compare_to_full(m, ComputeFullV, fullSvd);
+  CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullV, fullSvd) ));
-  jacobisvd_compare_to_full(m, 0, fullSvd);
+  CALL_SUBTEST(( jacobisvd_compare_to_full(m, 0, fullSvd) ));
 
   if (MatrixType::ColsAtCompileTime == Dynamic) {
     // thin U/V are only available with dynamic number of columns
-    jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd);
+    CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd) ));
-    jacobisvd_compare_to_full(m,              ComputeThinV, fullSvd);
+    CALL_SUBTEST(( jacobisvd_compare_to_full(m,              ComputeThinV, fullSvd) ));
-    jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd);
+    CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd) ));
-    jacobisvd_compare_to_full(m, ComputeThinU             , fullSvd);
+    CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU             , fullSvd) ));
-    jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd);
+    CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd) ));
-    jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeThinV);
+    CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeThinV) ));
-    jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeFullV);
+    CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeFullV) ));
-    jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeThinV);
+    CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeThinV) ));
 
     // test reconstruction
     typedef typename MatrixType::Index Index;
@@ -129,12 +210,29 @@ void jacobisvd_test_all_computation_options(const MatrixType& m)
 template<typename MatrixType>
 void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
 {
-  MatrixType m = pickrandom ? MatrixType::Random(a.rows(), a.cols()) : a;
+  MatrixType m = a;
+  if(pickrandom)
+  {
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef typename MatrixType::Index Index;
+    Index diagSize = (std::min)(a.rows(), a.cols());
+    RealScalar s = std::numeric_limits<RealScalar>::max_exponent10/4;
+    s = internal::random<RealScalar>(1,s);
+    Matrix<RealScalar,Dynamic,1> d =  Matrix<RealScalar,Dynamic,1>::Random(diagSize);
+    for(Index k=0; k<diagSize; ++k)
+      d(k) = d(k)*std::pow(RealScalar(10),internal::random<RealScalar>(-s,s));
+    m = Matrix<Scalar,Dynamic,Dynamic>::Random(a.rows(),diagSize) * d.asDiagonal() * Matrix<Scalar,Dynamic,Dynamic>::Random(diagSize,a.cols());
+    // cancel some coeffs
+    Index n  = internal::random<Index>(0,m.size()-1);
+    for(Index i=0; i<n; ++i)
+      m(internal::random<Index>(0,m.rows()-1), internal::random<Index>(0,m.cols()-1)) = Scalar(0);
+  }
 
-  jacobisvd_test_all_computation_options<MatrixType, FullPivHouseholderQRPreconditioner>(m);
+  CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, FullPivHouseholderQRPreconditioner>(m) ));
-  jacobisvd_test_all_computation_options<MatrixType, ColPivHouseholderQRPreconditioner>(m);
+  CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, ColPivHouseholderQRPreconditioner>(m) ));
-  jacobisvd_test_all_computation_options<MatrixType, HouseholderQRPreconditioner>(m);
+  CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, HouseholderQRPreconditioner>(m) ));
-  jacobisvd_test_all_computation_options<MatrixType, NoQRPreconditioner>(m);
+  CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, NoQRPreconditioner>(m) ));
 }
 
 template<typename MatrixType> void jacobisvd_verify_assert(const MatrixType& m)
@@ -328,6 +426,7 @@ void test_jacobisvd()
     TEST_SET_BUT_UNUSED_VARIABLE(r)
     TEST_SET_BUT_UNUSED_VARIABLE(c)
     
+    CALL_SUBTEST_10(( jacobisvd<MatrixXd>(MatrixXd(r,c)) ));
     CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(r,c)) ));
     CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(r,c)) ));
     (void) r;

gtsam/3rdparty/Eigen/test/ref.cpp

@@ -154,59 +154,79 @@ template<typename PlainObjectType> void check_const_correctness(const PlainObjec
   VERIFY( !(Ref<ConstPlainObjectType, Aligned>::Flags & LvalueBit) );
 }
 
-EIGEN_DONT_INLINE void call_ref_1(Ref<VectorXf> ) { }
+template<typename B>
-EIGEN_DONT_INLINE void call_ref_2(const Ref<const VectorXf>& ) { }
+EIGEN_DONT_INLINE void call_ref_1(Ref<VectorXf> a, const B &b) { VERIFY_IS_EQUAL(a,b); }
-EIGEN_DONT_INLINE void call_ref_3(Ref<VectorXf,0,InnerStride<> > ) { }
+template<typename B>
-EIGEN_DONT_INLINE void call_ref_4(const Ref<const VectorXf,0,InnerStride<> >& ) { }
+EIGEN_DONT_INLINE void call_ref_2(const Ref<const VectorXf>& a, const B &b) { VERIFY_IS_EQUAL(a,b); }
-EIGEN_DONT_INLINE void call_ref_5(Ref<MatrixXf,0,OuterStride<> > ) { }
+template<typename B>
-EIGEN_DONT_INLINE void call_ref_6(const Ref<const MatrixXf,0,OuterStride<> >& ) { }
+EIGEN_DONT_INLINE void call_ref_3(Ref<VectorXf,0,InnerStride<> > a, const B &b) { VERIFY_IS_EQUAL(a,b); }
+template<typename B>
+EIGEN_DONT_INLINE void call_ref_4(const Ref<const VectorXf,0,InnerStride<> >& a, const B &b) { VERIFY_IS_EQUAL(a,b); }
+template<typename B>
+EIGEN_DONT_INLINE void call_ref_5(Ref<MatrixXf,0,OuterStride<> > a, const B &b) { VERIFY_IS_EQUAL(a,b); }
+template<typename B>
+EIGEN_DONT_INLINE void call_ref_6(const Ref<const MatrixXf,0,OuterStride<> >& a, const B &b) { VERIFY_IS_EQUAL(a,b); }
+template<typename B>
+EIGEN_DONT_INLINE void call_ref_7(Ref<Matrix<float,Dynamic,3> > a, const B &b) { VERIFY_IS_EQUAL(a,b); }
 
 void call_ref()
 {
-  VectorXcf ca(10);
+  VectorXcf ca  = VectorXcf::Random(10);
-  VectorXf a(10);
+  VectorXf a    = VectorXf::Random(10);
+  RowVectorXf b = RowVectorXf::Random(10);
+  MatrixXf A    = MatrixXf::Random(10,10);
+  RowVector3f c = RowVector3f::Random();
   const VectorXf& ac(a);
   VectorBlock<VectorXf> ab(a,0,3);
-  MatrixXf A(10,10);
   const VectorBlock<VectorXf> abc(a,0,3);
+  
 
-  VERIFY_EVALUATION_COUNT( call_ref_1(a), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0);
-  //call_ref_1(ac);           // does not compile because ac is const
+  VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_1(ab), 0);
+//   call_ref_1(ac);           // does not compile because ac is const
-  VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_1(abc), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0);
-  // call_ref_1(A.row(3));    // does not compile because innerstride!=1
+  VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3)), 0);
+//   call_ref_1(A.row(3));    // does not compile because innerstride!=1
-  VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0);
-  //call_ref_1(a+a);          // does not compile for obvious reason
+  VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0);
+//   call_ref_1(a+a);          // does not compile for obvious reason
 
-  VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1)), 1);     // evaluated into a temp
+  MatrixXf tmp = A*A.col(1);
-  VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1);     // evaluated into a temp
-  VERIFY_EVALUATION_COUNT( call_ref_2(ac), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_2(a), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_2(ab), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_2(a+a), 1);            // evaluated into a temp
+  VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag()), 1);      // evaluated into a temp
+  tmp = a+a;
+  VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1);            // evaluated into a temp
+  VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1);      // evaluated into a temp
 
-  VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_4(a+a), 1);           // evaluated into a temp
+  tmp = a+a;
-  VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag()), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1);           // evaluated into a temp
+  VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0);
 
-  VERIFY_EVALUATION_COUNT( call_ref_5(a), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_5(A), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0);
-  // call_ref_5(A.transpose());   // does not compile
+//   call_ref_5(A.transpose());   // does not compile
-  VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0);             // storage order do not match, but this is a degenerate case that should work
+  VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0);
 
-  VERIFY_EVALUATION_COUNT( call_ref_6(a), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3)), 1);           // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
+  VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1);           // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
-  VERIFY_EVALUATION_COUNT( call_ref_6(A+A), 1);                // evaluated into a temp
+  tmp = A+A;
-  VERIFY_EVALUATION_COUNT( call_ref_6(A), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1);                // evaluated into a temp
-  VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose()), 1);      // evaluated into a temp because the storage orders do not match
+  VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0);
-  VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2)), 0);
+  VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1);      // evaluated into a temp because the storage orders do not match
+  VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
+  
+  VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0);
 }
 
 void test_ref()

gtsam/3rdparty/Eigen/test/simplicial_cholesky.cpp

@@ -11,26 +11,31 @@
 
 template<typename T> void test_simplicial_cholesky_T()
 {
-  SimplicialCholesky<SparseMatrix<T>, Lower> chol_colmajor_lower;
+  SimplicialCholesky<SparseMatrix<T>, Lower> chol_colmajor_lower_amd;
-  SimplicialCholesky<SparseMatrix<T>, Upper> chol_colmajor_upper;
+  SimplicialCholesky<SparseMatrix<T>, Upper> chol_colmajor_upper_amd;
-  SimplicialLLT<SparseMatrix<T>, Lower> llt_colmajor_lower;
+  SimplicialLLT<SparseMatrix<T>, Lower> llt_colmajor_lower_amd;
-  SimplicialLDLT<SparseMatrix<T>, Upper> llt_colmajor_upper;
+  SimplicialLLT<SparseMatrix<T>, Upper> llt_colmajor_upper_amd;
-  SimplicialLDLT<SparseMatrix<T>, Lower> ldlt_colmajor_lower;
+  SimplicialLDLT<SparseMatrix<T>, Lower> ldlt_colmajor_lower_amd;
-  SimplicialLDLT<SparseMatrix<T>, Upper> ldlt_colmajor_upper;
+  SimplicialLDLT<SparseMatrix<T>, Upper> ldlt_colmajor_upper_amd;
+  SimplicialLDLT<SparseMatrix<T>, Lower, NaturalOrdering<int> > ldlt_colmajor_lower_nat;
+  SimplicialLDLT<SparseMatrix<T>, Upper, NaturalOrdering<int> > ldlt_colmajor_upper_nat;
 
-  check_sparse_spd_solving(chol_colmajor_lower);
+  check_sparse_spd_solving(chol_colmajor_lower_amd);
-  check_sparse_spd_solving(chol_colmajor_upper);
+  check_sparse_spd_solving(chol_colmajor_upper_amd);
-  check_sparse_spd_solving(llt_colmajor_lower);
+  check_sparse_spd_solving(llt_colmajor_lower_amd);
-  check_sparse_spd_solving(llt_colmajor_upper);
+  check_sparse_spd_solving(llt_colmajor_upper_amd);
-  check_sparse_spd_solving(ldlt_colmajor_lower);
+  check_sparse_spd_solving(ldlt_colmajor_lower_amd);
-  check_sparse_spd_solving(ldlt_colmajor_upper);
+  check_sparse_spd_solving(ldlt_colmajor_upper_amd);
   
-  check_sparse_spd_determinant(chol_colmajor_lower);
+  check_sparse_spd_determinant(chol_colmajor_lower_amd);
-  check_sparse_spd_determinant(chol_colmajor_upper);
+  check_sparse_spd_determinant(chol_colmajor_upper_amd);
-  check_sparse_spd_determinant(llt_colmajor_lower);
+  check_sparse_spd_determinant(llt_colmajor_lower_amd);
-  check_sparse_spd_determinant(llt_colmajor_upper);
+  check_sparse_spd_determinant(llt_colmajor_upper_amd);
-  check_sparse_spd_determinant(ldlt_colmajor_lower);
+  check_sparse_spd_determinant(ldlt_colmajor_lower_amd);
-  check_sparse_spd_determinant(ldlt_colmajor_upper);
+  check_sparse_spd_determinant(ldlt_colmajor_upper_amd);
+  
+  check_sparse_spd_solving(ldlt_colmajor_lower_nat);
+  check_sparse_spd_solving(ldlt_colmajor_upper_nat);
 }
 
 void test_simplicial_cholesky()

gtsam/3rdparty/Eigen/test/sparseqr.cpp

@@ -2,24 +2,24 @@
 // for linear algebra.
 //
 // Copyright (C) 2012 Desire Nuentsa Wakam <desire.nuentsa_wakam@inria.fr>
+// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
 #include "sparse.h"
 #include <Eigen/SparseQR>
 
-
 template<typename MatrixType,typename DenseMat>
-int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 300)
+int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150)
 {
   eigen_assert(maxRows >= maxCols);
   typedef typename MatrixType::Scalar Scalar;
   int rows = internal::random<int>(1,maxRows);
-  int cols = internal::random<int>(1,rows);
+  int cols = internal::random<int>(1,maxCols);
   double density = (std::max)(8./(rows*cols), 0.01);
   
-  A.resize(rows,rows);
+  A.resize(rows,cols);
-  dA.resize(rows,rows);
+  dA.resize(rows,cols);
   initSparse<Scalar>(density, dA, A,ForceNonZeroDiag);
   A.makeCompressed();
   int nop = internal::random<int>(0, internal::random<double>(0,1) > 0.5 ? cols/2 : 0);
@@ -31,6 +31,13 @@ int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows
     A.col(j0)  = s * A.col(j1);
     dA.col(j0) = s * dA.col(j1);
   }
+  
+//   if(rows<cols) {
+//     A.conservativeResize(cols,cols);
+//     dA.conservativeResize(cols,cols);
+//     dA.bottomRows(cols-rows).setZero();
+//   }
+  
   return rows;
 }
 
@@ -42,11 +49,10 @@ template<typename Scalar> void test_sparseqr_scalar()
   MatrixType A;
   DenseMat dA;
   DenseVector refX,x,b; 
-  SparseQR<MatrixType, AMDOrdering<int> > solver; 
+  SparseQR<MatrixType, COLAMDOrdering<int> > solver; 
   generate_sparse_rectangular_problem(A,dA);
   
-  int n = A.cols();
+  b = dA * DenseVector::Random(A.cols());
-  b = DenseVector::Random(n);
   solver.compute(A);
   if (solver.info() != Success)
   {
@@ -60,17 +66,19 @@ template<typename Scalar> void test_sparseqr_scalar()
     std::cerr << "sparse QR factorization failed\n";
     exit(0);
     return;
-  } 
+  }
+  
+  VERIFY_IS_APPROX(A * x, b);
+  
   //Compare with a dense QR solver
   ColPivHouseholderQR<DenseMat> dqr(dA);
   refX = dqr.solve(b);
   
   VERIFY_IS_EQUAL(dqr.rank(), solver.rank());
-  
+  if(solver.rank()==A.cols()) // full rank
-  if(solver.rank()<A.cols())
-    VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );
-  else
     VERIFY_IS_APPROX(x, refX);
+//   else
+//     VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );
 
   // Compute explicitly the matrix Q
   MatrixType Q, QtQ, idM;
@@ -88,3 +96,4 @@ void test_sparseqr()
     CALL_SUBTEST_2(test_sparseqr_scalar<std::complex<double> >());
   }
 }
+

gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h

@@ -2,7 +2,7 @@
 // for linear algebra.
 //
 // Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2012 Kolja Brix <brix@igpm.rwth-aaachen.de>
+// Copyright (C) 2012, 2014 Kolja Brix <brix@igpm.rwth-aaachen.de>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -72,16 +72,20 @@ bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Precondition
 
 	VectorType p0 = rhs - mat*x;
 	VectorType r0 = precond.solve(p0);
-// 	RealScalar r0_sqnorm = r0.squaredNorm();
+ 
+	// is initial guess already good enough?
+	if(abs(r0.norm()) < tol) {
+		return true; 
+	}
 
 	VectorType w = VectorType::Zero(restart + 1);
 
-	FMatrixType H = FMatrixType::Zero(m, restart + 1);
+	FMatrixType H = FMatrixType::Zero(m, restart + 1); // Hessenberg matrix
 	VectorType tau = VectorType::Zero(restart + 1);
 	std::vector < JacobiRotation < Scalar > > G(restart);
 
 	// generate first Householder vector
-	VectorType e;
+	VectorType e(m-1);
 	RealScalar beta;
 	r0.makeHouseholder(e, tau.coeffRef(0), beta);
 	w(0)=(Scalar) beta;

gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp

@@ -127,46 +127,47 @@ template<typename Func> void forward_jacobian(const Func& f)
     VERIFY_IS_APPROX(j, jref);
 }
 
+
+// TODO also check actual derivatives!
 void test_autodiff_scalar()
 {
-  std::cerr << foo<float>(1,2) << "\n";
+  Vector2f p = Vector2f::Random();
   typedef AutoDiffScalar<Vector2f> AD;
-  AD ax(1,Vector2f::UnitX());
+  AD ax(p.x(),Vector2f::UnitX());
-  AD ay(2,Vector2f::UnitY());
+  AD ay(p.y(),Vector2f::UnitY());
   AD res = foo<AD>(ax,ay);
-  std::cerr << res.value() << " <> "
+  VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y()));
-            << res.derivatives().transpose() << "\n\n";
 }
 
+// TODO also check actual derivatives!
 void test_autodiff_vector()
 {
-  std::cerr << foo<Vector2f>(Vector2f(1,2)) << "\n";
+  Vector2f p = Vector2f::Random();
   typedef AutoDiffScalar<Vector2f> AD;
   typedef Matrix<AD,2,1> VectorAD;
-  VectorAD p(AD(1),AD(-1));
+  VectorAD ap = p.cast<AD>();
-  p.x().derivatives() = Vector2f::UnitX();
+  ap.x().derivatives() = Vector2f::UnitX();
-  p.y().derivatives() = Vector2f::UnitY();
+  ap.y().derivatives() = Vector2f::UnitY();
   
-  AD res = foo<VectorAD>(p);
+  AD res = foo<VectorAD>(ap);
-  std::cerr << res.value() << " <> "
+  VERIFY_IS_APPROX(res.value(), foo(p));
-            << res.derivatives().transpose() << "\n\n";
 }
 
 void test_autodiff_jacobian()
 {
-  for(int i = 0; i < g_repeat; i++) {
+  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) ));
-    CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) ));
+  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) ));
-    CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) ));
+  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) ));
-    CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) ));
+  CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) ));
-    CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) ));
+  CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) ));
-    CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) ));
-  }
 }
 
 void test_autodiff()
 {
-    test_autodiff_scalar();
+  for(int i = 0; i < g_repeat; i++) {
-    test_autodiff_vector();
+    CALL_SUBTEST_1( test_autodiff_scalar() );
-//     test_autodiff_jacobian();
+    CALL_SUBTEST_2( test_autodiff_vector() );
+    CALL_SUBTEST_3( test_autodiff_jacobian() );
+  }
 }
 

gtsam/3rdparty/gtsam_eigen_includes.h.in

@@ -17,6 +17,10 @@
 
 #pragma once
 
+#ifndef MKL_BLAS
+#define MKL_BLAS MKL_DOMAIN_BLAS
+#endif
+
 #cmakedefine EIGEN_USE_MKL_ALL // This is also defined in config.h
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense>
 #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR>

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

@@ -9,7 +9,9 @@ if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
   endif()
 endif()
 
-add_definitions(-Wno-unknown-pragmas)
+if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC"))
+  #add_definitions(-Wno-unknown-pragmas)
+endif()
 
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.6 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.6)

gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h

@@ -59,9 +59,10 @@ typedef ptrdiff_t ssize_t;
 #endif
 
 #ifdef __MSC__
+#if(_MSC_VER < 1700)
 /* MSC does not have rint() function */
 #define rint(x) ((int)((x)+0.5))  
-
+#endif
 /* MSC does not have INFINITY defined */
 #ifndef INFINITY
 #define INFINITY FLT_MAX

gtsam/base/DerivedValue.h

@@ -16,8 +16,9 @@
  */
 
 #pragma once
-#include <boost/make_shared.hpp>
+
 #include <gtsam/base/Value.h>
+#include <boost/make_shared.hpp>
 
 //////////////////
 // The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR

gtsam/base/LieMatrix.h

@@ -19,9 +19,9 @@
 
 #include <cstdarg>
 
+#include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/base/DerivedValue.h>
 #include <boost/serialization/nvp.hpp>
 
 namespace gtsam {
@@ -40,9 +40,12 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
   /** initialize from a normal matrix */
   LieMatrix(const Matrix& v) : Matrix(v) {}
 
+// Currently TMP constructor causes ICE on MSVS 2013
+#if (_MSC_VER < 1800)
   /** initialize from a fixed size normal vector */
   template<int M, int N>
   LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
+#endif
 
   /** constructor with size and initial data, row order ! */
   LieMatrix(size_t m, size_t n, const double* const data) :
@@ -82,6 +85,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
   inline LieMatrix retract(const Vector& v) const {
     if(v.size() != this->size())
       throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");
+
     return LieMatrix(*this +
       Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
       &v(0), this->rows(), this->cols()));

gtsam/base/LieVector.h

@@ -33,10 +33,13 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
 
   /** initialize from a normal vector */
   LieVector(const Vector& v) : Vector(v) {}
-
+  
+// Currently TMP constructor causes ICE on MSVS 2013
+#if (_MSC_VER < 1800)
   /** initialize from a fixed size normal vector */
   template<int N>
   LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
+#endif
 
   /** wrap a double */
   LieVector(double d) : Vector((Vector(1) << d)) {}

gtsam/base/Matrix.h

@@ -467,7 +467,7 @@ GTSAM_EXPORT Matrix Cayley(const Matrix& A);
 /// Implementation of Cayley transform using fixed size matrices to let
 /// Eigen do more optimization
 template<int N>
-Eigen::Matrix<double, N, N> Cayley(const Eigen::Matrix<double, N, N>& A) {
+Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
   typedef Eigen::Matrix<double, N, N> FMat;
   return (FMat::Identity() - A)*(FMat::Identity() + A).inverse();
 }

gtsam/base/Value.h

@@ -36,18 +36,19 @@ namespace gtsam {
    * Values can operate generically on Value objects, retracting or computing
    * local coordinates for many Value objects of different types.
    *
-   * When you implement retract_(), localCoordinates_(), and equals_(), we
+   * Inhereting from the DerivedValue class templated provides a generic implementation of
-   * suggest first implementing versions of these functions that work directly
+   * the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating
-   * with derived objects, then using the provided helper functions to
+   * the need to implement these functions in your class. Note that you must inheret from 
-   * implement the generic Value versions.  This makes your implementation
+   * DerivedValue templated on the class you are defining. For example you cannot define
-   * easier, and also improves performance in situations where the derived type
+   * the following
-   * is in fact known, such as in most implementations of \c evaluateError() in
+   * \code
-   * classes derived from NonlinearFactor.
+   * class Rot3 : public DerivedValue<Point3>{ \\classdef }
+   * \endcode
    *
    * Using the above practice, here is an example of implementing a typical
    * class derived from Value:
    * \code
-     class Rot3 : public Value {
+     class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
      public:
        // Constructor, there is never a need to call the Value base class constructor.
        Rot3() { ... }
@@ -74,27 +75,6 @@ namespace gtsam {
          // Math to implement 3D rotation localCoordinates, e.g. logarithm map
          return Vector(result);
        }
-
-       // Equals implementing the generic Value interface (virtual, implements Value::equals_())
-       virtual bool equals_(const Value& other, double tol = 1e-9) const {
-         // Call our provided helper function to call your Rot3-specific
-         // equals with appropriate casting.
-         return CallDerivedEquals(this, other, tol);
-       }
-
-       // retract implementing the generic Value interface (virtual, implements Value::retract_())
-       virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
-         // Call our provided helper function to call your Rot3-specific
-         // retract and do the appropriate casting and allocation.
-         return CallDerivedRetract(this, delta);
-       }
-
-       // localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_())
-       virtual Vector localCoordinates_(const Value& value) const {
-         // Call our provided helper function to call your Rot3-specific
-         // localCoordinates and do the appropriate casting.
-         return CallDerivedLocalCoordinates(this, value);
-       }
      };
      \endcode
    */

gtsam/base/Vector.cpp

@@ -30,55 +30,11 @@
 
 #include <gtsam/base/Vector.h>
 
-//#ifdef WIN32
-//#include <Windows.h>
-//#endif
 
 using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-void odprintf_(const char *format, ostream& stream, ...) {
-  char buf[4096], *p = buf;
-
-  va_list args;
-  va_start(args, stream);
-#ifdef WIN32
-  _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
-#else
-  vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
-#endif
-  va_end(args);
-
-//#ifdef WIN32
-//  OutputDebugString(buf);
-//#else
-  stream << buf;
-//#endif
-}
-
-/* ************************************************************************* */
-
-void odprintf(const char *format, ...) {
-  char buf[4096], *p = buf;
-
-  va_list args;
-  va_start(args, format);
-#ifdef WIN32
-  _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
-#else
-  vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
-#endif
-  va_end(args);
-
-//#ifdef WIN32
-//  OutputDebugString(buf);
-//#else
-  cout << buf;
-//#endif
-}
-
 /* ************************************************************************* */
 bool zero(const Vector& v) {
   bool result = true;
@@ -101,10 +57,12 @@ Vector delta(size_t n, size_t i, double value) {
 /* ************************************************************************* */
 void print(const Vector& v, const string& s, ostream& stream) {
   size_t n = v.size();
-  odprintf_("%s [", stream, s.c_str());
+
-  for(size_t i=0; i<n; i++)
+  stream << s << "[";
-    odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
+  for(size_t i=0; i<n; i++) {
-  odprintf_("];\n", stream);
+      stream << setprecision(9) << v(i) << (i<n-1 ? "; " : "");
+  }
+  stream << "];" << endl;
 }
 
 /* ************************************************************************* */

gtsam/base/Vector.h

@@ -41,11 +41,6 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
 
-/**
- * An auxiliary function to printf for Win32 compatibility, added by Kai
- */
-GTSAM_EXPORT void odprintf(const char *format, ...);
-
 /**
  * Create vector initialized to a constant value
  * @param n is the size of the vector

gtsam/base/tests/testMatrix.cpp

@@ -1127,6 +1127,12 @@ TEST( matrix, svd2 )
 
   svd(sampleA, U, s, V);
 
+  // take care of sign ambiguity
+  if (U(0, 1) > 0) {
+    U = -U;
+    V = -V;
+  }
+
   EXPECT(assert_equal(expectedU,U));
   EXPECT(assert_equal(expected_s,s,1e-9));
   EXPECT(assert_equal(expectedV,V));
@@ -1143,6 +1149,13 @@ TEST( matrix, svd3 )
   Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.);
 
   svd(sampleAt, U, s, V);
+
+  // take care of sign ambiguity
+  if (U(0, 0) > 0) {
+    U = -U;
+    V = -V;
+  }
+
   Matrix S = diag(s);
   Matrix t = U * S;
   Matrix Vt = trans(V);
@@ -1176,6 +1189,17 @@ TEST( matrix, svd4 )
      0.6723,   0.7403);
 
   svd(A, U, s, V);
+
+  // take care of sign ambiguity
+  if (U(0, 0) < 0) {
+    U.col(0) = -U.col(0);
+    V.col(0) = -V.col(0);
+  }
+  if (U(0, 1) < 0) {
+    U.col(1) = -U.col(1);
+    V.col(1) = -V.col(1);
+  }
+
   Matrix reconstructed = U * diag(s) * trans(V);
 
   EXPECT(assert_equal(A, reconstructed, 1e-4));

gtsam/geometry/Cal3_S2.h

@@ -165,6 +165,16 @@ public:
    */
   Vector3 calibrate(const Vector3& p) const;
 
+  /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
+  inline Cal3_S2 between(const Cal3_S2& q,
+      boost::optional<Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H2=boost::none) const {
+    if(H1) *H1 = -eye(5);
+    if(H2) *H2 = eye(5);
+    return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
+  }
+
+
   /// @}
   /// @name Manifold
   /// @{

gtsam/geometry/Rot3M.cpp

@@ -240,7 +240,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
     return retractCayley(omega);
   } else if(mode == Rot3::SLOW_CAYLEY) {
     Matrix Omega = skewSymmetric(omega);
-    return (*this)*Cayley<3>(-Omega/2);
+    return (*this)*CayleyFixed<3>(-Omega/2);
   } else {
     assert(false);
     exit(1);
@@ -269,7 +269,7 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
     // Create a fixed-size matrix
     Eigen::Matrix3d A(between(T).matrix());
     // using templated version of Cayley
-    Eigen::Matrix3d Omega = Cayley<3>(A);
+    Eigen::Matrix3d Omega = CayleyFixed<3>(A);
     return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
   } else {
     assert(false);

gtsam/geometry/tests/testCal3_S2.cpp

@@ -93,6 +93,17 @@ TEST( Cal3_S2, retract)
   CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
 }
 
+/* ************************************************************************* */
+TEST(Cal3_S2, between) {
+  Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
+  Matrix H1, H2;
+
+  EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
+  EXPECT(assert_equal(-eye(5), H1));
+  EXPECT(assert_equal(eye(5), H2));
+
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -173,6 +173,12 @@ TEST (EssentialMatrix, epipoles) {
   Vector S;
   gtsam::svd(E.matrix(), U, S, V);
 
+  // take care of SVD sign ambiguity
+  if (U(0, 2) > 0) {
+    U = -U;
+    V = -V;
+  }
+
   // check rank 2 constraint
   CHECK(fabs(S(2))<1e-10);
 
@@ -182,8 +188,15 @@ TEST (EssentialMatrix, epipoles) {
   // Check epipoles
 
   // Epipole in image 1 is just E.direction()
-  Unit3 e1(U(0, 2), U(1, 2), U(2, 2));
+  Unit3 e1(-U(0, 2), -U(1, 2), -U(2, 2));
-  EXPECT(assert_equal(e1, E.epipole_a()));
+  Unit3 actual = E.epipole_a();
+  EXPECT(assert_equal(e1, actual));
+
+  // take care of SVD sign ambiguity
+  if (V(0, 2) < 0) {
+    U = -U;
+    V = -V;
+  }
 
   // Epipole in image 2 is E.rotation().unrotate(E.direction())
   Unit3 e2(V(0, 2), V(1, 2), V(2, 2));

gtsam/geometry/tests/testTriangulation.cpp

@@ -268,7 +268,7 @@ TEST( triangulation, TriangulationFactor ) {
   Key pointKey(1);
   SharedNoiseModel model;
   typedef TriangulationFactor<> Factor;
-  Factor factor(camera1, z1, model, pointKey, sharedCal);
+  Factor factor(camera1, z1, model, pointKey);
 
   // Use the factor to calculate the Jacobians
   Matrix HActual;

gtsam/linear/Errors.cpp

@@ -38,7 +38,7 @@ Errors::Errors(const VectorValues& V) {
 
 /* ************************************************************************* */
 void Errors::print(const std::string& s) const {
-  odprintf("%s:\n", s.c_str());
+  cout << s << endl;
   BOOST_FOREACH(const Vector& v, *this)
     gtsam::print(v);
 }

gtsam/linear/JacobianFactor-inl.h

@@ -19,10 +19,6 @@
 #pragma once
 
 #include <gtsam/linear/linearExceptions.h>
-#include <boost/assign/list_of.hpp>
-#include <boost/range/adaptor/transformed.hpp>
-#include <boost/range/algorithm/for_each.hpp>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 
@@ -59,20 +55,10 @@ namespace gtsam {
     model_ = model;
   }
 
-  /* ************************************************************************* */
-  namespace internal {
-    static inline DenseIndex getColsJF(const std::pair<Key,Matrix>& p) {
-      return p.second.cols();
-    }
-  }
-
   /* ************************************************************************* */
   template<typename TERMS>
   void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
   {
-    using boost::adaptors::transformed;
-    namespace br = boost::range;
-
     // Check noise model dimension
     if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
       throw InvalidNoiseModel(b.size(), noiseModel->dim());
@@ -80,25 +66,32 @@ namespace gtsam {
     // Resize base class key vector
     Base::keys_.resize(terms.size());
 
-    // Construct block matrix - this uses the boost::range 'transformed' construct to apply
+    // Get dimensions of matrices
-    // internal::getColsJF (defined just above here in this file) to each term.  This
+    std::vector<size_t> dimensions;
-    // transforms the list of terms into a list of variable dimensions, by extracting the
+    dimensions.reserve(terms.size());
-    // number of columns in each matrix.  This is done to avoid separately allocating an
+    for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
-    // array of dimensions before constructing the VerticalBlockMatrix.
+      const std::pair<Key, Matrix>& term = *it;
-    Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true);
+      const Matrix& Ai = term.second;
+      dimensions.push_back(Ai.cols());
+    }
+
+    // Construct block matrix
+    Ab_ = VerticalBlockMatrix(dimensions, b.size(), true);
 
     // Check and add terms
     DenseIndex i = 0; // For block index
-    for(typename TERMS::const_iterator termIt = terms.begin(); termIt != terms.end(); ++termIt) {
+    for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
-      const std::pair<Key, Matrix>& term = *termIt;
+      const std::pair<Key, Matrix>& term = *it;
+      Key key = term.first;
+      const Matrix& Ai = term.second;
 
       // Check block rows
-      if(term.second.rows() != Ab_.rows())
+      if(Ai.rows() != Ab_.rows())
-        throw InvalidMatrixBlock(Ab_.rows(), term.second.rows());
+        throw InvalidMatrixBlock(Ab_.rows(), Ai.rows());
 
       // Assign key and matrix
-      Base::keys_[i] = term.first;
+      Base::keys_[i] = key;
-      Ab_(i) = term.second;
+      Ab_(i) = Ai;
 
       // Increment block index
       ++ i;

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -15,6 +15,17 @@
  * @author  Frank Dellaert
  */
 
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/LieVector.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <boost/assign/list_of.hpp>
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
+
 // STL/C++
 #include <iostream>
 #include <sstream>
@@ -22,16 +33,6 @@
 #include <boost/tuple/tuple.hpp>
 #include <boost/foreach.hpp>
 
-#include <boost/assign/std/list.hpp> // for operator +=
-using namespace boost::assign;
-
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-
 using namespace std;
 using namespace gtsam;
 

gtsam/linear/tests/testGaussianConditional.cpp

@@ -25,13 +25,15 @@
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 
-#include <iostream>
-#include <sstream>
-#include <vector>
 #include <boost/assign/std/list.hpp>
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/list_inserter.hpp>
 #include <boost/make_shared.hpp>
+#include <boost/assign/list_of.hpp>
+
+#include <iostream>
+#include <sstream>
+#include <vector>
 
 using namespace gtsam;
 using namespace std;

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -18,20 +18,21 @@
  *  @author Richard Roberts
  **/
 
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/inference/VariableSlots.h>
+#include <gtsam/inference/VariableIndex.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+
+#include <boost/assign/list_of.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
 using namespace boost::assign;
 
 #include <gtsam/base/TestableAssertions.h>
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/base/debug.h>
-#include <gtsam/base/VerticalBlockMatrix.h>
-#include <gtsam/inference/VariableSlots.h>
-#include <gtsam/inference/VariableIndex.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/GaussianConditional.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-
 using namespace std;
 using namespace gtsam;
 

gtsam/linear/tests/testHessianFactor.cpp

@@ -15,24 +15,25 @@
  * @date    Dec 15, 2010
  */
 
-#include <vector>
-#include <utility>
-
-#include <boost/assign/std/vector.hpp>
-#include <boost/assign/std/map.hpp>
-
-#include <gtsam/base/debug.h>
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/VectorValues.h>
-
+#include <gtsam/base/debug.h>
 #include <gtsam/base/TestableAssertions.h>
+
 #include <CppUnitLite/TestHarness.h>
 
-using namespace std;
+#include <boost/assign/list_of.hpp>
+#include <boost/assign/std/vector.hpp>
+#include <boost/assign/std/map.hpp>
 using namespace boost::assign;
+
+#include <vector>
+#include <utility>
+
+using namespace std;
 using namespace gtsam;
 
 const double tol = 1e-5;

gtsam/linear/tests/testJacobianFactor.cpp

@@ -105,6 +105,24 @@ TEST(JacobianFactor, constructors_and_accessors)
     EXPECT(noise == expected.get_model());
     EXPECT(noise == actual.get_model());
   }
+  {
+    // Test three-term constructor with std::map
+    JacobianFactor expected(
+      boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise);
+    map<Key,Matrix> mapTerms;
+    // note order of insertion plays no role: order will be determined by keys
+    mapTerms.insert(terms[2]);
+    mapTerms.insert(terms[1]);
+    mapTerms.insert(terms[0]);
+    JacobianFactor actual(mapTerms, b, noise);
+    EXPECT(assert_equal(expected, actual));
+    LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
+    EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
+    EXPECT(assert_equal(b, expected.getb()));
+    EXPECT(assert_equal(b, actual.getb()));
+    EXPECT(noise == expected.get_model());
+    EXPECT(noise == actual.get_model());
+  }
   {
     // VerticalBlockMatrix constructor
     JacobianFactor expected(

gtsam/navigation/MagFactor.h

@@ -37,7 +37,15 @@ class MagFactor: public NoiseModelFactor1<Rot2> {
 
 public:
 
-  /** Constructor */
+  /**
+   * Constructor of factor that estimates nav to body rotation bRn
+   * @param key of the unknown rotation bRn in the factor graph
+   * @param measured magnetometer reading, a 3-vector
+   * @param scale by which a unit vector is scaled to yield a magnetometer reading
+   * @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
+   * @param bias of the magnetometer, modeled as purely additive (after scaling)
+   * @param model of the additive Gaussian noise that is assumed
+   */
   MagFactor(Key key, const Point3& measured, double scale,
       const Unit3& direction, const Point3& bias,
       const SharedNoiseModel& model) :

gtsam/nonlinear/LinearContainerFactor.h

@@ -117,7 +117,7 @@ public:
 
   // casting syntactic sugar
 
-  inline bool hasLinearizationPoint() const { return linearizationPoint_; }
+  inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); }
 
   /**
    * Simple checks whether this is a Jacobian or Hessian factor

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -76,9 +76,6 @@ void NonlinearOptimizer::defaultOptimize() {
       !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
             params.errorTol, currentError, this->error(), params.verbosity));
 
-  if (params.verbosity >= NonlinearOptimizerParams::TERMINATION)
-    cout << "Number of iterations: " << this->iterations() << endl;
-
   // Printing if verbose
   if (params.verbosity >= NonlinearOptimizerParams::TERMINATION &&
       this->iterations() >= params.maxIterations)

gtsam/slam/SmartFactorBase.h

@@ -572,7 +572,7 @@ public:
 
     FastMap<Key,size_t> KeySlotMap;
     for (size_t slot=0; slot < allKeys.size(); slot++)
-      KeySlotMap.insert(std::make_pair<Key,size_t>(allKeys[slot],slot));
+      KeySlotMap.insert(std::make_pair(allKeys[slot],slot));
 
     // a single point is observed in numKeys cameras
     size_t numKeys = this->keys_.size(); // cameras observing current point

gtsam/slam/dataset.cpp

@@ -177,7 +177,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
   string tag;
 
   // load the poses
-  while (is) {
+  while (!is.eof()) {
     if (!(is >> tag))
       break;
 
@@ -213,7 +213,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
   // Parse the pose constraints
   int id1, id2;
   bool haveLandmark = false;
-  while (is) {
+  while (!is.eof()) {
     if (!(is >> tag))
       break;
 
@@ -461,12 +461,12 @@ GraphAndValues load3D(const string& filename) {
 
   ifstream is(filename.c_str());
   if (!is)
-    throw invalid_argument("load2D: can not find file " + filename);
+    throw invalid_argument("load3D: can not find file " + filename);
 
   Values::shared_ptr initial(new Values);
   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
 
-  while (is) {
+  while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
     istringstream ls(buf);
@@ -493,7 +493,7 @@ GraphAndValues load3D(const string& filename) {
   is.clear(); /* clears the end-of-file and error flags */
   is.seekg(0, ios::beg);
 
-  while (is) {
+  while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
     istringstream ls(buf);

gtsam/symbolic/tests/testSymbolicBayesTree.cpp

@@ -251,7 +251,7 @@ TEST( BayesTree, shortcutCheck )
   // Check if all the cached shortcuts are cleared
   rootClique->deleteCachedShortcuts();
   BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
-    bool notCleared = clique->cachedSeparatorMarginal();
+    bool notCleared = clique->cachedSeparatorMarginal().is_initialized();
     CHECK( notCleared == false);
   }
   EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals());

gtsam_unstable/gtsam_unstable.h

@@ -359,6 +359,11 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
   Vector calcIndicatorProb(const gtsam::Values& x);
   void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
 
+  void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
+  void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12);
+  Matrix get_model_inlier_cov();
+  Matrix get_model_outlier_cov();
+
   void serializable() const; // enabling serialization functionality
 };
 

gtsam_unstable/slam/BiasedGPSFactor.h

@@ -0,0 +1,105 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  BiasedGPSFactor.h
+ *  @author Luca Carlone
+ **/
+#pragma once
+
+#include <ostream>
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+  /**
+   * A class to model GPS measurements, including a bias term which models
+   * common-mode errors and that can be partially corrected if other sensors are used
+   * @addtogroup SLAM
+   */
+  class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
+
+  private:
+
+    typedef BiasedGPSFactor This;
+    typedef NoiseModelFactor2<Pose3, Point3> Base;
+
+    Point3 measured_; /** The measurement */
+
+  public:
+
+    // shorthand for a smart pointer to a factor
+    typedef typename boost::shared_ptr<BiasedGPSFactor> shared_ptr;
+
+    /** default constructor - only use for serialization */
+    BiasedGPSFactor() {}
+
+    /** Constructor */
+    BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured,
+        const SharedNoiseModel& model) :
+      Base(model, posekey, biaskey), measured_(measured) {
+    }
+
+    virtual ~BiasedGPSFactor() {}
+
+    /** implement functions needed for Testable */
+
+    /** print */
+    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      std::cout << s << "BiasedGPSFactor("
+          << keyFormatter(this->key1()) << ","
+          << keyFormatter(this->key2()) << ")\n";
+      measured_.print("  measured: ");
+      this->noiseModel_->print("  noise model: ");
+    }
+
+    /** equals */
+    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+      const This *e =  dynamic_cast<const This*> (&expected);
+      return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
+    }
+
+    /** implement functions needed to derive from Factor */
+
+    /** vector of errors */
+    Vector evaluateError(const Pose3& pose, const Point3& bias,
+        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+            boost::none) const {
+
+      if (H1 || H2){
+        H1->resize(3,6); // jacobian wrt pose
+        (*H1) << Matrix3::Zero(),  pose.rotation().matrix();
+        H2->resize(3,3); // jacobian wrt bias
+        (*H2) << Matrix3::Identity();
+      }
+      return pose.translation().vector() + bias.vector() - measured_.vector();
+    }
+
+    /** return the measured */
+    const Point3 measured() const {
+      return measured_;
+    }
+
+  private:
+
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template<class ARCHIVE>
+    void serialize(ARCHIVE & ar, const unsigned int version) {
+      ar & boost::serialization::make_nvp("NoiseModelFactor2",
+          boost::serialization::base_object<Base>(*this));
+      ar & BOOST_SERIALIZATION_NVP(measured_);
+    }
+  }; // \class BiasedGPSFactor
+
+} /// namespace gtsam

gtsam_unstable/slam/GaussMarkov1stOrderFactor.h

@@ -0,0 +1,135 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   GaussMarkov1stOrderFactor.h
+ *  @author Vadim Indelman, Stephen Williams, Luca Carlone
+ *  @date   Jan 17, 2012
+ **/
+#pragma once
+
+#include <ostream>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/NoiseModel.h>
+
+namespace gtsam {
+
+/*
+ * - The 1st order GaussMarkov factor relates two keys of the same type. This relation is given via
+ *      		key_2 = exp(-1/tau*delta_t) * key1 + w_d
+ * 		where tau is the time constant and delta_t is the time difference between the two keys.
+ * 		w_d is the equivalent discrete noise, whose covariance is calculated from the continuous noise model and delta_t.
+ * - w_d is approximated as a Gaussian noise.
+ * - In the multi-dimensional case, tau is a vector, and the above equation is applied on each element
+ *      in the state (represented by keys), using the appropriate time constant in the vector tau.
+ */
+
+/*
+ * A class for a measurement predicted by "GaussMarkov1stOrderFactor(config[key1],config[key2])"
+ * KEY1::Value is the Lie Group type
+ * T is the measurement type, by default the same
+ */
+template<class VALUE>
+class GaussMarkov1stOrderFactor: public NoiseModelFactor2<VALUE, VALUE> {
+
+private:
+
+  typedef GaussMarkov1stOrderFactor<VALUE> This;
+  typedef NoiseModelFactor2<VALUE, VALUE> Base;
+
+  double dt_;
+  Vector tau_;
+
+public:
+
+  // shorthand for a smart pointer to a factor
+  typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
+
+  /** default constructor - only use for serialization */
+  GaussMarkov1stOrderFactor() {}
+
+  /** Constructor */
+  GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau,
+      const SharedGaussian& model) :
+        Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) {
+  }
+
+  virtual ~GaussMarkov1stOrderFactor() {}
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    std::cout << s << "GaussMarkov1stOrderFactor("
+        << keyFormatter(this->key1()) << ","
+        << keyFormatter(this->key2()) << ")\n";
+    this->noiseModel_->print("  noise model");
+  }
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    const This *e =	dynamic_cast<const This*> (&expected);
+    return e != NULL && Base::equals(*e, tol);
+  }
+
+  /** implement functions needed to derive from Factor */
+
+  /** vector of errors */
+  Vector evaluateError(const VALUE& p1, const VALUE& p2,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const {
+
+    Vector v1( VALUE::Logmap(p1) );
+    Vector v2( VALUE::Logmap(p2) );
+
+    Vector alpha(tau_.size());
+    Vector alpha_v1(tau_.size());
+    for(int i=0; i<tau_.size(); i++){
+      alpha(i) = exp(- 1/tau_(i)*dt_ );
+      alpha_v1(i) = alpha(i) * v1(i);
+    }
+
+    Vector hx(v2 - alpha_v1);
+
+    if(H1) *H1 = - diag(alpha);
+    if(H2) *H2 = eye(v2.size());
+
+    return hx;
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+    ar & BOOST_SERIALIZATION_NVP(dt_);
+    ar & BOOST_SERIALIZATION_NVP(tau_);
+  }
+
+  SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){
+    /* Q_d (approx)= Q * delta_t */
+    /* In practice, square root of the information matrix is represented, so that:
+     *  R_d (approx)= R / sqrt(delta_t)
+     * */
+    noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
+    SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t)));
+    return model_d;
+  }
+
+}; // \class GaussMarkov1stOrderFactor
+
+} /// namespace gtsam

gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h

@@ -21,6 +21,8 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
@@ -302,6 +304,101 @@ namespace gtsam {
       return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
     }
 
+    /* ************************************************************************* */
+    SharedGaussian get_model_inlier() const {
+      return model_inlier_;
+    }
+
+    /* ************************************************************************* */
+    SharedGaussian get_model_outlier() const {
+      return model_outlier_;
+    }
+
+    /* ************************************************************************* */
+    Matrix get_model_inlier_cov() const {
+      return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+    }
+
+    /* ************************************************************************* */
+    Matrix get_model_outlier_cov() const {
+      return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+    }
+
+    /* ************************************************************************* */
+    void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) {
+      /* given marginals version, don't need to marginal multiple times if update a lot */
+
+      std::vector<gtsam::Key> Keys;
+      Keys.push_back(keyA_);
+      Keys.push_back(keyB_);
+      JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
+      Matrix cov1 = joint_marginal12(keyA_, keyA_);
+      Matrix cov2 = joint_marginal12(keyB_, keyB_);
+      Matrix cov12 = joint_marginal12(keyA_, keyB_);
+
+      updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
+    }
+
+    /* ************************************************************************* */
+    void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
+      /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
+       * (note these are given in the E step, where indicator probabilities are calculated).
+       *
+       * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
+       * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
+       *
+       * TODO: improve efficiency (info form)
+       */
+
+       // get joint covariance of the involved states
+
+       Marginals marginals(graph, values, Marginals::QR);
+
+       this->updateNoiseModels(values, marginals);
+    }
+
+    /* ************************************************************************* */
+    void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
+      /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
+       * (note these are given in the E step, where indicator probabilities are calculated).
+       *
+       * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
+       * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
+       *
+       * TODO: improve efficiency (info form)
+       */
+
+      const T& p1 = values.at<T>(keyA_);
+      const T& p2 = values.at<T>(keyB_);
+
+      Matrix H1, H2;
+      T hx = p1.between(p2, H1, H2); // h(x)
+
+      Matrix H;
+      H.resize(H1.rows(), H1.rows()+H2.rows());
+      H << H1, H2; // H = [H1 H2]
+
+      Matrix joint_cov;
+      joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
+      joint_cov << cov1, cov12,
+          cov12.transpose(), cov2;
+
+      Matrix cov_state = H*joint_cov*H.transpose();
+
+      //       model_inlier_->print("before:");
+
+      // update inlier and outlier noise models
+      Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+      model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
+
+      Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+      model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
+
+      //       model_inlier_->print("after:");
+      //       std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
+    }
+
+
     /* ************************************************************************* */
 
     /** number of variables attached to this factor */

gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp

@@ -0,0 +1,85 @@
+/**
+ * @file    testBiasedGPSFactor.cpp
+ * @brief  
+ * @author Luca Carlone
+ * @date   July 30, 2014
+ */
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam_unstable/slam/BiasedGPSFactor.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace gtsam;
+using namespace gtsam::symbol_shorthand;
+using namespace gtsam::noiseModel;
+// Convenience for named keys
+
+using symbol_shorthand::X;
+using symbol_shorthand::B;
+
+TEST(BiasedGPSFactor, errorNoiseless) {
+
+  Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
+  Point3 t(1.0, 0.5, 0.2);
+  Pose3 pose(R,t);
+  Point3 bias(0.0,0.0,0.0);
+  Point3 noise(0.0,0.0,0.0);
+  Point3 measured = t + noise;
+
+  BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
+  Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 );
+  Vector actualError = factor.evaluateError(pose,bias);
+  EXPECT(assert_equal(expectedError,actualError, 1E-5));
+}
+
+TEST(BiasedGPSFactor, errorNoisy) {
+
+  Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
+  Point3 t(1.0, 0.5, 0.2);
+  Pose3 pose(R,t);
+  Point3 bias(0.0,0.0,0.0);
+  Point3 noise(1.0,2.0,3.0);
+  Point3 measured = t - noise;
+
+  BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
+  Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 );
+  Vector actualError = factor.evaluateError(pose,bias);
+  EXPECT(assert_equal(expectedError,actualError, 1E-5));
+}
+
+TEST(BiasedGPSFactor, jacobian) {
+
+  Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
+  Point3 t(1.0, 0.5, 0.2);
+  Pose3 pose(R,t);
+  Point3 bias(0.0,0.0,0.0);
+
+  Point3 noise(0.0,0.0,0.0);
+  Point3 measured = t + noise;
+
+  BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
+
+  Matrix actualH1, actualH2;
+  factor.evaluateError(pose,bias, actualH1, actualH2);
+
+  Matrix numericalH1 = numericalDerivative21(
+      boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
+          &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
+          boost::none)), pose, bias, 1e-5);
+  EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
+
+  Matrix numericalH2 = numericalDerivative22(
+      boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
+          &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
+          boost::none)), pose, bias, 1e-5);
+  EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp

@@ -0,0 +1,122 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testGaussMarkov1stOrderFactor.cpp
+ * @brief   Unit tests for the GaussMarkov1stOrder factor
+ * @author  Vadim Indelman
+ * @date    Jan 17, 2012
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/base/numericalDerivative.h>
+
+using namespace std;
+using namespace gtsam;
+
+//! Factors
+typedef GaussMarkov1stOrderFactor<gtsam::LieVector> GaussMarkovFactor;
+
+/* ************************************************************************* */
+gtsam::LieVector predictionError(const gtsam::LieVector& v1, const gtsam::LieVector& v2, const GaussMarkovFactor factor) {
+  return factor.evaluateError(v1, v2);
+}
+
+/* ************************************************************************* */
+TEST( GaussMarkovFactor, equals )
+{
+  // Create two identical factors and make sure they're equal
+  gtsam::Key x1(1);
+  gtsam::Key x2(2);
+  double delta_t = 0.10;
+  Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
+  gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0);
+
+  GaussMarkovFactor factor1(x1, x2, delta_t, tau, model);
+  GaussMarkovFactor factor2(x1, x2, delta_t, tau, model);
+
+  CHECK(gtsam::assert_equal(factor1, factor2));
+}
+
+/* ************************************************************************* */
+TEST( GaussMarkovFactor, error )
+{
+  gtsam::Values linPoint;
+  gtsam::Key x1(1);
+  gtsam::Key x2(2);
+  double delta_t = 0.10;
+  Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
+  gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0);
+
+  gtsam::LieVector v1 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 12.0, 13.0));
+  gtsam::LieVector v2 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 15.0, 14.0));
+
+  // Create two nodes
+  linPoint.insert(x1, v1);
+  linPoint.insert(x2, v2);
+
+  GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
+  gtsam::Vector Err1( factor.evaluateError(v1, v2) );
+
+  // Manually calculate the error
+  Vector alpha(tau.size());
+  Vector alpha_v1(tau.size());
+  for(int i=0; i<tau.size(); i++){
+    alpha(i) = exp(- 1/tau(i)*delta_t );
+    alpha_v1(i) = alpha(i) * v1(i);
+  }
+  gtsam::Vector Err2( v2 - alpha_v1 );
+
+  CHECK(gtsam::assert_equal(Err1, Err2, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST (GaussMarkovFactor, jacobian ) {
+
+  gtsam::Values linPoint;
+  gtsam::Key x1(1);
+  gtsam::Key x2(2);
+  double delta_t = 0.10;
+  Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
+  gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0);
+
+  GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
+
+  // Update the linearization point
+  gtsam::LieVector v1_upd = gtsam::LieVector((gtsam::Vector(3) << 0.5, -0.7, 0.3));
+  gtsam::LieVector v2_upd = gtsam::LieVector((gtsam::Vector(3) << -0.7, 0.4, 0.9));
+
+  // Calculate the Jacobian matrix using the factor
+  Matrix computed_H1, computed_H2;
+  factor.evaluateError(v1_upd, v2_upd, computed_H1, computed_H2);
+
+  // Calculate the Jacobian matrices H1 and H2 using the numerical derivative function
+  gtsam::Matrix numerical_H1, numerical_H2;
+  numerical_H1 = gtsam::numericalDerivative21<gtsam::LieVector, gtsam::LieVector,
+      gtsam::LieVector>(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
+  numerical_H2 = gtsam::numericalDerivative22<gtsam::LieVector, gtsam::LieVector,
+      gtsam::LieVector>(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
+
+  // Verify they are equal for this choice of state
+  CHECK( gtsam::assert_equal(numerical_H1, computed_H1, 1e-9));
+  CHECK( gtsam::assert_equal(numerical_H2, computed_H2, 1e-9));
+}
+
+/* ************************************************************************* */
+int main()
+{
+  TestResult tr; return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp

@@ -49,12 +49,12 @@ using symbol_shorthand::T;
 typedef ProjectionFactorPPP<Pose3, Point3> TestProjectionFactor;
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, nonStandard ) {
+TEST( ProjectionFactorPPP, nonStandard ) {
   ProjectionFactorPPP<Pose3, Point3, Cal3DS2> f;
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Constructor) {
+TEST( ProjectionFactorPPP, Constructor) {
   Key poseKey(X(1));
   Key transformKey(T(1));
   Key pointKey(L(1));
@@ -65,7 +65,7 @@ TEST( ProjectionFactor, Constructor) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, ConstructorWithTransform) {
+TEST( ProjectionFactorPPP, ConstructorWithTransform) {
   Key poseKey(X(1));
   Key transformKey(T(1));
   Key pointKey(L(1));
@@ -75,7 +75,7 @@ TEST( ProjectionFactor, ConstructorWithTransform) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Equals ) {
+TEST( ProjectionFactorPPP, Equals ) {
   // Create two identical factors and make sure they're equal
   Point2 measurement(323.0, 240.0);
 
@@ -86,7 +86,7 @@ TEST( ProjectionFactor, Equals ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, EqualsWithTransform ) {
+TEST( ProjectionFactorPPP, EqualsWithTransform ) {
   // Create two identical factors and make sure they're equal
   Point2 measurement(323.0, 240.0);
   Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
@@ -98,7 +98,7 @@ TEST( ProjectionFactor, EqualsWithTransform ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Error ) {
+TEST( ProjectionFactorPPP, Error ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Key poseKey(X(1));
   Key transformKey(T(1));
@@ -121,7 +121,7 @@ TEST( ProjectionFactor, Error ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, ErrorWithTransform ) {
+TEST( ProjectionFactorPPP, ErrorWithTransform ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Key poseKey(X(1));
   Key transformKey(T(1));
@@ -145,7 +145,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Jacobian ) {
+TEST( ProjectionFactorPPP, Jacobian ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Key poseKey(X(1));
   Key transformKey(T(1));
@@ -179,7 +179,7 @@ TEST( ProjectionFactor, Jacobian ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, JacobianWithTransform ) {
+TEST( ProjectionFactorPPP, JacobianWithTransform ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Key poseKey(X(1));
   Key transformKey(T(1));

gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp

@@ -50,19 +50,19 @@ using symbol_shorthand::K;
 typedef ProjectionFactorPPPC<Pose3, Point3, Cal3_S2> TestProjectionFactor;
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, nonStandard ) {
+TEST( ProjectionFactorPPPC, nonStandard ) {
   ProjectionFactorPPPC<Pose3, Point3, Cal3DS2> f;
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Constructor) {
+TEST( ProjectionFactorPPPC, Constructor) {
   Point2 measurement(323.0, 240.0);
   TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
   // TODO: Actually check something
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Equals ) {
+TEST( ProjectionFactorPPPC, Equals ) {
   // Create two identical factors and make sure they're equal
   Point2 measurement(323.0, 240.0);
 
@@ -73,7 +73,7 @@ TEST( ProjectionFactor, Equals ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Error ) {
+TEST( ProjectionFactorPPPC, Error ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Point2 measurement(323.0, 240.0);
   TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
@@ -93,7 +93,7 @@ TEST( ProjectionFactor, Error ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, ErrorWithTransform ) {
+TEST( ProjectionFactorPPPC, ErrorWithTransform ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Point2 measurement(323.0, 240.0);
   Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
@@ -114,7 +114,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, Jacobian ) {
+TEST( ProjectionFactorPPPC, Jacobian ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Point2 measurement(323.0, 240.0);
   TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
@@ -149,7 +149,7 @@ TEST( ProjectionFactor, Jacobian ) {
 }
 
 /* ************************************************************************* */
-TEST( ProjectionFactor, JacobianWithTransform ) {
+TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
   // Create the factor with a measurement that is 3 pixels off in x
   Point2 measurement(323.0, 240.0);
   Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));

package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gtsam</name>
+  <version>3.1.0</version>
+  <description>gtsam</description>
+
+  <maintainer email="gtsam@lists.gatech.edu">Frank Dellaert</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>cmake</buildtool_depend>
+  <export>
+    <!-- Specify that this is not really a Catkin package-->
+    <build_type>cmake</build_type>
+  </export>
+</package>
+