Merge remote-tracking branch 'origin/develop' into feature/BAD

Conflicts:
	.cproject
	gtsam/geometry/Cal3DS2.cpp
	gtsam/geometry/Cal3DS2.h
	gtsam/geometry/Cal3Unified.h
	gtsam/navigation/CombinedImuFactor.h
	gtsam/navigation/ImuFactor.h
	gtsam/nonlinear/NonlinearFactor.h
	gtsam/slam/tests/testPoseRotationPrior.cpp

Modified: testGaussMarkov1stOrderFactor.cpp, testPoseRotationPrior.cpp

.cproject

@@ -848,18 +848,26 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGaussianFactorGraph.run</buildTarget>
+				<buildTarget>testGaussMarkov1stOrderFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianBayesNet.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGaussianBayesNet.run</buildTarget>
+				<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -2830,10 +2838,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
+				<buildTarget>testImplicitSchurFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>

.gitignore

@@ -3,4 +3,5 @@
 *.pyc
 *.DS_Store
 /examples/Data/dubrovnik-3-7-pre-rewritten.txt
-/examples/Data/pose2example-rewritten.txt
+/examples/Data/pose2example-rewritten.txt
+/examples/Data/pose3example-rewritten.txt

CMakeLists.txt

@@ -2,6 +2,12 @@
 project(GTSAM CXX C)
 cmake_minimum_required(VERSION 2.6)
 
+# new feature to Cmake Version > 2.8.12
+# Mac ONLY. Define Relative Path on Mac OS
+if(NOT DEFINED CMAKE_MACOSX_RPATH)
+  set(CMAKE_MACOSX_RPATH 0)
+endif()
+
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 3)
 set (GTSAM_VERSION_MINOR 1)

cmake/FindMKL.cmake

@@ -137,13 +137,16 @@ ELSE() # UNIX and macOS
                 ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
                 ${MKL_ROOT_DIR}/lib/
         )
-
+        
-        FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
+        # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
-          mkl_gnu_thread
+        IF(NOT APPLE)
-          PATHS
+            FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
-                ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
+              mkl_gnu_thread
-                ${MKL_ROOT_DIR}/lib/
+              PATHS
-        )
+                    ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
+                    ${MKL_ROOT_DIR}/lib/
+            )
+        ENDIF()
 
         # Intel Libraries
         IF("${MKL_ARCH_DIR}" STREQUAL "32")
@@ -227,7 +230,12 @@ ELSE() # UNIX and macOS
                 endforeach()
         endforeach()
 
-        SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
+        IF(APPLE)
+            SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES})
+        ELSE()
+            SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
+        ENDIF()
+        
         MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
                 MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
 ENDIF()

examples/Data/pose3example-rewritten.txt

@@ -1,11 +1,11 @@
 VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
 VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
-VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446
+VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
 VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024
 VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488
 EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
-EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
+EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
 EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
-EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592076 0.30338 -0.513225 0.542222 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
+EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.30338 -0.513226 0.542221 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
 EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
 EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000

examples/Data/simpleGraph10gradIter.txt

@@ -0,0 +1,11 @@
+VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816
+VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026
+VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610
+VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180
+VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502
+EDGE_SE3:QUAT 1 2   1.000000 2.000000 0.000000   0.0000000 0.0000000 0.7071068 0.7071068   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000   100.000000 0.000000   100.000000
+EDGE_SE3:QUAT 2 3   -0.000000 1.000000 0.000000   0.0000000 0.0000000 0.7071068 0.7071068   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000   100.000000 0.000000   100.000000
+EDGE_SE3:QUAT 3 4   1.000000 1.000000 0.000000   0.0000000 0.0000000 0.7071068 0.7071068   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000   100.000000 0.000000   100.000000
+EDGE_SE3:QUAT 3 1   0.000001 2.000000 0.000000   0.0000000 0.0000000 1.0000000 0.0000002   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000   100.000000 0.000000   100.000000
+EDGE_SE3:QUAT 1 4   -1.000000 1.000000 0.000000   0.0000000 0.0000000 -0.7071068 0.7071068   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000   100.000000 0.000000   100.000000
+EDGE_SE3:QUAT 0 1   0.000000 0.000000 0.000000   0.0000000 0.0000000 0.0000000 1.0000000   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000   100.000000 0.000000   100.000000

examples/Pose2SLAMExample_g2o.cpp

@@ -26,36 +26,72 @@
 using namespace std;
 using namespace gtsam;
 
+// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
 int main(const int argc, const char *argv[]) {
 
-  // Read graph from file
+  string kernelType = "none";
-  string g2oFile;
+  int maxIterations = 100; // default
-  if (argc < 2)
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
-    g2oFile = findExampleDataFile("noisyToyGraph.txt");
-  else
-    g2oFile = argv[1];
 
+  // Parse user's inputs
+  if (argc > 1){
+    g2oFile = argv[1]; // input dataset filename
+    // outputFile = g2oFile = argv[2]; // done later
+  }
+  if (argc > 3){
+    maxIterations = atoi(argv[3]); // user can specify either tukey or huber
+  }
+  if (argc > 4){
+    kernelType = argv[4]; // user can specify either tukey or huber
+  }
+
+  // reading file and creating factor graph
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
-  boost::tie(graph, initial) = readG2o(g2oFile);
+  bool is3D = false;
+  if(kernelType.compare("none") == 0){
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D);
+  }
+  if(kernelType.compare("huber") == 0){
+    std::cout << "Using robust kernel: huber " << std::endl;
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
+  }
+  if(kernelType.compare("tukey") == 0){
+    std::cout << "Using robust kernel: tukey " << std::endl;
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
+  }
 
   // Add prior on the pose having index (key) = 0
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  std::cout << "Adding prior on pose 0 " << std::endl;
+
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION");
+  if (argc > 3) {
+    params.maxIterations = maxIterations;
+    std::cout << "User required to perform maximum  " << params.maxIterations << " iterations "<< std::endl;
+  }
 
   std::cout << "Optimizing the factor graph" << std::endl;
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
+  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
+  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "final error=" <<graph->error(result)<< std::endl;
+
   if (argc < 3) {
     result.print("result");
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, result, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;

examples/Pose3SLAMExample_changeKeys.cpp

@@ -0,0 +1,89 @@
+/* ----------------------------------------------------------------------------
+
+ * 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_changeKeys input.g2o rewritted.g2o
+ * @date Aug 25, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.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);
+
+  bool add = false;
+  Key firstKey = 8646911284551352320;
+
+  std::cout << "Using reference key: " <<  firstKey  << std::endl;
+  if(add)
+    std::cout << "adding key "  << std::endl;
+  else
+    std::cout << "subtracting key "  << std::endl;
+
+
+  if (argc < 3) {
+    std::cout << "Please provide output file to write "   << std::endl;
+  } else {
+    const string inputFileRewritten = argv[2];
+    std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
+    // Additional: rewrite input with simplified keys 0,1,...
+    Values simpleInitial;
+    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+      Key key;
+      if(add)
+        key = key_value.key + firstKey;
+      else
+        key = key_value.key - firstKey;
+
+      simpleInitial.insert(key, initial->at(key_value.key));
+    }
+    NonlinearFactorGraph simpleGraph;
+    BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
+      boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+          boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+      if (pose3Between){
+        Key key1, key2;
+        if(add){
+          key1 = pose3Between->key1() + firstKey;
+          key2 = pose3Between->key2() + firstKey;
+        }else{
+          key1 = pose3Between->key1() - firstKey;
+          key2 = pose3Between->key2() - firstKey;
+        }
+        NonlinearFactor::shared_ptr simpleFactor(
+            new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
+        simpleGraph.add(simpleFactor);
+      }
+    }
+    writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
+  }
+  return 0;
+}

examples/Pose3SLAMExample_g2o.cpp

@@ -0,0 +1,74 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/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);
+
+  // Add prior on the first key
+  NonlinearFactorGraph graphWithPrior = *graph;
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+  Key firstKey = 0;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    break;
+  }
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
+  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
+  Values result = optimizer.optimize();
+  std::cout << "Optimization complete" << std::endl;
+
+  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "final error=" <<graph->error(result)<< std::endl;
+
+  if (argc < 3) {
+    result.print("result");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    writeG2o(*graph, result, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+  return 0;
+}

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 <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);
+
+  // Add prior on the first key
+  NonlinearFactorGraph graphWithPrior = *graph;
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+  Key firstKey = 0;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    break;
+  }
+
+  std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
+  Values initialization = InitializePose3::initialize(graphWithPrior);
+  std::cout << "done!" << std::endl;
+
+  if (argc < 3) {
+    initialization.print("initialization");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile  << std::endl;
+    writeG2o(*graph, initialization, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+  return 0;
+}

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 <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);
+
+  // Add prior on the first key
+  NonlinearFactorGraph graphWithPrior = *graph;
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+  Key firstKey = 0;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    break;
+  }
+
+  std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
+  bool useGradient = true;
+  Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
+  std::cout << "done!" << std::endl;
+
+  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
+
+  if (argc < 3) {
+    initialization.print("initialization");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile  << std::endl;
+    writeG2o(*graph, initialization, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+  return 0;
+}

gtsam/base/LieMatrix.h

@@ -157,7 +157,7 @@ struct LieMatrix : public Matrix {
         result.data(), p.rows(), p.cols()) = p;
     return result;
   }
-	
+  
   /// @}
 
 private:

gtsam/base/Value.h

@@ -36,9 +36,9 @@ namespace gtsam {
    * Values can operate generically on Value objects, retracting or computing
    * local coordinates for many Value objects of different types.
    *
-   * Inhereting from the DerivedValue class templated provides a generic implementation of
+   * Inheriting from the DerivedValue class templated provides a generic implementation of
    * the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating
-   * the need to implement these functions in your class. Note that you must inheret from 
+   * the need to implement these functions in your class. Note that you must inherit from
    * DerivedValue templated on the class you are defining. For example you cannot define
    * the following
    * \code

gtsam/geometry/Cal3DS2.cpp

@@ -23,24 +23,9 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-Cal3DS2::Cal3DS2(const Vector &v):
-    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
-
-/* ************************************************************************* */
-Matrix Cal3DS2::K() const {
-  return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
-}
-
-/* ************************************************************************* */
-Vector Cal3DS2::vector() const {
-  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
-}
-
 /* ************************************************************************* */
 void Cal3DS2::print(const std::string& s_) const {
-  gtsam::print(K(), s_ + ".K");
+  Base::print(s_);
-  gtsam::print(Vector(k()), s_ + ".k");
 }
 
 /* ************************************************************************* */
@@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
   return true;
 }
 
-/* ************************************************************************* */
-static Matrix29 D2dcalibration(double x, double y, double xx,
-    double yy, double xy, double rr, double r4, double pnx, double pny,
-    const Matrix2& DK) {
-  Matrix25 DR1;
-  DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
-  Matrix24 DR2;
-  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
-         y * rr, y * r4, rr + 2 * yy, 2 * xy;
-  Matrix29 D;
-  D << DR1, DK * DR2;
-  return D;
-}
-
-/* ************************************************************************* */
-static Matrix2 D2dintrinsic(double x, double y, double rr,
-    double g, double k1, double k2, double p1, double p2,
-    const Matrix2& DK) {
-  const double drdx = 2. * x;
-  const double drdy = 2. * y;
-  const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
-  const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
-
-  // Dx = 2*p1*xy + p2*(rr+2*xx);
-  // Dy = 2*p2*xy + p1*(rr+2*yy);
-  const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
-  const double dDxdy = 2. * p1 * x + p2 * drdy;
-  const double dDydx = 2. * p2 * y + p1 * drdx;
-  const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
-
-  Matrix2 DR;
-  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
-        y * dgdx + dDydx, g + y * dgdy + dDydy;
-
-  return DK * DR;
-}
-
-/* ************************************************************************* */
-Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
-
-  //  rr = x^2 + y^2;
-  //  g = (1 + k(1)*rr + k(2)*rr^2);
-  //  dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
-  //  pi(:,i) = g * pn(:,i) + dp;
-  const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
-  const double rr = xx + yy;
-  const double r4 = rr * rr;
-  const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
-
-  // tangential component
-  const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
-  const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
-
-  // Radial and tangential distortion applied
-  const double pnx = g * x + dx;
-  const double pny = g * y + dy;
-
-  Matrix2 DK;
-  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
-
-  // Derivative for calibration
-  if (H1)
-    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
-
-  // Derivative for points
-  if (H2)
-    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
-
-  // Regular uncalibrate after distortion
-  return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
-}
-
-/* ************************************************************************* */
-Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
-  // Use the following fixed point iteration to invert the radial distortion.
-  // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
-
-  const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
-                       (1 / fy_) * (pi.y() - v0_));
-
-  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
-  Point2 pn = invKPi;
-
-  // iterate until the uncalibrate is close to the actual pixel coordinate
-  const int maxIterations = 10;
-  int iteration;
-  for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (uncalibrate(pn).distance(pi) <= tol) break;
-    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
-    const double rr = xx + yy;
-    const double g = (1 + k1_ * rr + k2_ * rr * rr);
-    const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
-    const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
-    pn = (invKPi - Point2(dx, dy)) / g;
-  }
-
-  if ( iteration >= maxIterations )
-    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
-
-  return pn;
-}
-
-/* ************************************************************************* */
-Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
-  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
-  const double rr = xx + yy;
-  const double r4 = rr * rr;
-  const double g = (1 + k1_ * rr + k2_ * r4);
-  Matrix2 DK;
-  DK << fx_, s_, 0.0, fy_;
-  return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
-}
-
-/* ************************************************************************* */
-Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
-  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
-  const double rr = xx + yy;
-  const double r4 = rr * rr;
-  const double g = (1 + k1_ * rr + k2_ * r4);
-  const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
-  const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
-  const double pnx = g * x + dx;
-  const double pny = g * y + dy;
-  Matrix2 DK;
-  DK << fx_, s_, 0.0, fy_;
-  return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
-}
-
 /* ************************************************************************* */
 Cal3DS2 Cal3DS2::retract(const Vector& d) const {
   return Cal3DS2(vector() + d);

gtsam/geometry/Cal3DS2.h

@@ -11,7 +11,7 @@
 
 /**
  * @file Cal3DS2.h
- * @brief Calibration of a camera with radial distortion
+ * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
  */
@@ -19,7 +19,7 @@
 #pragma once
 
 #include <gtsam/base/DerivedValue.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
 
 namespace gtsam {
 
@@ -37,29 +37,21 @@ namespace gtsam {
  *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
-class GTSAM_EXPORT Cal3DS2 {
+class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
 
-protected:
+  typedef Cal3DS2_Base Base;
-
-  double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
-  double k1_, k2_ ; // radial 2nd-order and 4th-order
-  double p1_, p2_ ; // tangential distortion
 
 public:
 
-  Matrix K() const ;
-  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
-  Vector vector() const ;
-
   /// @name Standard Constructors
   /// @{
 
   /// Default Constructor with only unit focal length
-  Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
+  Cal3DS2() : Base() {}
 
   Cal3DS2(double fx, double fy, double s, double u0, double v0,
       double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
-  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
+        Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
 
   virtual ~Cal3DS2() {}
 
@@ -67,7 +59,7 @@ public:
   /// @name Advanced Constructors
   /// @{
 
-  Cal3DS2(const Vector &v) ;
+  Cal3DS2(const Vector &v) : Base(v) {}
 
   /// @}
   /// @name Testable
@@ -79,57 +71,6 @@ public:
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2& K, double tol = 10e-9) const;
 
-  /// @}
-  /// @name Standard Interface
-  /// @{
-
-  /// focal length x
-  inline double fx() const { return fx_;}
-
-  /// focal length x
-  inline double fy() const { return fy_;}
-
-  /// skew
-  inline double skew() const { return s_;}
-
-  /// image center in x
-  inline double px() const { return u0_;}
-
-  /// image center in y
-  inline double py() const { return v0_;}
-
-  /// First distortion coefficient
-  inline double k1() const { return k1_;}
-
-  /// Second distortion coefficient
-  inline double k2() const { return k2_;}
-
-  /// First tangential distortion coefficient
-  inline double p1() const { return p1_;}
-
-  /// Second tangential distortion coefficient
-  inline double p2() const { return p2_;}
-
-  /**
-   * convert intrinsic coordinates xy to (distorted) image coordinates uv
-   * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in (distorted) image coordinates
-   */
-  Point2 uncalibrate(const Point2& p,
-      boost::optional<Matrix&> Dcal = boost::none,
-      boost::optional<Matrix&> Dp = boost::none) const ;
-
-  /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
-  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
-
-  /// Derivative of uncalibrate wrpt intrinsic coordinates
-  Matrix D2d_intrinsic(const Point2& p) const ;
-
-  /// Derivative of uncalibrate wrpt the calibration parameters
-  Matrix D2d_calibration(const Point2& p) const ;
-
   /// @}
   /// @name Manifold
   /// @{
@@ -155,15 +96,10 @@ private:
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
-    ar & BOOST_SERIALIZATION_NVP(fx_);
+    ar & boost::serialization::make_nvp("Cal3DS2",
-    ar & BOOST_SERIALIZATION_NVP(fy_);
+        boost::serialization::base_object<Value>(*this));
-    ar & BOOST_SERIALIZATION_NVP(s_);
+    ar & boost::serialization::make_nvp("Cal3DS2",
-    ar & BOOST_SERIALIZATION_NVP(u0_);
+        boost::serialization::base_object<Cal3DS2_Base>(*this));
-    ar & BOOST_SERIALIZATION_NVP(v0_);
-    ar & BOOST_SERIALIZATION_NVP(k1_);
-    ar & BOOST_SERIALIZATION_NVP(k2_);
-    ar & BOOST_SERIALIZATION_NVP(p1_);
-    ar & BOOST_SERIALIZATION_NVP(p2_);
   }
 
 };

gtsam/geometry/Cal3DS2_Base.cpp

@@ -0,0 +1,187 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Cal3DS2_Base.cpp
+ * @date Feb 28, 2010
+ * @author ydjian
+ */
+
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
+    fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
+
+/* ************************************************************************* */
+Matrix Cal3DS2_Base::K() const {
+  return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
+}
+
+/* ************************************************************************* */
+Vector Cal3DS2_Base::vector() const {
+  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
+}
+
+/* ************************************************************************* */
+void Cal3DS2_Base::print(const std::string& s_) const {
+  gtsam::print(K(), s_ + ".K");
+  gtsam::print(Vector(k()), s_ + ".k");
+}
+
+/* ************************************************************************* */
+bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
+  if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
+      fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
+      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
+    return false;
+  return true;
+}
+
+/* ************************************************************************* */
+static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
+    double yy, double xy, double rr, double r4, double pnx, double pny,
+    const Eigen::Matrix<double, 2, 2>& DK) {
+  Eigen::Matrix<double, 2, 5> DR1;
+  DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
+  Eigen::Matrix<double, 2, 4> DR2;
+  DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
+         y * rr, y * r4, rr + 2 * yy, 2 * xy;
+  Eigen::Matrix<double, 2, 9> D;
+  D << DR1, DK * DR2;
+  return D;
+}
+
+/* ************************************************************************* */
+static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
+    double g, double k1, double k2, double p1, double p2,
+    const Eigen::Matrix<double, 2, 2>& DK) {
+  const double drdx = 2. * x;
+  const double drdy = 2. * y;
+  const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
+  const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
+
+  // Dx = 2*p1*xy + p2*(rr+2*xx);
+  // Dy = 2*p2*xy + p1*(rr+2*yy);
+  const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
+  const double dDxdy = 2. * p1 * x + p2 * drdy;
+  const double dDydx = 2. * p2 * y + p1 * drdx;
+  const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
+
+  Eigen::Matrix<double, 2, 2> DR;
+  DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
+        y * dgdx + dDydx, g + y * dgdy + dDydy;
+
+  return DK * DR;
+}
+
+/* ************************************************************************* */
+Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
+    boost::optional<Matrix&> H2) const {
+
+  //  rr = x^2 + y^2;
+  //  g = (1 + k(1)*rr + k(2)*rr^2);
+  //  dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
+  //  pi(:,i) = g * pn(:,i) + dp;
+  const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
+  const double rr = xx + yy;
+  const double r4 = rr * rr;
+  const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
+
+  // tangential component
+  const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
+  const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
+
+  // Radial and tangential distortion applied
+  const double pnx = g * x + dx;
+  const double pny = g * y + dy;
+
+  Eigen::Matrix<double, 2, 2> DK;
+  if (H1 || H2) DK << fx_, s_, 0.0, fy_;
+
+  // Derivative for calibration
+  if (H1)
+    *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+
+  // Derivative for points
+  if (H2)
+    *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+
+  // Regular uncalibrate after distortion
+  return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
+}
+
+/* ************************************************************************* */
+Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
+  // Use the following fixed point iteration to invert the radial distortion.
+  // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
+
+  const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
+                       (1 / fy_) * (pi.y() - v0_));
+
+  // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
+  Point2 pn = invKPi;
+
+  // iterate until the uncalibrate is close to the actual pixel coordinate
+  const int maxIterations = 10;
+  int iteration;
+  for (iteration = 0; iteration < maxIterations; ++iteration) {
+    if (uncalibrate(pn).distance(pi) <= tol) break;
+    const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
+    const double rr = xx + yy;
+    const double g = (1 + k1_ * rr + k2_ * rr * rr);
+    const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
+    const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
+    pn = (invKPi - Point2(dx, dy)) / g;
+  }
+
+  if ( iteration >= maxIterations )
+    throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
+
+  return pn;
+}
+
+/* ************************************************************************* */
+Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
+  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
+  const double rr = xx + yy;
+  const double r4 = rr * rr;
+  const double g = (1 + k1_ * rr + k2_ * r4);
+  Eigen::Matrix<double, 2, 2> DK;
+  DK << fx_, s_, 0.0, fy_;
+  return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
+}
+
+/* ************************************************************************* */
+Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
+  const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
+  const double rr = xx + yy;
+  const double r4 = rr * rr;
+  const double g = (1 + k1_ * rr + k2_ * r4);
+  const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
+  const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
+  const double pnx = g * x + dx;
+  const double pny = g * y + dy;
+  Eigen::Matrix<double, 2, 2> DK;
+  DK << fx_, s_, 0.0, fy_;
+  return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
+}
+
+}
+/* ************************************************************************* */
+
+

gtsam/geometry/Cal3DS2_Base.h

@@ -0,0 +1,158 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Cal3DS2.h
+ * @brief Calibration of a camera with radial distortion
+ * @date Feb 28, 2010
+ * @author ydjian
+ */
+
+#pragma once
+
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/geometry/Point2.h>
+
+namespace gtsam {
+
+/**
+ * @brief Calibration of a camera with radial distortion
+ * @addtogroup geometry
+ * \nosubgrouping
+ *
+ * Uses same distortionmodel as OpenCV, with
+ * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+ * but using only k1,k2,p1, and p2 coefficients.
+ * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
+ * rr = Pn.x^2 + Pn.y^2
+ * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
+ * pi = K*pn
+ */
+class GTSAM_EXPORT Cal3DS2_Base {
+
+protected:
+
+  double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
+  double k1_, k2_ ; // radial 2nd-order and 4th-order
+  double p1_, p2_ ; // tangential distortion
+
+public:
+  Matrix K() const ;
+  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
+  Vector vector() const ;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /// Default Constructor with only unit focal length
+  Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
+
+  Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
+      double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
+  fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  Cal3DS2_Base(const Vector &v) ;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// print with optional string
+  void print(const std::string& s = "") const ;
+
+  /// assert equality up to a tolerance
+  bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// focal length x
+  inline double fx() const { return fx_;}
+
+  /// focal length x
+  inline double fy() const { return fy_;}
+
+  /// skew
+  inline double skew() const { return s_;}
+
+  /// image center in x
+  inline double px() const { return u0_;}
+
+  /// image center in y
+  inline double py() const { return v0_;}
+
+  /// First distortion coefficient
+  inline double k1() const { return k1_;}
+
+  /// Second distortion coefficient
+  inline double k2() const { return k2_;}
+
+  /// First tangential distortion coefficient
+  inline double p1() const { return p1_;}
+
+  /// Second tangential distortion coefficient
+  inline double p2() const { return p2_;}
+
+  /**
+   * convert intrinsic coordinates xy to (distorted) image coordinates uv
+   * @param p point in intrinsic coordinates
+   * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
+   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
+   * @return point in (distorted) image coordinates
+   */
+  Point2 uncalibrate(const Point2& p,
+      boost::optional<Matrix&> Dcal = boost::none,
+      boost::optional<Matrix&> Dp = boost::none) const ;
+
+  /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
+  Point2 calibrate(const Point2& p, const double tol=1e-5) const;
+
+  /// Derivative of uncalibrate wrpt intrinsic coordinates
+  Matrix D2d_intrinsic(const Point2& p) const ;
+
+  /// Derivative of uncalibrate wrpt the calibration parameters
+  Matrix D2d_calibration(const Point2& p) const ;
+
+
+private:
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version)
+  {
+    ar & BOOST_SERIALIZATION_NVP(fx_);
+    ar & BOOST_SERIALIZATION_NVP(fy_);
+    ar & BOOST_SERIALIZATION_NVP(s_);
+    ar & BOOST_SERIALIZATION_NVP(u0_);
+    ar & BOOST_SERIALIZATION_NVP(v0_);
+    ar & BOOST_SERIALIZATION_NVP(k1_);
+    ar & BOOST_SERIALIZATION_NVP(k2_);
+    ar & BOOST_SERIALIZATION_NVP(p1_);
+    ar & BOOST_SERIALIZATION_NVP(p2_);
+  }
+
+  /// @}
+
+};
+
+}
+

gtsam/geometry/Cal3Unified.h

@@ -22,8 +22,8 @@
 
 #pragma once
 
-#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/base/DerivedValue.h>
 
 namespace gtsam {
 
@@ -40,20 +40,18 @@ namespace gtsam {
  *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
-class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
+class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
 
   typedef Cal3Unified This;
-  typedef Cal3DS2 Base;
+  typedef Cal3DS2_Base Base;
 
 private:
 
   double xi_;  // mirror parameter
 
 public:
-  /// dimension of the variable - used to autodetect sizes
-  static const size_t dimension = 10;
 
-  Vector vector() const ;
+    Vector vector() const ;
 
   /// @name Standard Constructors
   /// @{
@@ -91,7 +89,7 @@ public:
   /**
    * convert intrinsic coordinates xy to image coordinates uv
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
+   * @param Dcal optional 2*10 Jacobian wrpt Cal3Unified parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
@@ -131,6 +129,10 @@ private:
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
+    ar & boost::serialization::make_nvp("Cal3Unified",
+        boost::serialization::base_object<Value>(*this));
+    ar & boost::serialization::make_nvp("Cal3Unified",
+        boost::serialization::base_object<Cal3DS2_Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(xi_);
   }
 

gtsam/geometry/Unit3.cpp

@@ -21,7 +21,16 @@
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
 #include <boost/random/mersenne_twister.hpp>
+
+#ifdef __clang__
+#  pragma clang diagnostic push
+#  pragma clang diagnostic ignored "-Wunused-variable"
+#endif
 #include <boost/random/uniform_on_sphere.hpp>
+#ifdef __clang__
+#  pragma clang diagnostic pop
+#endif
+
 #include <boost/random/variate_generator.hpp>
 #include <iostream>
 
@@ -58,11 +67,11 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
 }
 
 /* ************************************************************************* */
-const Matrix& Unit3::basis() const {
+const Unit3::Matrix32& Unit3::basis() const {
 
   // Return cached version if exists
-  if (B_.rows() == 3)
+  if (B_)
-    return B_;
+    return *B_;
 
   // Get the axis of rotation with the minimum projected length of the point
   Point3 axis;
@@ -83,9 +92,9 @@ const Matrix& Unit3::basis() const {
   b2 = b2 / b2.norm();
 
   // Create the basis matrix
-  B_ = Matrix(3, 2);
+  B_.reset(Unit3::Matrix32());
-  B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
+  (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
-  return B_;
+  return *B_;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Unit3.h

@@ -23,6 +23,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/DerivedValue.h>
 #include <boost/random/mersenne_twister.hpp>
+#include <boost/optional.hpp>
 
 namespace gtsam {
 
@@ -31,8 +32,10 @@ class GTSAM_EXPORT Unit3{
 
 private:
 
+  typedef Eigen::Matrix<double,3,2> Matrix32;
+
   Point3 p_; ///< The location of the point on the unit sphere
-  mutable Matrix B_; ///< Cached basis
+  mutable boost::optional<Matrix32> B_; ///< Cached basis
 
 public:
 
@@ -84,7 +87,7 @@ public:
    * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
    * tangent to the sphere at the current direction.
    */
-  const Matrix& basis() const;
+  const Matrix32& basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
   Matrix skew() const;

gtsam/geometry/tests/testCal3Unified.cpp

@@ -19,6 +19,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unified.h>
 
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Key.h>
+
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified)
@@ -97,6 +100,19 @@ TEST( Cal3Unified, retract)
   CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
 }
 
+/* ************************************************************************* */
+TEST( Cal3Unified, DerivedValue)
+{
+  Values values;
+  Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
+  Key key = 1;
+  values.insert(key, cal);
+
+  Cal3Unified calafter = values.at<Cal3Unified>(key);
+
+  CHECK(assert_equal(cal,calafter,1e-9));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject)
 }
 
 /* ************************************************************************* */
-//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
+static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
-//  Point3 point(point2D.x(), point2D.y(), 1.0);
+  return Camera(pose,cal).projectPointAtInfinity(point3D);
-//  return Camera(pose,cal).projectPointAtInfinity(point);
+}
-//}
+
-//
+TEST( PinholeCamera, Dproject_Infinity)
-//TEST( PinholeCamera, Dproject_Infinity)
+{
-//{
+  Matrix Dpose, Dpoint, Dcal;
-//  Matrix Dpose, Dpoint, Dcal;
+  Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
-//  Point2 point2D(-0.08,-0.08);
+
-//  Point3 point3D(point1.x(), point1.y(), 1.0);
+  // test Projection
-//  Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
+  Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
-//  Matrix numerical_pose  = numericalDerivative31(projectInfinity3, pose1, point2D, K);
+  Point2 expected(-5.0, 5.0);
-//  Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K);
+  CHECK(assert_equal(actual, expected,  1e-7));
-//  Matrix numerical_cal   = numericalDerivative33(projectInfinity3, pose1, point2D, K);
+
-//  CHECK(assert_equal(numerical_pose,  Dpose,  1e-7));
+  // test Jacobians
-//  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+  Matrix numerical_pose     = numericalDerivative31(projectInfinity3, pose1, point3D, K);
-//  CHECK(assert_equal(numerical_cal,   Dcal,   1e-7));
+  Matrix numerical_point    = numericalDerivative32(projectInfinity3, pose1, point3D, K);
-//}
+  Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
-//
+  Matrix numerical_cal      = numericalDerivative33(projectInfinity3, pose1, point3D, K);
+  CHECK(assert_equal(numerical_pose,     Dpose,  1e-7));
+  CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
+  CHECK(assert_equal(numerical_cal,      Dcal,   1e-7));
+}
+
 /* ************************************************************************* */
 static Point2 project4(const Camera& camera, const Point3& point) {
   return camera.project2(point);

gtsam/geometry/tests/testRot3.cpp

@@ -184,7 +184,15 @@ TEST(Rot3, log)
   CHECK_OMEGA(  PI,   0,   0)
   CHECK_OMEGA(   0,  PI,   0)
   CHECK_OMEGA(   0,   0,  PI)
+
+  // Windows and Linux have flipped sign in quaternion mode
+#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
+  w = (Vector(3) << x*PI, y*PI, z*PI); 
+  R = Rot3::rodriguez(w); 
+  EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
+#else
   CHECK_OMEGA(x*PI,y*PI,z*PI)
+#endif
 
   // Check 360 degree rotations
 #define CHECK_OMEGA_ZERO(X,Y,Z) \

gtsam/geometry/tests/testRot3M.cpp

@@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
 
 static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
 static Point3 P(0.2, 0.7, -2.0);
-static double error = 1e-9, epsilon = 0.001;
 static const Matrix I3 = eye(3);
 
 /* ************************************************************************* */

gtsam/geometry/tests/testSerializationGeometry.cpp

@@ -25,6 +25,7 @@
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
 
@@ -46,6 +47,7 @@ static Cal3Bundler cal3(1.0, 2.0, 3.0);
 static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
 static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
 static CalibratedCamera cal5(Pose3(rt3, pt3));
+static Cal3Unified cal6(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0);
 
 static PinholeCamera<Cal3_S2> cam1(pose3, cal1);
 static StereoCamera cam2(pose3, cal4ptr);
@@ -66,6 +68,7 @@ TEST (Serialization, text_geometry) {
   EXPECT(equalsObj(cal3));
   EXPECT(equalsObj(cal4));
   EXPECT(equalsObj(cal5));
+  EXPECT(equalsObj(cal6));
 
   EXPECT(equalsObj(cam1));
   EXPECT(equalsObj(cam2));

gtsam/geometry/triangulation.h

@@ -174,7 +174,7 @@ Point3 triangulateNonlinear(
  * @param poses A vector of camera poses
  * @param sharedCal shared pointer to single calibration object
  * @param measurements A vector of camera measurements
- * @param rank tolerance, default 1e-9
+ * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
  * @return Returns a Point3
  */
@@ -222,7 +222,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
  * no other checks to verify the quality of the triangulation.
  * @param cameras pinhole cameras
  * @param measurements A vector of camera measurements
- * @param rank tolerance, default 1e-9
+ * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
  * @return Returns a Point3
  */

gtsam/linear/GaussianConditional.h

@@ -111,7 +111,7 @@ namespace gtsam {
     * assumed to have already been solved in and their values are read from \c x.
     * This function works for multiple frontal variables.
     *
-    * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
+    * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2 \f$,
     * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
     * variables of this conditional, this solve function computes
     * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.

gtsam/linear/GaussianFactorGraph.cpp

@@ -85,7 +85,7 @@ namespace gtsam {
       dims_accumulated.resize(dims.size()+1,0);
       dims_accumulated[0]=0;
       for (size_t i=1; i<dims_accumulated.size(); i++)
-    	  dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
+        dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
       return dims_accumulated;
     }
 
@@ -358,8 +358,8 @@ namespace gtsam {
   /* ************************************************************************* */
   void GaussianFactorGraph::multiplyHessianAdd(double alpha,
       const double* x, double* y) const {
-	vector<size_t> FactorKeys = getkeydim();
+  vector<size_t> FactorKeys = getkeydim();
-	BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
       f->multiplyHessianAdd(alpha, x, y, FactorKeys);
 
   }

gtsam/linear/HessianFactor.cpp

@@ -538,7 +538,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
 
   // copy to yvalues
   for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) {
-	bool didNotExist;
+  bool didNotExist;
     VectorValues::iterator it;
     boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
     if (didNotExist)

gtsam/navigation/CombinedImuFactor.h

@@ -344,12 +344,12 @@ namespace gtsam {
 
     /** Constructor */
     CombinedImuFactor(
-    	Key pose_i, ///< previous pose key
+      Key pose_i, ///< previous pose key
-    	Key vel_i, ///< previous velocity key
+      Key vel_i, ///< previous velocity key
-    	Key pose_j, ///< current pose key
+      Key pose_j, ///< current pose key
-    	Key vel_j, ///< current velocity key
+      Key vel_j, ///< current velocity key
-    	Key bias_i, ///< previous bias key
+      Key bias_i, ///< previous bias key
-    	Key bias_j, ///< current bias key
+      Key bias_j, ///< current bias key
         const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
         const Vector3& gravity, ///< gravity vector
         const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
@@ -479,33 +479,33 @@ namespace gtsam {
         Matrix3 dfPdPi;
         Matrix3 dfVdPi;
         if(use2ndOrderCoriolis_){
-        	dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
+          dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-        	dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
+          dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
         }
         else{
-        	dfPdPi = - Rot_i.matrix();
+          dfPdPi = - Rot_i.matrix();
-        	dfVdPi = Matrix3::Zero();
+          dfVdPi = Matrix3::Zero();
         }
 
-		(*H1) <<
+    (*H1) <<
-			// dfP/dRi
+      // dfP/dRi
-			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
-				+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
+        + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
-			// dfP/dPi
+      // dfP/dPi
-			dfPdPi,
+      dfPdPi,
-			// dfV/dRi
+      // dfV/dRi
-			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
-				+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
+        + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
-			// dfV/dPi
+      // dfV/dPi
-			dfVdPi,
+      dfVdPi,
-			// dfR/dRi
+      // dfR/dRi
-			Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+      Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-			// dfR/dPi
+      // dfR/dPi
-			Matrix3::Zero(),
+      Matrix3::Zero(),
-			//dBiasAcc/dPi
+      //dBiasAcc/dPi
-			Matrix3::Zero(), Matrix3::Zero(),
+      Matrix3::Zero(), Matrix3::Zero(),
-			//dBiasOmega/dPi
+      //dBiasOmega/dPi
-			Matrix3::Zero(), Matrix3::Zero();
+      Matrix3::Zero(), Matrix3::Zero();
       }
 
       if(H2) {
@@ -516,13 +516,13 @@ namespace gtsam {
             + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
             // dfV/dVi
             - Matrix3::Identity()
-			+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
+      + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-			// dfR/dVi
+      // dfR/dVi
-			Matrix3::Zero(),
+      Matrix3::Zero(),
-			//dBiasAcc/dVi
+      //dBiasAcc/dVi
-			Matrix3::Zero(),
+      Matrix3::Zero(),
-			//dBiasOmega/dVi
+      //dBiasOmega/dVi
-			Matrix3::Zero();
+      Matrix3::Zero();
       }
 
       if(H3) {
@@ -642,21 +642,21 @@ namespace gtsam {
       // Predict state at time j
       /* ---------------------------------------------------------------------------------------------------- */
        Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
-		  + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+      + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
-		  + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+      + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
-		  + vel_i * deltaTij
+      + vel_i * deltaTij
-		  - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-		  + 0.5 * gravity * deltaTij*deltaTij;
+      + 0.5 * gravity * deltaTij*deltaTij;
 
-	  vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+    vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
-		  + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+      + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
-		  + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
+      + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
-		  - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-		  + gravity * deltaTij);
+      + gravity * deltaTij);
 
       if(use2ndOrderCoriolis){
-    	  pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;	// 2nd order coriolis term for position
+        pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-    	  vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+        vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
       }
 
       const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);

gtsam/navigation/ImuFactor.h

@@ -307,11 +307,11 @@ namespace gtsam {
 
     /** Constructor */
     ImuFactor(
-    	Key pose_i, ///< previous pose key
+      Key pose_i, ///< previous pose key
-    	Key vel_i, ///< previous velocity key
+      Key vel_i, ///< previous velocity key
-    	Key pose_j, ///< current pose key
+      Key pose_j, ///< current pose key
-    	Key vel_j, ///< current velocity key
+      Key vel_j, ///< current velocity key
-    	Key bias, ///< previous bias key
+      Key bias, ///< previous bias key
         const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
         const Vector3& gravity, ///< gravity vector
         const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
@@ -418,29 +418,29 @@ namespace gtsam {
         Matrix3 dfPdPi;
         Matrix3 dfVdPi;
         if(use2ndOrderCoriolis_){
-        	dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
+          dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-        	dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
+          dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
         }
         else{
-        	dfPdPi = - Rot_i.matrix();
+          dfPdPi = - Rot_i.matrix();
-        	dfVdPi = Matrix3::Zero();
+          dfVdPi = Matrix3::Zero();
         }
 
-		(*H1) <<
+    (*H1) <<
-			// dfP/dRi
+      // dfP/dRi
-			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
-				+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
+        + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
-			// dfP/dPi
+      // dfP/dPi
-			dfPdPi,
+      dfPdPi,
-			// dfV/dRi
+      // dfV/dRi
-			Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
-				+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
+        + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
-			// dfV/dPi
+      // dfV/dPi
-			dfVdPi,
+      dfVdPi,
-			// dfR/dRi
+      // dfR/dRi
-			Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+      Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-			// dfR/dPi
+      // dfR/dPi
-			Matrix3::Zero();
+      Matrix3::Zero();
       }
 
       if(H2) {
@@ -539,22 +539,22 @@ namespace gtsam {
 
       // Predict state at time j
       /* ---------------------------------------------------------------------------------------------------- */
-	  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+    Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
-		  + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+      + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
-		  + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+      + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
-		  + vel_i * deltaTij
+      + vel_i * deltaTij
-		  - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-		  + 0.5 * gravity * deltaTij*deltaTij;
+      + 0.5 * gravity * deltaTij*deltaTij;
 
-	  vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+    vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
-		  + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+      + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
-		  + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
+      + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
-		  - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-		  + gravity * deltaTij);
+      + gravity * deltaTij);
 
       if(use2ndOrderCoriolis){
-    	  pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;	// 2nd order coriolis term for position
+        pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-    	  vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+        vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
       }
 
       const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -176,11 +176,11 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
     stm << "];\n";
 
     if (firstTimePoses) {
-    	lastKey = key;
+      lastKey = key;
-    	firstTimePoses = false;
+      firstTimePoses = false;
     } else {
-    	stm << "  var" << key << "--" << "var" << lastKey << ";\n";
+      stm << "  var" << key << "--" << "var" << lastKey << ";\n";
-    	lastKey = key;
+      lastKey = key;
     }
   }
   stm << "\n";
@@ -219,37 +219,37 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
     // Create factors and variable connections
     for(size_t i = 0; i < this->size(); ++i) {
       if(graphvizFormatting.plotFactorPoints){
-		// Make each factor a dot
+    // Make each factor a dot
-		stm << "  factor" << i << "[label=\"\", shape=point";
+    stm << "  factor" << i << "[label=\"\", shape=point";
-		{
+    {
-		map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
+    map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
-		if(pos != graphvizFormatting.factorPositions.end())
+    if(pos != graphvizFormatting.factorPositions.end())
-		  stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
+      stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
-		}
+    }
-		stm << "];\n";
+    stm << "];\n";
 
-		// Make factor-variable connections
+    // Make factor-variable connections
-		if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
+    if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
-		  BOOST_FOREACH(Key key, *this->at(i)) {
+      BOOST_FOREACH(Key key, *this->at(i)) {
-		  stm << "  var" << key << "--" << "factor" << i << ";\n";
+      stm << "  var" << key << "--" << "factor" << i << ";\n";
-		  }
+      }
-		}
+    }
 
-	  }
+    }
       else {
-    	if(this->at(i)) {
+      if(this->at(i)) {
-    	  Key k;
+        Key k;
-    	  bool firstTime = true;
+        bool firstTime = true;
-    	  BOOST_FOREACH(Key key, *this->at(i)) {
+        BOOST_FOREACH(Key key, *this->at(i)) {
-    		if(firstTime){
+        if(firstTime){
-    		  k = key;
+          k = key;
-    		  firstTime = false;
+          firstTime = false;
-    		  continue;
+          continue;
-    		}
+        }
-    		stm << "  var" << key << "--" << "var" << k << ";\n";
+        stm << "  var" << key << "--" << "var" << k << ";\n";
-    		k = key;
+        k = key;
-    	  }
+        }
-    	}
+      }
       }
     }
   }

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -77,9 +77,11 @@ void NonlinearOptimizer::defaultOptimize() {
             params.errorTol, currentError, this->error(), params.verbosity));
 
   // Printing if verbose
-  if (params.verbosity >= NonlinearOptimizerParams::TERMINATION &&
+  if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
-      this->iterations() >= params.maxIterations)
+    cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl;
-    cout << "Terminating because reached maximum iterations" << endl;
+    if (this->iterations() >= params.maxIterations)
+      cout << "Terminating because reached maximum iterations" << endl;
+  }
 }
 
 /* ************************************************************************* */

gtsam/slam/EssentialMatrixFactor.h

@@ -28,6 +28,7 @@ public:
 
   /**
    *  Constructor
+   *  @param key Essential Matrix variable key
    *  @param pA point in first camera, in calibrated coordinates
    *  @param pB point in second camera, in calibrated coordinates
    *  @param model noise model is about dot product in ideal, homogeneous coordinates
@@ -41,6 +42,7 @@ public:
 
   /**
    *  Constructor
+   *  @param key Essential Matrix variable key
    *  @param pA point in first camera, in pixel coordinates
    *  @param pB point in second camera, in pixel coordinates
    *  @param model noise model is about dot product in ideal, homogeneous coordinates
@@ -97,6 +99,8 @@ public:
 
   /**
    *  Constructor
+   *  @param key1 Essential Matrix variable key
+   *  @param key2 Inverse depth variable key
    *  @param pA point in first camera, in calibrated coordinates
    *  @param pB point in second camera, in calibrated coordinates
    *  @param model noise model should be in pixels, as well
@@ -111,6 +115,8 @@ public:
 
   /**
    *  Constructor
+   *  @param key1 Essential Matrix variable key
+   *  @param key2 Inverse depth variable key
    *  @param pA point in first camera, in pixel coordinates
    *  @param pB point in second camera, in pixel coordinates
    *  @param K calibration object, will be used only in constructor
@@ -216,6 +222,8 @@ public:
 
   /**
    *  Constructor
+   *  @param key1 Essential Matrix variable key
+   *  @param key2 Inverse depth variable key
    *  @param pA point in first camera, in calibrated coordinates
    *  @param pB point in second camera, in calibrated coordinates
    *  @param bRc extra rotation between "body" and "camera" frame
@@ -228,6 +236,8 @@ public:
 
   /**
    *  Constructor
+   *  @param key1 Essential Matrix variable key
+   *  @param key2 Inverse depth variable key
    *  @param pA point in first camera, in pixel coordinates
    *  @param pB point in second camera, in pixel coordinates
    *  @param K calibration object, will be used only in constructor

gtsam/slam/ImplicitSchurFactor.h

@@ -80,7 +80,7 @@ public:
   }
 
   /// Get matrix P
-  inline const Matrix& getPointCovariance() const {
+  inline const Matrix3& getPointCovariance() const {
     return PointCovariance_;
   }
 
@@ -285,26 +285,27 @@ public:
     return 0.5 * (result + f);
   }
 
-  /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
+  // needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
-  // This is wrong and does not match the definition in Hessian
+  // This is wrong and does not match the definition in Hessian,
-  //  virtual double error(const VectorValues& x) const {
+  // but it matches the definition of the Jacobian factor (JF)
-  //
+  double errorJF(const VectorValues& x) const {
-  //    // resize does not do malloc if correct size
+
-  //    e1.resize(size());
+    // resize does not do malloc if correct size
-  //    e2.resize(size());
+    e1.resize(size());
-  //
+    e2.resize(size());
-  //    // e1 = F * x - b = (2m*dm)*dm
+
-  //    for (size_t k = 0; k < size(); ++k)
+    // e1 = F * x - b = (2m*dm)*dm
-  //      e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
+    for (size_t k = 0; k < size(); ++k)
-  //    projectError(e1, e2);
+      e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
-  //
+    projectError(e1, e2);
-  //    double result = 0;
+
-  //    for (size_t k = 0; k < size(); ++k)
+    double result = 0;
-  //      result += dot(e2[k], e2[k]);
+    for (size_t k = 0; k < size(); ++k)
-  //
+      result += dot(e2[k], e2[k]);
-  //    std::cout << "implicitFactor::error result " << result << std::endl;
+
-  //    return 0.5 * result;
+    // std::cout << "implicitFactor::error result " << result << std::endl;
-  //  }
+    return 0.5 * result;
+  }
   /**
    * @brief Calculate corrected error Q*e = (I - E*P*E')*e
    */

gtsam/slam/InitializePose3.cpp

@@ -0,0 +1,410 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  InitializePose3.h
+ *  @author Luca Carlone
+ *  @author Frank Dellaert
+ *  @date   August, 2014
+ */
+
+#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/timing.h>
+
+#include <boost/math/special_functions.hpp>
+
+using namespace std;
+
+namespace gtsam {
+namespace InitializePose3 {
+
+//static const Matrix I = eye(1);
+static const Matrix I9 = eye(9);
+static const Vector zero9 = Vector::Zero(9);
+static const Matrix zero33= Matrix::Zero(3,3);
+
+static const Key keyAnchor = symbol('Z', 9999999);
+
+/* ************************************************************************* */
+GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
+
+  GaussianFactorGraph linearGraph;
+  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
+
+  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
+    Matrix3 Rij;
+
+    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    if (pose3Between)
+      Rij = pose3Between->measured().rotation().matrix();
+    else
+      std::cout << "Error in buildLinearOrientationGraph" << std::endl;
+
+    // std::cout << "Rij \n" << Rij << std::endl;
+
+    const FastVector<Key>& keys = factor->keys();
+    Key key1 = keys[0], key2 = keys[1];
+    Matrix M9 = Matrix::Zero(9,9);
+    M9.block(0,0,3,3) = Rij;
+    M9.block(3,3,3,3) = Rij;
+    M9.block(6,6,3,3) = Rij;
+    linearGraph.add(key1, -I9, key2, M9, zero9, model);
+  }
+  // prior on the anchor orientation
+  linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0), model);
+  return linearGraph;
+}
+
+/* ************************************************************************* */
+// Transform VectorValues into valid Rot3
+Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
+  gttic(InitializePose3_computeOrientationsChordal);
+
+  Matrix ppm = Matrix::Zero(3,3); // plus plus minus
+  ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
+
+  Values validRot3;
+  BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) {
+    Key key = it.first;
+    if (key != keyAnchor) {
+      const Vector& rotVector = it.second;
+      Matrix3 rotMat;
+      rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
+      rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
+      rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
+
+      Matrix U, V; Vector s;
+      svd(rotMat, U, s, V);
+      Matrix3 normalizedRotMat = U * V.transpose();
+
+      //      std::cout << "rotMat \n" << rotMat << std::endl;
+      //      std::cout << "U V' \n" << U * V.transpose() << std::endl;
+      //      std::cout << "V \n" << V << std::endl;
+
+      if(normalizedRotMat.determinant() < 0)
+        normalizedRotMat = U * ppm * V.transpose();
+
+      Rot3 initRot = Rot3(normalizedRotMat);
+      validRot3.insert(key, initRot);
+    }
+  }
+  return validRot3;
+}
+
+/* ************************************************************************* */
+// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
+NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
+  gttic(InitializePose3_buildPose3graph);
+  NonlinearFactorGraph pose3Graph;
+
+  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
+
+    // recast to a between on Pose3
+    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    if (pose3Between)
+      pose3Graph.add(pose3Between);
+
+    // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
+    boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
+        boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
+    if (pose3Prior)
+      pose3Graph.add(
+          BetweenFactor<Pose3>(keyAnchor, pose3Prior->keys()[0],
+              pose3Prior->prior(), pose3Prior->get_noiseModel()));
+  }
+  return pose3Graph;
+}
+
+/* ************************************************************************* */
+// Return the orientations of a graph including only BetweenFactors<Pose3>
+Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
+  gttic(InitializePose3_computeOrientationsChordal);
+
+  // regularize measurements and plug everything in a factor graph
+  GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
+
+  // Solve the LFG
+  VectorValues relaxedRot3 = relaxedGraph.optimize();
+
+  // normalize and compute Rot3
+  return normalizeRelaxedRotations(relaxedRot3);
+}
+
+/* ************************************************************************* */
+// Return the orientations of a graph including only BetweenFactors<Pose3>
+Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
+  gttic(InitializePose3_computeOrientationsGradient);
+
+  // this works on the inverse rotations, according to Tron&Vidal,2011
+  Values inverseRot;
+  inverseRot.insert(keyAnchor, Rot3());
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) {
+    Key key = key_value.key;
+    const Pose3& pose = givenGuess.at<Pose3>(key);
+    inverseRot.insert(key, pose.rotation().inverse());
+  }
+
+  // Create the map of edges incident on each node
+  KeyVectorMap adjEdgesMap;
+  KeyRotMap factorId2RotMap;
+
+  createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
+
+  // calculate max node degree & allocate gradient
+  size_t maxNodeDeg = 0;
+  VectorValues grad;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
+    Key key = key_value.key;
+    grad.insert(key,Vector3::Zero());
+    size_t currNodeDeg = (adjEdgesMap.at(key)).size();
+    if(currNodeDeg > maxNodeDeg)
+      maxNodeDeg = currNodeDeg;
+  }
+
+  // Create parameters
+  double b = 1;
+  double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
+  double a = (M_PI*M_PI)/(2*f0);
+  double rho = 2*a*b;
+  double mu_max = maxNodeDeg * rho;
+  double stepsize = 2/mu_max; // = 1/(a b dG)
+
+  std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
+  double maxGrad;
+  // gradient iterations
+  size_t it;
+  for(it=0; it < maxIter; it++){
+    //////////////////////////////////////////////////////////////////////////
+    // compute the gradient at each node
+    //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
+    //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
+    maxGrad = 0;
+    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
+      Key key = key_value.key;
+      //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
+      Vector gradKey = Vector3::Zero();
+      // collect the gradient for each edge incident on key
+      BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){
+        Rot3 Rij = factorId2RotMap.at(factorId);
+        Rot3 Ri = inverseRot.at<Rot3>(key);
+        if( key == (pose3Graph.at(factorId))->keys()[0] ){
+          Key key1 = (pose3Graph.at(factorId))->keys()[1];
+          Rot3 Rj = inverseRot.at<Rot3>(key1);
+          gradKey = gradKey + gradientTron(Ri, Rij  * Rj, a, b);
+          //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij  * Rj, a, b) \n " << gradientTron(Ri, Rij  * Rj, a, b) << std::endl;
+        }else if( key == (pose3Graph.at(factorId))->keys()[1] ){
+          Key key0 = (pose3Graph.at(factorId))->keys()[0];
+          Rot3 Rj = inverseRot.at<Rot3>(key0);
+          gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
+          //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse()  * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
+        }else{
+          std::cout << "Error in gradient computation" << std::endl;
+        }
+      } // end of i-th gradient computation
+      grad.at(key) = stepsize *  gradKey;
+
+      double normGradKey = (gradKey).norm();
+      //std::cout << "key  " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
+      if(normGradKey>maxGrad)
+        maxGrad = normGradKey;
+    } // end of loop over nodes
+
+    //////////////////////////////////////////////////////////////////////////
+    // update estimates
+    inverseRot = inverseRot.retract(grad);
+
+    //////////////////////////////////////////////////////////////////////////
+    // check stopping condition
+    if (it>20 && maxGrad < 5e-3)
+      break;
+  } // enf of gradient iterations
+
+  std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad <<  std::endl;
+
+  // Return correct rotations
+  const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
+  Values estimateRot;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
+    Key key = key_value.key;
+    if (key != keyAnchor) {
+      const Rot3& R = inverseRot.at<Rot3>(key);
+      if(setRefFrame)
+        estimateRot.insert(key, Rref.compose(R.inverse()));
+      else
+        estimateRot.insert(key, R.inverse());
+    }
+  }
+  return estimateRot;
+}
+
+/* ************************************************************************* */
+void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
+  size_t factorId = 0;
+  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose3Graph) {
+    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    if (pose3Between){
+      Rot3 Rij = pose3Between->measured().rotation();
+      factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
+
+      Key key1 = pose3Between->key1();
+      if (adjEdgesMap.find(key1) != adjEdgesMap.end()){  // key is already in
+        adjEdgesMap.at(key1).push_back(factorId);
+      }else{
+        vector<size_t> edge_id;
+        edge_id.push_back(factorId);
+        adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
+      }
+      Key key2 = pose3Between->key2();
+      if (adjEdgesMap.find(key2) != adjEdgesMap.end()){  // key is already in
+        adjEdgesMap.at(key2).push_back(factorId);
+      }else{
+        vector<size_t> edge_id;
+        edge_id.push_back(factorId);
+        adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
+      }
+    }else{
+      std::cout << "Error in computeOrientationsGradient" << std::endl;
+    }
+    factorId++;
+  }
+}
+
+/* ************************************************************************* */
+Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
+  Vector3 logRot = Rot3::Logmap(R1.between(R2));
+
+  double th = logRot.norm();
+  if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
+    Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation
+    logRot = Rot3::Logmap(R1pert.between(R2));
+    th = logRot.norm();
+  }
+  // exclude small or invalid rotations
+  if (th > 1e-5 && th == th){ // nonzero valid rotations
+    logRot = logRot / th;
+  }else{
+    logRot = Vector3::Zero();
+    th = 0.0;
+  }
+
+  double fdot = a*b*th*exp(-b*th);
+  return fdot*logRot;
+}
+
+/* ************************************************************************* */
+Values initializeOrientations(const NonlinearFactorGraph& graph) {
+
+  // We "extract" the Pose3 subgraph of the original graph: this
+  // is done to properly model priors and avoiding operating on a larger graph
+  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
+
+  // Get orientations from relative orientation measurements
+  return computeOrientationsChordal(pose3Graph);
+}
+
+///* ************************************************************************* */
+Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
+  gttic(InitializePose3_computePoses);
+
+  // put into Values structure
+  Values initialPose;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
+    Key key = key_value.key;
+    const Rot3& rot = initialRot.at<Rot3>(key);
+    Pose3 initializedPose = Pose3(rot, Point3());
+    initialPose.insert(key, initializedPose);
+  }
+  // add prior
+  noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
+  initialPose.insert(keyAnchor, Pose3());
+  pose3graph.add(PriorFactor<Pose3>(keyAnchor, Pose3(), priorModel));
+
+  // Create optimizer
+  GaussNewtonParams params;
+  bool singleIter = true;
+  if(singleIter){
+    params.maxIterations = 1;
+  }else{
+    std::cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" <<std::endl;
+    params.setVerbosity("TERMINATION");
+  }
+  GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
+  Values GNresult = optimizer.optimize();
+
+  // put into Values structure
+  Values estimate;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) {
+    Key key = key_value.key;
+    if (key != keyAnchor) {
+      const Pose3& pose = GNresult.at<Pose3>(key);
+      estimate.insert(key, pose);
+    }
+  }
+  return estimate;
+}
+
+/* ************************************************************************* */
+Values initialize(const NonlinearFactorGraph& graph) {
+  gttic(InitializePose3_initialize);
+
+  // We "extract" the Pose3 subgraph of the original graph: this
+  // is done to properly model priors and avoiding operating on a larger graph
+  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
+
+  // Get orientations from relative orientation measurements
+  Values valueRot3 = computeOrientationsChordal(pose3Graph);
+
+  // Compute the full poses (1 GN iteration on full poses)
+  return computePoses(pose3Graph, valueRot3);
+}
+
+/* ************************************************************************* */
+Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
+  Values initialValues;
+
+  // We "extract" the Pose3 subgraph of the original graph: this
+  // is done to properly model priors and avoiding operating on a larger graph
+  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
+
+  // Get orientations from relative orientation measurements
+  Values orientations;
+  if(useGradient)
+    orientations = computeOrientationsGradient(pose3Graph, givenGuess);
+  else
+    orientations = computeOrientationsChordal(pose3Graph);
+
+//  orientations.print("orientations\n");
+
+  // Compute the full poses (1 GN iteration on full poses)
+  return computePoses(pose3Graph, orientations);
+
+  //  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
+  //    Key key = key_value.key;
+  //    if (key != keyAnchor) {
+  //      const Point3& pos = givenGuess.at<Pose3>(key).translation();
+  //      const Rot3& rot = orientations.at<Rot3>(key);
+  //      Pose3 initializedPoses = Pose3(rot, pos);
+  //      initialValues.insert(key, initializedPoses);
+  //    }
+  //  }
+  //  return initialValues;
+}
+
+} // end of namespace lago
+} // end of namespace gtsam

gtsam/slam/InitializePose3.h

@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  InitializePose3.h
+ *  @brief Initialize Pose3 in a factor graph
+ *
+ *  @author Luca Carlone
+ *  @author Frank Dellaert
+ *  @date   August, 2014
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/inference/graph.h>
+#include <gtsam/geometry/Rot3.h>
+
+namespace gtsam {
+
+typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
+typedef std::map<Key, Rot3 > KeyRotMap;
+
+namespace InitializePose3 {
+
+GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g);
+
+GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
+
+GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
+
+GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
+    const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true);
+
+GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+    const NonlinearFactorGraph& pose3Graph);
+
+GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b);
+
+GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
+
+GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot);
+
+GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
+
+GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false);
+
+} // end of namespace lago
+} // end of namespace gtsam

gtsam/slam/PoseRotationPrior.h

@@ -77,7 +77,7 @@ public:
       (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
     }
 
-    return Rotation::Logmap(newR) - Rotation::Logmap(measured_);
+    return measured_.localCoordinates(newR);
   }
 
 private:

gtsam/slam/dataset.cpp

@@ -101,6 +101,25 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
   double v1, v2, v3, v4, v5, v6;
   is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
 
+   if (noiseFormat == NoiseFormatAUTO)
+   {
+     // Try to guess covariance matrix layout
+     if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
+     {
+       // NoiseFormatGRAPH
+       noiseFormat = NoiseFormatGRAPH;
+     }
+     else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
+     {
+       // NoiseFormatCOV
+       noiseFormat = NoiseFormatCOV;
+     }
+     else
+     {
+       throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
+     }
+  }
+
   // Read matrix and check that diagonal entries are non-zero
   Matrix M(3, 3);
   switch (noiseFormat) {
@@ -162,7 +181,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
 }
 
 /* ************************************************************************* */
-GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
+GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
     bool addNoise, bool smart, NoiseFormat noiseFormat,
     KernelFunctionType kernelFunctionType) {
 
@@ -210,7 +229,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
   }
 
   // Parse the pose constraints
-  int id1, id2;
+  Key id1, id2;
   bool haveLandmark = false;
   while (!is.eof()) {
     if (!(is >> tag))

gtsam/slam/dataset.h

@@ -57,7 +57,8 @@ enum NoiseFormat {
   NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33
   NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
   NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
-  NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
+  NoiseFormatCOV, ///< Covariance matrix C11, C12, C13, C22, C23, C33
+  NoiseFormatAUTO  ///< Try to guess covariance matrix layout
 };
 
 /// Robust kernel type to wrap around quadratic noise model
@@ -79,7 +80,7 @@ GTSAM_EXPORT GraphAndValues load2D(
     std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
     bool addNoise = false,
     bool smart = true, //
-    NoiseFormat noiseFormat = NoiseFormatGRAPH,
+    NoiseFormat noiseFormat = NoiseFormatAUTO,
     KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /**
@@ -94,8 +95,8 @@ GTSAM_EXPORT GraphAndValues load2D(
  * @return graph and initial values
  */
 GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
-    SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
+    SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise =
-        false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
+        false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
     KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel

gtsam/slam/tests/testDataset.cpp

@@ -56,6 +56,17 @@ TEST( dataSet, load2D)
   EXPECT(assert_equal(expected, *actual));
 }
 
+/* ************************************************************************* */
+TEST( dataSet, load2DVictoriaPark)
+{
+  const string filename = findExampleDataFile("victoria_park.txt");
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  boost::tie(graph, initial) = load2D(filename);
+  EXPECT_LONGS_EQUAL(10608,graph->size());
+  EXPECT_LONGS_EQUAL(7120,initial->size());
+}
+
 /* ************************************************************************* */
 TEST( dataSet, Balbianello)
 {

gtsam/slam/tests/testImplicitSchurFactor.cpp

@@ -0,0 +1,259 @@
+/**
+ * @file    testImplicitSchurFactor.cpp
+ * @brief   unit test implicit jacobian factors
+ * @author  Frank Dellaert
+ * @date    Oct 20, 2013
+ */
+
+//#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
+#include <gtsam/slam/ImplicitSchurFactor.h>
+//#include <gtsam_unstable/slam/JacobianFactorQ.h>
+#include <gtsam/slam/JacobianFactorQ.h>
+//#include "gtsam_unstable/slam/JacobianFactorQR.h"
+#include "gtsam/slam/JacobianFactorQR.h"
+
+#include <gtsam/base/timing.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/linear/GaussianFactor.h>
+
+#include <boost/assign/list_of.hpp>
+#include <boost/assign/std/vector.hpp>
+#include <boost/range/iterator_range.hpp>
+#include <boost/range/adaptor/map.hpp>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace boost::assign;
+using namespace gtsam;
+
+// F
+typedef Eigen::Matrix<double, 2, 6> Matrix26;
+const Matrix26 F0 = Matrix26::Ones();
+const Matrix26 F1 = 2 * Matrix26::Ones();
+const Matrix26 F3 = 3 * Matrix26::Ones();
+const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
+    (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3));
+// RHS and sigmas
+const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);
+
+//*************************************************************************************
+TEST( implicitSchurFactor, creation ) {
+  // Matrix E = Matrix::Ones(6,3);
+  Matrix E = zeros(6, 3);
+  E.block<2,2>(0, 0) = eye(2);
+  E.block<2,3>(2, 0) = 2 * ones(2, 3);
+  Matrix3 P = (E.transpose() * E).inverse();
+  ImplicitSchurFactor<6> expected(Fblocks, E, P, b);
+  Matrix expectedP = expected.getPointCovariance();
+  EXPECT(assert_equal(expectedP, P));
+}
+
+/* ************************************************************************* */
+TEST( implicitSchurFactor, addHessianMultiply ) {
+
+  Matrix E = zeros(6, 3);
+  E.block<2,2>(0, 0) = eye(2);
+  E.block<2,3>(2, 0) = 2 * ones(2, 3);
+  E.block<2,2>(4, 1) = eye(2);
+  Matrix3 P = (E.transpose() * E).inverse();
+
+  double alpha = 0.5;
+  VectorValues xvalues = map_list_of //
+  (0, gtsam::repeat(6, 2))//
+  (1, gtsam::repeat(6, 4))//
+  (2, gtsam::repeat(6, 0))// distractor
+  (3, gtsam::repeat(6, 8));
+
+  VectorValues yExpected = map_list_of//
+  (0, gtsam::repeat(6, 27))//
+  (1, gtsam::repeat(6, -40))//
+  (2, gtsam::repeat(6, 0))// distractor
+  (3, gtsam::repeat(6, 279));
+
+  // Create full F
+  size_t M=4, m = 3, d = 6;
+  Matrix F(2 * m, d * M);
+  F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
+
+  // Calculate expected result F'*alpha*(I - E*P*E')*F*x
+  FastVector<Key> keys;
+  keys += 0,1,2,3;
+  Vector x = xvalues.vector(keys);
+  Vector expected = zero(24);
+  ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
+  EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
+
+  // Create ImplicitSchurFactor
+  ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
+
+  VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
+  { // First Version
+    VectorValues yActual = zero;
+    implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
+    EXPECT(assert_equal(yExpected, yActual, 1e-8));
+    implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
+    EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
+    implicitFactor.multiplyHessianAdd(-1, xvalues, yActual);
+    EXPECT(assert_equal(zero, yActual, 1e-8));
+  }
+
+  typedef Eigen::Matrix<double, 24, 1> DeltaX;
+  typedef Eigen::Map<DeltaX> XMap;
+  double* y = new double[24];
+  double* xdata = x.data();
+
+  { // Raw memory Version
+    std::fill(y, y + 24, 0);// zero y !
+    implicitFactor.multiplyHessianAdd(alpha, xdata, y);
+    EXPECT(assert_equal(expected, XMap(y), 1e-8));
+    implicitFactor.multiplyHessianAdd(alpha, xdata, y);
+    EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
+    implicitFactor.multiplyHessianAdd(-1, xdata, y);
+    EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
+  }
+
+  // Create JacobianFactor with same error
+  const SharedDiagonal model;
+  JacobianFactorQ<6> jf(Fblocks, E, P, b, model);
+
+  { // error
+    double expectedError = jf.error(xvalues);
+    double actualError = implicitFactor.errorJF(xvalues);
+    DOUBLES_EQUAL(expectedError,actualError,1e-7)
+  }
+
+  { // JacobianFactor with same error
+    VectorValues yActual = zero;
+    jf.multiplyHessianAdd(alpha, xvalues, yActual);
+    EXPECT(assert_equal(yExpected, yActual, 1e-8));
+    jf.multiplyHessianAdd(alpha, xvalues, yActual);
+    EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
+    jf.multiplyHessianAdd(-1, xvalues, yActual);
+    EXPECT(assert_equal(zero, yActual, 1e-8));
+  }
+
+  { // check hessian Diagonal
+    VectorValues diagExpected = jf.hessianDiagonal();
+    VectorValues diagActual = implicitFactor.hessianDiagonal();
+    EXPECT(assert_equal(diagExpected, diagActual, 1e-8));
+  }
+
+  { // check hessian Block Diagonal
+    map<Key,Matrix> BD = jf.hessianBlockDiagonal();
+    map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal();
+    LONGS_EQUAL(3,actualBD.size());
+    EXPECT(assert_equal(BD[0],actualBD[0]));
+    EXPECT(assert_equal(BD[1],actualBD[1]));
+    EXPECT(assert_equal(BD[3],actualBD[3]));
+  }
+
+  { // Raw memory Version
+    std::fill(y, y + 24, 0);// zero y !
+    jf.multiplyHessianAdd(alpha, xdata, y);
+    EXPECT(assert_equal(expected, XMap(y), 1e-8));
+    jf.multiplyHessianAdd(alpha, xdata, y);
+    EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
+    jf.multiplyHessianAdd(-1, xdata, y);
+    EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
+  }
+
+  { // Check gradientAtZero
+    VectorValues expected = jf.gradientAtZero();
+    VectorValues actual = implicitFactor.gradientAtZero();
+    EXPECT(assert_equal(expected, actual, 1e-8));
+  }
+
+  // Create JacobianFactorQR
+  JacobianFactorQR<6> jfq(Fblocks, E, P, b, model);
+  {
+    const SharedDiagonal model;
+    VectorValues yActual = zero;
+    jfq.multiplyHessianAdd(alpha, xvalues, yActual);
+    EXPECT(assert_equal(yExpected, yActual, 1e-8));
+    jfq.multiplyHessianAdd(alpha, xvalues, yActual);
+    EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
+    jfq.multiplyHessianAdd(-1, xvalues, yActual);
+    EXPECT(assert_equal(zero, yActual, 1e-8));
+  }
+
+  { // Raw memory Version
+    std::fill(y, y + 24, 0);// zero y !
+    jfq.multiplyHessianAdd(alpha, xdata, y);
+    EXPECT(assert_equal(expected, XMap(y), 1e-8));
+    jfq.multiplyHessianAdd(alpha, xdata, y);
+    EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
+    jfq.multiplyHessianAdd(-1, xdata, y);
+    EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
+  }
+  delete [] y;
+}
+
+/* ************************************************************************* */
+TEST(implicitSchurFactor, hessianDiagonal)
+{
+  /* TESTED AGAINST MATLAB
+   *  F = [ones(2,6) zeros(2,6) zeros(2,6)
+        zeros(2,6) 2*ones(2,6) zeros(2,6)
+        zeros(2,6) zeros(2,6) 3*ones(2,6)]
+      E = [[1:6] [1:6] [0.5 1:5]];
+      E = reshape(E',3,6)'
+      P = inv(E' * E)
+      H = F' * (eye(6) - E * P * E') * F
+      diag(H)
+   */
+  Matrix E(6,3);
+  E.block<2,3>(0, 0) << 1,2,3,4,5,6;
+  E.block<2,3>(2, 0) << 1,2,3,4,5,6;
+  E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
+  Matrix3 P = (E.transpose() * E).inverse();
+  ImplicitSchurFactor<6> factor(Fblocks, E, P, b);
+
+  // hessianDiagonal
+  VectorValues expected;
+  expected.insert(0, 1.195652*ones(6));
+  expected.insert(1, 4.782608*ones(6));
+  expected.insert(3, 7.043478*ones(6));
+  EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5));
+
+  // hessianBlockDiagonal
+  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
+  LONGS_EQUAL(3,actualBD.size());
+  Matrix FtE0 = F0.transpose() * E.block<2,3>(0, 0);
+  Matrix FtE1 = F1.transpose() * E.block<2,3>(2, 0);
+  Matrix FtE3 = F3.transpose() * E.block<2,3>(4, 0);
+
+  // variant one
+  EXPECT(assert_equal(F0.transpose()*F0-FtE0*P*FtE0.transpose(),actualBD[0]));
+  EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
+  EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
+
+  // variant two
+  Matrix I2 = eye(2);
+  Matrix E0 = E.block<2,3>(0, 0);
+  Matrix F0t = F0.transpose();
+  EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0]));
+  EXPECT(assert_equal(F0t*(F0-E0*P*E0.transpose()*F0),actualBD[0]));
+
+  Matrix M1 = F0t*(F0-E0*P*E0.transpose()*F0);
+  Matrix M2 = F0t*F0-F0t*E0*P*E0.transpose()*F0;
+
+  EXPECT(assert_equal(  M1 , actualBD[0] ));
+  EXPECT(assert_equal(  M1 , M2 ));
+
+  Matrix M1b = F0t*(E0*P*E0.transpose()*F0);
+  Matrix M2b = F0t*E0*P*E0.transpose()*F0;
+  EXPECT(assert_equal(  M1b , M2b ));
+
+  EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
+  EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
+  EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
+}
+
+/* ************************************************************************* */
+int main(void) {
+  TestResult tr;
+  int result = TestRegistry::runAllTests(tr);
+  return result;
+}
+//*************************************************************************************

gtsam/slam/tests/testInitializePose3.cpp

@@ -0,0 +1,259 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testInitializePose3.cpp
+ *  @brief Unit tests for 3D SLAM initialization, using rotation relaxation
+ *
+ *  @author Luca Carlone
+ *  @author Frank Dellaert
+ *  @date   August, 2014
+ */
+
+#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/Pose3.h>
+#include <CppUnitLite/TestHarness.h>
+
+#include <cmath>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::assign;
+
+static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1));
+
+namespace simple {
+// We consider a small graph:
+//                            symbolic FG
+//               x2               0  1
+//             / | \              1  2
+//            /  |  \             2  3
+//          x3   |   x1           2  0
+//           \   |   /            0  3
+//            \  |  /
+//               x0
+//
+static Point3 p0 = Point3(0,0,0);
+static Rot3   R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) );
+static Point3 p1 = Point3(1,2,0);
+static Rot3   R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) );
+static Point3 p2 = Point3(0,2,0);
+static Rot3   R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) );
+static Point3 p3 = Point3(-1,1,0);
+static Rot3   R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) );
+
+static Pose3 pose0 = Pose3(R0,p0);
+static Pose3 pose1 = Pose3(R1,p1);
+static Pose3 pose2 = Pose3(R2,p2);
+static Pose3 pose3 = Pose3(R3,p3);
+
+NonlinearFactorGraph graph() {
+  NonlinearFactorGraph g;
+  g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), model));
+  g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), model));
+  g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), model));
+  g.add(BetweenFactor<Pose3>(x2, x0, pose2.between(pose0), model));
+  g.add(BetweenFactor<Pose3>(x0, x3, pose0.between(pose3), model));
+  g.add(PriorFactor<Pose3>(x0, pose0, model));
+  return g;
+}
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, buildPose3graph ) {
+  NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
+  // pose3graph.print("");
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, orientations ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+
+  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
+  EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
+  EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
+  EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+
+  KeyVectorMap adjEdgesMap;
+  KeyRotMap factorId2RotMap;
+
+  InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
+
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[2], 4, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[3], 5, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0).size(), 4, 1e-9);
+
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[0], 0, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[1], 1, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1).size(), 2, 1e-9);
+
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[0], 1, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[1], 2, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[2], 3, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2).size(), 3, 1e-9);
+
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[0], 2, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[1], 4, 1e-9);
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3).size(), 2, 1e-9);
+
+  // This includes the anchor
+  EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9);
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, singleGradient ) {
+  Rot3 R1 = Rot3();
+  Matrix M = Matrix3::Zero();
+  M(0,1) = -1; M(1,0) = 1; M(2,2) = 1;
+  Rot3 R2 = Rot3(M);
+  double a = 6.010534238540223;
+  double b = 1.0;
+
+  Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
+  Vector expected = Vector3::Zero();
+  expected(2) = 1.962658662803917;
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(expected, actual, 1e-6));
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, iterationGradient ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+
+  // Wrong initial guess - initialization should fix the rotations
+  Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
+  Values givenPoses;
+  givenPoses.insert(x0,simple::pose0);
+  givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
+  givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
+  givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
+
+  size_t maxIter = 1; // test gradient at the first iteration
+  bool setRefFrame = false;
+  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
+
+  Matrix M0 = (Matrix(3,3) <<  0.999435813876064,  -0.033571481675497,   0.001004768630281,
+      0.033572116359134,   0.999436104312325,  -0.000621610948719,
+     -0.000983333645009,   0.000654992453817,   0.999999302019670);
+  Rot3 R0Expected = Rot3(M0);
+  EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-5));
+
+  Matrix M1 = (Matrix(3,3) <<  0.999905367545392,  -0.010866391403031,   0.008436675399114,
+      0.010943459008004,   0.999898317528125,  -0.009143047050380,
+     -0.008336465609239,   0.009234508232789,   0.999922610604863);
+  Rot3 R1Expected = Rot3(M1);
+  EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-5));
+
+  Matrix M2 = (Matrix(3,3) <<   0.998936644682875,   0.045376417678595,  -0.008158469732553,
+      -0.045306446926148,   0.998936408933058,   0.008566024448664,
+       0.008538487960253,  -0.008187284445083,   0.999930028850403);
+  Rot3 R2Expected = Rot3(M2);
+  EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-5));
+
+  Matrix M3 = (Matrix(3,3) <<   0.999898767273093,  -0.010834701971459,   0.009223038487275,
+      0.010911315499947,   0.999906044037258,  -0.008297366559388,
+     -0.009132272433995,   0.008397162077148,   0.999923041673329);
+  Rot3 R3Expected = Rot3(M3);
+  EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-5));
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, orientationsGradient ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
+
+  // Wrong initial guess - initialization should fix the rotations
+  Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
+  Values givenPoses;
+  givenPoses.insert(x0,simple::pose0);
+  givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
+  givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
+  givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
+  // do 10 gradient iterations
+  bool setRefFrame = false;
+  Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
+
+  //  const Key keyAnchor = symbol('Z', 9999999);
+  //  givenPoses.insert(keyAnchor,simple::pose0);
+  //  string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
+  //  writeG2o(pose3Graph, givenPoses, g2oFile);
+
+  const string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter");
+  NonlinearFactorGraph::shared_ptr matlabGraph;
+  Values::shared_ptr matlabValues;
+  bool is3D = true;
+  boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D);
+
+  Rot3 R0Expected = matlabValues->at<Pose3>(1).rotation();
+  EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-4));
+
+  Rot3 R1Expected = matlabValues->at<Pose3>(2).rotation();
+  EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-4));
+
+  Rot3 R2Expected = matlabValues->at<Pose3>(3).rotation();
+  EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-3));
+
+  Rot3 R3Expected = matlabValues->at<Pose3>(4).rotation();
+  EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-4));
+}
+
+/* *************************************************************************** */
+TEST( InitializePose3, posesWithGivenGuess ) {
+  Values givenPoses;
+  givenPoses.insert(x0,simple::pose0);
+  givenPoses.insert(x1,simple::pose1);
+  givenPoses.insert(x2,simple::pose2);
+  givenPoses.insert(x3,simple::pose3);
+
+  Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(givenPoses, initial, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST( InitializePose3, initializePoses )
+{
+  const string g2oFile = findExampleDataFile("pose3example-grid");
+  NonlinearFactorGraph::shared_ptr inputGraph;
+  Values::shared_ptr expectedValues;
+  bool is3D = true;
+  boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
+  noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
+  inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
+
+  Values initial = InitializePose3::initialize(*inputGraph);
+  EXPECT(assert_equal(*expectedValues,initial,1e-4));
+}
+
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/slam/tests/testLago.cpp

@@ -36,7 +36,7 @@ using namespace boost::assign;
 static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
 
-namespace simple {
+namespace simpleLago {
 // We consider a small graph:
 //                            symbolic FG
 //               x2               0  1
@@ -67,7 +67,7 @@ NonlinearFactorGraph graph() {
 
 /* *************************************************************************** */
 TEST( Lago, checkSTandChords ) {
-  NonlinearFactorGraph g = simple::graph();
+  NonlinearFactorGraph g = simpleLago::graph();
   PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
       BetweenFactor<Pose2> >(g);
 
@@ -84,7 +84,7 @@ TEST( Lago, checkSTandChords ) {
 
 /* *************************************************************************** */
 TEST( Lago, orientationsOverSpanningTree ) {
-  NonlinearFactorGraph g = simple::graph();
+  NonlinearFactorGraph g = simpleLago::graph();
   PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
       BetweenFactor<Pose2> >(g);
 
@@ -115,7 +115,7 @@ TEST( Lago, orientationsOverSpanningTree ) {
 
 /* *************************************************************************** */
 TEST( Lago, regularizedMeasurements ) {
-  NonlinearFactorGraph g = simple::graph();
+  NonlinearFactorGraph g = simpleLago::graph();
   PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
       BetweenFactor<Pose2> >(g);
 
@@ -141,7 +141,7 @@ TEST( Lago, regularizedMeasurements ) {
 /* *************************************************************************** */
 TEST( Lago, smallGraphVectorValues ) {
   bool useOdometricPath = false;
-  VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath);
+  VectorValues initial = lago::initializeOrientations(simpleLago::graph(), useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
@@ -153,7 +153,7 @@ TEST( Lago, smallGraphVectorValues ) {
 /* *************************************************************************** */
 TEST( Lago, smallGraphVectorValuesSP ) {
 
-  VectorValues initial = lago::initializeOrientations(simple::graph());
+  VectorValues initial = lago::initializeOrientations(simpleLago::graph());
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
   EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
@@ -165,8 +165,8 @@ TEST( Lago, smallGraphVectorValuesSP ) {
 /* *************************************************************************** */
 TEST( Lago, multiplePosePriors ) {
   bool useOdometricPath = false;
-  NonlinearFactorGraph g = simple::graph();
+  NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
+  g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
   VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -178,8 +178,8 @@ TEST( Lago, multiplePosePriors ) {
 
 /* *************************************************************************** */
 TEST( Lago, multiplePosePriorsSP ) {
-  NonlinearFactorGraph g = simple::graph();
+  NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
+  g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
   VectorValues initial = lago::initializeOrientations(g);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -192,8 +192,8 @@ TEST( Lago, multiplePosePriorsSP ) {
 /* *************************************************************************** */
 TEST( Lago, multiplePoseAndRotPriors ) {
   bool useOdometricPath = false;
-  NonlinearFactorGraph g = simple::graph();
+  NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
+  g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
   VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -205,8 +205,8 @@ TEST( Lago, multiplePoseAndRotPriors ) {
 
 /* *************************************************************************** */
 TEST( Lago, multiplePoseAndRotPriorsSP ) {
-  NonlinearFactorGraph g = simple::graph();
+  NonlinearFactorGraph g = simpleLago::graph();
-  g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
+  g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
   VectorValues initial = lago::initializeOrientations(g);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@@ -221,20 +221,20 @@ TEST( Lago, smallGraphValues ) {
 
   // we set the orientations in the initial guess to zero
   Values initialGuess;
-  initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0));
+  initialGuess.insert(x0,Pose2(simpleLago::pose0.x(),simpleLago::pose0.y(),0.0));
-  initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0));
+  initialGuess.insert(x1,Pose2(simpleLago::pose1.x(),simpleLago::pose1.y(),0.0));
-  initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0));
+  initialGuess.insert(x2,Pose2(simpleLago::pose2.x(),simpleLago::pose2.y(),0.0));
-  initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
+  initialGuess.insert(x3,Pose2(simpleLago::pose3.x(),simpleLago::pose3.y(),0.0));
 
   // lago does not touch the Cartesian part and only fixed the orientations
-  Values actual = lago::initialize(simple::graph(), initialGuess);
+  Values actual = lago::initialize(simpleLago::graph(), initialGuess);
 
   // we are in a noiseless case
   Values expected;
-  expected.insert(x0,simple::pose0);
+  expected.insert(x0,simpleLago::pose0);
-  expected.insert(x1,simple::pose1);
+  expected.insert(x1,simpleLago::pose1);
-  expected.insert(x2,simple::pose2);
+  expected.insert(x2,simpleLago::pose2);
-  expected.insert(x3,simple::pose3);
+  expected.insert(x3,simpleLago::pose3);
 
   EXPECT(assert_equal(expected, actual, 1e-6));
 }
@@ -243,14 +243,14 @@ TEST( Lago, smallGraphValues ) {
 TEST( Lago, smallGraph2 ) {
 
   // lago does not touch the Cartesian part and only fixed the orientations
-  Values actual = lago::initialize(simple::graph());
+  Values actual = lago::initialize(simpleLago::graph());
 
   // we are in a noiseless case
   Values expected;
-  expected.insert(x0,simple::pose0);
+  expected.insert(x0,simpleLago::pose0);
-  expected.insert(x1,simple::pose1);
+  expected.insert(x1,simpleLago::pose1);
-  expected.insert(x2,simple::pose2);
+  expected.insert(x2,simpleLago::pose2);
-  expected.insert(x3,simple::pose3);
+  expected.insert(x3,simpleLago::pose3);
 
   EXPECT(assert_equal(expected, actual, 1e-6));
 }

gtsam/slam/tests/testPoseRotationPrior.cpp

@@ -35,6 +35,7 @@ const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3)
 // Pose2 examples
 const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
 const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
+const Rot2 rot2C = Rot2::fromAngle(M_PI-0.01), rot2D = Rot2::fromAngle(M_PI+0.01);
 
 /* ************************************************************************* */
 Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
@@ -61,9 +62,15 @@ TEST( testPoseRotationFactor, level3_error ) {
   Pose3 pose1(rot3A, point3A);
   Pose3RotationPrior factor(poseKey, rot3C, model3);
   Matrix actH1;
-  EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
+#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
+  EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1)));
+#else
+  EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2));
+#endif
   Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
-  EXPECT(assert_equal(expH1, actH1, tol));
+  // the derivative is more complex, but is close to the identity for Rot3 around the origin
+  // If not using true expmap will be close, but not exact around the origin
+  // EXPECT(assert_equal(expH1, actH1, tol));
 }
 
 /* ************************************************************************* */
@@ -86,6 +93,16 @@ TEST( testPoseRotationFactor, level2_error ) {
   EXPECT(assert_equal(expH1, actH1, tol));
 }
 
+/* ************************************************************************* */
+TEST( testPoseRotationFactor, level2_error_wrap ) {
+  Pose2 pose1(rot2C, point2A);
+  Pose2RotationPrior factor(poseKey, rot2D, model1);
+  Matrix actH1;
+  EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1)));
+  Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
+  EXPECT(assert_equal(expH1, actH1, tol));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam_unstable/base/DSFMap.h

@@ -34,89 +34,89 @@ class DSFMap {
 
 protected:
 
-	/// We store the forest in an STL map, but parents are done with pointers
+  /// We store the forest in an STL map, but parents are done with pointers
-	struct Entry {
+  struct Entry {
-		typename std::map<KEY, Entry>::iterator parent_;
+    typename std::map<KEY, Entry>::iterator parent_;
-		size_t rank_;
+    size_t rank_;
-		Entry() {}
+    Entry() {}
-	};
+  };
 
     typedef typename std::map<KEY, Entry> Map;
-	typedef typename Map::iterator iterator;
+  typedef typename Map::iterator iterator;
-	mutable Map entries_;
+  mutable Map entries_;
 
-	/// Given key, find iterator to initial entry
+  /// Given key, find iterator to initial entry
-	iterator find__(const KEY& key) const {
+  iterator find__(const KEY& key) const {
-		static const Entry empty;
+    static const Entry empty;
-		iterator it = entries_.find(key);
+    iterator it = entries_.find(key);
-		// if key does not exist, create and return itself
+    // if key does not exist, create and return itself
-		if (it == entries_.end()) {
+    if (it == entries_.end()) {
-			it = entries_.insert(std::make_pair(key, empty)).first;
+      it = entries_.insert(std::make_pair(key, empty)).first;
-			it->second.parent_ = it;
+      it->second.parent_ = it;
-			it->second.rank_ = 0;
+      it->second.rank_ = 0;
-		}
+    }
-		return it;
+    return it;
-	}
+  }
 
-	/// Given iterator to initial entry, find the root Entry
+  /// Given iterator to initial entry, find the root Entry
-	iterator find_(const iterator& it) const {
+  iterator find_(const iterator& it) const {
-		// follow parent pointers until we reach set representative
+    // follow parent pointers until we reach set representative
-		iterator& parent = it->second.parent_;
+    iterator& parent = it->second.parent_;
-		if (parent != it)
+    if (parent != it)
-			parent = find_(parent); // not yet, recurse!
+      parent = find_(parent); // not yet, recurse!
-		return parent;
+    return parent;
-	}
+  }
 
-	/// Given key, find the root Entry
+  /// Given key, find the root Entry
-	inline iterator find_(const KEY& key) const {
+  inline iterator find_(const KEY& key) const {
-		iterator initial = find__(key);
+    iterator initial = find__(key);
-		return find_(initial);
+    return find_(initial);
-	}
+  }
 
 public:
 
-	typedef std::set<KEY> Set;
+  typedef std::set<KEY> Set;
 
-	/// constructor
+  /// constructor
-	DSFMap() {
+  DSFMap() {
-	}
+  }
 
-	/// Given key, find the representative key for the set in which it lives
+  /// Given key, find the representative key for the set in which it lives
-	inline KEY find(const KEY& key) const {
+  inline KEY find(const KEY& key) const {
-		iterator root = find_(key);
+    iterator root = find_(key);
-		return root->first;
+    return root->first;
-	}
+  }
 
-	/// Merge two sets
+  /// Merge two sets
-	void merge(const KEY& x, const KEY& y) {
+  void merge(const KEY& x, const KEY& y) {
 
-		// straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
+    // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
-		iterator xRoot = find_(x);
+    iterator xRoot = find_(x);
-		iterator yRoot = find_(y);
+    iterator yRoot = find_(y);
-		if (xRoot == yRoot)
+    if (xRoot == yRoot)
-			return;
+      return;
 
-		// Merge sets
+    // Merge sets
-		if (xRoot->second.rank_ < yRoot->second.rank_)
+    if (xRoot->second.rank_ < yRoot->second.rank_)
-			xRoot->second.parent_ = yRoot;
+      xRoot->second.parent_ = yRoot;
-		else if (xRoot->second.rank_ > yRoot->second.rank_)
+    else if (xRoot->second.rank_ > yRoot->second.rank_)
-			yRoot->second.parent_ = xRoot;
+      yRoot->second.parent_ = xRoot;
-		else {
+    else {
-			yRoot->second.parent_ = xRoot;
+      yRoot->second.parent_ = xRoot;
-			xRoot->second.rank_ = xRoot->second.rank_ + 1;
+      xRoot->second.rank_ = xRoot->second.rank_ + 1;
-		}
+    }
-	}
+  }
 
-	/// return all sets, i.e. a partition of all elements
+  /// return all sets, i.e. a partition of all elements
-	std::map<KEY, Set> sets() const {
+  std::map<KEY, Set> sets() const {
-		std::map<KEY, Set> sets;
+    std::map<KEY, Set> sets;
-		iterator it = entries_.begin();
+    iterator it = entries_.begin();
-		for (; it != entries_.end(); it++) {
+    for (; it != entries_.end(); it++) {
-			iterator root = find_(it);
+      iterator root = find_(it);
-			sets[root->first].insert(it->first);
+      sets[root->first].insert(it->first);
-		}
+    }
-		return sets;
+    return sets;
-	}
+  }
 
 };
 

gtsam_unstable/discrete/tests/testLoopyBelief.cpp

@@ -1,8 +1,8 @@
 /**
- * @file 	 testLoopyBelief.cpp
+ * @file    testLoopyBelief.cpp
  * @brief  
  * @author Duy-Nguyen Ta
- * @date 	 Oct 11, 2013
+ * @date    Oct 11, 2013
  */
 
 #include <gtsam/inference/VariableIndex.h>

gtsam_unstable/partition/FindSeparator-inl.h

@@ -26,539 +26,539 @@ extern "C" {
 
 namespace gtsam { namespace partition {
 
-	typedef boost::shared_array<idx_t> sharedInts;
+  typedef boost::shared_array<idx_t> sharedInts;
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	/**
+  /**
-	 * Return the size of the separator and the partiion indices {part}
+   * Return the size of the separator and the partiion indices {part}
-	 * Part [j] is 0, 1, or 2, depending on
+   * Part [j] is 0, 1, or 2, depending on
-	 * whether node j is in the left part of the graph, the right part, or the
+   * whether node j is in the left part of the graph, the right part, or the
-	 * separator, respectively
+   * separator, respectively
-	 */
+   */
-	std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
+  std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
-		const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
+    const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
 
-		// control parameters
+    // control parameters
-		idx_t vwgt[n];                  // the weights of the vertices
+    idx_t vwgt[n];                  // the weights of the vertices
-		idx_t options[METIS_NOPTIONS];
+    idx_t options[METIS_NOPTIONS];
-		METIS_SetDefaultOptions(options);	// use defaults
+    METIS_SetDefaultOptions(options);  // use defaults
-		idx_t sepsize;                      // the size of the separator, output
+    idx_t sepsize;                      // the size of the separator, output
-		sharedInts part_(new idx_t[n]);      // the partition of each vertex, output
+    sharedInts part_(new idx_t[n]);      // the partition of each vertex, output
 
-		// set uniform weights on the vertices
+    // set uniform weights on the vertices
-		std::fill(vwgt, vwgt+n, 1);
+    std::fill(vwgt, vwgt+n, 1);
 
-		// TODO: Fix at later time
+    // TODO: Fix at later time
-		//boost::timer::cpu_timer TOTALTmr;
+    //boost::timer::cpu_timer TOTALTmr;
-		if (verbose) {
+    if (verbose) {
-			printf("**********************************************************************\n");
+      printf("**********************************************************************\n");
-			printf("Graph Information ---------------------------------------------------\n");
+      printf("Graph Information ---------------------------------------------------\n");
-			printf("  #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
+      printf("  #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
-			printf("\nND Partitioning... -------------------------------------------\n");
+      printf("\nND Partitioning... -------------------------------------------\n");
-			//TOTALTmr.start()
+      //TOTALTmr.start()
-		}
+    }
 
-		// call metis parition routine
+    // call metis parition routine
-		METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), 
+    METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), 
            vwgt, options, &sepsize, part_.get());
 
-		if (verbose) {
+    if (verbose) {
-			//boost::cpu_times const elapsed_times(timer.elapsed());
+      //boost::cpu_times const elapsed_times(timer.elapsed());
-			//printf("\nTiming Information --------------------------------------------------\n");
+      //printf("\nTiming Information --------------------------------------------------\n");
-			//printf("  Total:        \t\t %7.3f\n", elapsed_times);
+      //printf("  Total:        \t\t %7.3f\n", elapsed_times);
-			printf("  Sep size:        \t\t %d\n", sepsize);
+      printf("  Sep size:        \t\t %d\n", sepsize);
-			printf("**********************************************************************\n");
+      printf("**********************************************************************\n");
-		}
+    }
 
-		return std::make_pair(sepsize, part_);
+    return std::make_pair(sepsize, part_);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt,
+  void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt,
-			idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part)
+      idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part)
-	{
+  {
     idx_t i, ncon;
-		graph_t *graph;
+    graph_t *graph;
-		real_t *tpwgts2;
+    real_t *tpwgts2;
-		ctrl_t *ctrl;
+    ctrl_t *ctrl;
-		ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
+    ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
-		ctrl->iptype = METIS_IPTYPE_GROW;
+    ctrl->iptype = METIS_IPTYPE_GROW;
-		//if () == NULL)
+    //if () == NULL)
-		//  return METIS_ERROR_INPUT;
+    //  return METIS_ERROR_INPUT;
 
-		InitRandom(ctrl->seed);
+    InitRandom(ctrl->seed);
 
-		graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
+    graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
 
-		AllocateWorkSpace(ctrl, graph);
+    AllocateWorkSpace(ctrl, graph);
 
-  	ncon = graph->ncon;
+    ncon = graph->ncon;
-  	ctrl->ncuts = 1;
+    ctrl->ncuts = 1;
-	
+  
-  	/* determine the weights of the two partitions as a function of the weight of the
+    /* determine the weights of the two partitions as a function of the weight of the
-  	   target partition weights */
+       target partition weights */
 
-  	tpwgts2 = rwspacemalloc(ctrl, 2*ncon);
+    tpwgts2 = rwspacemalloc(ctrl, 2*ncon);
-  	for (i=0; i<ncon; i++) {
+    for (i=0; i<ncon; i++) {
-  	  tpwgts2[i]      = rsum((2>>1), ctrl->tpwgts+i, ncon);
+      tpwgts2[i]      = rsum((2>>1), ctrl->tpwgts+i, ncon);
-  	  tpwgts2[ncon+i] = 1.0 - tpwgts2[i];
+      tpwgts2[ncon+i] = 1.0 - tpwgts2[i];
-  	}
+    }
-  	/* perform the bisection */
+    /* perform the bisection */
-  	*edgecut = MultilevelBisect(ctrl, graph, tpwgts2);
+    *edgecut = MultilevelBisect(ctrl, graph, tpwgts2);
 
-		//  ConstructMinCoverSeparator(&ctrl, &graph, 1.05);
+    //  ConstructMinCoverSeparator(&ctrl, &graph, 1.05);
-	//  *edgecut = graph->mincut;
+  //  *edgecut = graph->mincut;
-	//  *sepsize = graph.pwgts[2];
+  //  *sepsize = graph.pwgts[2];
-	  icopy(*nvtxs, graph->where, part);
+    icopy(*nvtxs, graph->where, part);
-		std::cout << "Finished bisection:" << *edgecut << std::endl;
+    std::cout << "Finished bisection:" << *edgecut << std::endl;
-		FreeGraph(&graph);
+    FreeGraph(&graph);
 
-		FreeCtrl(&ctrl);
+    FreeCtrl(&ctrl);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	/**
+  /**
-	 * Return the number of edge cuts and the partition indices {part}
+   * Return the number of edge cuts and the partition indices {part}
-	 * Part [j] is 0 or 1, depending on
+   * Part [j] is 0 or 1, depending on
-	 * whether node j is in the left part of the graph or the right part respectively
+   * whether node j is in the left part of the graph or the right part respectively
-	 */
+   */
-	std::pair<int, sharedInts> edgeMetis(idx_t n, const sharedInts& xadj,	const sharedInts& adjncy,
+  std::pair<int, sharedInts> edgeMetis(idx_t n, const sharedInts& xadj,  const sharedInts& adjncy,
-		const sharedInts& adjwgt, bool verbose) {
+    const sharedInts& adjwgt, bool verbose) {
 
-		// control parameters
+    // control parameters
-		idx_t vwgt[n];                  // the weights of the vertices
+    idx_t vwgt[n];                  // the weights of the vertices
-		idx_t options[METIS_NOPTIONS];
+    idx_t options[METIS_NOPTIONS];
-		METIS_SetDefaultOptions(options);	// use defaults
+    METIS_SetDefaultOptions(options);  // use defaults
-		idx_t edgecut;                      // the number of edge cuts, output
+    idx_t edgecut;                      // the number of edge cuts, output
-		sharedInts part_(new idx_t[n]);      // the partition of each vertex, output
+    sharedInts part_(new idx_t[n]);      // the partition of each vertex, output
 
-		// set uniform weights on the vertices
+    // set uniform weights on the vertices
-		std::fill(vwgt, vwgt+n, 1);
+    std::fill(vwgt, vwgt+n, 1);
 
-		//TODO: Fix later
+    //TODO: Fix later
-		//boost::timer TOTALTmr;
+    //boost::timer TOTALTmr;
-		if (verbose) {
+    if (verbose) {
-			printf("**********************************************************************\n");
+      printf("**********************************************************************\n");
-			printf("Graph Information ---------------------------------------------------\n");
+      printf("Graph Information ---------------------------------------------------\n");
-			printf("  #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
+      printf("  #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
-			printf("\nND Partitioning... -------------------------------------------\n");
+      printf("\nND Partitioning... -------------------------------------------\n");
-			//cleartimer(TOTALTmr);
+      //cleartimer(TOTALTmr);
-			//starttimer(TOTALTmr);
+      //starttimer(TOTALTmr);
-		}
+    }
 
-		//int wgtflag = 1; // only edge weights
+    //int wgtflag = 1; // only edge weights
-		//int numflag = 0; // c style numbering starting from 0
+    //int numflag = 0; // c style numbering starting from 0
-		//int nparts = 2; // partition the graph to 2 submaps
+    //int nparts = 2; // partition the graph to 2 submaps
-		modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
+    modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
-				options, &edgecut, part_.get());
+        options, &edgecut, part_.get());
 
-		
+    
-		if (verbose) {
+    if (verbose) {
-			//stoptimer(TOTALTmr);
+      //stoptimer(TOTALTmr);
-			printf("\nTiming Information --------------------------------------------------\n");
+      printf("\nTiming Information --------------------------------------------------\n");
-			//printf("  Total:        \t\t %7.3f\n", gettimer(TOTALTmr));
+      //printf("  Total:        \t\t %7.3f\n", gettimer(TOTALTmr));
-			printf("  Edge cuts:    \t\t %d\n", edgecut);
+      printf("  Edge cuts:    \t\t %d\n", edgecut);
-			printf("**********************************************************************\n");
+      printf("**********************************************************************\n");
-		}
+    }
 
-		return std::make_pair(edgecut, part_);
+    return std::make_pair(edgecut, part_);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	/**
+  /**
-	 * Prepare the data structure {xadj} and {adjncy} required by metis
+   * Prepare the data structure {xadj} and {adjncy} required by metis
-	 * xadj always has the size equal to the no. of the nodes plus 1
+   * xadj always has the size equal to the no. of the nodes plus 1
-	 * adjncy always has the size equal to two times of the no. of the edges in the Metis graph
+   * adjncy always has the size equal to two times of the no. of the edges in the Metis graph
-	 */
+   */
-	template <class GenericGraph>
+  template <class GenericGraph>
-	void prepareMetisGraph(const GenericGraph& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
+  void prepareMetisGraph(const GenericGraph& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
-			sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {
+      sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {
 
-		typedef int Weight;
+    typedef int Weight;
-		typedef std::vector<int> Weights;
+    typedef std::vector<int> Weights;
-		typedef std::vector<int> Neighbors;
+    typedef std::vector<int> Neighbors;
-		typedef std::pair<Neighbors, Weights> NeighborsInfo;
+    typedef std::pair<Neighbors, Weights> NeighborsInfo;
 
-		// set up dictionary
+    // set up dictionary
-		std::vector<int>& dictionary = workspace.dictionary;
+    std::vector<int>& dictionary = workspace.dictionary;
-		workspace.prepareDictionary(keys);
+    workspace.prepareDictionary(keys);
 
-		// prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights
+    // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights
-		int numNodes = keys.size();
+    int numNodes = keys.size();
-		int numEdges = 0;
+    int numEdges = 0;
-		std::vector<NeighborsInfo> adjacencyMap;
+    std::vector<NeighborsInfo> adjacencyMap;
-		adjacencyMap.resize(numNodes);
+    adjacencyMap.resize(numNodes);
-		std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl;
+    std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl;
-		int index1, index2;
+    int index1, index2;
 
-		BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
+    BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
-			index1 = dictionary[factor->key1.index];
+      index1 = dictionary[factor->key1.index];
-			index2 = dictionary[factor->key2.index];
+      index2 = dictionary[factor->key2.index];
-			std::cout << "index1: " << index1 << std::endl;
+      std::cout << "index1: " << index1 << std::endl;
-			std::cout << "index2: " << index2 << std::endl;
+      std::cout << "index2: " << index2 << std::endl;
-			// if both nodes are in the current graph, i.e. not a joint factor between frontal and separator
+      // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator
-			if (index1 >= 0 && index2 >= 0) {       
+      if (index1 >= 0 && index2 >= 0) {       
-			  std::pair<Neighbors, Weights>& adjacencyMap1 = adjacencyMap[index1];
+        std::pair<Neighbors, Weights>& adjacencyMap1 = adjacencyMap[index1];
-			  std::pair<Neighbors, Weights>& adjacencyMap2 = adjacencyMap[index2];
+        std::pair<Neighbors, Weights>& adjacencyMap2 = adjacencyMap[index2];
-				try{
+        try{
-				 adjacencyMap1.first.push_back(index2);
+         adjacencyMap1.first.push_back(index2);
-				 adjacencyMap1.second.push_back(factor->weight);
+         adjacencyMap1.second.push_back(factor->weight);
-				 adjacencyMap2.first.push_back(index1);
+         adjacencyMap2.first.push_back(index1);
-				 adjacencyMap2.second.push_back(factor->weight);
+         adjacencyMap2.second.push_back(factor->weight);
-				}catch(std::exception& e){
+        }catch(std::exception& e){
-					std::cout << e.what() << std::endl;
+          std::cout << e.what() << std::endl;
-				}
+        }
-				numEdges++;
+        numEdges++;
-			}
+      }
-		}
+    }
 
-		// prepare for {xadj}, {adjncy}, and {adjwgt}
+    // prepare for {xadj}, {adjncy}, and {adjwgt}
-		*ptr_xadj   = sharedInts(new idx_t[numNodes+1]);
+    *ptr_xadj   = sharedInts(new idx_t[numNodes+1]);
-		*ptr_adjncy = sharedInts(new idx_t[numEdges*2]);
+    *ptr_adjncy = sharedInts(new idx_t[numEdges*2]);
-		*ptr_adjwgt = sharedInts(new idx_t[numEdges*2]);
+    *ptr_adjwgt = sharedInts(new idx_t[numEdges*2]);
-		sharedInts& xadj = *ptr_xadj;
+    sharedInts& xadj = *ptr_xadj;
-		sharedInts& adjncy = *ptr_adjncy;
+    sharedInts& adjncy = *ptr_adjncy;
-		sharedInts& adjwgt = *ptr_adjwgt;
+    sharedInts& adjwgt = *ptr_adjwgt;
-		int ind_xadj = 0, ind_adjncy = 0;
+    int ind_xadj = 0, ind_adjncy = 0;
-		BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) {
+    BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) {
-			*(xadj.get() + ind_xadj) = ind_adjncy;
+      *(xadj.get() + ind_xadj) = ind_adjncy;
-			std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy);
+      std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy);
-			std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy);
+      std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy);
-			assert(info.first.size() == info.second.size());
+      assert(info.first.size() == info.second.size());
-			ind_adjncy += info.first.size();
+      ind_adjncy += info.first.size();
-			ind_xadj ++;
+      ind_xadj ++;
-		}
+    }
-		if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes");
+    if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes");
-		*(xadj.get() + ind_xadj) = ind_adjncy;
+    *(xadj.get() + ind_xadj) = ind_adjncy;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template<class GenericGraph>
+  template<class GenericGraph>
-	boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph,
+  boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph,
-		const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
+    const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
-		// create a metis graph
+    // create a metis graph
-		size_t numKeys = keys.size();
+    size_t numKeys = keys.size();
-		if (verbose) 
+    if (verbose) 
-			std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
+      std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
 
-		sharedInts xadj, adjncy, adjwgt;
+    sharedInts xadj, adjncy, adjwgt;
 
-		prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
+    prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
 
-		// run ND on the graph
+    // run ND on the graph
-		size_t sepsize;
+    size_t sepsize;
-		sharedInts part;
+    sharedInts part;
-		boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
+    boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
-		if (!sepsize)	return boost::optional<MetisResult>();
+    if (!sepsize)  return boost::optional<MetisResult>();
 
-		// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later
+    // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later
-		//  we will have more submaps
+    //  we will have more submaps
-		MetisResult result;
+    MetisResult result;
-		result.C.reserve(sepsize);
+    result.C.reserve(sepsize);
-		result.A.reserve(numKeys - sepsize);
+    result.A.reserve(numKeys - sepsize);
-		result.B.reserve(numKeys - sepsize);
+    result.B.reserve(numKeys - sepsize);
-		int* ptr_part = part.get();
+    int* ptr_part = part.get();
-		std::vector<size_t>::const_iterator itKey = keys.begin();
+    std::vector<size_t>::const_iterator itKey = keys.begin();
-		std::vector<size_t>::const_iterator itKeyLast = keys.end();
+    std::vector<size_t>::const_iterator itKeyLast = keys.end();
-		while(itKey != itKeyLast) {
+    while(itKey != itKeyLast) {
-			switch(*(ptr_part++)) {
+      switch(*(ptr_part++)) {
-			case 0: result.A.push_back(*(itKey++)); break;
+      case 0: result.A.push_back(*(itKey++)); break;
-			case 1: result.B.push_back(*(itKey++)); break;
+      case 1: result.B.push_back(*(itKey++)); break;
-			case 2: result.C.push_back(*(itKey++)); break;
+      case 2: result.C.push_back(*(itKey++)); break;
-			default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!");
+      default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!");
-			}
+      }
-		}
+    }
 
-		if (verbose) {
+    if (verbose) {
-			std::cout << "total key: " << keys.size()
+      std::cout << "total key: " << keys.size()
-										<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", "
+                    << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", "
-										<< result.C.size() << "; sepsize from Metis = " << sepsize << std::endl;
+                    << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl;
-			//throw runtime_error("separatorPartitionByMetis:stop for debug");
+      //throw runtime_error("separatorPartitionByMetis:stop for debug");
-		}
+    }
 
-		if(result.C.size() != sepsize) {
+    if(result.C.size() != sepsize) {
-			std::cout << "total key: " << keys.size()
+      std::cout << "total key: " << keys.size()
-					<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size()
+          << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size()
-					<< "; sepsize from Metis = " << sepsize << std::endl;
+          << "; sepsize from Metis = " << sepsize << std::endl;
-			throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!");
+      throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!");
-		}
+    }
 
-		return boost::make_optional<MetisResult >(result);
+    return boost::make_optional<MetisResult >(result);
-	}
+  }
 
-	/* *************************************************************************/ 
+  /* *************************************************************************/ 
-	template<class GenericGraph>
+  template<class GenericGraph>
-	boost::optional<MetisResult> edgePartitionByMetis(const GenericGraph& graph,
+  boost::optional<MetisResult> edgePartitionByMetis(const GenericGraph& graph,
-	 const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
+   const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
 
-		// a small hack for handling the camera1-camera2 case used in the unit tests
+    // a small hack for handling the camera1-camera2 case used in the unit tests
-		if (graph.size() == 1 && keys.size() == 2) {
+    if (graph.size() == 1 && keys.size() == 2) {
-			MetisResult result;
+      MetisResult result;
-			result.A.push_back(keys.front());
+      result.A.push_back(keys.front());
-			result.B.push_back(keys.back());
+      result.B.push_back(keys.back());
-			return result;
+      return result;
-		}
+    }
 
-		// create a metis graph
+    // create a metis graph
-		size_t numKeys = keys.size();
+    size_t numKeys = keys.size();
-		if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
+    if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
-		sharedInts xadj, adjncy, adjwgt;
+    sharedInts xadj, adjncy, adjwgt;
-		prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
+    prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
 
-		// run metis on the graph
+    // run metis on the graph
-		int edgecut;
+    int edgecut;
-		sharedInts part;
+    sharedInts part;
-		boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
+    boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
 
-		// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps
+    // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps
-		MetisResult result;
+    MetisResult result;
-		result.A.reserve(numKeys);
+    result.A.reserve(numKeys);
-		result.B.reserve(numKeys);
+    result.B.reserve(numKeys);
-		int* ptr_part = part.get();
+    int* ptr_part = part.get();
-		std::vector<size_t>::const_iterator itKey = keys.begin();
+    std::vector<size_t>::const_iterator itKey = keys.begin();
-		std::vector<size_t>::const_iterator itKeyLast = keys.end();
+    std::vector<size_t>::const_iterator itKeyLast = keys.end();
-		while(itKey != itKeyLast) {
+    while(itKey != itKeyLast) {
-			if (*ptr_part != 0 && *ptr_part != 1)
+      if (*ptr_part != 0 && *ptr_part != 1)
-				std::cout << *ptr_part << "!!!" << std::endl;
+        std::cout << *ptr_part << "!!!" << std::endl;
-			switch(*(ptr_part++)) {
+      switch(*(ptr_part++)) {
-			case 0: result.A.push_back(*(itKey++)); break;
+      case 0: result.A.push_back(*(itKey++)); break;
-			case 1: result.B.push_back(*(itKey++)); break;
+      case 1: result.B.push_back(*(itKey++)); break;
-			default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!");
+      default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!");
-			}
+      }
-		}
+    }
 
-		if (verbose) {
+    if (verbose) {
-			std::cout << "the size of two submaps in the reduced graph: " << result.A.size()
+      std::cout << "the size of two submaps in the reduced graph: " << result.A.size()
-				<< " " << result.B.size() << std::endl;
+        << " " << result.B.size() << std::endl;
-			int edgeCut = 0;
+      int edgeCut = 0;
 
-			BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
+      BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
-				int key1 = factor->key1.index;
+        int key1 = factor->key1.index;
-				int key2 = factor->key2.index;
+        int key2 = factor->key2.index;
-				// print keys and their subgraph assignment
+        // print keys and their subgraph assignment
-				std::cout << key1;
+        std::cout << key1;
-				if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A ";
+        if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A ";
-				if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B ";
+        if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B ";
 
-				std::cout << key2;
+        std::cout << key2;
-				if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A ";
+        if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A ";
-				if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B ";
+        if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B ";
         std::cout << "weight " << factor->weight;;
 
-				// find vertices that were assigned to sets A & B. Their edge will be cut
+        // find vertices that were assigned to sets A & B. Their edge will be cut
-				if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() &&
+        if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() &&
-  					 std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) ||
+             std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) ||
-  					(std::find(result.B.begin(), result.B.end(), key1) != result.B.end() &&
+            (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() &&
-   					 std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){
+              std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){
-					edgeCut ++;
+          edgeCut ++;
-					std::cout << " CUT ";
+          std::cout << " CUT ";
-				}
+        }
-				std::cout << std::endl;
+        std::cout << std::endl;
-			}
+      }
-			std::cout << "edgeCut: " << edgeCut << std::endl;
+      std::cout << "edgeCut: " << edgeCut << std::endl;
-		}
+    }
 
-		return boost::make_optional<MetisResult >(result);
+    return boost::make_optional<MetisResult >(result);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	bool isLargerIsland(const std::vector<size_t>& island1, const std::vector<size_t>& island2) {
+  bool isLargerIsland(const std::vector<size_t>& island1, const std::vector<size_t>& island2) {
-		return island1.size() > island2.size();
+    return island1.size() > island2.size();
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	// debug functions
+  // debug functions
-	void printIsland(const std::vector<size_t>& island) {
+  void printIsland(const std::vector<size_t>& island) {
-		std::cout << "island: ";
+    std::cout << "island: ";
-		BOOST_FOREACH(const size_t key, island)
+    BOOST_FOREACH(const size_t key, island)
-			std::cout << key << " ";
+      std::cout << key << " ";
-		std::cout << std::endl;
+    std::cout << std::endl;
-	}
+  }
 
-	void printIslands(const std::list<std::vector<size_t> >& islands) {
+  void printIslands(const std::list<std::vector<size_t> >& islands) {
-		BOOST_FOREACH(const std::vector<std::size_t>& island, islands)
+    BOOST_FOREACH(const std::vector<std::size_t>& island, islands)
-				printIsland(island);
+        printIsland(island);
-	}
+  }
 
-	void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) {
+  void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) {
-		int numCamera = 0, numLandmark = 0;
+    int numCamera = 0, numLandmark = 0;
-		BOOST_FOREACH(const size_t key, keys)
+    BOOST_FOREACH(const size_t key, keys)
-		if (int2symbol[key].chr() == 'x')
+    if (int2symbol[key].chr() == 'x')
-			numCamera++;
+      numCamera++;
-		else
+    else
-			numLandmark++;
+      numLandmark++;
-		std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl;
+    std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template<class GenericGraph>
+  template<class GenericGraph>
-	void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector<size_t>& landmarkKeys,
+  void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector<size_t>& landmarkKeys,
-			MetisResult& partitionResult, WorkSpace& workspace) {
+      MetisResult& partitionResult, WorkSpace& workspace) {
 
-		// set up cameras in the dictionary
+    // set up cameras in the dictionary
-		std::vector<size_t>& A = partitionResult.A;
+    std::vector<size_t>& A = partitionResult.A;
-		std::vector<size_t>& B = partitionResult.B;
+    std::vector<size_t>& B = partitionResult.B;
-		std::vector<size_t>& C = partitionResult.C;
+    std::vector<size_t>& C = partitionResult.C;
-		std::vector<int>& dictionary = workspace.dictionary;
+    std::vector<int>& dictionary = workspace.dictionary;
-		std::fill(dictionary.begin(), dictionary.end(), -1);
+    std::fill(dictionary.begin(), dictionary.end(), -1);
-		BOOST_FOREACH(const size_t a, A)
+    BOOST_FOREACH(const size_t a, A)
-			dictionary[a] = 1;
+      dictionary[a] = 1;
-		BOOST_FOREACH(const size_t b, B)
+    BOOST_FOREACH(const size_t b, B)
-			dictionary[b] = 2;
+      dictionary[b] = 2;
-		if (!C.empty())
+    if (!C.empty())
-			throw std::runtime_error("addLandmarkToPartitionResult: C is not empty");
+      throw std::runtime_error("addLandmarkToPartitionResult: C is not empty");
 
-		// set up landmarks
+    // set up landmarks
-		size_t i,j;
+    size_t i,j;
-		BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) {
+    BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) {
-			i = factor->key1.index;
+      i = factor->key1.index;
-			j = factor->key2.index;
+      j = factor->key2.index;
-			if (dictionary[j] == 0) // if the landmark is already in the separator, continue
+      if (dictionary[j] == 0) // if the landmark is already in the separator, continue
-				continue;
+        continue;
-			else if (dictionary[j] == -1)
+      else if (dictionary[j] == -1)
-				dictionary[j] = dictionary[i];
+        dictionary[j] = dictionary[i];
-			else {
+      else {
-				if (dictionary[j] != dictionary[i])
+        if (dictionary[j] != dictionary[i])
-					dictionary[j] = 0;
+          dictionary[j] = 0;
-			}
+      }
-//			if (j == 67980)
+//      if (j == 67980)
-//				std::cout << "dictionary[67980]" << dictionary[j] << std::endl;
+//        std::cout << "dictionary[67980]" << dictionary[j] << std::endl;
-		}
+    }
 
-		BOOST_FOREACH(const size_t j, landmarkKeys) {
+    BOOST_FOREACH(const size_t j, landmarkKeys) {
-			switch(dictionary[j]) {
+      switch(dictionary[j]) {
-			case 0: C.push_back(j); break;
+      case 0: C.push_back(j); break;
-			case 1: A.push_back(j); break;
+      case 1: A.push_back(j); break;
-			case 2: B.push_back(j); break;
+      case 2: B.push_back(j); break;
-			default: std::cout << j << ": " << dictionary[j] << std::endl;
+      default: std::cout << j << ": " << dictionary[j] << std::endl;
-			throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark");
+      throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark");
-			}
+      }
-		}
+    }
-	}
+  }
 
 #define REDUCE_CAMERA_GRAPH
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template<class GenericGraph>
+  template<class GenericGraph>
-	boost::optional<MetisResult> findPartitoning(const GenericGraph& graph, const std::vector<size_t>& keys,
+  boost::optional<MetisResult> findPartitoning(const GenericGraph& graph, const std::vector<size_t>& keys,
-			WorkSpace& workspace, bool verbose,
+      WorkSpace& workspace, bool verbose,
-			const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph) {
+      const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph) {
-		boost::optional<MetisResult> result;
+    boost::optional<MetisResult> result;
-		GenericGraph reducedGraph;
+    GenericGraph reducedGraph;
-		std::vector<size_t> keyToPartition;
+    std::vector<size_t> keyToPartition;
-		std::vector<size_t> cameraKeys, landmarkKeys;
+    std::vector<size_t> cameraKeys, landmarkKeys;
-		if (reduceGraph) {
+    if (reduceGraph) {
-			if (!int2symbol.is_initialized())
+      if (!int2symbol.is_initialized())
-				throw std::invalid_argument("findSeparator: int2symbol must be valid!");
+        throw std::invalid_argument("findSeparator: int2symbol must be valid!");
 
-			// find out all the landmark keys, which are to be eliminated
+      // find out all the landmark keys, which are to be eliminated
-			cameraKeys.reserve(keys.size());
+      cameraKeys.reserve(keys.size());
-			landmarkKeys.reserve(keys.size());
+      landmarkKeys.reserve(keys.size());
-			BOOST_FOREACH(const size_t key, keys) {
+      BOOST_FOREACH(const size_t key, keys) {
-				if((*int2symbol)[key].chr() == 'x')
+        if((*int2symbol)[key].chr() == 'x')
-					cameraKeys.push_back(key);
+          cameraKeys.push_back(key);
-				else
+        else
-					landmarkKeys.push_back(key);
+          landmarkKeys.push_back(key);
-			}
+      }
 
-			keyToPartition = cameraKeys;
+      keyToPartition = cameraKeys;
-			workspace.prepareDictionary(keyToPartition);
+      workspace.prepareDictionary(keyToPartition);
-			const std::vector<int>& dictionary = workspace.dictionary;
+      const std::vector<int>& dictionary = workspace.dictionary;
-			reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph);
+      reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph);
-			std::cout << "original graph: V" << keys.size() << ", E" << graph.size()
+      std::cout << "original graph: V" << keys.size() << ", E" << graph.size()
-			<< " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl;
+      << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl;
-			result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose);
+      result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose);
-		}	else // call Metis to partition the graph to A, B, C
+    }  else // call Metis to partition the graph to A, B, C
-			result = separatorPartitionByMetis(graph, keys, workspace, verbose);
+      result = separatorPartitionByMetis(graph, keys, workspace, verbose);
 
-		if (!result.is_initialized()) {
+    if (!result.is_initialized()) {
-			std::cout << "metis failed!" << std::endl;
+      std::cout << "metis failed!" << std::endl;
-			return 0;
+      return 0;
-		}
+    }
 
-		if (reduceGraph) {
+    if (reduceGraph) {
-			addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace);
+      addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace);
-			std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl;
+      std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl;
-		}
+    }
 
-		return result;
+    return result;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template<class GenericGraph>
+  template<class GenericGraph>
-	int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
+  int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
-			const int minNodesPerMap, WorkSpace& workspace, bool verbose,
+      const int minNodesPerMap, WorkSpace& workspace, bool verbose,
-			const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph,
+      const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph,
-			const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
+      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
 
-		boost::optional<MetisResult> result = findPartitoning(graph, keys, workspace, 
+    boost::optional<MetisResult> result = findPartitoning(graph, keys, workspace, 
-			verbose, int2symbol, reduceGraph);
+      verbose, int2symbol, reduceGraph);
 
-		// find the island in A and B, and make them separated submaps
+    // find the island in A and B, and make them separated submaps
-		typedef std::vector<size_t> Island;
+    typedef std::vector<size_t> Island;
-		std::list<Island> islands;
+    std::list<Island> islands;
 
-		std::list<Island> islands_in_A = findIslands(graph, result->A, workspace,
+    std::list<Island> islands_in_A = findIslands(graph, result->A, workspace,
-			minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
+      minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
 
-		std::list<Island> islands_in_B = findIslands(graph, result->B, workspace,
+    std::list<Island> islands_in_B = findIslands(graph, result->B, workspace,
-			minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
+      minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
 
-		islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end());
+    islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end());
-		islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end());
+    islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end());
-		islands.sort(isLargerIsland);
+    islands.sort(isLargerIsland);
-		size_t numIsland0 = islands.size();
+    size_t numIsland0 = islands.size();
 
 #ifdef NDEBUG
-//		verbose = true;
+//    verbose = true;
-//		if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!");
+//    if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!");
-//		std::cout << "sep size: " << result->C.size() << "; ";
+//    std::cout << "sep size: " << result->C.size() << "; ";
-//		printNumCamerasLandmarks(result->C, *int2symbol);
+//    printNumCamerasLandmarks(result->C, *int2symbol);
-//		std::cout << "no. of island: " << islands.size() << "; ";
+//    std::cout << "no. of island: " << islands.size() << "; ";
-//		std::cout << "island size: ";
+//    std::cout << "island size: ";
-//		BOOST_FOREACH(const Island& island, islands)
+//    BOOST_FOREACH(const Island& island, islands)
-//			std::cout << island.size() << " ";
+//      std::cout << island.size() << " ";
-//		std::cout << std::endl;
+//    std::cout << std::endl;
 
-//		BOOST_FOREACH(const Island& island, islands) {
+//    BOOST_FOREACH(const Island& island, islands) {
-//			printNumCamerasLandmarks(island, int2symbol);
+//      printNumCamerasLandmarks(island, int2symbol);
-//		}
+//    }
 #endif
 
-		// absorb small components into the separator
+    // absorb small components into the separator
-		size_t oldSize = islands.size();
+    size_t oldSize = islands.size();
-		while(true) {
+    while(true) {
-			if (islands.size() < 2) {
+      if (islands.size() < 2) {
-				std::cout << "numIsland: " << numIsland0 << std::endl;
+        std::cout << "numIsland: " << numIsland0 << std::endl;
-				throw std::runtime_error("findSeparator: found fewer than 2 submaps!");
+        throw std::runtime_error("findSeparator: found fewer than 2 submaps!");
-			}
+      }
 
-			std::list<Island>::reference island = islands.back();
+      std::list<Island>::reference island = islands.back();
-			if ((int)island.size() >= minNodesPerMap) break;
+      if ((int)island.size() >= minNodesPerMap) break;
-			result->C.insert(result->C.end(), island.begin(), island.end());
+      result->C.insert(result->C.end(), island.begin(), island.end());
-			islands.pop_back();
+      islands.pop_back();
-		}
+    }
-		if (islands.size() != oldSize){
+    if (islands.size() != oldSize){
-			if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl;
+      if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl;
-		}
+    }
-		else{
+    else{
-			if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl;
+      if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl;
-		}
+    }
 
-		// generate the node map
+    // generate the node map
-		std::vector<int>& partitionTable = workspace.partitionTable;
+    std::vector<int>& partitionTable = workspace.partitionTable;
-		std::fill(partitionTable.begin(), partitionTable.end(), -1);
+    std::fill(partitionTable.begin(), partitionTable.end(), -1);
-		BOOST_FOREACH(const size_t key, result->C)
+    BOOST_FOREACH(const size_t key, result->C)
-			partitionTable[key] = 0;
+      partitionTable[key] = 0;
-		int idx = 0;
+    int idx = 0;
-		BOOST_FOREACH(const Island& island, islands) {
+    BOOST_FOREACH(const Island& island, islands) {
-			idx++;
+      idx++;
-			BOOST_FOREACH(const size_t key, island) {
+      BOOST_FOREACH(const size_t key, island) {
-				partitionTable[key] = idx;
+        partitionTable[key] = idx;
-			}
+      }
-		}
+    }
 
-		return islands.size();
+    return islands.size();
-	}
+  }
 
 }} //namespace

gtsam_unstable/partition/FindSeparator.h

@@ -16,29 +16,29 @@
 
 namespace gtsam { namespace partition {
 
-//	typedef std::map<size_t, size_t> PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id
+//  typedef std::map<size_t, size_t> PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id
 
-	/** the metis Nest dissection result */
+  /** the metis Nest dissection result */
-	struct MetisResult {
+  struct MetisResult {
-		std::vector<size_t> A, B;  // frontals
+    std::vector<size_t> A, B;  // frontals
-		std::vector<size_t> C;     // separator
+    std::vector<size_t> C;     // separator
-	};
+  };
 
-	/**
+  /**
-	 * use Metis library to partition, return the size of separator and the optional partition table
+   * use Metis library to partition, return the size of separator and the optional partition table
-	 * the size of dictionary mush be equal to the number of variables in the original graph (the largest one)
+   * the size of dictionary mush be equal to the number of variables in the original graph (the largest one)
-	 */
+   */
-	template<class GenericGraph>
+  template<class GenericGraph>
-	boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const std::vector<size_t>& keys,
+  boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const std::vector<size_t>& keys,
-			WorkSpace& workspace, bool verbose);
+      WorkSpace& workspace, bool verbose);
 
-	/**
+  /**
-	 * return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**).
+   * return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**).
-	 * return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator.
+   * return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator.
-	 */
+   */
-	template<class GenericGraph>
+  template<class GenericGraph>
-	int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
+  int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
-			const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional<std::vector<Symbol> >& int2symbol,
+      const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional<std::vector<Symbol> >& int2symbol,
-			const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
+      const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
 
 }} //namespace

gtsam_unstable/partition/GenericGraph.cpp

@@ -19,459 +19,459 @@ using namespace std;
 
 namespace gtsam { namespace partition {
 
-	/**
+  /**
-	 * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys}
+   * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys}
-	 */
+   */
-	list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
+  list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
-			const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
+      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
-	{
+  {
-		typedef pair<int, int> IntPair;
+    typedef pair<int, int> IntPair;
-		typedef list<sharedGenericFactor2D> FactorList;
+    typedef list<sharedGenericFactor2D> FactorList;
-		typedef map<IntPair, FactorList::iterator> Connections;
+    typedef map<IntPair, FactorList::iterator> Connections;
 
-		// create disjoin set forest
+    // create disjoin set forest
-		DSFVector dsf(workspace.dsf, keys);
+    DSFVector dsf(workspace.dsf, keys);
 
-		FactorList factors(graph.begin(), graph.end());
+    FactorList factors(graph.begin(), graph.end());
-		size_t nrFactors = factors.size();
+    size_t nrFactors = factors.size();
-		FactorList::iterator itEnd;
+    FactorList::iterator itEnd;
-		workspace.prepareDictionary(keys);
+    workspace.prepareDictionary(keys);
-		while (nrFactors) {
+    while (nrFactors) {
-			Connections connections;
+      Connections connections;
-			bool succeed = false;
+      bool succeed = false;
-			itEnd = factors.end();
+      itEnd = factors.end();
-			list<FactorList::iterator> toErase;
+      list<FactorList::iterator> toErase;
-			for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
+      for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
 
-				// remove invalid factors
+        // remove invalid factors
-				GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
+        GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
-				if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
+        if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
-					toErase.push_back(itFactor);	nrFactors--; continue;
+          toErase.push_back(itFactor);  nrFactors--; continue;
-				}
+        }
 
-				size_t label1 = dsf.findSet(key1.index);
+        size_t label1 = dsf.findSet(key1.index);
-				size_t label2 = dsf.findSet(key2.index);
+        size_t label2 = dsf.findSet(key2.index);
-				if (label1 == label2) {	toErase.push_back(itFactor);	nrFactors--; continue; }
+        if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }
 
-				// merge two trees if the connection is strong enough, otherwise cache it
+        // merge two trees if the connection is strong enough, otherwise cache it
-				// an odometry factor always merges two islands
+        // an odometry factor always merges two islands
-				if (key1.type == NODE_POSE_2D && key2.type  == NODE_POSE_2D) {
+        if (key1.type == NODE_POSE_2D && key2.type  == NODE_POSE_2D) {
-					toErase.push_back(itFactor); nrFactors--;
+          toErase.push_back(itFactor); nrFactors--;
-					dsf.makeUnionInPlace(label1, label2);
+          dsf.makeUnionInPlace(label1, label2);
-					succeed = true;
+          succeed = true;
-					break;
+          break;
-				}
+        }
 
-				// single landmark island only need one measurement
+        // single landmark island only need one measurement
-				if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
+        if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
-						(dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
+            (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
-					toErase.push_back(itFactor); nrFactors--;
+          toErase.push_back(itFactor); nrFactors--;
-					dsf.makeUnionInPlace(label1, label2);
+          dsf.makeUnionInPlace(label1, label2);
-					succeed = true;
+          succeed = true;
-					break;
+          break;
-				}
+        }
 
-				// stack the current factor with the cached constraint
+        // stack the current factor with the cached constraint
-				IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
+        IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
-				Connections::iterator itCached = connections.find(labels);
+        Connections::iterator itCached = connections.find(labels);
-				if (itCached == connections.end()) {
+        if (itCached == connections.end()) {
-					connections.insert(make_pair(labels, itFactor));
+          connections.insert(make_pair(labels, itFactor));
-					continue;
+          continue;
-				} else {
+        } else {
-					GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
+          GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
-					// if observe the same landmark, we can not merge, abandon the current factor
+          // if observe the same landmark, we can not merge, abandon the current factor
-					if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) ||
+          if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) ||
-							(key1.index == key22.index && key1.type == NODE_LANDMARK_2D) ||
+              (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) ||
-							(key2.index == key21.index && key2.type == NODE_LANDMARK_2D) ||
+              (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) ||
-							(key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) {
+              (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) {
-						toErase.push_back(itFactor); nrFactors--;
+            toErase.push_back(itFactor); nrFactors--;
-						continue;
+            continue;
-					} else {
+          } else {
-						toErase.push_back(itFactor); nrFactors--;
+            toErase.push_back(itFactor); nrFactors--;
-						toErase.push_back(itCached->second); nrFactors--;
+            toErase.push_back(itCached->second); nrFactors--;
-						dsf.makeUnionInPlace(label1, label2);
+            dsf.makeUnionInPlace(label1, label2);
-						connections.erase(itCached);
+            connections.erase(itCached);
-						succeed = true;
+            succeed = true;
-						break;
+            break;
-					}
+          }
-				}
+        }
-			}
+      }
 
-			// erase unused factors
+      // erase unused factors
-			BOOST_FOREACH(const FactorList::iterator& it, toErase)
+      BOOST_FOREACH(const FactorList::iterator& it, toErase)
-				factors.erase(it);
+        factors.erase(it);
 
-			if (!succeed) break;
+      if (!succeed) break;
-		}
+    }
 
-		list<vector<size_t> > islands;
+    list<vector<size_t> > islands;
-		map<size_t, vector<size_t> > arrays = dsf.arrays();
+    map<size_t, vector<size_t> > arrays = dsf.arrays();
-		size_t key; vector<size_t> array;
+    size_t key; vector<size_t> array;
-		BOOST_FOREACH(boost::tie(key, array), arrays)
+    BOOST_FOREACH(boost::tie(key, array), arrays)
-			islands.push_back(array);
+      islands.push_back(array);
-		return islands;
+    return islands;
-	}
+  }
 
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	void print(const GenericGraph2D& graph, const std::string name) {
+  void print(const GenericGraph2D& graph, const std::string name) {
-		cout << name << endl;
+    cout << name << endl;
-		BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
+    BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
-			cout << factor_->key1.index << " " << factor_->key2.index << endl;
+      cout << factor_->key1.index << " " << factor_->key2.index << endl;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	void print(const GenericGraph3D& graph, const std::string name) {
+  void print(const GenericGraph3D& graph, const std::string name) {
-		cout << name << endl;
+    cout << name << endl;
-		BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
+    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
-			cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
+      cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
-			factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
+      factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	// create disjoin set forest
+  // create disjoin set forest
-	DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
+  DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
-		DSFVector dsf(workspace.dsf, keys);
+    DSFVector dsf(workspace.dsf, keys);
-		typedef list<sharedGenericFactor3D> FactorList;
+    typedef list<sharedGenericFactor3D> FactorList;
 
-		FactorList factors(graph.begin(), graph.end());
+    FactorList factors(graph.begin(), graph.end());
-		size_t nrFactors = factors.size();
+    size_t nrFactors = factors.size();
-		FactorList::iterator itEnd;
+    FactorList::iterator itEnd;
-		while (nrFactors) {
+    while (nrFactors) {
 
-			bool succeed = false;
+      bool succeed = false;
-			itEnd = factors.end();
+      itEnd = factors.end();
-			list<FactorList::iterator> toErase;
+      list<FactorList::iterator> toErase;
-			for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
+      for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
 
-				// remove invalid factors
+        // remove invalid factors
-				if (graph.size() == 178765) cout << "kai21" <<  endl;
+        if (graph.size() == 178765) cout << "kai21" <<  endl;
-				GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
+        GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
-				if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
+        if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
-				if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
+        if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
-					toErase.push_back(itFactor);	nrFactors--; continue;
+          toErase.push_back(itFactor);  nrFactors--; continue;
-				}
+        }
 
-				if (graph.size() == 178765) cout << "kai22" << endl;
+        if (graph.size() == 178765) cout << "kai22" << endl;
-				size_t label1 = dsf.findSet(key1.index);
+        size_t label1 = dsf.findSet(key1.index);
-				size_t label2 = dsf.findSet(key2.index);
+        size_t label2 = dsf.findSet(key2.index);
-				if (label1 == label2) {	toErase.push_back(itFactor);	nrFactors--; continue; }
+        if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }
 
-				if (graph.size() == 178765) cout << "kai23" << endl;
+        if (graph.size() == 178765) cout << "kai23" << endl;
-				// merge two trees if the connection is strong enough, otherwise cache it
+        // merge two trees if the connection is strong enough, otherwise cache it
-				// an odometry factor always merges two islands
+        // an odometry factor always merges two islands
-				if ((key1.type == NODE_POSE_3D && key2.type  == NODE_LANDMARK_3D) ||
+        if ((key1.type == NODE_POSE_3D && key2.type  == NODE_LANDMARK_3D) ||
-						(key1.type == NODE_POSE_3D && key2.type  == NODE_POSE_3D)) {
+            (key1.type == NODE_POSE_3D && key2.type  == NODE_POSE_3D)) {
-					toErase.push_back(itFactor); nrFactors--;
+          toErase.push_back(itFactor); nrFactors--;
-					dsf.makeUnionInPlace(label1, label2);
+          dsf.makeUnionInPlace(label1, label2);
-					succeed = true;
+          succeed = true;
-					break;
+          break;
-				}
+        }
 
-				if (graph.size() == 178765) cout << "kai24" << endl;
+        if (graph.size() == 178765) cout << "kai24" << endl;
 
 
-			}
+      }
 
-			// erase unused factors
+      // erase unused factors
-			BOOST_FOREACH(const FactorList::iterator& it, toErase)
+      BOOST_FOREACH(const FactorList::iterator& it, toErase)
-			factors.erase(it);
+      factors.erase(it);
 
-			if (!succeed) break;
+      if (!succeed) break;
-		}
+    }
-		return dsf;
+    return dsf;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	// first check the type of the key (pose or landmark), and then check whether it is singular
+  // first check the type of the key (pose or landmark), and then check whether it is singular
-	inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) {
+  inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) {
-		switch(node.type) {
+    switch(node.type) {
-		case NODE_POSE_3D:
+    case NODE_POSE_3D:
-			return singularCameras.find(node.index) != singularCameras.end(); break;
+      return singularCameras.find(node.index) != singularCameras.end(); break;
-		case NODE_LANDMARK_3D:
+    case NODE_LANDMARK_3D:
-			return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
+      return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
-		default:
+    default:
-			throw runtime_error("unrecognized key type!");
+      throw runtime_error("unrecognized key type!");
-		}
+    }
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
+  void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
-			const vector<bool>& isCamera, const vector<bool>& isLandmark,
+      const vector<bool>& isCamera, const vector<bool>& isLandmark,
-			set<size_t>& singularCameras, set<size_t>& singularLandmarks,	vector<int>& nrConstraints,
+      set<size_t>& singularCameras, set<size_t>& singularLandmarks,  vector<int>& nrConstraints,
-			bool& foundSingularCamera, bool& foundSingularLandmark,
+      bool& foundSingularCamera, bool& foundSingularLandmark,
-			const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
+      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
 
-		// compute the constraint number per camera
+    // compute the constraint number per camera
-		std::fill(nrConstraints.begin(),  nrConstraints.end(),    0);
+    std::fill(nrConstraints.begin(),  nrConstraints.end(),    0);
-		BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
-			const int& key1 = factor_->key1.index;
+      const int& key1 = factor_->key1.index;
-			const int& key2 = factor_->key2.index;
+      const int& key2 = factor_->key2.index;
-			if (workspace.dictionary[key1] != -1 &&	workspace.dictionary[key2] != -1 &&
+      if (workspace.dictionary[key1] != -1 &&  workspace.dictionary[key2] != -1 &&
-					!isSingular(singularCameras, singularLandmarks, factor_->key1) &&
+          !isSingular(singularCameras, singularLandmarks, factor_->key1) &&
-					!isSingular(singularCameras, singularLandmarks, factor_->key2)) {
+          !isSingular(singularCameras, singularLandmarks, factor_->key2)) {
-				nrConstraints[key1]++;
+        nrConstraints[key1]++;
-				nrConstraints[key2]++;
+        nrConstraints[key2]++;
 
-				// a single pose constraint is sufficient for stereo, so we add 2 to the counter
+        // a single pose constraint is sufficient for stereo, so we add 2 to the counter
-				// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
+        // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
-				if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
+        if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
-					nrConstraints[key1]+=2;
+          nrConstraints[key1]+=2;
-					nrConstraints[key2]+=2;
+          nrConstraints[key2]+=2;
-				}
+        }
-			}
+      }
-		}
+    }
 
