Merge remote-tracking branch 'origin/develop' into fix/38_SmartRangeExamples-2

2019-06-12 18:21:53 -04:00 · 2019-06-12 18:21:53 -04:00 · 5017f3e774
parent 6bcbfe2c67 39e08c9a4e
commit 5017f3e774
8 changed files with 153 additions and 74 deletions
--- a/.travis.sh
+++ b/.travis.sh
@ -1,46 +1,85 @@
 #!/bin/bash

-set -e   # Make sure any error makes the script to return an error code
-set -x   # echo
-
-SOURCE_DIR=`pwd`
-BUILD_DIR=build
-
-#CMAKE_C_FLAGS="-Wall -Wextra -Wabi -O2"
-#CMAKE_CXX_FLAGS="-Wall -Wextra -Wabi -O2"
-
-function build_and_test ()
+# common tasks before either build or test
+function prepare ()
 {
+  set -e   # Make sure any error makes the script to return an error code
+  set -x   # echo
+
+  SOURCE_DIR=`pwd`
+  BUILD_DIR=build
+
  #env
  git clean -fd || true
  rm -fr $BUILD_DIR || true
  mkdir $BUILD_DIR && cd $BUILD_DIR

+  if [ -z "$CMAKE_BUILD_TYPE" ]; then
+    CMAKE_BUILD_TYPE=Debug
+  fi
+
+  if [ -z "$GTSAM_ALLOW_DEPRECATED_SINCE_V4" ]; then
+    GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
+  fi
+
  if [ ! -z "$GCC_VERSION" ]; then
    sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-$GCC_VERSION 60 \
                         --slave /usr/bin/g++ g++ /usr/bin/g++-$GCC_VERSION
    sudo update-alternatives --set gcc /usr/bin/gcc-$GCC_VERSION
  fi
+}

-  cmake $SOURCE_DIR \
-      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
-      -DGTSAM_BUILD_TESTS=$GTSAM_BUILD_TESTS \
-      -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \
-      -DGTSAM_BUILD_EXAMPLES_ALWAYS=$GTSAM_BUILD_EXAMPLES_ALWAYS \
-      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4
-
-  # Actual build:
-  make -j2
-
-  # Run tests:
-  if [ "$GTSAM_BUILD_TESTS" == "ON" ]; then
-    make check
-  fi
-
+# common tasks after either build or test
+function finish ()
+{
  # Print ccache stats
  ccache -s

  cd $SOURCE_DIR
 }

-build_and_test
+# compile the code with the intent of populating the cache
+function build ()
+{
+  prepare
+
+  cmake $SOURCE_DIR \
+      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
+      -DGTSAM_BUILD_TESTS=OFF \
+      -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \
+      -DGTSAM_BUILD_EXAMPLES_ALWAYS=ON \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4
+
+  # Actual build:
+  make -j2
+
+  finish
+}
+
+# run the tests
+function test ()
+{
+  prepare
+
+  cmake $SOURCE_DIR \
+      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
+      -DGTSAM_BUILD_TESTS=ON \
+      -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \
+      -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
+
+  # Actual build:
+  make -j2 check
+
+  finish
+}
+
+# select between build or test
+case $1 in
+  -b)
+    build
+    ;;
+  -t)                      
+    test
+    ;;
+esac
--- a/.travis.yml
+++ b/.travis.yml
@ -23,9 +23,6 @@ install:
  - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache  ; fi
  - if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi

-script:
-  - bash .travis.sh
-
 # We first do the compile stage specified below, then the matrix expansion specified after.
 stages:
  - compile
@ -38,43 +35,52 @@ jobs:
  - stage: compile
    os: osx
    compiler: gcc
-    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
  - stage: compile
    os: osx
    compiler: gcc
-    env: CMAKE_BUILD_TYPE=Release GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
 # on Mac, CLANG
  - stage: compile
    os: osx
    compiler: clang
-    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
  - stage: compile
    os: osx
    compiler: clang
-    env: CMAKE_BUILD_TYPE=Release GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
 # on Linux, GCC
  - stage: compile
    os: linux
    compiler: gcc
