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

.travis.sh

@@ -1,46 +1,85 @@
 #!/bin/bash
 
-set -e   # Make sure any error makes the script to return an error code
+# common tasks before either build or test
-set -x   # echo
+function prepare ()
-
-SOURCE_DIR=`pwd`
-BUILD_DIR=build
-
-#CMAKE_C_FLAGS="-Wall -Wextra -Wabi -O2"
-#CMAKE_CXX_FLAGS="-Wall -Wextra -Wabi -O2"
-
-function build_and_test ()
 {
+  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 \
+# common tasks after either build or test
-      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
+function finish ()
-      -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
-
   # 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

.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_UNSTABLE=ON
-    - GTSAM_BUILD_TESTS=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
+# Exclude clang on Linux/clang in release until issue #57 is solved
-# matrix:
+matrix:
-#   exclude:
+  exclude:
-#   - os: linux
+    - os: linux
-#     compiler: clang
+      compiler: clang
+      env : CMAKE_BUILD_TYPE=Release

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();
   }
 

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();

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);
 

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;
 }
 /* ************************************************************************* */

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;
 }
 /* ************************************************************************* */

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;