-		// find singular cameras and landmarks
+    // find singular cameras and landmarks
-		foundSingularCamera = false;
+    foundSingularCamera = false;
-		foundSingularLandmark = false;
+    foundSingularLandmark = false;
-		for (size_t i=0; i<nrConstraints.size(); i++) {
+    for (size_t i=0; i<nrConstraints.size(); i++) {
-			if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
+      if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
-					singularCameras.find(i) == singularCameras.end()) {
+          singularCameras.find(i) == singularCameras.end()) {
-				singularCameras.insert(i);
+        singularCameras.insert(i);
-				foundSingularCamera = true;
+        foundSingularCamera = true;
-			}
+      }
-			if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
+      if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
-					singularLandmarks.find(i) == singularLandmarks.end()) {
+          singularLandmarks.find(i) == singularLandmarks.end()) {
-				singularLandmarks.insert(i);
+        singularLandmarks.insert(i);
-				foundSingularLandmark = true;
+        foundSingularLandmark = true;
-			}
+      }
-		}
+    }
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
+  list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
-			const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
+      const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
 
-		// create disjoint set forest
+    // create disjoint set forest
-		workspace.prepareDictionary(keys);
+    workspace.prepareDictionary(keys);
-		DSFVector dsf = createDSF(graph, keys, workspace);
+    DSFVector dsf = createDSF(graph, keys, workspace);
 
-		const bool verbose = false;
+    const bool verbose = false;
-		bool foundSingularCamera = true;
+    bool foundSingularCamera = true;
-		bool foundSingularLandmark = true;
+    bool foundSingularLandmark = true;
 
-		list<vector<size_t> > islands;
+    list<vector<size_t> > islands;
-		set<size_t> singularCameras, singularLandmarks;
+    set<size_t> singularCameras, singularLandmarks;
-		vector<bool> isCamera(workspace.dictionary.size(), false);
+    vector<bool> isCamera(workspace.dictionary.size(), false);
-		vector<bool> isLandmark(workspace.dictionary.size(), false);
+    vector<bool> isLandmark(workspace.dictionary.size(), false);
 
-	  // check the constraint number of every variable
+    // check the constraint number of every variable
-		// find the camera and landmark keys
+    // find the camera and landmark keys
-		BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
-			//assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
+      //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
-			if (workspace.dictionary[factor_->key1.index] != -1) {
+      if (workspace.dictionary[factor_->key1.index] != -1) {
-				if (factor_->key1.type == NODE_POSE_3D)
+        if (factor_->key1.type == NODE_POSE_3D)
-					isCamera[factor_->key1.index] = true;
+          isCamera[factor_->key1.index] = true;
-				else
+        else
-					isLandmark[factor_->key1.index] = true;
+          isLandmark[factor_->key1.index] = true;
-			}
+      }
             if (workspace.dictionary[factor_->key2.index] != -1) {
-				if (factor_->key2.type == NODE_POSE_3D)
+        if (factor_->key2.type == NODE_POSE_3D)
-					isCamera[factor_->key2.index] = true;
+          isCamera[factor_->key2.index] = true;
-				else
+        else
-					isLandmark[factor_->key2.index] = true;
+          isLandmark[factor_->key2.index] = true;
             }
-		}
+    }
 
-		vector<int> nrConstraints(workspace.dictionary.size(), 0);
+    vector<int> nrConstraints(workspace.dictionary.size(), 0);
-		// iterate until all singular variables have been removed. Removing a singular variable
+    // iterate until all singular variables have been removed. Removing a singular variable
-		// can cause another to become singular, so this will probably run several times
+    // can cause another to become singular, so this will probably run several times
-		while (foundSingularCamera || foundSingularLandmark) {
+    while (foundSingularCamera || foundSingularLandmark) {
-			findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark,      // input
+      findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark,      // input
-					singularCameras, singularLandmarks, nrConstraints,                    // output
+          singularCameras, singularLandmarks, nrConstraints,                    // output
-					foundSingularCamera, foundSingularLandmark,                           // output
+          foundSingularCamera, foundSingularLandmark,                           // output
-					minNrConstraintsPerCamera,  minNrConstraintsPerLandmark);             // input
+          minNrConstraintsPerCamera,  minNrConstraintsPerLandmark);             // input
-		}
+    }
 
-		// add singular variables directly as islands
+    // add singular variables directly as islands
-		if (!singularCameras.empty()) {
+    if (!singularCameras.empty()) {
-			if (verbose) cout << "singular cameras:";
+      if (verbose) cout << "singular cameras:";
-			BOOST_FOREACH(const size_t i, singularCameras) {
+      BOOST_FOREACH(const size_t i, singularCameras) {
-				islands.push_back(vector<size_t>(1, i)); // <---------------------------
+        islands.push_back(vector<size_t>(1, i)); // <---------------------------
-				if (verbose) cout << i << " ";
+        if (verbose) cout << i << " ";
-			}
+      }
-			if (verbose) cout << endl;
+      if (verbose) cout << endl;
-		}
+    }
-		if (!singularLandmarks.empty()) {
+    if (!singularLandmarks.empty()) {
-			if (verbose) cout << "singular landmarks:";
+      if (verbose) cout << "singular landmarks:";
-			BOOST_FOREACH(const size_t i, singularLandmarks) {
+      BOOST_FOREACH(const size_t i, singularLandmarks) {
-				islands.push_back(vector<size_t>(1, i)); // <---------------------------
+        islands.push_back(vector<size_t>(1, i)); // <---------------------------
-				if (verbose) cout << i << " ";
+        if (verbose) cout << i << " ";
-			}
+      }
-			if (verbose) cout << endl;
+      if (verbose) cout << endl;
-		}
+    }
 
 
-		// regenerating islands
+    // regenerating islands
-		map<size_t, vector<size_t> > labelIslands = dsf.arrays();
+    map<size_t, vector<size_t> > labelIslands = dsf.arrays();
-		size_t label; vector<size_t> island;
+    size_t label; vector<size_t> island;
-		BOOST_FOREACH(boost::tie(label, island), labelIslands) {
+    BOOST_FOREACH(boost::tie(label, island), labelIslands) {
-			vector<size_t> filteredIsland; // remove singular cameras from array
+      vector<size_t> filteredIsland; // remove singular cameras from array
-			filteredIsland.reserve(island.size());
+      filteredIsland.reserve(island.size());
-			BOOST_FOREACH(const size_t key, island) {
+      BOOST_FOREACH(const size_t key, island) {
-				if ((isCamera[key]   && singularCameras.find(key) == singularCameras.end()) ||        // not singular
+        if ((isCamera[key]   && singularCameras.find(key) == singularCameras.end()) ||        // not singular
-						(isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) ||    // not singular
+            (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) ||    // not singular
-						(!isCamera[key] && !isLandmark[key])) {   // the key is not involved in any factor, so the type is undertermined
+            (!isCamera[key] && !isLandmark[key])) {   // the key is not involved in any factor, so the type is undertermined
-					filteredIsland.push_back(key);
+          filteredIsland.push_back(key);
-				}
+        }
-			}
+      }
-			islands.push_back(filteredIsland);
+      islands.push_back(filteredIsland);
-		}
+    }
 
-		// sanity check
+    // sanity check
-		size_t nrKeys = 0;
+    size_t nrKeys = 0;
-		BOOST_FOREACH(const vector<size_t>& island, islands)
+    BOOST_FOREACH(const vector<size_t>& island, islands)
-			nrKeys += island.size();
+      nrKeys += island.size();
-		if (nrKeys != keys.size())	{
+    if (nrKeys != keys.size())  {
-			cout << nrKeys << " vs " << keys.size() << endl;
+      cout << nrKeys << " vs " << keys.size() << endl;
-			throw runtime_error("findIslands: the number of keys is inconsistent!");
+      throw runtime_error("findIslands: the number of keys is inconsistent!");
-		}
+    }
 
 
-		if (verbose) cout << "found " << islands.size() << " islands!" << endl;
+    if (verbose) cout << "found " << islands.size() << " islands!" << endl;
-		return islands;
+    return islands;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	// return the number of intersection between two **sorted** landmark vectors
+  // return the number of intersection between two **sorted** landmark vectors
-	inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
+  inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
-		size_t i1 = 0, i2 = 0;
+    size_t i1 = 0, i2 = 0;
-		int nrCommonLandmarks = 0;
+    int nrCommonLandmarks = 0;
-		while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
+    while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
-			if (landmarks1[i1] < landmarks2[i2])
+      if (landmarks1[i1] < landmarks2[i2])
-				i1 ++;
+        i1 ++;
-			else if (landmarks1[i1] > landmarks2[i2])
+      else if (landmarks1[i1] > landmarks2[i2])
-				i2 ++;
+        i2 ++;
-			else {
+      else {
-				i1++; i2++;
+        i1++; i2++;
-				nrCommonLandmarks ++;
+        nrCommonLandmarks ++;
-			}
+      }
-		}
+    }
-		return nrCommonLandmarks;
+    return nrCommonLandmarks;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys,	const std::vector<size_t>& landmarkKeys,
+  void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys,  const std::vector<size_t>& landmarkKeys,
-			const std::vector<int>& dictionary,	GenericGraph3D& reducedGraph) {
+      const std::vector<int>& dictionary,  GenericGraph3D& reducedGraph) {
 
-		typedef size_t CameraKey;
+    typedef size_t CameraKey;
-		typedef pair<CameraKey, CameraKey> CameraPair;
+    typedef pair<CameraKey, CameraKey> CameraPair;
-		typedef size_t LandmarkKey;
+    typedef size_t LandmarkKey;
-		// get a mapping from each landmark to its connected cameras
+    // get a mapping from each landmark to its connected cameras
-		vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
+    vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
-		// for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
+    // for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
-		vector<int> cameraToCamera(dictionary.size(), -1);
+    vector<int> cameraToCamera(dictionary.size(), -1);
-		size_t key_i, key_j;
+    size_t key_i, key_j;
-		BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
-			if (factor_->key1.type == NODE_POSE_3D) {
+      if (factor_->key1.type == NODE_POSE_3D) {
-				if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
+        if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
-					cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
+          cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
-				}
+        }
-				else { // odometry factor
+        else { // odometry factor
-					if (factor_->key1.index < factor_->key2.index) {
+          if (factor_->key1.index < factor_->key2.index) {
-						key_i = factor_->key1.index;
+            key_i = factor_->key1.index;
-						key_j = factor_->key2.index;
+            key_j = factor_->key2.index;
-					} else {
+          } else {
-						key_i = factor_->key2.index;
+            key_i = factor_->key2.index;
-						key_j = factor_->key1.index;
+            key_j = factor_->key1.index;
-					}
+          }
-					cameraToCamera[key_i] = key_j;
+          cameraToCamera[key_i] = key_j;
-				}
+        }
-			}
+      }
-		}
+    }
 
-		// sort the landmark keys for the late getNrCommonLandmarks call
+    // sort the landmark keys for the late getNrCommonLandmarks call
-		BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
+    BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
-			if (!landmarks.empty())
+      if (!landmarks.empty())
-				std::sort(landmarks.begin(), landmarks.end());
+        std::sort(landmarks.begin(), landmarks.end());
-		}
+    }
 
-		// generate the reduced graph
+    // generate the reduced graph
-		reducedGraph.clear();
+    reducedGraph.clear();
-		int factorIndex = 0;
+    int factorIndex = 0;
-		int camera1, camera2, nrTotalConstraints;
+    int camera1, camera2, nrTotalConstraints;
-		bool hasOdometry;
+    bool hasOdometry;
-		for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
+    for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
-			for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
+      for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
-				camera1 = cameraKeys[i1];
+        camera1 = cameraKeys[i1];
-				camera2 = cameraKeys[i2];
+        camera2 = cameraKeys[i2];
-				int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
+        int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
-				hasOdometry =  cameraToCamera[camera1] == camera2;
+        hasOdometry =  cameraToCamera[camera1] == camera2;
-				if (nrCommonLandmarks > 0 || hasOdometry) {
+        if (nrCommonLandmarks > 0 || hasOdometry) {
-					nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
+          nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
-					reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
+          reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
-							factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
+              factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
-				}
+        }
-			}
+      }
-		}
+    }
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
+  void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
-			WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
+      WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
-		workspace.prepareDictionary(frontals);
+    workspace.prepareDictionary(frontals);
-		vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
+    vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
 
-		// summarize the constraint number
+    // summarize the constraint number
-		const vector<int>& dictionary = workspace.dictionary;
+    const vector<int>& dictionary = workspace.dictionary;
-		vector<bool> isValidCamera(workspace.dictionary.size(), false);
+    vector<bool> isValidCamera(workspace.dictionary.size(), false);
-		vector<bool> isValidLandmark(workspace.dictionary.size(), false);
+    vector<bool> isValidLandmark(workspace.dictionary.size(), false);
-		BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
+    BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
-			assert(factor_->key1.type == NODE_POSE_3D);
+      assert(factor_->key1.type == NODE_POSE_3D);
-			//assert(factor_->key2.type == NODE_LANDMARK_3D);
+      //assert(factor_->key2.type == NODE_LANDMARK_3D);
-			const size_t& key1 = factor_->key1.index;
+      const size_t& key1 = factor_->key1.index;
-			const size_t& key2 = factor_->key2.index;
+      const size_t& key2 = factor_->key2.index;
-			if (dictionary[key1] == -1 || dictionary[key2] == -1)
+      if (dictionary[key1] == -1 || dictionary[key2] == -1)
-				continue;
+        continue;
 
-			isValidCamera[key1] = true;
+      isValidCamera[key1] = true;
-			if(factor_->key2.type == NODE_LANDMARK_3D)
+      if(factor_->key2.type == NODE_LANDMARK_3D)
-			  isValidLandmark[key2] = true;
+        isValidLandmark[key2] = true;
-			else
+      else
-				isValidCamera[key2] = true;
+        isValidCamera[key2] = true;
 
-			nrConstraints[key1]++;
+      nrConstraints[key1]++;
-			nrConstraints[key2]++;
+      nrConstraints[key2]++;
 
-			// a single pose constraint is sufficient for stereo, so we add 2 to the counter
+      // a single pose constraint is sufficient for stereo, so we add 2 to the counter
-			// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
+      // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
-			if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
+      if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
-				nrConstraints[key1]+=2;
+        nrConstraints[key1]+=2;
-				nrConstraints[key2]+=2;
+        nrConstraints[key2]+=2;
-			}
+      }
-		}
+    }
 
-		// find the minimum constraint for cameras and landmarks
+    // find the minimum constraint for cameras and landmarks
-		size_t minFoundConstraintsPerCamera = 10000;
+    size_t minFoundConstraintsPerCamera = 10000;
-		size_t minFoundConstraintsPerLandmark = 10000;
+    size_t minFoundConstraintsPerLandmark = 10000;
 
-		for (size_t i=0; i<isValidCamera.size(); i++) {
+    for (size_t i=0; i<isValidCamera.size(); i++) {
-			if (isValidCamera[i]) {
+      if (isValidCamera[i]) {
-				minFoundConstraintsPerCamera   = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
+        minFoundConstraintsPerCamera   = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
-				if (nrConstraints[i] < minNrConstraintsPerCamera)
+        if (nrConstraints[i] < minNrConstraintsPerCamera)
-							cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
+              cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
-			}
+      }
 
-		}
+    }
-		for (size_t j=0; j<isValidLandmark.size(); j++) {
+    for (size_t j=0; j<isValidLandmark.size(); j++) {
-			if (isValidLandmark[j]) {
+      if (isValidLandmark[j]) {
-				minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
+        minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
-			  if (nrConstraints[j] < minNrConstraintsPerLandmark)
+        if (nrConstraints[j] < minNrConstraintsPerLandmark)
-				  cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
+          cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
-			}
+      }
-		}
+    }
 
-		// debug info
+    // debug info
-		BOOST_FOREACH(const size_t key, frontals) {
+    BOOST_FOREACH(const size_t key, frontals) {
-			if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
+      if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
-				cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
+        cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
-		}
+    }
 
- 		if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
+     if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
-			throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
+      throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
-		if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
+    if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
-			throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
+      throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
-	}
+  }
 
 }} // namespace

gtsam_unstable/partition/GenericGraph.h

@@ -17,133 +17,133 @@
 
 namespace gtsam { namespace partition {
 
-	/***************************************************
+  /***************************************************
-	 * 2D generic factors and their factor graph
+   * 2D generic factors and their factor graph
-	 ***************************************************/
+   ***************************************************/
-	enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
+  enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
 
-	/** the index of the node and the type of the node */
+  /** the index of the node and the type of the node */
-	struct GenericNode2D {
+  struct GenericNode2D {
-		std::size_t index;
+    std::size_t index;
-		GenericNode2DType type;
+    GenericNode2DType type;
-		GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
+    GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
-	};
+  };
 
-	/** a factor always involves two nodes/variables for now */
+  /** a factor always involves two nodes/variables for now */
-	struct GenericFactor2D {
+  struct GenericFactor2D {
-		GenericNode2D key1;
+    GenericNode2D key1;
-		GenericNode2D key2;
+    GenericNode2D key2;
-		int index;          // the factor index in the original nonlinear factor graph
+    int index;          // the factor index in the original nonlinear factor graph
-		int weight;         // the weight of the edge
+    int weight;         // the weight of the edge
-		GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1)
+    GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1)
-		: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
+    : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
-		GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1)
+    GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1)
-				: key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
+        : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
-				  key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
+          key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
-	};
+  };
 
-	/** graph is a collection of factors */
+  /** graph is a collection of factors */
-	typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
+  typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
-	typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
+  typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
 
-	/** merge nodes in DSF using constraints captured by the given graph */
+  /** merge nodes in DSF using constraints captured by the given graph */
-	std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
+  std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
-			const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
+      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
 
-	/** eliminate the sensors from generic graph */
+  /** eliminate the sensors from generic graph */
-	inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys,	const std::vector<size_t>& landmarkKeys,
+  inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys,  const std::vector<size_t>& landmarkKeys,
-			const std::vector<int>& dictionary,	GenericGraph2D& reducedGraph) {
+      const std::vector<int>& dictionary,  GenericGraph2D& reducedGraph) {
-		throw std::runtime_error("reduceGenericGraph 2d not implemented");
+    throw std::runtime_error("reduceGenericGraph 2d not implemented");
-	}
+  }
 
-	/** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */
+  /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */
-	inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
+  inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
-			WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {	return; }
+      WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {  return; }
 
-	/** print the graph **/
+  /** print the graph **/
-	void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
+  void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
 
-	/***************************************************
+  /***************************************************
-	 * 3D generic factors and their factor graph
+   * 3D generic factors and their factor graph
-	 ***************************************************/
+   ***************************************************/
-	enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
+  enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
 
-//	const int minNrConstraintsPerCamera = 7;
+//  const int minNrConstraintsPerCamera = 7;
-//	const int minNrConstraintsPerLandmark = 2;
+//  const int minNrConstraintsPerLandmark = 2;
 
-	/** the index of the node and the type of the node */
+  /** the index of the node and the type of the node */
-	struct GenericNode3D {
+  struct GenericNode3D {
-		std::size_t index;
+    std::size_t index;
-		GenericNode3DType type;
+    GenericNode3DType type;
-		GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
+    GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
-	};
+  };
 
-	/** a factor always involves two nodes/variables for now */
+  /** a factor always involves two nodes/variables for now */
-	struct GenericFactor3D {
+  struct GenericFactor3D {
-		GenericNode3D key1;
+    GenericNode3D key1;
-		GenericNode3D key2;
+    GenericNode3D key2;
-		int index; // the index in the entire graph, 0-based
+    int index; // the index in the entire graph, 0-based
-		int weight; // the weight of the edge
+    int weight; // the weight of the edge
-		GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {}
+    GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {}
-		GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1,
+    GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1,
-				const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1)
+        const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1)
-		: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
+    : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
-	};
+  };
 
-	/** graph is a collection of factors */
+  /** graph is a collection of factors */
-	typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
+  typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
-	typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
+  typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
 
-	/** merge nodes in DSF using constraints captured by the given graph */
+  /** merge nodes in DSF using constraints captured by the given graph */
-	std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
+  std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
-			const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
+      const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
 
-	/** eliminate the sensors from generic graph */
+  /** eliminate the sensors from generic graph */
-	void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys,	const std::vector<size_t>& landmarkKeys,
+  void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys,  const std::vector<size_t>& landmarkKeys,
-			const std::vector<int>& dictionary,	GenericGraph3D& reducedGraph);
+      const std::vector<int>& dictionary,  GenericGraph3D& reducedGraph);
 
-	/** check whether the 3D graph is singular (under constrained) */
+  /** check whether the 3D graph is singular (under constrained) */
-	void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
+  void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
-			WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
+      WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
 
 
-	/** print the graph **/
+  /** print the graph **/
-	void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
+  void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
 
-	/***************************************************
+  /***************************************************
-	 * unary generic factors and their factor graph
+   * unary generic factors and their factor graph
-	 ***************************************************/
+   ***************************************************/
-	/** a factor involves a single variable */
+  /** a factor involves a single variable */
-	struct GenericUnaryFactor {
+  struct GenericUnaryFactor {
-		GenericNode2D key;
+    GenericNode2D key;
-		int index;          // the factor index in the original nonlinear factor graph
+    int index;          // the factor index in the original nonlinear factor graph
-		GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
+    GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
-		: key(key_, type_), index(index_) {}
+    : key(key_, type_), index(index_) {}
-		GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
+    GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
-		: key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
+    : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
-	};
+  };
 
-	/** graph is a collection of factors */
+  /** graph is a collection of factors */
-	typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
+  typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
-	typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
+  typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
 
-	/***************************************************
+  /***************************************************
-	 *               utility functions
+   *               utility functions
-	***************************************************/
+  ***************************************************/
-	inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
+  inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
-		if (cameras1.empty() || cameras2.empty())
+    if (cameras1.empty() || cameras2.empty())
-			throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
+      throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
-		std::set<size_t>::const_iterator it1 = cameras1.begin();
+    std::set<size_t>::const_iterator it1 = cameras1.begin();
-		std::set<size_t>::const_iterator it2 = cameras2.begin();
+    std::set<size_t>::const_iterator it2 = cameras2.begin();
-		while (it1 != cameras1.end() && it2 != cameras2.end()) {
+    while (it1 != cameras1.end() && it2 != cameras2.end()) {
-			if (*it1 == *it2)
+      if (*it1 == *it2)
-				return true;
+        return true;
-			else if (*it1 < *it2)
+      else if (*it1 < *it2)
-				it1++;
+        it1++;
-			else
+      else
-				it2++;
+        it2++;
-		}
+    }
-		return false;
+    return false;
-	}
+  }
 
 }} // namespace

gtsam_unstable/partition/NestedDissection-inl.h

@@ -16,236 +16,236 @@
 
 namespace gtsam { namespace partition {
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
+  NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
-			const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
+      const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
-	fg_(fg), ordering_(ordering){
+  fg_(fg), ordering_(ordering){
-		GenericUnaryGraph unaryFactors;
+    GenericUnaryGraph unaryFactors;
-		GenericGraph gfg;
+    GenericGraph gfg;
-		boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
+    boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
 
-		// build reverse mapping from integer to symbol
+    // build reverse mapping from integer to symbol
-		int numNodes = ordering.size();
+    int numNodes = ordering.size();
-		int2symbol_.resize(numNodes);
+    int2symbol_.resize(numNodes);
-		Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
+    Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
-		while(it != itLast)
+    while(it != itLast)
-			int2symbol_[it->second] = (it++)->first;
+      int2symbol_[it->second] = (it++)->first;
 
-		vector<size_t> keys;
+    vector<size_t> keys;
-		keys.reserve(numNodes);
+    keys.reserve(numNodes);
-		for(int i=0; i<ordering.size(); ++i)
+    for(int i=0; i<ordering.size(); ++i)
-			keys.push_back(i);
+      keys.push_back(i);
 
-		WorkSpace workspace(numNodes);
+    WorkSpace workspace(numNodes);
-		root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
+    root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
+  NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
-			const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
+      const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
-		GenericUnaryGraph unaryFactors;
+    GenericUnaryGraph unaryFactors;
-		GenericGraph gfg;
+    GenericGraph gfg;
-		boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
+    boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
 
-		// build reverse mapping from integer to symbol
+    // build reverse mapping from integer to symbol
-		int numNodes = ordering.size();
+    int numNodes = ordering.size();
-		int2symbol_.resize(numNodes);
+    int2symbol_.resize(numNodes);
-		Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
+    Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
-		while(it != itLast)
+    while(it != itLast)
-			int2symbol_[it->second] = (it++)->first;
+      int2symbol_[it->second] = (it++)->first;
 
-		vector<size_t> keys;
+    vector<size_t> keys;
-		keys.reserve(numNodes);
+    keys.reserve(numNodes);
-		for(int i=0; i<ordering.size(); ++i)
+    for(int i=0; i<ordering.size(); ++i)
-			keys.push_back(i);
+      keys.push_back(i);
 
-		WorkSpace workspace(numNodes);
+    WorkSpace workspace(numNodes);
-		root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
+    root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
+  boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
-			const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const {
+      const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const {
-		OrderedSymbols frontalKeys;
+    OrderedSymbols frontalKeys;
-		BOOST_FOREACH(const size_t index, frontals)
+    BOOST_FOREACH(const size_t index, frontals)
-			frontalKeys.push_back(int2symbol_[index]);
+      frontalKeys.push_back(int2symbol_[index]);
 
-		UnorderedSymbols sepKeys;
+    UnorderedSymbols sepKeys;
-		BOOST_FOREACH(const size_t index, sep)
+    BOOST_FOREACH(const size_t index, sep)
-			sepKeys.insert(int2symbol_[index]);
+      sepKeys.insert(int2symbol_[index]);
 
-		return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
+    return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
+  void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
-			const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable,  // input
+      const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable,  // input
-			vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs
+      vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs
-			typename SubNLG::Weeklinks& weeklinks) const {                                                              // the links between child cliques
+      typename SubNLG::Weeklinks& weeklinks) const {                                                              // the links between child cliques
-		list<size_t> sep_; // the separator variables involved in the current factor
+    list<size_t> sep_; // the separator variables involved in the current factor
-		int partition1 = partitionTable[factor->key1.index];
+    int partition1 = partitionTable[factor->key1.index];
-		int partition2 = partitionTable[factor->key2.index];
+    int partition2 = partitionTable[factor->key2.index];
-		if (partition1 <= 0 && partition2 <= 0) {                                // is a factor in the current clique
+    if (partition1 <= 0 && partition2 <= 0) {                                // is a factor in the current clique
-			sepFactors.push_back(fg_[factor->index]);
+      sepFactors.push_back(fg_[factor->index]);
-		}
+    }
-		else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) {  // is a weeklink (factor between two child cliques)
+    else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) {  // is a weeklink (factor between two child cliques)
-			weeklinks.push_back(fg_[factor->index]);
+      weeklinks.push_back(fg_[factor->index]);
-		}
+    }
-		else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
+    else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
-			frontalFactors[partition1 - 1].push_back(factor);
+      frontalFactors[partition1 - 1].push_back(factor);
-		}
+    }
-		else {                                                          // is a joint factor in the child clique (involving varaibles in the current clique)
+    else {                                                          // is a joint factor in the child clique (involving varaibles in the current clique)
-			if (partition1 > 0 && partition2 <= 0) {
+      if (partition1 > 0 && partition2 <= 0) {
-				frontalFactors[partition1 - 1].push_back(factor);
+        frontalFactors[partition1 - 1].push_back(factor);
-				childSeps[partition1 - 1].insert(factor->key2.index);
+        childSeps[partition1 - 1].insert(factor->key2.index);
-			} else if (partition1 <= 0 && partition2 > 0) {
+      } else if (partition1 <= 0 && partition2 > 0) {
-				frontalFactors[partition2 - 1].push_back(factor);
+        frontalFactors[partition2 - 1].push_back(factor);
-				childSeps[partition2 - 1].insert(factor->key1.index);
+        childSeps[partition2 - 1].insert(factor->key1.index);
-			} else
+      } else
-				throw runtime_error("processFactor: unexpected entries in the partition table!");
+        throw runtime_error("processFactor: unexpected entries in the partition table!");
-		}
+    }
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	/**
+  /**
-	 * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors})
+   * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors})
-	 *  and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals})
+   *  and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals})
-	 *  and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into
+   *  and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into
-	 *  the correspoding ordering in {childSeps}.
+   *  the correspoding ordering in {childSeps}.
-	 */
+   */
-	// TODO: frontalFactors and localFrontals should be generated in findSeparator
+  // TODO: frontalFactors and localFrontals should be generated in findSeparator
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
+  void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
-			const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
+      const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
-			const std::vector<int>& partitionTable, const int numSubmaps,                                   // input
+      const std::vector<int>& partitionTable, const int numSubmaps,                                   // input
-			vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors,  NLG& sepFactors,     // output factor graphs
+      vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors,  NLG& sepFactors,     // output factor graphs
-			vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals,  // output sub-orderings
+      vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals,  // output sub-orderings
-			typename SubNLG::Weeklinks& weeklinks) const {                                                             // the links between child cliques
+      typename SubNLG::Weeklinks& weeklinks) const {                                                             // the links between child cliques
 
-		// make three lists of variables A, B, and C
+    // make three lists of variables A, B, and C
-		int partition;
+    int partition;
-		childFrontals.resize(numSubmaps);
+    childFrontals.resize(numSubmaps);
-		BOOST_FOREACH(const size_t key, keys){
+    BOOST_FOREACH(const size_t key, keys){
-			partition = partitionTable[key];
+      partition = partitionTable[key];
-			switch (partition) {
+      switch (partition) {
-			case -1: break;                                        // the separator of the separator variables
+      case -1: break;                                        // the separator of the separator variables
-			case 0:	 localFrontals.push_back(key); break;          // the separator variables
+      case 0:   localFrontals.push_back(key); break;          // the separator variables
-			default: childFrontals[partition-1].push_back(key);    // the frontal variables
+      default: childFrontals[partition-1].push_back(key);    // the frontal variables
-			}
+      }
-		}
+    }
 
-		// group the factors to {frontalFactors} and {sepFactors},and find the joint variables
+    // group the factors to {frontalFactors} and {sepFactors},and find the joint variables
-		vector<set<size_t> > childSeps_;
+    vector<set<size_t> > childSeps_;
-		childSeps_.resize(numSubmaps);
+    childSeps_.resize(numSubmaps);
-		childSeps.reserve(numSubmaps);
+    childSeps.reserve(numSubmaps);
-		frontalFactors.resize(numSubmaps);
+    frontalFactors.resize(numSubmaps);
-		frontalUnaryFactors.resize(numSubmaps);
+    frontalUnaryFactors.resize(numSubmaps);
-		BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
+    BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
-			processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
+      processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
-		BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
+    BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
-			childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
+      childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
 
-		// add unary factor to the current cluster or pass it to one of the child clusters
+    // add unary factor to the current cluster or pass it to one of the child clusters
-		BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
+    BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
-			partition = partitionTable[unaryFactor_->key.index];
+      partition = partitionTable[unaryFactor_->key.index];
-			if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
+      if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
-			else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
+      else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
-		}
+    }
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
+  NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
-			const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
+      const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
-		NLG sepFactors;
+    NLG sepFactors;
-		typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
+    typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
-		while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
+    while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
-		BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
+    BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
-			sepFactors.push_back(fg_[unaryFactor_->index]);
+      sepFactors.push_back(fg_[unaryFactor_->index]);
-		return sepFactors;
+    return sepFactors;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
+  boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
-			const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
+      const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
-			const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
+      const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
 
