Merge remote-tracking branch 'origin/develop' into feature/concurrent-calibration

CMakeLists.txt

@@ -46,7 +46,6 @@ endif()
 # Set up options
 
 # Configurable Options
-option(GTSAM_BUILD_TIMING                "Enable/Disable building of timing scripts" OFF) # These do not currently work
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
 endif()
@@ -304,6 +303,9 @@ add_subdirectory(tests)
 # Build examples
 add_subdirectory(examples)
 
+# Build timing
+add_subdirectory(timing)
+
 # Matlab toolbox
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 	add_subdirectory(matlab)
@@ -361,7 +363,7 @@ message(STATUS "================  Configuration Options  ======================"
 message(STATUS "Build flags                                               ")
 print_config_flag(${GTSAM_BUILD_TESTS}                 "Build Tests                    ")
 print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS}       "Build examples with 'make all' ")
-print_config_flag(${GTSAM_BUILD_TIMING}                "Build Timing scripts           ")
+print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS}         "Build timing scripts with 'make all'")
 if (DOXYGEN_FOUND)
     print_config_flag(${GTSAM_BUILD_DOCS}              "Build Docs                     ")
 endif()

cmake/GtsamTesting.cmake

@@ -38,21 +38,46 @@ endmacro()
 # 
 # Add scripts that will serve as examples of how to use the library.  A list of files or
 # glob patterns is specified, and one executable will be created for each matching .cpp
-# file.  These executables will not be installed.  They are build with 'make all' if
+# file.  These executables will not be installed.  They are built with 'make all' if
 # GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.
 #
 # Usage example:
 #   gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
 #
 # Arguments:
-#   globPatterns:  The list of files or glob patterns from which to create unit tests, with
-#                  one test created for each cpp file.  e.g. "*.cpp", or
+#   globPatterns:  The list of files or glob patterns from which to create examples, with
+#                  one program created for each cpp file.  e.g. "*.cpp", or
 #                  "A*.cpp;B*.cpp;MyExample.cpp".
 #   excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
 #                  an empty string "" if nothing needs to be excluded.
 #   linkLibraries: The list of libraries to link to.
 macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries)
-	gtsamAddExamplesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}")
+	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
+endmacro()
+
+
+###############################################################################
+# Macro:
+#
+# gtsamAddTimingGlob(globPatterns excludedFiles linkLibraries)
+# 
+# Add scripts that time aspects of the library.  A list of files or
+# glob patterns is specified, and one executable will be created for each matching .cpp
+# file.  These executables will not be installed.  They are not built with 'make all',
+# but they may be built with 'make timing'.
+#
+# Usage example:
+#   gtsamAddTimingGlob("*.cpp" "DisabledTimingScript.cpp" "gtsam;GeographicLib")
+#
+# Arguments:
+#   globPatterns:  The list of files or glob patterns from which to create programs, with
+#                  one program created for each cpp file.  e.g. "*.cpp", or
+#                  "A*.cpp;B*.cpp;MyExample.cpp".
+#   excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
+#                  an empty string "" if nothing needs to be excluded.
+#   linkLibraries: The list of libraries to link to.
+macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries)
+	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS})
 endmacro()
 
 
@@ -63,6 +88,7 @@ enable_testing()
 
 option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
+option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
 
 # Add option for combining unit tests
 if(MSVC OR XCODE_VERSION)
@@ -80,6 +106,9 @@ endif()
 # Add examples target
 add_custom_target(examples)
 
+# Add timing target
+add_custom_target(timing)
+
 # Include obsolete macros - will be removed in the near future
 include(GtsamTestingObsolete)
 
@@ -180,7 +209,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 endmacro()
 
 
-macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries)
+macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName buildWithAll)
     # Get all script files
     file(GLOB script_files ${globPatterns})
 
@@ -220,7 +249,7 @@ macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries)
 		target_link_libraries(${script_name} ${linkLibraries})
 	
 		# Add target dependencies
-		add_dependencies(examples ${script_name})
+		add_dependencies(${groupName} ${script_name})
 		if(NOT MSVC AND NOT XCODE_VERSION)
 		  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
 		endif()
@@ -228,12 +257,14 @@ macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries)
 		# Add TOPSRCDIR
 		set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
 
-		if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS)
+        # Exclude from all or not - note weird variable assignment because we're in a macro	
+	    set(buildWithAll_on ${buildWithAll})
+		if(NOT buildWithAll_on)
 			# Exclude from 'make all' and 'make install'
-			set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
+			set_target_properties("${script_name}" PROPERTIES EXCLUDE_FROM_ALL ON)
 		endif()
 
 		# Configure target folder (for MSVC and Xcode)
-		set_property(TARGET ${script_name} PROPERTY FOLDER "Examples")
+		set_property(TARGET ${script_name} PROPERTY FOLDER "${groupName}")
 	endforeach()
 endmacro()

examples/SFMExample_SmartFactor.cpp

@@ -14,6 +14,7 @@
  * @brief   A structure-from-motion problem on a simulated dataset, using smart projection factor
  * @author  Duy-Nguyen Ta
  * @author  Jing Dong
+ * @author  Frank Dellaert
  */
 
 /**
@@ -28,11 +29,6 @@
 // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
 #include <gtsam/geometry/Point2.h>
 
-// Each variable in the system (poses and landmarks) must be identified with a unique key.
-// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
-// Here we will use Symbols
-#include <gtsam/inference/Symbol.h>
-
 // In GTSAM, measurement functions are represented as 'factors'.
 // The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark,
 // The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration,
@@ -65,8 +61,8 @@ using namespace std;
 using namespace gtsam;
 
 // Make the typename short so it looks much cleaner
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>
-  SmartFactor;
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
+    gtsam::Cal3_S2> SmartFactor;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
@@ -75,93 +71,87 @@ int main(int argc, char* argv[]) {
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  noiseModel::Isotropic::shared_ptr measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
-  // Create the set of ground-truth landmarks
+  // Create the set of ground-truth landmarks and poses
   vector<Point3> points = createPoints();
-
-  // Create the set of ground-truth poses
   vector<Pose3> poses = createPoses();
 
   // Create a factor graph
   NonlinearFactorGraph graph;
 
-  // A vector saved all Smart factors (for get landmark position after optimization)
-  vector<SmartFactor::shared_ptr> smartfactors_ptr;
-
   // Simulated measurements from each camera pose, adding them to the factor graph
-  for (size_t i = 0; i < points.size(); ++i) {
+  for (size_t j = 0; j < points.size(); ++j) {
 
     // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
     SmartFactor::shared_ptr smartfactor(new SmartFactor());
 
-    for (size_t j = 0; j < poses.size(); ++j) {
+    for (size_t i = 0; i < poses.size(); ++i) {
 
       // generate the 2D measurement
-      SimpleCamera camera(poses[j], *K);
-      Point2 measurement = camera.project(points[i]);
+      SimpleCamera camera(poses[i], *K);
+      Point2 measurement = camera.project(points[j]);
 
-      // call add() function to add measurment into a single factor, here we need to add:
+      // call add() function to add measurement into a single factor, here we need to add:
       //    1. the 2D measurement
       //    2. the corresponding camera's key
       //    3. camera noise model
       //    4. camera calibration
-      smartfactor->add(measurement, Symbol('x', j), measurementNoise, K);
+      smartfactor->add(measurement, i, measurementNoise, K);
     }
 
-    // save smartfactors to get landmark position
-    smartfactors_ptr.push_back(smartfactor);
-
     // insert the smart factor in the graph
     graph.push_back(smartfactor);
   }
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
+  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
+  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
 
-  // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
-  // Here we add a prior on the second pose x1, so this will fix the scale by indicating the distance between x0 and x1.
-  // Because these two are fixed, the rest poses will be alse fixed.
-  graph.push_back(PriorFactor<Pose3>(Symbol('x', 1), poses[1], poseNoise)); // add directly to graph
+  // Because the structure-from-motion problem has a scale ambiguity, the problem is
+  // still under-constrained. Here we add a prior on the second pose x1, so this will
+  // fix the scale by indicating the distance between x0 and x1.
+  // Because these two are fixed, the rest of the poses will be also be fixed.
+  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
 
   graph.print("Factor Graph:\n");
 
-  // Create the data structure to hold the initial estimate to the solution
+  // Create the initial estimate to the solution
   // Intentionally initialize the variables off from the ground truth
   Values initialEstimate;
+  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(i, poses[i].compose(delta));
   initialEstimate.print("Initial Estimates:\n");
 
   // Optimize the graph and print results
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();
   result.print("Final results:\n");
 
-
-  // Notice: Smart factor represent the 3D point as a factor, not a variable.
+  // A smart factor represent the 3D point inside the factor, not as a variable.
   // The 3D position of the landmark is not explicitly calculated by the optimizer.
-  // If you do want to output the landmark's 3D position, you should use the internal function point()
-  // of the smart factor to get the 3D point.
+  // To obtain the landmark's 3D position, we use the "point" method of the smart factor.
   Values landmark_result;
-  for (size_t i = 0; i < points.size(); ++i) {
+  for (size_t j = 0; j < points.size(); ++j) {
 
-    // The output of point() is in boost::optional<gtsam::Point3>, since sometimes
-    // the triangulation opterations inside smart factor will encounter degeneracy.
-    // Check the manual of boost::optional for more details for the usages.
+    // The output of point() is in boost::optional<gtsam::Point3>, as sometimes
+    // the triangulation operation inside smart factor will encounter degeneracy.
     boost::optional<Point3> point;
 
-    // here we use the saved smart factors to call, pass in all optimized pose to calculate landmark positions
-    point = smartfactors_ptr.at(i)->point(result);
-
-    // ignore if boost::optional return NULL
-    if (point)
-      landmark_result.insert(Symbol('l', i), *point);
+    // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
+    SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
+    if (smart) {
+      point = smart->point(result);
+      if (point) // ignore if boost::optional return NULL
+        landmark_result.insert(j, *point);
+    }
   }
 
   landmark_result.print("Landmark results:\n");
 
-
   return 0;
 }
 /* ************************************************************************* */

examples/SFMExample_bal.cpp

@@ -43,8 +43,7 @@ int main (int argc, char* argv[]) {
 
   // Load the SfM data from file
   SfM_data mydata;
-  const bool success = readBAL(filename, mydata);
-  assert(success);
+  assert(readBAL(filename, mydata));
   cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph

gtsam/base/CMakeLists.txt

@@ -7,9 +7,3 @@ install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
 
 # Build tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(base "gtsam" "gtsam" "${base_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)
-

gtsam/discrete/CMakeLists.txt

@@ -6,9 +6,3 @@ install(FILES ${discrete_headers} DESTINATION include/gtsam/discrete)
 
 # Add all tests
 add_subdirectory(tests)
-
-# Build timing scripts
-#if (GTSAM_BUILD_TIMING)
-#    gtsam_add_timing(discrete "${discrete_local_libs}")
-#endif(GTSAM_BUILD_TIMING)
-

gtsam/geometry/CMakeLists.txt

@@ -4,9 +4,3 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry)
 
 # Build tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(geometry "gtsam" "gtsam" "${geometry_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)
-

gtsam/geometry/EssentialMatrix.h

@@ -112,6 +112,16 @@ public:
     return E_;
   }
 
+  /// Return epipole in image_a , as Unit3 to allow for infinity
+  inline const Unit3& epipole_a() const {
+    return aTb_; // == direction()
+  }
+
+  /// Return epipole in image_b, as Unit3 to allow for infinity
+  inline Unit3 epipole_b() const {
+    return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
+  }
+
   /**
    * @brief takes point in world coordinates and transforms it to pose with |t|==1
    * @param p point in world coordinates

gtsam/geometry/PinholeCamera.h

@@ -275,11 +275,12 @@ public:
     if (P.z() <= 0)
       throw CheiralityException();
 #endif
+    double d = 1.0 / P.z();
+    const double u = P.x() * d, v = P.y() * d;
     if (Dpoint) {
-      double d = 1.0 / P.z(), d2 = d * d;
-      *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
+      *Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d);
     }
-    return Point2(P.x() / P.z(), P.y() / P.z());
+    return Point2(u, v);
   }
 
   /// Project a point into the image and check depth

gtsam/geometry/Unit3.cpp

@@ -58,7 +58,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
 }
 
 /* ************************************************************************* */
-Matrix Unit3::basis() const {
+const Matrix& Unit3::basis() const {
 
   // Return cached version if exists
   if (B_.rows() == 3)

gtsam/geometry/Unit3.h

@@ -84,7 +84,7 @@ public:
    * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
    * tangent to the sphere at the current direction.
    */
-  Matrix basis() const;
+  const Matrix& basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
   Matrix skew() const;

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -144,9 +144,9 @@ TEST (EssentialMatrix, FromPose3_b) {
   Matrix actualH;
   Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
   Point3 c1Tc2(0.4, 0.5, 0.6);
-  EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
+  EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
   Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
-  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
   Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
       boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
   EXPECT(assert_equal(expectedH, actualH, 1e-5));
@@ -161,6 +161,35 @@ TEST (EssentialMatrix, streaming) {
   EXPECT(assert_equal(expected, actual));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, epipoles) {
+  // Create an E
+  Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
+  Point3 c1Tc2(0.4, 0.5, 0.6);
+  EssentialMatrix E(c1Rc2, Unit3(c1Tc2));
+
+  // Calculate expected values through SVD
+  Matrix U, V;
+  Vector S;
+  gtsam::svd(E.matrix(), U, S, V);
+
+  // check rank 2 constraint
+  CHECK(fabs(S(2))<1e-10);
+
+  // check epipoles not at infinity
+  CHECK(fabs(U(2,2))>1e-10 && fabs(V(2,2))>1e-10);
+
+  // Check epipoles
+
+  // Epipole in image 1 is just E.direction()
+  Unit3 e1(U(0, 2), U(1, 2), U(2, 2));
+  EXPECT(assert_equal(e1, E.epipole_a()));
+
+  // Epipole in image 2 is E.rotation().unrotate(E.direction())
+  Unit3 e2(V(0, 2), V(1, 2), V(2, 2));
+  EXPECT(assert_equal(e2, E.epipole_b()));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/inference/CMakeLists.txt

@@ -4,9 +4,3 @@ install(FILES ${inference_headers} DESTINATION include/gtsam/inference)
 
 # Build tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(inference "gtsam" "gtsam" "${inference_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)
-

gtsam/linear/CMakeLists.txt

@@ -4,8 +4,3 @@ install(FILES ${linear_headers} DESTINATION include/gtsam/linear)
 
 # Build tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(linear "gtsam" "gtsam" "${linear_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)

gtsam/linear/IterativeSolver.h

@@ -104,6 +104,7 @@ namespace gtsam {
   class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> {
   public:
     typedef boost::tuple<Key,size_t,Key> Base;
+    KeyInfoEntry(){}
     KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
     const size_t index() const { return this->get<0>(); }
     const size_t dim() const { return this->get<1>(); }

gtsam/linear/SubgraphPreconditioner.h

@@ -157,8 +157,6 @@ namespace gtsam {
     Weights weights(const GaussianFactorGraph &gfg) const;
     SubgraphBuilderParameters parameters_;
 
-  private:
-    SubgraphBuilder() {}
   };
 
   /*******************************************************************************************/

gtsam/linear/tests/timeSLAMlike.cpp

@@ -1,144 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file    timeSLAMlike.cpp
- * @brief   Times solving of random SLAM-like graphs
- * @author  Richard Roberts
- * @date    Aug 30, 2010
- */
-
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/NoiseModel.h>
-
-#include <boost/random.hpp>
-#include <boost/timer.hpp>
-#include <boost/bind.hpp>
-#include <boost/lambda/lambda.hpp>
-#include <vector>
-
-using namespace gtsam;
-using namespace std;
-using namespace boost::lambda;
-
-typedef EliminationTree<JacobianFactor> GaussianEliminationTree;
-
-static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
-
-bool _pair_compare(const pair<Index, Matrix>& a1, const pair<Index, Matrix>& a2) { return a1.first < a2.first; }
-
-int main(int argc, char *argv[]) {
-
-  size_t vardim = 3;
-  size_t blockdim = 3;
-  int nVars = 500;
-  size_t blocksPerVar = 5;
-  size_t varsPerBlock = 2;
-  size_t varSpread = 10;
-
-  size_t nTrials = 10;
-
-  double blockbuild, blocksolve;
-
-  cout << "\n" << nVars << " variables of dimension " << vardim << ", " <<
-      blocksPerVar << " blocks for each variable, blocks of dimension " << blockdim << " measure " << varsPerBlock << " variables\n";
-  cout << nTrials << " trials\n";
-
-  boost::variate_generator<boost::mt19937, boost::uniform_int<> > ri(boost::mt19937(), boost::uniform_int<>(-varSpread, varSpread));
-
-  /////////////////////////////////////////////////////////////////////////////
-  // Timing test with blockwise Gaussian factor graphs
-
-  {
-    // Build GFG's
-    cout << "Building SLAM-like Gaussian factor graphs... ";
-    cout.flush();
-    boost::timer timer;
-    timer.restart();
-    vector<GaussianFactorGraph> blockGfgs;
-    blockGfgs.reserve(nTrials);
-    for(size_t trial=0; trial<nTrials; ++trial) {
-      blockGfgs.push_back(GaussianFactorGraph());
-      SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
-      for(int c=0; c<nVars; ++c) {
-        for(size_t d=0; d<blocksPerVar; ++d) {
-          vector<pair<Index, Matrix> > terms; terms.reserve(varsPerBlock);
-          if(c == 0 && d == 0)
-            // If it's the first factor, just make a prior
-            terms.push_back(make_pair(0, eye(vardim)));
-          else if(c != 0) {
-            // Generate a random Gaussian factor
-            for(size_t h=0; h<varsPerBlock; ++h) {
-              int var;
-              // If it's the first factor for this variable, make it "odometry"
-              if(d == 0 && h == 0)
-                var = c-1;
-              else if(d == 0 && h == 1)
-                var = c;
-              else
-                // Choose random variable ids
-                do
-                  var = c + ri();
-                while(var < 0 || var > nVars-1 || find_if(terms.begin(), terms.end(),
-                    boost::bind(&pair<Index, Matrix>::first, boost::lambda::_1) == Index(var)) != terms.end());
-              Matrix A(blockdim, vardim);
-              for(size_t j=0; j<blockdim; ++j)
-                for(size_t k=0; k<vardim; ++k)
-                  A(j,k) = rg();
-              terms.push_back(make_pair(Index(var), A));
-            }
-          }
-          Vector b(blockdim);
-          sort(terms.begin(), terms.end(), &_pair_compare);
-          for(size_t j=0; j<blockdim; ++j)
-            b(j) = rg();
-          if(!terms.empty())
-            blockGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(terms, b, noise)));
-        }
-      }
-
-//      if(trial == 0)
-//        blockGfgs.front().print("GFG: ");
-    }
-    blockbuild = timer.elapsed();
-    cout << blockbuild << " s" << endl;
-
-    // Solve GFG's
-    cout << "Solving SLAM-like Gaussian factor graphs... ";
-    cout.flush();
-    timer.restart();
-    for(size_t trial=0; trial<nTrials; ++trial) {
-//      cout << "Trial " << trial << endl;
-      VectorValues soln(*GaussianMultifrontalSolver(blockGfgs[trial]).optimize());
-    }
-    blocksolve = timer.elapsed();
-    cout << blocksolve << " s" << endl;
-  }
-
-
-  /////////////////////////////////////////////////////////////////////////////
-  // Print per-graph times
-  cout << "\nPer-factor-graph times for building and solving\n";
-  cout << "  total " << (1000.0*(blockbuild+blocksolve)/double(nTrials)) <<
-      "  build " << (1000.0*blockbuild/double(nTrials)) <<
-      "  solve " << (1000.0*blocksolve/double(nTrials)) << " ms/graph\n";
-  cout << endl;
-
-  return 0;
-}
-
-/**
- * @file    timeSLAMlike.cpp
- * @brief   
- * @author  Richard Roberts
- * @date Aug 30, 2010
- */
-

gtsam/navigation/CMakeLists.txt

@@ -4,8 +4,3 @@ install(FILES ${navigation_headers} DESTINATION include/gtsam/navigation)
 
 # Add all tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(navigation "gtsam" "gtsam" "${navigation_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)

gtsam/nonlinear/CMakeLists.txt

@@ -4,9 +4,3 @@ install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear)
 
 # Build tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)
-

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -37,7 +37,7 @@ using boost::adaptors::map_values;
 
 /* ************************************************************************* */
 LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(
-    const std::string &src) const {
+    const std::string &src) {
   std::string s = src;
   boost::algorithm::to_upper(s);
   if (s == "SILENT")
@@ -59,7 +59,7 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans
 
 /* ************************************************************************* */
 std::string LevenbergMarquardtParams::verbosityLMTranslator(
-    VerbosityLM value) const {
+    VerbosityLM value) {
   std::string s;
   switch (value) {
   case LevenbergMarquardtParams::SILENT:

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -41,9 +41,8 @@ public:
     SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA
   };
 
-private:
-  VerbosityLM verbosityLMTranslator(const std::string &s) const;
-  std::string verbosityLMTranslator(VerbosityLM value) const;
+  static VerbosityLM verbosityLMTranslator(const std::string &s);
+  static std::string verbosityLMTranslator(VerbosityLM value);
 
 public:
 

gtsam/nonlinear/NonlinearOptimizerParams.cpp

@@ -14,7 +14,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslator(
-    const std::string &src) const {
+    const std::string &src) {
   std::string s = src;
   boost::algorithm::to_upper(s);
   if (s == "SILENT")
@@ -36,7 +36,7 @@ NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslato
 
 /* ************************************************************************* */
 std::string NonlinearOptimizerParams::verbosityTranslator(
-    Verbosity value) const {
+    Verbosity value) {
   std::string s;
   switch (value) {
   case NonlinearOptimizerParams::SILENT:

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -84,9 +84,8 @@ public:
     verbosity = verbosityTranslator(src);
   }
 
-private:
-  Verbosity verbosityTranslator(const std::string &s) const;
-  std::string verbosityTranslator(Verbosity value) const;
+  static Verbosity verbosityTranslator(const std::string &s) ;
+  static std::string verbosityTranslator(Verbosity value) ;
 
   // Successive Linearization Parameters
 

gtsam/slam/CMakeLists.txt

@@ -8,8 +8,3 @@ install(FILES ${slam_headers} DESTINATION include/gtsam/slam)
 
 # Build tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(slam "gtsam" "gtsam" "${slam_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)

gtsam/symbolic/CMakeLists.txt

@@ -4,9 +4,3 @@ install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic)
 
 # Build tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)
-

gtsam_unstable/CMakeLists.txt

@@ -119,3 +119,5 @@ endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
 # Build examples
 add_subdirectory(examples)
 
+# Build timing
+add_subdirectory(timing)

gtsam_unstable/base/CMakeLists.txt

@@ -4,8 +4,3 @@ install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base)
 
 # Add all tests
 add_subdirectory(tests)
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    gtsam_add_subdir_timing(base_unstable "${base_full_libs}" "${base_full_libs}" "${base_excluded_files}") 
-endif(GTSAM_BUILD_TIMING)

gtsam_unstable/gtsam_unstable.h

@@ -333,6 +333,10 @@ virtual class BetweenFactorEM : gtsam::NonlinearFactor {
   void set_flag_bump_up_near_zero_probs(bool flag);
   bool get_flag_bump_up_near_zero_probs() const;
 
+  void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
+  Matrix get_model_inlier_cov();
+  Matrix get_model_outlier_cov();
+
   void serializable() const; // enabling serialization functionality
 };
 

gtsam_unstable/partition/CMakeLists.txt

@@ -2,6 +2,4 @@
 file(GLOB partition_headers "*.h")
 install(FILES ${partition_headers} DESTINATION include/gtsam_unstable/parition)
 
-set(ignore_test "tests/testNestedDissection.cpp")
-# Add all tests
-gtsamAddTestsGlob(partition_unstable "tests/*.cpp" "${ignore_test}" "gtsam_unstable;metis")
+add_subdirectory(tests)

gtsam_unstable/partition/tests/CMakeLists.txt

@@ -0,0 +1,2 @@
+set(ignore_test "testNestedDissection.cpp")
+gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable")

gtsam_unstable/slam/BetweenFactorEM.h

@@ -21,6 +21,7 @@
 #include <gtsam/base/Lie.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/nonlinear/Marginals.h>
 
 namespace gtsam {
 
@@ -292,6 +293,77 @@ namespace gtsam {
       return flag_bump_up_near_zero_probs_;
     }
 
+    /* ************************************************************************* */
+    SharedGaussian get_model_inlier() const {
+    	return model_inlier_;
+    }
+
+    /* ************************************************************************* */
+    SharedGaussian get_model_outlier() const {
+    	return model_outlier_;
+    }
+
+    /* ************************************************************************* */
+    Matrix get_model_inlier_cov() const {
+    	return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+    }
+
+    /* ************************************************************************* */
+    Matrix get_model_outlier_cov() const {
+    	return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+    }
+
+    /* ************************************************************************* */
+    void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
+    	/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
+    	 * (note these are given in the E step, where indicator probabilities are calculated).
+    	 *
+    	 * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
+    	 * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
+    	 *
+    	 * TODO: improve efficiency (info form)
+    	 */
+
+    	 const T& p1 = values.at<T>(key1_);
+    	 const T& p2 = values.at<T>(key2_);
+
+    	 Matrix H1, H2;
+    	 T hx = p1.between(p2, H1, H2); // h(x)
+
+    	 // get joint covariance of the involved states
+    	 std::vector<gtsam::Key> Keys;
+    	 Keys.push_back(key1_);
+    	 Keys.push_back(key2_);
+    	 Marginals marginals( graph, values, Marginals::QR );
+    	 JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
+    	 Matrix cov1 = joint_marginal12(key1_, key1_);
+    	 Matrix cov2 = joint_marginal12(key2_, key2_);
+    	 Matrix cov12 = joint_marginal12(key1_, key2_);
+
+    	 Matrix H;
+    	 H.resize(H1.rows(), H1.rows()+H2.rows());
+    	 H << H1, H2; // H = [H1 H2]
+
+    	 Matrix joint_cov;
+    	 joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
+    	 joint_cov << cov1, cov12,
+    			 cov12.transpose(), cov2;
+
+    	 Matrix cov_state = H*joint_cov*H.transpose();
+
+    	 //    	 model_inlier_->print("before:");
+
+    	 // update inlier and outlier noise models
+    	 Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
+    	 model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
+
+    	 Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
+    	 model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
+
+    	 //    	 model_inlier_->print("after:");
+    	 //    	 std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
+    }
+
     /* ************************************************************************* */
     /** return the measured */
     const VALUE& measured() const {

gtsam_unstable/slam/tests/testBetweenFactorEM.cpp

@@ -14,11 +14,13 @@
 #include <gtsam/base/numericalDerivative.h>
 
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
 
 
 using namespace std;
 using namespace gtsam;
 
+
 // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
 // to reenable the test.
 #if 0
@@ -251,6 +253,49 @@ TEST( BetweenFactorEM, CaseStudy)
   }
 }
 
+
+///* ************************************************************************** */
+TEST (BetweenFactorEM, updateNoiseModel ) {
+	gtsam::Key key1(1);
+	gtsam::Key key2(2);
+
+	gtsam::Pose2 p1(10.0, 15.0, 0.1);
+	gtsam::Pose2 p2(15.0, 15.0, 0.3);
+	gtsam::Pose2 noise(0.5, 0.4, 0.01);
+	gtsam::Pose2 rel_pose_ideal = p1.between(p2);
+	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_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0)));
+
+	gtsam::Values values;
+	values.insert(key1, p1);
+	values.insert(key2, p2);
+
+	double prior_outlier = 0.0;
+	double prior_inlier = 1.0;
+
+	BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
+			prior_inlier, prior_outlier);
+
+	SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
+
+	NonlinearFactorGraph graph;
+	graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
+	graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
+
+	f.updateNoiseModels(values, graph);
+
+	SharedGaussian model_inlier_new = f.get_model_inlier();
+	SharedGaussian model_outlier_new = f.get_model_outlier();
+
+	model_inlier->print("model_inlier:");
+	model_outlier->print("model_outlier:");
+	model_inlier_new->print("model_inlier_new:");
+	model_outlier_new->print("model_outlier_new:");
+}
+
+
 #endif
 
 /* ************************************************************************* */