-    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
  - stage: compile
    os: linux
    compiler: gcc
-    env: CMAKE_BUILD_TYPE=Release GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
 # on Linux, CLANG
  - stage: compile
    os: linux
    compiler: clang
-    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
  - stage: compile
    os: linux
    compiler: clang
-    env: CMAKE_BUILD_TYPE=Release GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
 # on Linux, with deprecated ON to make sure that path still compiles
  - stage: compile
    os: linux
    compiler: clang
-    env: GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON GTSAM_BUILD_TESTS=OFF GTSAM_BUILD_EXAMPLES_ALWAYS=ON
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
+    script: bash .travis.sh -b

 # Matrix configuration:
 os:
@ -87,16 +93,17 @@ env:
  global:
    - MAKEFLAGS="-j2"
    - CCACHE_SLOPPINESS=pch_defines,time_macros
-    - GTSAM_BUILD_UNSTABLE=ON
    - GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
-    - GTSAM_BUILD_EXAMPLES_ALWAYS=OFF
-    - GTSAM_BUILD_TESTS=ON
+    - GTSAM_BUILD_UNSTABLE=ON
  matrix:
-    - CMAKE_BUILD_TYPE=Debug
+    - CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
    - CMAKE_BUILD_TYPE=Release
+script:
+  - bash .travis.sh -t

-# Uncomment this if you want to exclude clang on linux
-# matrix:
-#   exclude:
-#   - os: linux
-#     compiler: clang
+# Exclude clang on Linux/clang in release until issue #57 is solved
+matrix:
+  exclude:
+    - os: linux
+      compiler: clang
+      env : CMAKE_BUILD_TYPE=Release
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@ -95,7 +95,7 @@ class GTSAM_EXPORT VariableIndex {
  }

  /// Return true if no factors associated with a variable
-  const bool empty(Key variable) const {
+  bool empty(Key variable) const {
    return (*this)[variable].empty();
  }

--- a/gtsam/navigation/ScenarioRunner.cpp
+++ b/gtsam/navigation/ScenarioRunner.cpp
@ -31,6 +31,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;

 PreintegratedImuMeasurements ScenarioRunner::integrate(
    double T, const Bias& estimatedBias, bool corrupted) const {
+  gttic_(integrate);
  PreintegratedImuMeasurements pim(p_, estimatedBias);

  const double dt = imuSampleTime();
--- a/gtsam/navigation/tests/testCombinedImuFactor.cpp
+++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp
@ -163,11 +163,11 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
  // Measurements
  const Vector3 measuredOmega(0, 0.1, 0);  // M_PI/10.0+0.3;
  const Vector3 measuredAcc(0, 1.1, -kGravity);
-  const double deltaT = 0.001;
+  const double deltaT = 0.01;

  PreintegratedCombinedMeasurements pim(p, bias);

-  for (int i = 0; i < 1000; ++i)
+  for (int i = 0; i < 100; ++i)
    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
@ -190,9 +190,9 @@ TEST(CombinedImuFactor, PredictRotation) {
  PreintegratedCombinedMeasurements pim(p, bias);
  const Vector3 measuredAcc = - kGravityAlongNavZDown;
  const Vector3 measuredOmega(0, 0, M_PI / 10.0);
-  const double deltaT = 0.001;
+  const double deltaT = 0.01;
  const double tol = 1e-4;
-  for (int i = 0; i < 1000; ++i)
+  for (int i = 0; i < 100; ++i)
    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);

--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@ -18,6 +18,8 @@
 * @author  Stephen Williams
 */

+// #define ENABLE_TIMING // uncomment for timing results
+
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/geometry/Pose3.h>
@ -111,7 +113,7 @@ TEST(ImuFactor, Accelerating) {
  PreintegratedImuMeasurements pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

@ -569,6 +571,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {

 /* ************************************************************************* */
 TEST(ImuFactor, PredictPositionAndVelocity) {
+  gttic(PredictPositionAndVelocity);
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)

  // Measurements
@ -597,6 +600,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {

 /* ************************************************************************* */
 TEST(ImuFactor, PredictRotation) {
+  gttic(PredictRotation);
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)

  // Measurements
@ -623,6 +627,7 @@ TEST(ImuFactor, PredictRotation) {

 /* ************************************************************************* */
 TEST(ImuFactor, PredictArbitrary) {
+  gttic(PredictArbitrary);
  Pose3 x1;
  const Vector3 v1(0, 0, 0);

@ -675,6 +680,7 @@ TEST(ImuFactor, PredictArbitrary) {

 /* ************************************************************************* */
 TEST(ImuFactor, bodyPSensorNoBias) {
+  gttic(bodyPSensorNoBias);
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)

  // Rotate sensor (z-down) to body (same as navigation) i.e. z-up
@ -716,10 +722,11 @@ TEST(ImuFactor, bodyPSensorNoBias) {
 #include <gtsam/nonlinear/Marginals.h>

 TEST(ImuFactor, bodyPSensorWithBias) {
+  gttic(bodyPSensorWithBias);
  using noiseModel::Diagonal;
  typedef Bias Bias;

-  int numFactors = 80;
+  int numFactors = 10;
  Vector6 noiseBetweenBiasSigma;
  noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
      3.0e-6, 3.0e-6);
@ -899,6 +906,7 @@ TEST(ImuFactor, MergeWithCoriolis) {
 /* ************************************************************************* */
 // Same values as pre-integration test but now testing covariance
 TEST(ImuFactor, CheckCovariance) {
+  gttic(CheckCovariance);
  // Measurements
  Vector3 measuredAcc(0.1, 0.0, 0.0);
  Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
@ -922,6 +930,10 @@ TEST(ImuFactor, CheckCovariance) {
 /* ************************************************************************* */
 int main() {
  TestResult tr;
-  return TestRegistry::runAllTests(tr);
+  auto result = TestRegistry::runAllTests(tr);
+#ifdef ENABLE_TIMING
+  tictoc_print_();
+#endif
+  return result;
 }
 /* ************************************************************************* */
--- a/gtsam/navigation/tests/testScenarios.cpp
+++ b/gtsam/navigation/tests/testScenarios.cpp
@ -15,6 +15,8 @@
 * @author  Frank Dellaert
 */

+// #define ENABLE_TIMING // uncomment for timing results
+
 #include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/base/timing.h>
 #include <CppUnitLite/TestHarness.h>
@ -46,6 +48,7 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {

 /* ************************************************************************* */
 TEST(ScenarioRunner, Spin) {
+  gttic(Spin);
  //  angular velocity 6 degree/sec
  const double w = 6 * kDegree;
  const Vector3 W(0, 0, w), V(0, 0, 0);
@ -63,12 +66,12 @@ TEST(ScenarioRunner, Spin) {
  Matrix6 expected;
  expected << p->accelerometerCovariance / kDt, Z_3x3,  //
      Z_3x3, p->gyroscopeCovariance / kDt;
-  Matrix6 actual = runner.estimateNoiseCovariance(10000);
+  Matrix6 actual = runner.estimateNoiseCovariance(1000);
  EXPECT(assert_equal(expected, actual, 1e-2));
 #endif

  // Check calculated covariance against Monte Carlo estimate
-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
 }

@ -79,6 +82,7 @@ ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0));
 }
 /* ************************************************************************* */
 TEST(ScenarioRunner, Forward) {
+  gttic(Forward);
  using namespace forward;
  ScenarioRunner runner(scenario, defaultParams(), kDt);
  const double T = 0.1;  // seconds
@ -86,24 +90,26 @@ TEST(ScenarioRunner, Forward) {
  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, ForwardWithBias) {
+  gttic(ForwardWithBias);
  using namespace forward;
  ScenarioRunner runner(scenario, defaultParams(), kDt);
  const double T = 0.1;  // seconds

  auto pim = runner.integrate(T, kNonZeroBias);
-  Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, kNonZeroBias);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, Circle) {
+  gttic(Circle);
  // Forward velocity 2m/s, angular velocity 6 degree/sec
  const double v = 2, w = 6 * kDegree;
  ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
@ -114,13 +120,14 @@ TEST(ScenarioRunner, Circle) {
  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, Loop) {
+  gttic(Loop);
  // Forward velocity 2m/s
  // Pitch up with angular velocity 6 degree/sec (negative in FLU)
  const double v = 2, w = 6 * kDegree;
@ -132,7 +139,7 @@ TEST(ScenarioRunner, Loop) {
  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
 }
@ -156,29 +163,32 @@ const double T = 3;  // seconds

 /* ************************************************************************* */
 TEST(ScenarioRunner, Accelerating) {
+  gttic(Accelerating);
  using namespace accelerating;
  ScenarioRunner runner(scenario, defaultParams(), T / 10);

  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingWithBias) {
+  gttic(AcceleratingWithBias);
  using namespace accelerating;
  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);

  auto pim = runner.integrate(T, kNonZeroBias);
-  Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingAndRotating) {
+  gttic(AcceleratingAndRotating);
  using namespace initial;
  const double a = 0.2;  // m/s^2
  const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -190,7 +200,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }
@ -215,29 +225,32 @@ const double T = 3;  // seconds

 /* ************************************************************************* */
 TEST(ScenarioRunner, Accelerating2) {
+  gttic(Accelerating2);
  using namespace accelerating2;
  ScenarioRunner runner(scenario, defaultParams(), T / 10);

  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingWithBias2) {
+  gttic(AcceleratingWithBias2);
  using namespace accelerating2;
  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);

  auto pim = runner.integrate(T, kNonZeroBias);
-  Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingAndRotating2) {
+  gttic(AcceleratingAndRotating2);
  using namespace initial2;
  const double a = 0.2;  // m/s^2
  const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -249,7 +262,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) {
  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }
@ -275,29 +288,32 @@ const double T = 3;  // seconds

 /* ************************************************************************* */
 TEST(ScenarioRunner, Accelerating3) {
+  gttic(Accelerating3);
  using namespace accelerating3;
  ScenarioRunner runner(scenario, defaultParams(), T / 10);

  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingWithBias3) {
+  gttic(AcceleratingWithBias3);
  using namespace accelerating3;
  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);

  auto pim = runner.integrate(T, kNonZeroBias);
-  Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingAndRotating3) {
+  gttic(AcceleratingAndRotating3);
  using namespace initial3;
  const double a = 0.2;  // m/s^2
  const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -309,7 +325,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) {
  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }
@ -336,29 +352,32 @@ const double T = 3;  // seconds

 /* ************************************************************************* */
 TEST(ScenarioRunner, Accelerating4) {
+  gttic(Accelerating4);
  using namespace accelerating4;
  ScenarioRunner runner(scenario, defaultParams(), T / 10);

  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingWithBias4) {
+  gttic(AcceleratingWithBias4);
  using namespace accelerating4;
  ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);

  auto pim = runner.integrate(T, kNonZeroBias);
-  Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100, kNonZeroBias);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }

 /* ************************************************************************* */
 TEST(ScenarioRunner, AcceleratingAndRotating4) {
+  gttic(AcceleratingAndRotating4);
  using namespace initial4;
  const double a = 0.2;  // m/s^2
  const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -370,7 +389,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
  auto pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

-  Matrix9 estimatedCov = runner.estimateCovariance(T);
+  Matrix9 estimatedCov = runner.estimateCovariance(T, 100);
  EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
 }
@ -379,7 +398,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
 int main() {
  TestResult tr;
  auto result = TestRegistry::runAllTests(tr);
+#ifdef ENABLE_TIMING
  tictoc_print_();
+#endif
  return result;
 }
 /* ************************************************************************* */
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@ -175,7 +175,6 @@ namespace gtsam { namespace partition {
  void prepareMetisGraph(const GenericGraph& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
      sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {

-    typedef int Weight;
    typedef std::vector<int> Weights;
    typedef std::vector<int> Neighbors;
    typedef std::pair<Neighbors, Weights> NeighborsInfo;