-		// if no split needed
+    // if no split needed
-		NLG sepFactors; // factors that should remain in the current cluster
+    NLG sepFactors; // factors that should remain in the current cluster
-		if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
+    if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
-			sepFactors = collectOriginalFactors(gfg, unaryFactors);
+      sepFactors = collectOriginalFactors(gfg, unaryFactors);
-			return makeSubNLG(sepFactors, frontals, sep, parent);
+      return makeSubNLG(sepFactors, frontals, sep, parent);
-		}
+    }
 
-		// find the nested dissection separator
+    // find the nested dissection separator
-		int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
+    int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
-				NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
+        NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
-		partition::PartitionTable& partitionTable = workspace.partitionTable;
+    partition::PartitionTable& partitionTable = workspace.partitionTable;
-		if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
+    if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
 
-		// split the factors between child cliques and the current clique
+    // split the factors between child cliques and the current clique
-		vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
+    vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
-		vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
+    vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
-		partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
+    partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
-				frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
+        frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
 
-		// make a new cluster
+    // make a new cluster
-		boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
+    boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
-		current->setWeeklinks(weeklinks);
+    current->setWeeklinks(weeklinks);
 
-		// check whether all the submaps are fully constrained
+    // check whether all the submaps are fully constrained
-		for (int i=0; i<numSubmaps; i++) {
+    for (int i=0; i<numSubmaps; i++) {
-			checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
+      checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
-		}
+    }
 
-		// create child clusters
+    // create child clusters
-		for (int i=0; i<numSubmaps; i++) {
+    for (int i=0; i<numSubmaps; i++) {
-			boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
+      boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
-					numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
+          numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
-			current->addChild(child);
+      current->addChild(child);
-		}
+    }
 
-		return current;
+    return current;
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
+  boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
-			const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
+      const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
-			const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
+      const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
 
-		// if there is no need to cut any more
+    // if there is no need to cut any more
-		NLG sepFactors; // factors that should remain in the current cluster
+    NLG sepFactors; // factors that should remain in the current cluster
-		if (!cuts.get()) {
+    if (!cuts.get()) {
-			sepFactors = collectOriginalFactors(gfg, unaryFactors);
+      sepFactors = collectOriginalFactors(gfg, unaryFactors);
-			return makeSubNLG(sepFactors, frontals, sep, parent);
+      return makeSubNLG(sepFactors, frontals, sep, parent);
-		}
+    }
 
-		// retrieve the current partitioning info
+    // retrieve the current partitioning info
-		int numSubmaps = 2;
+    int numSubmaps = 2;
-		partition::PartitionTable& partitionTable = cuts->partitionTable;
+    partition::PartitionTable& partitionTable = cuts->partitionTable;
 
-		// split the factors between child cliques and the current clique
+    // split the factors between child cliques and the current clique
-		vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
+    vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
-		vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
+    vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
-		partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
+    partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
-				frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
+        frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
 
-		// make a new cluster
+    // make a new cluster
-		boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
+    boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
-		current->setWeeklinks(weeklinks);
+    current->setWeeklinks(weeklinks);
 
-		// create child clusters
+    // create child clusters
-		for (int i=0; i<2; i++) {
+    for (int i=0; i<2; i++) {
-			boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
+      boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
-					cuts->children.empty() ? boost::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose);
+          cuts->children.empty() ? boost::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose);
-			current->addChild(child);
+      current->addChild(child);
-		}
+    }
-		return current;
+    return current;
-	}
+  }
 }} //namespace

gtsam_unstable/partition/NestedDissection.h

@@ -14,56 +14,56 @@
 
 namespace gtsam { namespace partition {
 
-	/**
+  /**
-	 *  Apply nested dissection algorithm to nonlinear factor graphs
+   *  Apply nested dissection algorithm to nonlinear factor graphs
-	 */
+   */
-	template <class NLG, class SubNLG, class GenericGraph>
+  template <class NLG, class SubNLG, class GenericGraph>
-	class NestedDissection {
+  class NestedDissection {
-	public:
+  public:
-		typedef boost::shared_ptr<SubNLG> sharedSubNLG;
+    typedef boost::shared_ptr<SubNLG> sharedSubNLG;
 
-	private:
+  private:
-		NLG fg_;                          // the original nonlinear factor graph
+    NLG fg_;                          // the original nonlinear factor graph
-		Ordering ordering_;               // the variable ordering in the nonlinear factor graph
+    Ordering ordering_;               // the variable ordering in the nonlinear factor graph
-		std::vector<Symbol> int2symbol_;  // the mapping from integer key to symbol
+    std::vector<Symbol> int2symbol_;  // the mapping from integer key to symbol
-		sharedSubNLG root_;               // the root of generated cluster tree
+    sharedSubNLG root_;               // the root of generated cluster tree
 
-	public:
+  public:
-		sharedSubNLG root() const { return root_; }
+    sharedSubNLG root() const { return root_; }
 
-	public:
+  public:
-		/* constructor with post-determined partitoning*/
+    /* constructor with post-determined partitoning*/
-		NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false);
+    NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false);
 
-		/* constructor with pre-determined cuts*/
+    /* constructor with pre-determined cuts*/
-		NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false);
+    NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false);
 
-	private:
+  private:
-		/* convert generic subgraph to nonlinear subgraph  */
+    /* convert generic subgraph to nonlinear subgraph  */
-		sharedSubNLG makeSubNLG(const NLG& fg, const std::vector<size_t>& frontals,	const std::vector<size_t>& sep,	const boost::shared_ptr<SubNLG>& parent) const;
+    sharedSubNLG makeSubNLG(const NLG& fg, const std::vector<size_t>& frontals,  const std::vector<size_t>& sep,  const boost::shared_ptr<SubNLG>& parent) const;
 
-		void processFactor(const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable,  // input
+    void processFactor(const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable,  // input
-					std::vector<GenericGraph>& frontalFactors, NLG& sepFactors, std::vector<std::set<size_t> >& childSeps, // output factor graphs
+          std::vector<GenericGraph>& frontalFactors, NLG& sepFactors, std::vector<std::set<size_t> >& childSeps, // output factor graphs
-					typename SubNLG::Weeklinks& weeklinks) const;
+          typename SubNLG::Weeklinks& weeklinks) const;
 
-		/* recursively partition the generic graph */
+    /* recursively partition the generic graph */
-		void partitionFactorsAndVariables(
+    void partitionFactorsAndVariables(
-				const GenericGraph& fg, const GenericUnaryGraph& unaryFactors,
+        const GenericGraph& fg, const GenericUnaryGraph& unaryFactors,
-				const std::vector<size_t>& keys, const std::vector<int>& partitionTable, const int numSubmaps,  // input
+        const std::vector<size_t>& keys, const std::vector<int>& partitionTable, const int numSubmaps,  // input
-				std::vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors,   // output factor graphs
+        std::vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors,   // output factor graphs
-				std::vector<std::vector<size_t> >& childFrontals, std::vector<std::vector<size_t> >& childSeps, std::vector<size_t>& localFrontals,  // output sub-orderings
+        std::vector<std::vector<size_t> >& childFrontals, std::vector<std::vector<size_t> >& childSeps, std::vector<size_t>& localFrontals,  // output sub-orderings
-				typename SubNLG::Weeklinks& weeklinks) const;
+        typename SubNLG::Weeklinks& weeklinks) const;
 
-		NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const;
+    NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const;
 
-		/* recursively partition the generic graph */
+    /* recursively partition the generic graph */
-		sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
+    sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
-					const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
+          const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
 
-		/* recursively partition the generic graph */
+    /* recursively partition the generic graph */
-		sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
+    sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
-				const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
+        const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
 
-	};
+  };
 
 }} //namespace

gtsam_unstable/partition/PartitionWorkSpace.h

@@ -13,32 +13,32 @@
 
 namespace gtsam { namespace partition {
 
-	typedef std::vector<int> PartitionTable;
+  typedef std::vector<int> PartitionTable;
 
-	// the work space, preallocated memory
+  // the work space, preallocated memory
-	struct WorkSpace {
+  struct WorkSpace {
-		std::vector<int> dictionary;                          // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs
+    std::vector<int> dictionary;                          // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs
-		boost::shared_ptr<std::vector<size_t> > dsf;          // a block memory pre-allocated for DSFVector
+    boost::shared_ptr<std::vector<size_t> > dsf;          // a block memory pre-allocated for DSFVector
-		PartitionTable partitionTable;                        // a mapping from a key to the submap index, 0 means the separator, i means the ith submap
+    PartitionTable partitionTable;                        // a mapping from a key to the submap index, 0 means the separator, i means the ith submap
 
-		// constructor
+    // constructor
-		WorkSpace(const size_t numNodes) : dictionary(numNodes,0),
+    WorkSpace(const size_t numNodes) : dictionary(numNodes,0),
-		 dsf(new std::vector<size_t>(numNodes, 0)), partitionTable(numNodes, -1) { }
+     dsf(new std::vector<size_t>(numNodes, 0)), partitionTable(numNodes, -1) { }
 
-		// set up dictionary: -1: no such key, none-zero: the corresponding 0-based index
+    // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index
-		inline void prepareDictionary(const std::vector<size_t>& keys) {
+    inline void prepareDictionary(const std::vector<size_t>& keys) {
-			int index = 0;
+      int index = 0;
-			std::fill(dictionary.begin(), dictionary.end(), -1);
+      std::fill(dictionary.begin(), dictionary.end(), -1);
-			std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
+      std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
-			while(it!=itLast)	dictionary[*(it++)] = index++;
+      while(it!=itLast)  dictionary[*(it++)] = index++;
-		}
+    }
-	};
+  };
 
 
-	// manually defined cuts
+  // manually defined cuts
-	struct Cuts {
+  struct Cuts {
-		PartitionTable partitionTable;
+    PartitionTable partitionTable;
-		std::vector<boost::shared_ptr<Cuts> > children;
+    std::vector<boost::shared_ptr<Cuts> > children;
-	};
+  };
 
 }} // namespace

gtsam_unstable/partition/tests/testFindSeparator.cpp

@@ -185,45 +185,45 @@ TEST ( Partition, findSeparator2 )
 // x25 x26             x27 x28
 TEST ( Partition, findSeparator3_with_reduced_camera )
 {
-	GenericGraph3D graph;
+  GenericGraph3D graph;
-	for (int j=1; j<=8; j++)
+  for (int j=1; j<=8; j++)
-		graph.push_back(boost::make_shared<GenericFactor3D>(25, j));
+    graph.push_back(boost::make_shared<GenericFactor3D>(25, j));
-	for (int j=1; j<=16; j++)
+  for (int j=1; j<=16; j++)
-		graph.push_back(boost::make_shared<GenericFactor3D>(26, j));
+    graph.push_back(boost::make_shared<GenericFactor3D>(26, j));
-	for (int j=9; j<=24; j++)
+  for (int j=9; j<=24; j++)
-		graph.push_back(boost::make_shared<GenericFactor3D>(27, j));
+    graph.push_back(boost::make_shared<GenericFactor3D>(27, j));
-	for (int j=17; j<=24; j++)
+  for (int j=17; j<=24; j++)
-		graph.push_back(boost::make_shared<GenericFactor3D>(28, j));
+    graph.push_back(boost::make_shared<GenericFactor3D>(28, j));
 
-	std::vector<size_t> keys;
+  std::vector<size_t> keys;
-	for(int i=1; i<=28; i++)
+  for(int i=1; i<=28; i++)
-		keys.push_back(i);
+    keys.push_back(i);
 
-	vector<Symbol> int2symbol;
+  vector<Symbol> int2symbol;
-	int2symbol.push_back(Symbol('x',0)); // dummy
+  int2symbol.push_back(Symbol('x',0)); // dummy
-	for(int i=1; i<=24; i++)
+  for(int i=1; i<=24; i++)
-		int2symbol.push_back(Symbol('l',i));
+    int2symbol.push_back(Symbol('l',i));
-	int2symbol.push_back(Symbol('x',25));
+  int2symbol.push_back(Symbol('x',25));
-	int2symbol.push_back(Symbol('x',26));
+  int2symbol.push_back(Symbol('x',26));
-	int2symbol.push_back(Symbol('x',27));
+  int2symbol.push_back(Symbol('x',27));
-	int2symbol.push_back(Symbol('x',28));
+  int2symbol.push_back(Symbol('x',28));
 
-	WorkSpace workspace(29);
+  WorkSpace workspace(29);
-	bool reduceGraph = true;
+  bool reduceGraph = true;
-	int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0);
+  int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0);
-	LONGS_EQUAL(2, numIsland);
+  LONGS_EQUAL(2, numIsland);
 
-	partition::PartitionTable& partitionTable = workspace.partitionTable;
+  partition::PartitionTable& partitionTable = workspace.partitionTable;
-	for (int j=1; j<=8; j++)
+  for (int j=1; j<=8; j++)
-		LONGS_EQUAL(1, partitionTable[j]);
+    LONGS_EQUAL(1, partitionTable[j]);
-	for (int j=9; j<=16; j++)
+  for (int j=9; j<=16; j++)
-		LONGS_EQUAL(0, partitionTable[j]);
+    LONGS_EQUAL(0, partitionTable[j]);
-	for (int j=17; j<=24; j++)
+  for (int j=17; j<=24; j++)
-		LONGS_EQUAL(2, partitionTable[j]);
+    LONGS_EQUAL(2, partitionTable[j]);
-	LONGS_EQUAL(1, partitionTable[25]);
+  LONGS_EQUAL(1, partitionTable[25]);
-	LONGS_EQUAL(1, partitionTable[26]);
+  LONGS_EQUAL(1, partitionTable[26]);
-	LONGS_EQUAL(2, partitionTable[27]);
+  LONGS_EQUAL(2, partitionTable[27]);
-	LONGS_EQUAL(2, partitionTable[28]);
+  LONGS_EQUAL(2, partitionTable[28]);
 }
 
 /* ************************************************************************* */

gtsam_unstable/partition/tests/testGenericGraph.cpp

@@ -29,29 +29,29 @@ using namespace gtsam::partition;
  */
 TEST ( GenerciGraph, findIslands )
 {
-	GenericGraph2D graph;
+  GenericGraph2D graph;
-	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
 
-	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
-	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
 
-	WorkSpace workspace(10); // from 0 to 9
+  WorkSpace workspace(10); // from 0 to 9
-	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
+  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
-	LONGS_EQUAL(2, islands.size());
+  LONGS_EQUAL(2, islands.size());
-	vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
+  vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
-	vector<size_t> island2; island2 += 4, 5, 6, 9;
+  vector<size_t> island2; island2 += 4, 5, 6, 9;
-	CHECK(island1 == islands.front());
+  CHECK(island1 == islands.front());
-	CHECK(island2 == islands.back());
+  CHECK(island2 == islands.back());
 }
 
 /* ************************************************************************* */
@@ -62,27 +62,27 @@ TEST ( GenerciGraph, findIslands )
  */
 TEST( GenerciGraph, findIslands2 )
 {
-	GenericGraph2D graph;
+  GenericGraph2D graph;
-	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
 
-	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
-	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
+  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
 
-	WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
+  WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
-	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
+  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
-	LONGS_EQUAL(1, islands.size());
+  LONGS_EQUAL(1, islands.size());
-	vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
+  vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
-	CHECK(island1 == islands.front());
+  CHECK(island1 == islands.front());
 }
 
 /* ************************************************************************* */
@@ -92,21 +92,21 @@ TEST( GenerciGraph, findIslands2 )
  */
 TEST ( GenerciGraph, findIslands3 )
 {
-	GenericGraph2D graph;
+  GenericGraph2D graph;
-	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
 
-	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
-	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
+  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
 
-	WorkSpace workspace(7); // from 0 to 9
+  WorkSpace workspace(7); // from 0 to 9
-	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
+  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
-	LONGS_EQUAL(2, islands.size());
+  LONGS_EQUAL(2, islands.size());
-	vector<size_t> island1; island1 += 1, 5;
+  vector<size_t> island1; island1 += 1, 5;
-	vector<size_t> island2; island2 += 2, 3, 4, 6;
+  vector<size_t> island2; island2 += 2, 3, 4, 6;
-	CHECK(island1 == islands.front());
+  CHECK(island1 == islands.front());
-	CHECK(island2 == islands.back());
+  CHECK(island2 == islands.back());
 }
 
 /* ************************************************************************* */
@@ -115,18 +115,18 @@ TEST ( GenerciGraph, findIslands3 )
  */
 TEST ( GenerciGraph, findIslands4 )
 {
-	GenericGraph2D graph;
+  GenericGraph2D graph;
-	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
-	std::vector<size_t> keys; keys += 3, 4, 7;
+  std::vector<size_t> keys; keys += 3, 4, 7;
 
-	WorkSpace workspace(8); // from 0 to 7
+  WorkSpace workspace(8); // from 0 to 7
-	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
+  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
-	LONGS_EQUAL(2, islands.size());
+  LONGS_EQUAL(2, islands.size());
-	vector<size_t> island1; island1 += 3, 4;
+  vector<size_t> island1; island1 += 3, 4;
-	vector<size_t> island2; island2 += 7;
+  vector<size_t> island2; island2 += 7;
-	CHECK(island1 == islands.front());
+  CHECK(island1 == islands.front());
-	CHECK(island2 == islands.back());
+  CHECK(island2 == islands.back());
 }
 
 /* ************************************************************************* */
@@ -137,24 +137,24 @@ TEST ( GenerciGraph, findIslands4 )
  */
 TEST ( GenerciGraph, findIslands5 )
 {
-	GenericGraph2D graph;
+  GenericGraph2D graph;
-	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
 
-	graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
-	graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
+  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
 
-	std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
+  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
 
-	WorkSpace workspace(6); // from 0 to 5
+  WorkSpace workspace(6); // from 0 to 5
-	list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
+  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
-	LONGS_EQUAL(2, islands.size());
+  LONGS_EQUAL(2, islands.size());
-	vector<size_t> island1; island1 += 1, 3, 5;
+  vector<size_t> island1; island1 += 1, 3, 5;
-	vector<size_t> island2; island2 += 2, 4;
+  vector<size_t> island2; island2 += 2, 4;
-	CHECK(island1 == islands.front());
+  CHECK(island1 == islands.front());
-	CHECK(island2 == islands.back());
+  CHECK(island2 == islands.back());
 }
 
 /* ************************************************************************* */
@@ -165,31 +165,31 @@ TEST ( GenerciGraph, findIslands5 )
  */
 TEST ( GenerciGraph, reduceGenericGraph )
 {
-	GenericGraph3D graph;
+  GenericGraph3D graph;
-	graph.push_back(boost::make_shared<GenericFactor3D>(1, 3));
+  graph.push_back(boost::make_shared<GenericFactor3D>(1, 3));
-	graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
+  graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
-	graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
+  graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
-	graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
+  graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
-	graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
+  graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
 
-	std::vector<size_t> cameraKeys, landmarkKeys;
+  std::vector<size_t> cameraKeys, landmarkKeys;
-	cameraKeys.push_back(1);
+  cameraKeys.push_back(1);
-	cameraKeys.push_back(2);
+  cameraKeys.push_back(2);
-	landmarkKeys.push_back(3);
+  landmarkKeys.push_back(3);
-	landmarkKeys.push_back(4);
+  landmarkKeys.push_back(4);
-	landmarkKeys.push_back(5);
+  landmarkKeys.push_back(5);
-	landmarkKeys.push_back(6);
+  landmarkKeys.push_back(6);
 
-	std::vector<int> dictionary;
+  std::vector<int> dictionary;
-	dictionary.resize(7, -1); // from 0 to 6
+  dictionary.resize(7, -1); // from 0 to 6
-	dictionary[1] = 0;
+  dictionary[1] = 0;
-	dictionary[2] = 1;
+  dictionary[2] = 1;
 
-	GenericGraph3D reduced;
+  GenericGraph3D reduced;
-	std::map<size_t, vector<size_t> > cameraToLandmarks;
+  std::map<size_t, vector<size_t> > cameraToLandmarks;
-	reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
+  reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
-	LONGS_EQUAL(1, reduced.size());
+  LONGS_EQUAL(1, reduced.size());
-	LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
+  LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
 }
 
 /* ************************************************************************* */
@@ -200,53 +200,53 @@ TEST ( GenerciGraph, reduceGenericGraph )
  */
 TEST ( GenericGraph, reduceGenericGraph2 )
 {
-	GenericGraph3D graph;
+  GenericGraph3D graph;
-	graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
+  graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
-	graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
+  graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
-	graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
+  graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
-	graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
+  graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
-	graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
+  graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
-	graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
+  graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
 
-	std::vector<size_t> cameraKeys, landmarkKeys;
+  std::vector<size_t> cameraKeys, landmarkKeys;
-	cameraKeys.push_back(1);
+  cameraKeys.push_back(1);
-	cameraKeys.push_back(2);
+  cameraKeys.push_back(2);
-	cameraKeys.push_back(7);
+  cameraKeys.push_back(7);
-	landmarkKeys.push_back(3);
+  landmarkKeys.push_back(3);
-	landmarkKeys.push_back(4);
+  landmarkKeys.push_back(4);
-	landmarkKeys.push_back(5);
+  landmarkKeys.push_back(5);
-	landmarkKeys.push_back(6);
+  landmarkKeys.push_back(6);
 
-	std::vector<int> dictionary;
+  std::vector<int> dictionary;
-	dictionary.resize(8, -1); // from 0 to 7
+  dictionary.resize(8, -1); // from 0 to 7
-	dictionary[1] = 0;
+  dictionary[1] = 0;
-	dictionary[2] = 1;
+  dictionary[2] = 1;
-	dictionary[7] = 6;
+  dictionary[7] = 6;
 
-	GenericGraph3D reduced;
+  GenericGraph3D reduced;
-	std::map<size_t, vector<size_t> > cameraToLandmarks;
+  std::map<size_t, vector<size_t> > cameraToLandmarks;
-	reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
+  reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
-	LONGS_EQUAL(2, reduced.size());
+  LONGS_EQUAL(2, reduced.size());
-	LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
+  LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
-	LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
+  LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
 }
 
 /* ************************************************************************* */
 TEST ( GenerciGraph, hasCommonCamera )
 {
-	std::set<size_t> cameras1;   cameras1 += 1, 2, 3, 4, 5;
+  std::set<size_t> cameras1;   cameras1 += 1, 2, 3, 4, 5;
-	std::set<size_t> cameras2;   cameras2 += 8, 7, 6, 5;
+  std::set<size_t> cameras2;   cameras2 += 8, 7, 6, 5;
-	bool actual = hasCommonCamera(cameras1, cameras2);
+  bool actual = hasCommonCamera(cameras1, cameras2);
-	CHECK(actual);
+  CHECK(actual);
 }
 
 /* ************************************************************************* */
 TEST ( GenerciGraph, hasCommonCamera2 )
 {
-	std::set<size_t> cameras1;   cameras1 += 1, 3, 5, 7;
+  std::set<size_t> cameras1;   cameras1 += 1, 3, 5, 7;
-	std::set<size_t> cameras2;   cameras2 += 2, 4, 6, 8, 10;
+  std::set<size_t> cameras2;   cameras2 += 2, 4, 6, 8, 10;
-	bool actual = hasCommonCamera(cameras1, cameras2);
+  bool actual = hasCommonCamera(cameras1, cameras2);
-	CHECK(!actual);
+  CHECK(!actual);
 }
 
 /* ************************************************************************* */

gtsam_unstable/partition/tests/testNestedDissection.cpp

@@ -32,22 +32,22 @@ using namespace gtsam::partition;
 //   l1
 TEST ( NestedDissection, oneIsland )
 {
-	using namespace submapPlanarSLAM;
+  using namespace submapPlanarSLAM;
-	typedef TSAM2D::SubNLG SubNLG;
+  typedef TSAM2D::SubNLG SubNLG;
-	Graph fg;
+  Graph fg;
-	fg.addOdometry(1, 2, Pose2(), odoNoise);
+  fg.addOdometry(1, 2, Pose2(), odoNoise);
-	fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
+  fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
-	fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
+  fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
-	fg.addPoseConstraint(1, Pose2());
+  fg.addPoseConstraint(1, Pose2());
 
-	Ordering ordering; ordering += x1, x2, l1;
+  Ordering ordering; ordering += x1, x2, l1;
 
-	int numNodeStopPartition = 1e3;
+  int numNodeStopPartition = 1e3;
-	int minNodesPerMap = 1e3;
+  int minNodesPerMap = 1e3;
-	NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
+  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
-	LONGS_EQUAL(4, nd.root()->size());
+  LONGS_EQUAL(4, nd.root()->size());
-	LONGS_EQUAL(3, nd.root()->frontal().size());
+  LONGS_EQUAL(3, nd.root()->frontal().size());
-	LONGS_EQUAL(0, nd.root()->children().size());
+  LONGS_EQUAL(0, nd.root()->children().size());
 }
 
 /* ************************************************************************* */
