diff --git a/.travis.sh b/.travis.sh index b27ec81b0..c2feb71f2 100755 --- 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 diff --git a/.travis.yml b/.travis.yml index d8d1c07b5..b2d44a9cc 100644 --- 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 diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 0d7320428..f2ba3e31c 100644 --- 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(); } diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index fcba92f60..3938ce86c 100644 --- 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(); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c1c17a6d4..3ef810cad 100644 --- 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); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 48e4dd5e3..460695ec6 100644 --- 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 #include #include @@ -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 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; } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index 2129d2d92..adac0fb53 100644 --- 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 #include #include @@ -46,6 +48,7 @@ static boost::shared_ptr 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; } /* ************************************************************************* */ diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 2088ee646..297057e3f 100644 --- 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& keys, WorkSpace& workspace, sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { - typedef int Weight; typedef std::vector Weights; typedef std::vector Neighbors; typedef std::pair NeighborsInfo;