diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9214772f7..ce866031a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -103,6 +103,7 @@ public: protected: /// Parameters. Declared mutable only for deprecated predict method. + /// TODO(frank): make const once deprecated method is removed #ifdef ALLOW_DEPRECATED_IN_GTSAM4 mutable #endif diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index bc9dfe8eb..2b2b3fddf 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -33,7 +33,7 @@ class Scenario { // Derived quantities: - virtual Rot3 rotation(double t) const { return pose(t).rotation(); } + Rot3 rotation(double t) const { return pose(t).rotation(); } Vector3 velocity_b(double t) const { const Rot3 nRb = rotation(t); @@ -47,20 +47,19 @@ class Scenario { }; /// Scenario with constant twist 3D trajectory. -class ExpmapScenario : public Scenario { +class ConstantTwistScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] - ExpmapScenario(const Vector3& w, const Vector3& v) + ConstantTwistScenario(const Vector3& w, const Vector3& v) : twist_((Vector6() << w, v).finished()), a_b_(twist_.head<3>().cross(twist_.tail<3>())) {} - Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } - Rot3 rotation(double t) const { return Rot3::Expmap(twist_.head<3>() * t); } - Vector3 omega_b(double t) const { return twist_.head<3>(); } - Vector3 velocity_n(double t) const { + Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } + Vector3 omega_b(double t) const override { return twist_.head<3>(); } + Vector3 velocity_n(double t) const override { return rotation(t).matrix() * twist_.tail<3>(); } - Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; } + Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; } private: const Vector6 twist_; @@ -77,12 +76,12 @@ class AcceleratingScenario : public Scenario { const Vector3& omega_b = Vector3::Zero()) : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} - Pose3 pose(double t) const { + Pose3 pose(double t) const override { return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); } - Vector3 omega_b(double t) const { return omega_b_; } - Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; } - Vector3 acceleration_n(double t) const { return a_n_; } + Vector3 omega_b(double t) const override { return omega_b_; } + Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; } + Vector3 acceleration_n(double t) const override { return a_n_; } private: const Rot3 nRb_; diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index a6fea095d..e71e13a23 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -16,6 +16,8 @@ */ #include +#include + #include using namespace std; @@ -35,9 +37,10 @@ AggregateImuReadings ScenarioRunner::integrate(double T, const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t); + Vector3 measuredOmega = + corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t); Vector3 measuredAcc = - corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t); + corrupted ? measuredSpecificForce(t) : actualSpecificForce(t); pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -52,6 +55,8 @@ NavState ScenarioRunner::predict(const AggregateImuReadings& pim, Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N, const Bias& estimatedBias) const { + gttic_(estimateCovariance); + // Get predict prediction from ground truth measurements NavState prediction = predict(integrate(T)); diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index c8b5efc15..00575d414 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -57,21 +57,23 @@ class ScenarioRunner { const Vector3& gravity_n() const { return p_->n_gravity; } // A gyro simply measures angular velocity in body frame - Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); } + Vector3 actualAngularVelocity(double t) const { + return scenario_->omega_b(t); + } // An accelerometer measures acceleration in body, but not gravity - Vector3 actual_specific_force_b(double t) const { + Vector3 actualSpecificForce(double t) const { Rot3 bRn = scenario_->rotation(t).transpose(); return scenario_->acceleration_b(t) - bRn * gravity_n(); } // versions corrupted by bias and noise - Vector3 measured_omega_b(double t) const { - return actual_omega_b(t) + estimatedBias_.gyroscope() + + Vector3 measuredAngularVelocity(double t) const { + return actualAngularVelocity(t) + estimatedBias_.gyroscope() + gyroSampler_.sample() / sqrt_dt_; } - Vector3 measured_specific_force_b(double t) const { - return actual_specific_force_b(t) + estimatedBias_.accelerometer() + + Vector3 measuredSpecificForce(double t) const { + return actualSpecificForce(t) + estimatedBias_.accelerometer() + accSampler_.sample() / sqrt_dt_; } diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index 4baa4a0ab..914970ea5 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -48,7 +48,7 @@ TEST(Scenario, Spin) { TEST(Scenario, Forward) { const double v = 2; // m/s const Vector3 W(0, 0, 0), V(v, 0, 0); - const ExpmapScenario scenario(W, V); + const ConstantTwistScenario scenario(W, V); const double T = 15; EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); @@ -65,7 +65,7 @@ TEST(Scenario, Circle) { // Forward velocity 2m/s, angular velocity 6 kDegree/sec around Z const double v = 2, w = 6 * kDegree; const Vector3 W(0, 0, w), V(v, 0, 0); - const ExpmapScenario scenario(W, V); + const ConstantTwistScenario scenario(W, V); const double T = 15; EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); @@ -85,7 +85,7 @@ TEST(Scenario, Loop) { // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) const double v = 2, w = 6 * kDegree; const Vector3 W(0, -w, 0), V(v, 0, 0); - const ExpmapScenario scenario(W, V); + const ConstantTwistScenario scenario(W, V); const double T = 30; EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 654a8876e..6b0ce8428 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -78,7 +79,7 @@ TEST(ScenarioRunner, Spin) { /* ************************************************************************* */ namespace forward { const double v = 2; // m/s -ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); } /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { @@ -110,7 +111,7 @@ TEST(ScenarioRunner, ForwardWithBias) { TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 kDegree/sec const double v = 2, w = 6 * kDegree; - ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); + ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); ScenarioRunner runner(&scenario, defaultParams(), kDt); const double T = 0.1; // seconds @@ -128,7 +129,7 @@ TEST(ScenarioRunner, Loop) { // Forward velocity 2m/s // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) const double v = 2, w = 6 * kDegree; - ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); + ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); ScenarioRunner runner(&scenario, defaultParams(), kDt); const double T = 0.1; // seconds @@ -398,6 +399,8 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { /* ************************************************************************* */ int main() { TestResult tr; - return TestRegistry::runAllTests(tr); + auto result = TestRegistry::runAllTests(tr); + tictoc_print_(); + return result; } /* ************************************************************************* */