@@ -56,35 +56,35 @@ TEST ( NestedDissection, oneIsland )
 // x2/  \x5
 TEST ( NestedDissection, TwoIslands )
 {
-	using namespace submapPlanarSLAM;
+  using namespace submapPlanarSLAM;
-	typedef TSAM2D::SubNLG SubNLG;
+  typedef TSAM2D::SubNLG SubNLG;
-	Graph fg;
+  Graph fg;
-	fg.addOdometry(1, 2, Pose2(), odoNoise);
+  fg.addOdometry(1, 2, Pose2(), odoNoise);
-	fg.addOdometry(1, 3, Pose2(), odoNoise);
+  fg.addOdometry(1, 3, Pose2(), odoNoise);
-	fg.addOdometry(2, 3, Pose2(), odoNoise);
+  fg.addOdometry(2, 3, Pose2(), odoNoise);
-	fg.addOdometry(3, 4, Pose2(), odoNoise);
+  fg.addOdometry(3, 4, Pose2(), odoNoise);
-	fg.addOdometry(4, 5, Pose2(), odoNoise);
+  fg.addOdometry(4, 5, Pose2(), odoNoise);
-	fg.addOdometry(3, 5, Pose2(), odoNoise);
+  fg.addOdometry(3, 5, Pose2(), odoNoise);
-	fg.addPoseConstraint(1, Pose2());
+  fg.addPoseConstraint(1, Pose2());
-	fg.addPoseConstraint(4, Pose2());
+  fg.addPoseConstraint(4, Pose2());
-	Ordering ordering; ordering += x1, x2, x3, x4, x5;
+  Ordering ordering; ordering += x1, x2, x3, x4, x5;
 
-	int numNodeStopPartition = 2;
+  int numNodeStopPartition = 2;
-	int minNodesPerMap = 1;
+  int minNodesPerMap = 1;
-	NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
+  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
-	// root submap
+  // root submap
-	LONGS_EQUAL(0, nd.root()->size());
+  LONGS_EQUAL(0, nd.root()->size());
-	LONGS_EQUAL(1, nd.root()->frontal().size());
+  LONGS_EQUAL(1, nd.root()->frontal().size());
-	LONGS_EQUAL(0, nd.root()->separator().size());
+  LONGS_EQUAL(0, nd.root()->separator().size());
-	LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
+  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
 
-	// the 1st submap
+  // the 1st submap
-	LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
+  LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
-	LONGS_EQUAL(4, nd.root()->children()[0]->size());
+  LONGS_EQUAL(4, nd.root()->children()[0]->size());
 
-	// the 2nd submap
+  // the 2nd submap
-	LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
+  LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
-	LONGS_EQUAL(4, nd.root()->children()[1]->size());
+  LONGS_EQUAL(4, nd.root()->children()[1]->size());
 }
 
 /* ************************************************************************* */
@@ -93,40 +93,40 @@ TEST ( NestedDissection, TwoIslands )
 // x2/  \x5
 TEST ( NestedDissection, FourIslands )
 {
-	using namespace submapPlanarSLAM;
+  using namespace submapPlanarSLAM;
-	typedef TSAM2D::SubNLG SubNLG;
+  typedef TSAM2D::SubNLG SubNLG;
-	Graph fg;
+  Graph fg;
-	fg.addOdometry(1, 3, Pose2(), odoNoise);
+  fg.addOdometry(1, 3, Pose2(), odoNoise);
-	fg.addOdometry(2, 3, Pose2(), odoNoise);
+  fg.addOdometry(2, 3, Pose2(), odoNoise);
-	fg.addOdometry(3, 4, Pose2(), odoNoise);
+  fg.addOdometry(3, 4, Pose2(), odoNoise);
-	fg.addOdometry(3, 5, Pose2(), odoNoise);
+  fg.addOdometry(3, 5, Pose2(), odoNoise);
-	fg.addPoseConstraint(1, Pose2());
+  fg.addPoseConstraint(1, Pose2());
-	fg.addPoseConstraint(4, Pose2());
+  fg.addPoseConstraint(4, Pose2());
-	Ordering ordering; ordering += x1, x2, x3, x4, x5;
+  Ordering ordering; ordering += x1, x2, x3, x4, x5;
 
-	int numNodeStopPartition = 2;
+  int numNodeStopPartition = 2;
-	int minNodesPerMap = 1;
+  int minNodesPerMap = 1;
-	NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
+  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
-	LONGS_EQUAL(0, nd.root()->size());
+  LONGS_EQUAL(0, nd.root()->size());
-	LONGS_EQUAL(1, nd.root()->frontal().size());
+  LONGS_EQUAL(1, nd.root()->frontal().size());
-	LONGS_EQUAL(0, nd.root()->separator().size());
+  LONGS_EQUAL(0, nd.root()->separator().size());
-	LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
+  LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
 
-	// the 1st submap
+  // the 1st submap
-	LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
+  LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
-	LONGS_EQUAL(2, nd.root()->children()[0]->size());
+  LONGS_EQUAL(2, nd.root()->children()[0]->size());
 
-	// the 2nd submap
+  // the 2nd submap
-	LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
+  LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
-	LONGS_EQUAL(2, nd.root()->children()[1]->size());
+  LONGS_EQUAL(2, nd.root()->children()[1]->size());
 
-	// the 3rd submap
+  // the 3rd submap
-	LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
+  LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
-	LONGS_EQUAL(1, nd.root()->children()[2]->size());
+  LONGS_EQUAL(1, nd.root()->children()[2]->size());
 
-	// the 4th submap
+  // the 4th submap
-	LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
+  LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
-	LONGS_EQUAL(1, nd.root()->children()[3]->size());
+  LONGS_EQUAL(1, nd.root()->children()[3]->size());
 }
 
 /* ************************************************************************* */
@@ -137,41 +137,41 @@ TEST ( NestedDissection, FourIslands )
 // x5
 TEST ( NestedDissection, weekLinks )
 {
-	using namespace submapPlanarSLAM;
+  using namespace submapPlanarSLAM;
-	typedef TSAM2D::SubNLG SubNLG;
+  typedef TSAM2D::SubNLG SubNLG;
-	Graph fg;
+  Graph fg;
-	fg.addOdometry(1, 2, Pose2(), odoNoise);
+  fg.addOdometry(1, 2, Pose2(), odoNoise);
-	fg.addOdometry(2, 3, Pose2(), odoNoise);
+  fg.addOdometry(2, 3, Pose2(), odoNoise);
-	fg.addOdometry(2, 4, Pose2(), odoNoise);
+  fg.addOdometry(2, 4, Pose2(), odoNoise);
-	fg.addOdometry(3, 4, Pose2(), odoNoise);
+  fg.addOdometry(3, 4, Pose2(), odoNoise);
-	fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
+  fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
-	fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
+  fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
-	fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
+  fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
-	fg.addPoseConstraint(1, Pose2());
+  fg.addPoseConstraint(1, Pose2());
-	fg.addPoseConstraint(4, Pose2());
+  fg.addPoseConstraint(4, Pose2());
-	fg.addPoseConstraint(5, Pose2());
+  fg.addPoseConstraint(5, Pose2());
-	Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
+  Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
 
-	int numNodeStopPartition = 2;
+  int numNodeStopPartition = 2;
-	int minNodesPerMap = 1;
+  int minNodesPerMap = 1;
-	NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
+  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
-	LONGS_EQUAL(0, nd.root()->size()); // one weeklink
+  LONGS_EQUAL(0, nd.root()->size()); // one weeklink
-	LONGS_EQUAL(1, nd.root()->frontal().size());
+  LONGS_EQUAL(1, nd.root()->frontal().size());
-	LONGS_EQUAL(0, nd.root()->separator().size());
+  LONGS_EQUAL(0, nd.root()->separator().size());
-	LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
+  LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
-	LONGS_EQUAL(1, nd.root()->weeklinks().size());
+  LONGS_EQUAL(1, nd.root()->weeklinks().size());
 
-	// the 1st submap
+  // the 1st submap
-	LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
+  LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
-	LONGS_EQUAL(4, nd.root()->children()[0]->size());
+  LONGS_EQUAL(4, nd.root()->children()[0]->size());
 
-	// the 2nd submap
+  // the 2nd submap
-	LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
+  LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
-	LONGS_EQUAL(4, nd.root()->children()[1]->size());
+  LONGS_EQUAL(4, nd.root()->children()[1]->size());
-	//
+  //
-	// the 3rd submap
+  // the 3rd submap
-	LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
+  LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
-	LONGS_EQUAL(1, nd.root()->children()[2]->size());
+  LONGS_EQUAL(1, nd.root()->children()[2]->size());
 }
 
 /* ************************************************************************* */
@@ -184,86 +184,86 @@ TEST ( NestedDissection, weekLinks )
  */
 TEST ( NestedDissection, manual_cuts )
 {
-	using namespace submapPlanarSLAM;
+  using namespace submapPlanarSLAM;
-	typedef partition::Cuts Cuts;
+  typedef partition::Cuts Cuts;
-	typedef TSAM2D::SubNLG SubNLG;
+  typedef TSAM2D::SubNLG SubNLG;
-	typedef partition::PartitionTable PartitionTable;
+  typedef partition::PartitionTable PartitionTable;
-	Graph fg;
+  Graph fg;
-	fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
+  fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
-	fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);
+  fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);
 
-	fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1,       bearingRangeNoise);
+  fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1,       bearingRangeNoise);
-	fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1,       bearingRangeNoise);
+  fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1,       bearingRangeNoise);
-	fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
-	fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
 
-	fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
-	fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
+  fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
-	fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4),     sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4),     sqrt(2), bearingRangeNoise);
-	fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
-	fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
+  fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
-	fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4),     sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4),     sqrt(2), bearingRangeNoise);
 
-	fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
-	fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
+  fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
-	fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
+  fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
-	fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2),     1,       bearingRangeNoise);
+  fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2),     1,       bearingRangeNoise);
 
-	fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise);
+  fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise);
 
-	// generate ordering
+  // generate ordering
-	Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
+  Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
 
-	// define cuts
+  // define cuts
-	boost::shared_ptr<Cuts> cuts(new Cuts());
+  boost::shared_ptr<Cuts> cuts(new Cuts());
-	cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
+  cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
-	//x0        x1         x2         l1         l2         l3         l4         l5         l6
+  //x0        x1         x2         l1         l2         l3         l4         l5         l6
-	(*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2;
+  (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2;
 
-	cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
+  cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
-	cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
+  cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
-	//x0        x1          x2          l1         l2          l3          l4         l5          l6
+  //x0        x1          x2          l1         l2          l3          l4         l5          l6
-	(*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1;
+  (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1;
 
-	cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
+  cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
-	cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
+  cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
-	//x0         x1          x2         l1          l2          l3         l4          l5          l6
+  //x0         x1          x2         l1          l2          l3         l4          l5          l6
-	(*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2;
+  (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2;
 
 
-	// nested dissection
+  // nested dissection
-	NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
+  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
-	LONGS_EQUAL(2, nd.root()->size());
+  LONGS_EQUAL(2, nd.root()->size());
-	LONGS_EQUAL(3, nd.root()->frontal().size());
+  LONGS_EQUAL(3, nd.root()->frontal().size());
-	LONGS_EQUAL(0, nd.root()->separator().size());
+  LONGS_EQUAL(0, nd.root()->separator().size());
-	LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
+  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
-	LONGS_EQUAL(0, nd.root()->weeklinks().size());
+  LONGS_EQUAL(0, nd.root()->weeklinks().size());
 
-	// the 1st submap
+  // the 1st submap
-	LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
+  LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
-	LONGS_EQUAL(4, nd.root()->children()[0]->size());
+  LONGS_EQUAL(4, nd.root()->children()[0]->size());
-	LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
+  LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
 
-	// the 1-1st submap
+  // the 1-1st submap
-	LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
+  LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
-	LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
+  LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
 
-	// the 1-2nd submap
+  // the 1-2nd submap
-	LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
+  LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
-	LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
+  LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
 
-	// the 2nd submap
+  // the 2nd submap
-	LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
+  LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
-	LONGS_EQUAL(3, nd.root()->children()[1]->size());
+  LONGS_EQUAL(3, nd.root()->children()[1]->size());
-	LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
+  LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
 
-	// the 2-1st submap
+  // the 2-1st submap
-	LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
+  LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
-	LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
+  LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
 
-	// the 2-2nd submap
+  // the 2-2nd submap
-	LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
+  LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
-	LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
+  LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
 
 }
 
@@ -272,65 +272,65 @@ TEST ( NestedDissection, manual_cuts )
 //    / |    /    \    | \
 // x0  x1             x2  x3
 TEST( NestedDissection, Graph3D) {
-	using namespace gtsam::submapVisualSLAM;
+  using namespace gtsam::submapVisualSLAM;
-	typedef TSAM3D::SubNLG SubNLG;
+  typedef TSAM3D::SubNLG SubNLG;
-	typedef partition::PartitionTable PartitionTable;
+  typedef partition::PartitionTable PartitionTable;
-	vector<GeneralCamera> cameras;
+  vector<GeneralCamera> cameras;
-	cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.))));
+  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.))));
-	cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.))));
+  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.))));
-	cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.))));
+  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.))));
-	cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.))));
+  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.))));
 
-	vector<Point3> points;
+  vector<Point3> points;
-	for (int cube_index = 0; cube_index <= 3; cube_index++) {
+  for (int cube_index = 0; cube_index <= 3; cube_index++) {
-		Point3 center((cube_index-1) * 3, 0.5, 10.);
+    Point3 center((cube_index-1) * 3, 0.5, 10.);
-		points.push_back(center + Point3(-0.5, -0.5, -0.5));
+    points.push_back(center + Point3(-0.5, -0.5, -0.5));
-		points.push_back(center + Point3(-0.5,  0.5, -0.5));
+    points.push_back(center + Point3(-0.5,  0.5, -0.5));
-		points.push_back(center + Point3( 0.5,  0.5, -0.5));
+    points.push_back(center + Point3( 0.5,  0.5, -0.5));
-		points.push_back(center + Point3( 0.5, -0.5, -0.5));
+    points.push_back(center + Point3( 0.5, -0.5, -0.5));
-		points.push_back(center + Point3(-0.5, -0.5,  0.5));
+    points.push_back(center + Point3(-0.5, -0.5,  0.5));
-		points.push_back(center + Point3(-0.5,  0.5,  0.5));
+    points.push_back(center + Point3(-0.5,  0.5,  0.5));
-		points.push_back(center + Point3( 0.5,  0.5,  0.5));
+    points.push_back(center + Point3( 0.5,  0.5,  0.5));
-		points.push_back(center + Point3( 0.5,  0.5,  0.5));
+    points.push_back(center + Point3( 0.5,  0.5,  0.5));
-	}
+  }
 
-	Graph graph;
+  Graph graph;
-	SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
+  SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
-	SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
+  SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
-	for (int j=1; j<=8; j++)
+  for (int j=1; j<=8; j++)
-		graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
+    graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
-	for (int j=1; j<=16; j++)
+  for (int j=1; j<=16; j++)
-		graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
+    graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
-	for (int j=9; j<=24; j++)
+  for (int j=9; j<=24; j++)
-		graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
+    graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
-	for (int j=17; j<=24; j++)
+  for (int j=17; j<=24; j++)
-		graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
+    graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
 
-	// make an easy ordering
+  // make an easy ordering
-	Ordering ordering; ordering += x0, x1, x2, x3;
+  Ordering ordering; ordering += x0, x1, x2, x3;
-	for (int j=1; j<=24; j++)
+  for (int j=1; j<=24; j++)
-		ordering += Symbol('l', j);
+    ordering += Symbol('l', j);
 
-	// nested dissection
+  // nested dissection
-	const int numNodeStopPartition = 10;
+  const int numNodeStopPartition = 10;
-	const int minNodesPerMap = 5;
+  const int minNodesPerMap = 5;
-	NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
+  NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
 
-	LONGS_EQUAL(0, nd.root()->size());
+  LONGS_EQUAL(0, nd.root()->size());
-	LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
+  LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
-	LONGS_EQUAL(0, nd.root()->separator().size());
+  LONGS_EQUAL(0, nd.root()->separator().size());
-	LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
+  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
-	LONGS_EQUAL(0, nd.root()->weeklinks().size());
+  LONGS_EQUAL(0, nd.root()->weeklinks().size());
 
-	// the 1st submap
+  // the 1st submap
-	LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8
+  LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8
-	LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16
+  LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16
-	LONGS_EQUAL(0, nd.root()->children()[0]->children().size());
+  LONGS_EQUAL(0, nd.root()->children()[0]->children().size());
 
-	// the 2nd submap
+  // the 2nd submap
-	LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8
+  LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8
-	LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8
+  LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8
-	LONGS_EQUAL(0, nd.root()->children()[1]->children().size());
+  LONGS_EQUAL(0, nd.root()->children()[1]->children().size());
 }
 
 

gtsam_unstable/slam/BetweenFactorEM.h

@@ -295,87 +295,87 @@ namespace gtsam {
 
     /* ************************************************************************* */
     SharedGaussian get_model_inlier() const {
-    	return model_inlier_;
+      return model_inlier_;
     }
 
     /* ************************************************************************* */
     SharedGaussian get_model_outlier() const {
-    	return model_outlier_;
+      return model_outlier_;
     }
 
     /* ************************************************************************* */
     Matrix get_model_inlier_cov() const {
-    	return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+      return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
     }
 
     /* ************************************************************************* */
     Matrix get_model_outlier_cov() const {
-    	return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+      return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
     }
 
     /* ************************************************************************* */
     void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
-    	/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
+      /* 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).
+       * (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
+       * 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).
+       * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
-    	 *
+       *
-    	 * TODO: improve efficiency (info form)
+       * TODO: improve efficiency (info form)
-    	 */
+       */
 
-    	 // get joint covariance of the involved states
+       // get joint covariance of the involved states
-    	 std::vector<gtsam::Key> Keys;
+       std::vector<gtsam::Key> Keys;
-    	 Keys.push_back(key1_);
+       Keys.push_back(key1_);
-    	 Keys.push_back(key2_);
+       Keys.push_back(key2_);
-    	 Marginals marginals( graph, values, Marginals::QR );
+       Marginals marginals( graph, values, Marginals::QR );
-    	 JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
+       JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
-    	 Matrix cov1 = joint_marginal12(key1_, key1_);
+       Matrix cov1 = joint_marginal12(key1_, key1_);
-    	 Matrix cov2 = joint_marginal12(key2_, key2_);
+       Matrix cov2 = joint_marginal12(key2_, key2_);
-    	 Matrix cov12 = joint_marginal12(key1_, key2_);
+       Matrix cov12 = joint_marginal12(key1_, key2_);
 
-    	 updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
+       updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
     }
 
     /* ************************************************************************* */
         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
+          /* 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).
+           * (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
+           * 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).
+           * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
-        	 *
+           *
-        	 * TODO: improve efficiency (info form)
+           * TODO: improve efficiency (info form)
-        	 */
+           */
 
-        	 const T& p1 = values.at<T>(key1_);
+           const T& p1 = values.at<T>(key1_);
-        	 const T& p2 = values.at<T>(key2_);
+           const T& p2 = values.at<T>(key2_);
 
-        	 Matrix H1, H2;
+           Matrix H1, H2;
-        	 T hx = p1.between(p2, H1, H2); // h(x)
+           T hx = p1.between(p2, H1, H2); // h(x)
 
-        	 Matrix H;
+           Matrix H;
-        	 H.resize(H1.rows(), H1.rows()+H2.rows());
+           H.resize(H1.rows(), H1.rows()+H2.rows());
-        	 H << H1, H2; // H = [H1 H2]
+           H << H1, H2; // H = [H1 H2]
 
-        	 Matrix joint_cov;
+           Matrix joint_cov;
-        	 joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
+           joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
-        	 joint_cov << cov1, cov12,
+           joint_cov << cov1, cov12,
-        			 cov12.transpose(), cov2;
+               cov12.transpose(), cov2;
 
-        	 Matrix cov_state = H*joint_cov*H.transpose();
+           Matrix cov_state = H*joint_cov*H.transpose();
 
-        	 //    	 model_inlier_->print("before:");
+           //       model_inlier_->print("before:");
 
-        	 // update inlier and outlier noise models
+           // update inlier and outlier noise models
-        	 Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+           Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
-        	 model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
+           model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
 
-        	 Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+           Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
-        	 model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
+           model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
 
-        	 //    	 model_inlier_->print("after:");
+           //       model_inlier_->print("after:");
-        	 //    	 std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
+           //       std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
         }
 
     /* ************************************************************************* */

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 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/tests/testBetweenFactorEM.cpp

@@ -255,43 +255,43 @@ TEST( BetweenFactorEM, CaseStudy)
 
 ///* ************************************************************************** */
 TEST (BetweenFactorEM, updateNoiseModel ) {
-	gtsam::Key key1(1);
+  gtsam::Key key1(1);
-	gtsam::Key key2(2);
+  gtsam::Key key2(2);
 
-	gtsam::Pose2 p1(10.0, 15.0, 0.1);
+  gtsam::Pose2 p1(10.0, 15.0, 0.1);
-	gtsam::Pose2 p2(15.0, 15.0, 0.3);
+  gtsam::Pose2 p2(15.0, 15.0, 0.3);
-	gtsam::Pose2 noise(0.5, 0.4, 0.01);
+  gtsam::Pose2 noise(0.5, 0.4, 0.01);
-	gtsam::Pose2 rel_pose_ideal = p1.between(p2);
+  gtsam::Pose2 rel_pose_ideal = p1.between(p2);
-	gtsam::Pose2 rel_pose_msr   = rel_pose_ideal.compose(noise);
+  gtsam::Pose2 rel_pose_msr   = rel_pose_ideal.compose(noise);
 
-	SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05)));
+  SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05)));
-	SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0)));
+  SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0)));
 
-	gtsam::Values values;
+  gtsam::Values values;
-	values.insert(key1, p1);
+  values.insert(key1, p1);
-	values.insert(key2, p2);
+  values.insert(key2, p2);
 
-	double prior_outlier = 0.0;
+  double prior_outlier = 0.0;
-	double prior_inlier = 1.0;
+  double prior_inlier = 1.0;
 
-	BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
+  BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
-			prior_inlier, prior_outlier);
+      prior_inlier, prior_outlier);
 
-	SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
+  SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
 
-	NonlinearFactorGraph graph;
+  NonlinearFactorGraph graph;
-	graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
+  graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
-	graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
+  graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
 
-	f.updateNoiseModels(values, graph);
+  f.updateNoiseModels(values, graph);
 
-	SharedGaussian model_inlier_new = f.get_model_inlier();
+  SharedGaussian model_inlier_new = f.get_model_inlier();
-	SharedGaussian model_outlier_new = f.get_model_outlier();
+  SharedGaussian model_outlier_new = f.get_model_outlier();
 
-	model_inlier->print("model_inlier:");
+  model_inlier->print("model_inlier:");
-	model_outlier->print("model_outlier:");
+  model_outlier->print("model_outlier:");
-	model_inlier_new->print("model_inlier_new:");
+  model_inlier_new->print("model_inlier_new:");
-	model_outlier_new->print("model_outlier_new:");
+  model_outlier_new->print("model_outlier_new:");
 }
 
 

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,124 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/LieVector.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+//! Factors
+typedef GaussMarkov1stOrderFactor<LieVector> GaussMarkovFactor;
+
+/* ************************************************************************* */
+LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) {
+  return factor.evaluateError(v1, v2);
+}
+
+/* ************************************************************************* */
+TEST( GaussMarkovFactor, equals )
+{
+  // Create two identical factors and make sure they're equal
+  Key x1(1);
+  Key x2(2);
+  double delta_t = 0.10;
+  Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
+  SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
+
+  GaussMarkovFactor factor1(x1, x2, delta_t, tau, model);
+  GaussMarkovFactor factor2(x1, x2, delta_t, tau, model);
+
+  CHECK(assert_equal(factor1, factor2));
+}
+
+/* ************************************************************************* */
+TEST( GaussMarkovFactor, error )
+{
+  Values linPoint;
+  Key x1(1);
+  Key x2(2);
+  double delta_t = 0.10;
+  Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
+  SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
+
+  LieVector v1 = LieVector((Vector(3) << 10.0, 12.0, 13.0));
+  LieVector v2 = LieVector((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);
+  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);
+  }
+  Vector Err2( v2 - alpha_v1 );
+
+  CHECK(assert_equal(Err1, Err2, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST (GaussMarkovFactor, jacobian ) {
+
+  Values linPoint;
+  Key x1(1);
+  Key x2(2);
+  double delta_t = 0.10;
+  Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
+  SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
+
+  GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
+
+  // Update the linearization point
+  LieVector v1_upd = LieVector((Vector(3) << 0.5, -0.7, 0.3));
+  LieVector v2_upd = LieVector((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
+  Matrix numerical_H1, numerical_H2;
+  numerical_H1 = numericalDerivative21<Vector3, Vector3, Vector3>(
+      boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
+  numerical_H2 = numericalDerivative22<Vector3, Vector3, Vector3>(
+      boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
+
+  // Verify they are equal for this choice of state
+  CHECK( assert_equal(numerical_H1, computed_H1, 1e-9));
+  CHECK( assert_equal(numerical_H2, computed_H2, 1e-9));
+}
+
+/* ************************************************************************* */
+int main()
+{
+  TestResult tr; return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

timing/timePinholeCamera.cpp

@@ -26,7 +26,7 @@ using namespace gtsam;
 
 int main()
 {
-  int n = 1000000;
+  int n = 1e6;
 
   const Pose3 pose1((Matrix)(Matrix(3,3) <<
       1., 0., 0.,
@@ -35,8 +35,6 @@ int main()
   ),
   Point3(0,0,0.5));
 
-//  static Cal3_S2 K(500, 100, 0.1, 320, 240);
-//  static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
   static Cal3Bundler K(500, 1e-3, 2.0*1e-3);
   const PinholeCamera<Cal3Bundler> camera(pose1,K);
   const Point3 point1(-0.08,-0.08, 0.0);
@@ -63,8 +61,18 @@ int main()
       camera.project(point1);
     long timeLog2 = clock();
     double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-    cout << ((double)n/seconds) << " calls/second" << endl;
+    cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
-    cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
+  }
+
+  // Oct 12 2014, Macbook Air
+  {
+    long timeLog = clock();
+    Point2 measurement(0,0);
+    for(int i = 0; i < n; i++)
+      measurement.localCoordinates(camera.project(point1));
+    long timeLog2 = clock();
+    double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+    cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
   }
 
   // Oct 12 2013, iMac 3.06GHz Core i3
@@ -84,8 +92,7 @@ int main()
       camera.project(point1, Dpose, Dpoint);
     long timeLog2 = clock();
     double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-    cout << ((double)n/seconds) << " calls/second" << endl;
+    cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
-    cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
   }
 
   // Oct 12 2013, iMac 3.06GHz Core i3
@@ -97,7 +104,7 @@ int main()
   // Cal3Bundler fix:   2.0946 musecs/call
   // June 24 2014, Macbook Pro 2.3GHz Core i7
   // GTSAM 3.1:         0.2294 musecs/call
-  // After project fix: 0.2093 musecs/call
+  // After project fix: 0.2093 nanosecs/call
   {
     Matrix Dpose, Dpoint, Dcal;
     long timeLog = clock();
@@ -105,8 +112,7 @@ int main()
       camera.project(point1, Dpose, Dpoint, Dcal);
     long timeLog2 = clock();
     double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
-    cout << ((double)n/seconds) << " calls/second" << endl;
+    cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
-    cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
   }
 
   return 0;