12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Reduced time to run very slow testScenario by reducing sample count for estimating covariance.

release/4.3a0
Frank Dellaert 2019-06-12 14:21:08 -04:00
parent e61ce989ca
commit 6f5e332a98
2 changed files with 40 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/navigation/ScenarioRunner.cpp
View File

@ -31,6 +31,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
PreintegratedImuMeasurements ScenarioRunner::integrate( PreintegratedImuMeasurements ScenarioRunner::integrate(
double T, const Bias& estimatedBias, bool corrupted) const { double T, const Bias& estimatedBias, bool corrupted) const {
gttic_(integrate);
PreintegratedImuMeasurements pim(p_, estimatedBias); PreintegratedImuMeasurements pim(p_, estimatedBias);
const double dt = imuSampleTime(); const double dt = imuSampleTime();

57
gtsam/navigation/tests/testScenarios.cpp
View File

@ -15,6 +15,8 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
// #define ENABLE_TIMING // uncomment for timing results
#include <gtsam/navigation/ScenarioRunner.h> #include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -46,6 +48,7 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Spin) { TEST(ScenarioRunner, Spin) {
gttic(Spin);
// angular velocity 6 degree/sec // angular velocity 6 degree/sec
const double w = 6 * kDegree; const double w = 6 * kDegree;
const Vector3 W(0, 0, w), V(0, 0, 0); const Vector3 W(0, 0, w), V(0, 0, 0);
@ -63,12 +66,12 @@ TEST(ScenarioRunner, Spin) {
Matrix6 expected; Matrix6 expected;
expected << p->accelerometerCovariance / kDt, Z_3x3, // expected << p->accelerometerCovariance / kDt, Z_3x3, //
Z_3x3, p->gyroscopeCovariance / kDt; Z_3x3, p->gyroscopeCovariance / kDt;
Matrix6 actual = runner.estimateNoiseCovariance(10000); Matrix6 actual = runner.estimateNoiseCovariance(1000);
EXPECT(assert_equal(expected, actual, 1e-2)); EXPECT(assert_equal(expected, actual, 1e-2));
#endif #endif
// Check calculated covariance against Monte Carlo estimate // 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)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
@ -79,6 +82,7 @@ ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Forward) {
gttic(Forward);
using namespace forward; using namespace forward;
ScenarioRunner runner(scenario, defaultParams(), kDt); ScenarioRunner runner(scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
@ -86,24 +90,26 @@ TEST(ScenarioRunner, Forward) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, ForwardWithBias) { TEST(ScenarioRunner, ForwardWithBias) {
gttic(ForwardWithBias);
using namespace forward; using namespace forward;
ScenarioRunner runner(scenario, defaultParams(), kDt); ScenarioRunner runner(scenario, defaultParams(), kDt);
const double T = 0.1; // seconds const double T = 0.1; // seconds
auto pim = runner.integrate(T, kNonZeroBias); 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)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Circle) { TEST(ScenarioRunner, Circle) {
gttic(Circle);
// Forward velocity 2m/s, angular velocity 6 degree/sec // Forward velocity 2m/s, angular velocity 6 degree/sec
const double v = 2, w = 6 * kDegree; const double v = 2, w = 6 * kDegree;
ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
@ -114,13 +120,14 @@ TEST(ScenarioRunner, Circle) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Loop) { TEST(ScenarioRunner, Loop) {
gttic(Loop);
// Forward velocity 2m/s // Forward velocity 2m/s
// Pitch up with angular velocity 6 degree/sec (negative in FLU) // Pitch up with angular velocity 6 degree/sec (negative in FLU)
const double v = 2, w = 6 * kDegree; const double v = 2, w = 6 * kDegree;
@ -132,7 +139,7 @@ TEST(ScenarioRunner, Loop) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
} }
@ -156,29 +163,32 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating) { TEST(ScenarioRunner, Accelerating) {
gttic(Accelerating);
using namespace accelerating; using namespace accelerating;
ScenarioRunner runner(scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias) { TEST(ScenarioRunner, AcceleratingWithBias) {
gttic(AcceleratingWithBias);
using namespace accelerating; using namespace accelerating;
ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, 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)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating) { TEST(ScenarioRunner, AcceleratingAndRotating) {
gttic(AcceleratingAndRotating);
using namespace initial; using namespace initial;
const double a = 0.2; // m/s^2 const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0), W(0, 0.1, 0); const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -190,7 +200,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
@ -215,29 +225,32 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating2) { TEST(ScenarioRunner, Accelerating2) {
gttic(Accelerating2);
using namespace accelerating2; using namespace accelerating2;
ScenarioRunner runner(scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias2) { TEST(ScenarioRunner, AcceleratingWithBias2) {
gttic(AcceleratingWithBias2);
using namespace accelerating2; using namespace accelerating2;
ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, 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)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating2) { TEST(ScenarioRunner, AcceleratingAndRotating2) {
gttic(AcceleratingAndRotating2);
using namespace initial2; using namespace initial2;
const double a = 0.2; // m/s^2 const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0), W(0, 0.1, 0); const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -249,7 +262,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
@ -275,29 +288,32 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating3) { TEST(ScenarioRunner, Accelerating3) {
gttic(Accelerating3);
using namespace accelerating3; using namespace accelerating3;
ScenarioRunner runner(scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias3) { TEST(ScenarioRunner, AcceleratingWithBias3) {
gttic(AcceleratingWithBias3);
using namespace accelerating3; using namespace accelerating3;
ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, 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)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating3) { TEST(ScenarioRunner, AcceleratingAndRotating3) {
gttic(AcceleratingAndRotating3);
using namespace initial3; using namespace initial3;
const double a = 0.2; // m/s^2 const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0), W(0, 0.1, 0); const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -309,7 +325,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
@ -336,29 +352,32 @@ const double T = 3; // seconds
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, Accelerating4) { TEST(ScenarioRunner, Accelerating4) {
gttic(Accelerating4);
using namespace accelerating4; using namespace accelerating4;
ScenarioRunner runner(scenario, defaultParams(), T / 10); ScenarioRunner runner(scenario, defaultParams(), T / 10);
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingWithBias4) { TEST(ScenarioRunner, AcceleratingWithBias4) {
gttic(AcceleratingWithBias4);
using namespace accelerating4; using namespace accelerating4;
ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias);
auto pim = runner.integrate(T, 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)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ScenarioRunner, AcceleratingAndRotating4) { TEST(ScenarioRunner, AcceleratingAndRotating4) {
gttic(AcceleratingAndRotating4);
using namespace initial4; using namespace initial4;
const double a = 0.2; // m/s^2 const double a = 0.2; // m/s^2
const Vector3 A(0, a, 0), W(0, 0.1, 0); const Vector3 A(0, a, 0), W(0, 0.1, 0);
@ -370,7 +389,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
auto pim = runner.integrate(T); auto pim = runner.integrate(T);
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); 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_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1)); EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
} }
@ -379,7 +398,9 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) {
int main() { int main() {
TestResult tr; TestResult tr;
auto result = TestRegistry::runAllTests(tr); auto result = TestRegistry::runAllTests(tr);
#ifdef ENABLE_TIMING
tictoc_print_(); tictoc_print_();
#endif
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */