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