gtsam_unstable/timing/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTimingGlob("*.cpp" "" "gtsam_unstable")

gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp

@@ -20,9 +20,9 @@
 #include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/nonlinear/Key.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/LieVector.h>
+#include <gtsam/inference/Key.h>
 
 using namespace std;
 using namespace gtsam;
@@ -32,7 +32,7 @@ gtsam::Rot3 world_R_ECEF(
     0.85173,     -0.52399,            0,
     0.41733,      0.67835,     -0.60471);
 
-gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
+gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
 gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
 /* ************************************************************************* */
@@ -54,16 +54,16 @@ int main() {
   gtsam::Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vec(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5));
+  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
+  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
+  gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
   gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   // Second test: zero angular motion, some acceleration - generated in matlab
-  Vector measurement_acc((Vec(3) << 6.501390843381716,  -6.763926150509185,  -2.300389940090343));
-  Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3));
+  Vector measurement_acc((Vector(3) << 6.501390843381716,  -6.763926150509185,  -2.300389940090343));
+  Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3));
 
   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
 
@@ -72,7 +72,7 @@ int main() {
       -0.652537293,  0.709880342,  0.265075427);
   Point3 t1(2.0,1.0,3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1(3,0.5,-0.5,0.4);
+  LieVector Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4));
   Rot3 R2(0.473618898,   0.119523052,  0.872582019,
        0.609241153,   0.67099888, -0.422594037,
       -0.636011287,  0.731761397,  0.244979388);
@@ -99,7 +99,7 @@ int main() {
   GaussianFactorGraph graph;
   gttic_(LinearizeTiming);
   for(size_t i = 0; i < 100000; ++i) {
-    GaussianFactor::shared_ptr g = f.linearize(values, ordering);
+    GaussianFactor::shared_ptr g = f.linearize(values);
     graph.push_back(g);
   }
   gttoc_(LinearizeTiming);

tests/CMakeLists.txt

@@ -14,16 +14,3 @@ if(MSVC)
 	set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp"
 		APPEND PROPERTY COMPILE_FLAGS "/bigobj")
 endif()
-
-# Build timing scripts
-if (GTSAM_BUILD_TIMING)
-    # Subdirectory target for timing - does not actually execute the scripts
-    add_custom_target(timing.tests)
-    set(is_test FALSE)
-
-    # Build grouped benchmarks
-    gtsam_add_grouped_scripts("tests"               # Use subdirectory as group label
-    "time*.cpp" timing "Timing Benchmark"         # Standard for all timing scripts
-    "${tests_full_libs}" "${tests_full_libs}" "${tests_exclude}"   # Pass in linking and exclusion lists
-    ${is_test})
-endif (GTSAM_BUILD_TIMING)

timing/CMakeLists.txt

@@ -0,0 +1,3 @@
+gtsamAddTimingGlob("*.cpp" "" "gtsam")
+
+target_link_libraries(timeGaussianFactorGraph CppUnitLite)

timing/timeCalibratedCamera.cpp

@@ -27,11 +27,11 @@ int main()
 {
   int n = 100000;
 
-  const Pose3 pose1((Matrix)(Mat(3,3) <<
+  const Pose3 pose1(Matrix3((Matrix(3,3) <<
       1., 0., 0.,
       0.,-1., 0.,
       0., 0.,-1.
-  ),
+  )),
   Point3(0,0,0.5));
 
   const CalibratedCamera camera(pose1);

timing/timeFactorOverhead.cpp

@@ -16,30 +16,29 @@
  * @date    Aug 20, 2010
  */
 
+#include <gtsam/base/timing.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/NoiseModel.h>
-#include <gtsam/inference/EliminationTree-inl.h>
+#include <gtsam/linear/VectorValues.h>
 
 #include <boost/random.hpp>
-#include <boost/timer.hpp>
 #include <vector>
 
 using namespace gtsam;
 using namespace std;
 
-typedef EliminationTree<GaussianFactor> GaussianEliminationTree;
-
 static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
 
 int main(int argc, char *argv[]) {
 
-  Index key = 0;
+  Key key = 0;
 
   size_t vardim = 2;
   size_t blockdim = 1;
-  size_t nBlocks = 2000;
+  size_t nBlocks = 4000;
 
-  size_t nTrials = 10;
+  size_t nTrials = 20;
 
   double blockbuild, blocksolve, combbuild, combsolve;
 
@@ -54,8 +53,7 @@ int main(int argc, char *argv[]) {
     // Build GFG's
     cout << "Building blockwise Gaussian factor graphs... ";
     cout.flush();
-    boost::timer timer;
-    timer.restart();
+    gttic_(blockbuild);
     vector<GaussianFactorGraph> blockGfgs;
     blockGfgs.reserve(nTrials);
     for(size_t trial=0; trial<nTrials; ++trial) {
@@ -70,22 +68,26 @@ int main(int argc, char *argv[]) {
         Vector b(blockdim);
         for(size_t j=0; j<blockdim; ++j)
           b(j) = rg();
-        blockGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(key, A, b, noise)));
+        blockGfgs[trial].push_back(boost::make_shared<JacobianFactor>(key, A, b, noise));
       }
     }
-    blockbuild = timer.elapsed();
+    gttoc_(blockbuild);
+    tictoc_getNode(blockbuildNode, blockbuild);
+    blockbuild = blockbuildNode->secs();
     cout << blockbuild << " s" << endl;
 
     // Solve GFG's
     cout << "Solving blockwise Gaussian factor graphs... ";
     cout.flush();
-    timer.restart();
+    gttic_(blocksolve);
     for(size_t trial=0; trial<nTrials; ++trial) {
 //      cout << "Trial " << trial << endl;
-      GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate(&EliminateQR));
-      VectorValues soln(optimize(*gbn));
+      GaussianBayesNet::shared_ptr gbn = blockGfgs[trial].eliminateSequential();
+      VectorValues soln = gbn->optimize();
     }
-    blocksolve = timer.elapsed();
+    gttoc_(blocksolve);
+    tictoc_getNode(blocksolveNode, blocksolve);
+    blocksolve = blocksolveNode->secs();
     cout << blocksolve << " s" << endl;
   }
 
@@ -97,8 +99,7 @@ int main(int argc, char *argv[]) {
     // Build GFG's
     cout << "Building combined-factor Gaussian factor graphs... ";
     cout.flush();
-    boost::timer timer;
-    timer.restart();
+    gttic_(combbuild);
     vector<GaussianFactorGraph> combGfgs;
     for(size_t trial=0; trial<nTrials; ++trial) {
       combGfgs.push_back(GaussianFactorGraph());
@@ -115,21 +116,25 @@ int main(int argc, char *argv[]) {
         for(size_t j=0; j<blockdim; ++j)
           bcomb(blockdim*i+j) = rg();
       }
-      combGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(key, Acomb, bcomb,
-          noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))));
+      combGfgs[trial].push_back(boost::make_shared<JacobianFactor>(key, Acomb, bcomb,
+          noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0)));
     }
-    combbuild = timer.elapsed();
+    gttoc(combbuild);
+    tictoc_getNode(combbuildNode, combbuild);
+    combbuild = combbuildNode->secs();
     cout << combbuild << " s" << endl;
 
     // Solve GFG's
     cout << "Solving combined-factor Gaussian factor graphs... ";
     cout.flush();
-    timer.restart();
+    gttic_(combsolve);
     for(size_t trial=0; trial<nTrials; ++trial) {
-      GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate(&EliminateQR));
-      VectorValues soln(optimize(*gbn));
+      GaussianBayesNet::shared_ptr gbn = combGfgs[trial].eliminateSequential();
+      VectorValues soln = gbn->optimize();
     }
-    combsolve = timer.elapsed();
+    gttoc_(combsolve);
+    tictoc_getNode(combsolveNode, combsolve);
+    combsolve = combsolveNode->secs();
     cout << combsolve << " s" << endl;
   }
 

timing/timeGaussianFactor.cpp

@@ -22,7 +22,7 @@
 using namespace std;
 
 #include <boost/tuple/tuple.hpp>
-#include <boost/assign/std/list.hpp> // for operator += in Ordering
+#include <boost/assign/list_of.hpp>
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/linear/JacobianFactor.h>
@@ -33,7 +33,7 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-static const Index _x1_=1, _x2_=2, _l1_=3;
+static const Key _x1_=1, _x2_=2, _l1_=3;
 
 /*
  * Alex's Machine
@@ -53,7 +53,7 @@ static const Index _x1_=1, _x2_=2, _l1_=3;
 int main()
 {
   // create a linear factor
-  Matrix Ax2 = (Mat(8, 2) <<
+  Matrix Ax2 = (Matrix(8, 2) <<
            // x2  
            -5., 0.,
            +0.,-5.,
@@ -65,7 +65,7 @@ int main()
            +0.,10.
            );
                      
-  Matrix Al1 = (Mat(8, 10) <<
+  Matrix Al1 = (Matrix(8, 10) <<
            // l1     
            5., 0.,1.,2.,3.,4.,5.,6.,7.,8.,
            0., 5.,1.,2.,3.,4.,5.,6.,7.,8.,
@@ -77,7 +77,7 @@ int main()
            0., 0.,1.,2.,3.,4.,5.,6.,7.,8.
            );
                      
-  Matrix Ax1 = (Mat(8, 2) <<
+  Matrix Ax1 = (Matrix(8, 2) <<
            // x1
            0.00,  0.,1.,2.,3.,4.,5.,6.,7.,8.,
            0.00,  0.,1.,2.,3.,4.,5.,6.,7.,8.,
@@ -108,7 +108,8 @@ int main()
   JacobianFactor::shared_ptr factor;
 
   for(int i = 0; i < n; i++)
-    conditional = JacobianFactor(combined).eliminateFirst();
+    boost::tie(conditional, factor) =
+        JacobianFactor(combined).eliminate(Ordering(boost::assign::list_of(_x2_)));
 
   long timeLog2 = clock();
   double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;

timing/timeGaussianFactorGraph.cpp

@@ -29,10 +29,10 @@ using namespace boost::assign;
 /* ************************************************************************* */
 // Create a Kalman smoother for t=1:T and optimize
 double timeKalmanSmoother(int T) {
-  pair<GaussianFactorGraph,Ordering> smoother_ordering = createSmoother(T);
-  GaussianFactorGraph& smoother(smoother_ordering.first);
+  GaussianFactorGraph smoother = createSmoother(T);
   clock_t start = clock();
-  GaussianSequentialSolver(smoother).optimize();
+  // Keys will come out sorted since keys() returns a set
+  smoother.optimize(Ordering(smoother.keys()));
   clock_t end = clock ();
   double dif = (double)(end - start) / CLOCKS_PER_SEC;
   return dif;
@@ -40,12 +40,10 @@ double timeKalmanSmoother(int T) {
 
 /* ************************************************************************* */
 // Create a planar factor graph and optimize
-// todo: use COLAMD ordering again (removed when linear baked-in ordering added)
 double timePlanarSmoother(int N, bool old = true) {
-  boost::tuple<GaussianFactorGraph, VectorValues> pg = planarGraph(N);
-  GaussianFactorGraph& fg(pg.get<0>());
+  GaussianFactorGraph fg = planarGraph(N).get<0>();
   clock_t start = clock();
-  GaussianSequentialSolver(fg).optimize();
+  fg.optimize();
   clock_t end = clock ();
   double dif = (double)(end - start) / CLOCKS_PER_SEC;
   return dif;
@@ -53,12 +51,10 @@ double timePlanarSmoother(int N, bool old = true) {
 
 /* ************************************************************************* */
 // Create a planar factor graph and eliminate
-// todo: use COLAMD ordering again (removed when linear baked-in ordering added)
 double timePlanarSmootherEliminate(int N, bool old = true) {
-  boost::tuple<GaussianFactorGraph, VectorValues> pg = planarGraph(N);
-  GaussianFactorGraph& fg(pg.get<0>());
+  GaussianFactorGraph fg = planarGraph(N).get<0>();
   clock_t start = clock();
-  GaussianSequentialSolver(fg).eliminate();
+  fg.eliminateMultifrontal();
   clock_t end = clock ();
   double dif = (double)(end - start) / CLOCKS_PER_SEC;
   return dif;

timing/timeMatrix.cpp

@@ -189,7 +189,7 @@ double timeColumn(size_t reps) {
  */
 double timeHouseholder(size_t reps) {
   // create a matrix
-  Matrix Abase = Mat(4, 7) <<
+  Matrix Abase = (Matrix(4, 7) <<
       -5,  0, 5, 0,  0,  0,  -1,
       00, -5, 0, 5,  0,  0, 1.5,
       10,  0, 0, 0,-10,  0,   2,

timing/timePinholeCamera.cpp

@@ -28,7 +28,7 @@ int main()
 {
   int n = 1000000;
 
-  const Pose3 pose1((Matrix)(Mat(3,3) <<
+  const Pose3 pose1((Matrix)(Matrix(3,3) <<
       1., 0., 0.,
       0.,-1., 0.,
       0., 0.,-1.
@@ -53,6 +53,10 @@ int main()
   // After Cal3DS2 fix: 0.12231 musecs/call
   // Cal3Bundler:       0.12000 musecs/call
   // Cal3Bundler fix:   0.14637 musecs/call
+  // June 24 2014, Macbook Pro 2.3GHz Core i7
+  // GTSAM 3.1:         0.04295 musecs/call
+  // After project fix: 0.04193 musecs/call
+
   {
     long timeLog = clock();
     for(int i = 0; i < n; i++)
@@ -70,6 +74,9 @@ int main()
   // After Cal3DS2 fix: 3.2857 musecs/call
   // Cal3Bundler:       2.6556 musecs/call
   // Cal3Bundler fix:   2.1613 musecs/call
+  // June 24 2014, Macbook Pro 2.3GHz Core i7
+  // GTSAM 3.1:         0.2322 musecs/call
+  // After project fix: 0.2094 musecs/call
   {
     Matrix Dpose, Dpoint;
     long timeLog = clock();
@@ -88,6 +95,9 @@ int main()
   // After Cal3DS2 fix: 3.4483 musecs/call
   // Cal3Bundler:       2.5112 musecs/call
   // 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
   {
     Matrix Dpose, Dpoint, Dcal;
     long timeLog = clock();

timing/timePose2.cpp

@@ -58,7 +58,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
   if (H1) {
     double dt1 = -s2 * x + c2 * y;
     double dt2 = -c2 * x - s2 * y;
-    *H1 = (Mat(3,3) <<
+    *H1 = (Matrix(3,3) <<
       -c,  -s,  dt1,
       s,  -c,  dt2,
       0.0, 0.0,-1.0);

timing/timePose3.cpp

@@ -37,8 +37,8 @@ int main()
 
   double norm=sqrt(1.0+16.0+4.0);
   double x=1.0/norm, y=4.0/norm, z=2.0/norm;
-  Vector v = (Vec(6) << x, y, z, 0.1, 0.2, -0.1);
-  Pose3 T = Pose3::Expmap((Vec(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v);
+  Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1);
+  Pose3 T = Pose3::Expmap((Vector(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v);
   Matrix H1,H2;
 
   TEST(retract, T.retract(v))

timing/timeStereoCamera.cpp

@@ -27,7 +27,7 @@ int main()
 {
   int n = 100000;
 
-  const Pose3 pose1((Matrix)(Mat(3,3) <<
+  const Pose3 pose1((Matrix)(Matrix(3,3) <<
       1., 0., 0.,
       0.,-1., 0.,
       0., 0.,-1.