From 076612365e7e257808635025cd80879b75d7acc5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 2 Jan 2016 16:11:11 -0800 Subject: [PATCH] Addressed review comments by Abe --- gtsam/navigation/PreintegrationBase.h | 1 + gtsam/navigation/Scenario.h | 23 +++++++++---------- gtsam/navigation/ScenarioRunner.cpp | 8 +++++-- gtsam/navigation/ScenarioRunner.h | 14 ++++++----- gtsam/navigation/tests/testScenario.cpp | 6 ++--- gtsam/navigation/tests/testScenarioRunner.cpp | 11 +++++---- 6 files changed, 36 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 26b8aca2a..7e6810110 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -110,6 +110,7 @@ protected: NavState deltaXij_; /// Parameters. Declared mutable only for deprecated predict method. + /// TODO(frank): make const once deprecated method is removed mutable boost::shared_ptr p_; /// Acceleration and gyro bias used for preintegration 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 3b0a763d8..91a92af2d 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -16,6 +16,7 @@ */ #include +#include #include @@ -37,9 +38,10 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( 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); } @@ -59,6 +61,8 @@ PoseVelocityBias ScenarioRunner::predict( Matrix6 ScenarioRunner::estimatePoseCovariance( double T, size_t N, const imuBias::ConstantBias& estimatedBias) const { + gttic_(estimatePoseCovariance); + // Get predict prediction from ground truth measurements Pose3 prediction = predict(integrate(T)).pose; diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index d32d7b836..c6b17f462 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -47,21 +47,23 @@ class ScenarioRunner { static Vector3 gravity_n() { return Vector3(0, 0, -10.0); } // 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) + bias_.gyroscope() + + Vector3 measuredAngularVelocity(double t) const { + return actualAngularVelocity(t) + bias_.gyroscope() + gyroSampler_.sample() / std::sqrt(imuSampleTime_); } - Vector3 measured_specific_force_b(double t) const { - return actual_specific_force_b(t) + bias_.accelerometer() + + Vector3 measuredSpecificForce(double t) const { + return actualSpecificForce(t) + bias_.accelerometer() + accSampler_.sample() / std::sqrt(imuSampleTime_); } diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index ab538e02a..afb62935e 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -31,7 +31,7 @@ static const double degree = M_PI / 180.0; 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)); @@ -48,7 +48,7 @@ TEST(Scenario, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec around Z const double v = 2, w = 6 * degree; 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)); @@ -68,7 +68,7 @@ TEST(Scenario, Loop) { // Pitch up with angular velocity 6 degree/sec (negative in FLU) const double v = 2, w = 6 * degree; 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 019d60f91..b09d7c05e 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -33,7 +34,7 @@ static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); /* ************************************************************************* */ 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) { @@ -63,7 +64,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, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds @@ -80,7 +81,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, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds @@ -154,6 +155,8 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { /* ************************************************************************* */ int main() { TestResult tr; - return TestRegistry::runAllTests(tr); + auto result = TestRegistry::runAllTests(tr); + tictoc_print_(); + return result; } /* ************************************************************************* */