Reduced time to run very slow testScenario by reducing sample count for estimating covariance.
Frank Dellaert
parent
e61ce989ca
commit
6f5e332a98
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue