From d0f911139dcf016b2f49c97432dbf2c2bf76d6aa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Dec 2015 13:57:26 -0800 Subject: [PATCH 01/40] First Scenario test --- gtsam/navigation/tests/testScenario.cpp | 69 +++++++++++++++++++++++++ 1 file changed, 69 insertions(+) create mode 100644 gtsam/navigation/tests/testScenario.cpp diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp new file mode 100644 index 000000000..0c18b4584 --- /dev/null +++ b/gtsam/navigation/tests/testScenario.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scenario.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#include + +namespace gtsam { +/// Simple class to test navigation scenarios +class Scenario { + public: + /// Construct scenario with constant twist [w,v] + Scenario(const Vector3& w, const Vector3& v) + : twist_((Vector6() << w, v).finished()) {} + + Pose3 poseAtTime(double t) { return Pose3::Expmap(twist_ * t); } + + private: + Vector6 twist_; +}; + +} // namespace gtsam + +/** + * @file testScenario.cpp + * @brief test ImuFacor with Scenario class + * @author Frank Dellaert + */ + +#include + +#include + +using namespace std; +using namespace gtsam; + +static const double degree = M_PI / 180.0; + +/* ************************************************************************* */ +TEST(Scenario, Circle) { + // Forward velocity 2m/s, angular velocity 6 degree/sec + const double v = 2, omega = 6 * degree; + Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); + + // R = v/omega, so test if circle is of right size + const double R = v / omega; + const Pose3 T15 = circle.poseAtTime(15); + EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From e5b8f982a14dcefa0db801698d805f70ac8c938b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Dec 2015 13:57:37 -0800 Subject: [PATCH 02/40] Ignore backup files --- doc/.gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/.gitignore b/doc/.gitignore index ac7af2e80..8a3139177 100644 --- a/doc/.gitignore +++ b/doc/.gitignore @@ -1 +1,3 @@ /html/ +*.lyx~ +*.bib~ From 988837be6ab92e479cbd5166ea40f24374fd9236 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Dec 2015 14:29:52 -0800 Subject: [PATCH 03/40] Moved to header, added some methods --- gtsam/navigation/Scenario.h | 41 +++++++++++++++++++++++++ gtsam/navigation/tests/testScenario.cpp | 28 ++--------------- 2 files changed, 43 insertions(+), 26 deletions(-) create mode 100644 gtsam/navigation/Scenario.h diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h new file mode 100644 index 000000000..4b25d827d --- /dev/null +++ b/gtsam/navigation/Scenario.h @@ -0,0 +1,41 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scenario.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#pragma once +#include + +namespace gtsam { + +/// Simple class with constant twist 3D trajectory +class Scenario { + public: + /// Construct scenario with constant twist [w,v] + Scenario(const Vector3& w, const Vector3& v, double imuSampleTime = 1e-2) + : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {} + + Vector3 groundTruthAcc() const { return twist_.tail<3>(); } + Vector3 groundTruthGyro() const { return twist_.head<3>(); } + const double& imuSampleTime() const { return imuSampleTime_; } + + Pose3 poseAtTime(double t) { return Pose3::Expmap(twist_ * t); } + + private: + Vector6 twist_; + double imuSampleTime_; +}; + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index 0c18b4584..b5461fbdc 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -9,38 +9,14 @@ * -------------------------------------------------------------------------- */ -/** - * @file Scenario.h - * @brief Simple class to test navigation scenarios - * @author Frank Dellaert - */ - -#include - -namespace gtsam { -/// Simple class to test navigation scenarios -class Scenario { - public: - /// Construct scenario with constant twist [w,v] - Scenario(const Vector3& w, const Vector3& v) - : twist_((Vector6() << w, v).finished()) {} - - Pose3 poseAtTime(double t) { return Pose3::Expmap(twist_ * t); } - - private: - Vector6 twist_; -}; - -} // namespace gtsam - /** * @file testScenario.cpp - * @brief test ImuFacor with Scenario class + * @brief Unit test Scenario class * @author Frank Dellaert */ +#include #include - #include using namespace std; From be47a2ef1560905fd9b3b14ccdd581958435ce85 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Dec 2015 14:49:52 -0800 Subject: [PATCH 04/40] Run Scenario and check mean --- gtsam/navigation/tests/testScenarioRunner.cpp | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 gtsam/navigation/tests/testScenarioRunner.cpp diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp new file mode 100644 index 000000000..00762793b --- /dev/null +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -0,0 +1,107 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ScenarioRunner.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#include +#include + +namespace gtsam { + +double accNoiseVar = 0.01; +double omegaNoiseVar = 0.03; +double intNoiseVar = 0.0001; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; + +/// Simple class to test navigation scenarios +class ScenarioRunner { + public: + ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} + + // Integrate measurements for T seconds + ImuFactor::PreintegratedMeasurements integrate(double T) { + // TODO(frank): allow non-zero + const imuBias::ConstantBias zeroBias; + const bool use2ndOrderCoriolis = true; + + ImuFactor::PreintegratedMeasurements result( + zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, use2ndOrderCoriolis); + + const Vector3 measuredAcc = scenario_.groundTruthAcc(); + const Vector3 measuredOmega = scenario_.groundTruthGyro(); + double deltaT = scenario_.imuSampleTime(); + for (double t = 0; t <= T; t += deltaT) { + result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + } + + return result; + } + + // Predict mean + Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) { + // TODO(frank): allow non-standard + const imuBias::ConstantBias zeroBias; + const Pose3 pose_i = Pose3::identity(); + const Vector3 vel_i = Vector3::Zero(); + const Vector3 gravity(0, 0, 9.81); + const Vector3 omegaCoriolis = Vector3::Zero(); + const bool use2ndOrderCoriolis = true; + const PoseVelocityBias prediction = integrated.predict( + pose_i, vel_i, zeroBias, gravity, omegaCoriolis, use2ndOrderCoriolis); + return prediction.pose; + } + + private: + Scenario scenario_; +}; + +} // namespace gtsam + +/** + * @file testScenario.cpp + * @brief test ImuFacor with ScenarioRunner class + * @author Frank Dellaert + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const double degree = M_PI / 180.0; + +/* ************************************************************************* */ +TEST(ScenarioRunner, Circle) { + // Forward velocity 2m/s, angular velocity 6 degree/sec + const double v = 2, omega = 6 * degree; + Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); + + ScenarioRunner runner(circle); + const double T = 15; // seconds + ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); + EXPECT(assert_equal(circle.poseAtTime(T), runner.mean(integrated), 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 84628cd511de83bd4ef724967172525b89579cb8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Dec 2015 15:11:35 -0800 Subject: [PATCH 05/40] Added Vector3 methods from develop --- gtsam/geometry/Rot3.h | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 608f41954..cc0dc309e 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -330,6 +330,17 @@ namespace gtsam { Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2 = boost::none) const; + /// operator* for Vector3 + inline Vector3 operator*(const Vector3& v) const { + return rotate(Point3(v)).vector(); + } + + /// rotate for Vector3 + Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + return rotate(Point3(v), H1, H2).vector(); + } + /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; @@ -337,6 +348,12 @@ namespace gtsam { Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2=boost::none) const; + /// unrotate for Vector3 + Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + return unrotate(Point3(v), H1, H2).vector(); + } + /// @} /// @name Group Action on Unit3 /// @{ From 846a7774915f9208bf2651f4157211805d132fa6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Dec 2015 15:11:48 -0800 Subject: [PATCH 06/40] Forward scenario --- gtsam/navigation/Scenario.h | 20 ++++++++++++++-- gtsam/navigation/tests/testScenario.cpp | 10 ++++++++ gtsam/navigation/tests/testScenarioRunner.cpp | 23 ++++++++++++++----- 3 files changed, 45 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 4b25d827d..7731e33df 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -27,12 +27,28 @@ class Scenario { Scenario(const Vector3& w, const Vector3& v, double imuSampleTime = 1e-2) : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {} - Vector3 groundTruthAcc() const { return twist_.tail<3>(); } - Vector3 groundTruthGyro() const { return twist_.head<3>(); } + // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) + // also, uses g=10 for easy debugging + Vector3 gravity() const { return Vector3(0, 0, -10.0); } + + Vector3 groundTruthGyroInBody() const { return twist_.head<3>(); } + Vector3 groundTruthVelocityInBody() const { return twist_.tail<3>(); } + + // All constant twist scenarios have zero acceleration + Vector3 groundTruthAccInBody() const { return Vector3::Zero(); } + const double& imuSampleTime() const { return imuSampleTime_; } + /// Pose of body in nav frame at time t Pose3 poseAtTime(double t) { return Pose3::Expmap(twist_ * t); } + /// Velocity in nav frame at time t + Vector3 velocityAtTime(double t) { + const Pose3 pose = poseAtTime(t); + const Rot3& nRb = pose.rotation(); + return nRb * groundTruthVelocityInBody(); + } + private: Vector6 twist_; double imuSampleTime_; diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index b5461fbdc..a4176be12 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -24,6 +24,16 @@ using namespace gtsam; static const double degree = M_PI / 180.0; +/* ************************************************************************* */ +TEST(Scenario, Forward) { + const double v = 2; // m/s + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + + const Pose3 T15 = forward.poseAtTime(15); + EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9)); +} + /* ************************************************************************* */ TEST(Scenario, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 00762793b..cd1bc2e35 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -42,8 +42,8 @@ class ScenarioRunner { zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, use2ndOrderCoriolis); - const Vector3 measuredAcc = scenario_.groundTruthAcc(); - const Vector3 measuredOmega = scenario_.groundTruthGyro(); + const Vector3 measuredAcc = scenario_.groundTruthAccInBody(); + const Vector3 measuredOmega = scenario_.groundTruthGyroInBody(); double deltaT = scenario_.imuSampleTime(); for (double t = 0; t <= T; t += deltaT) { result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -57,12 +57,12 @@ class ScenarioRunner { // TODO(frank): allow non-standard const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); - const Vector3 vel_i = Vector3::Zero(); - const Vector3 gravity(0, 0, 9.81); + const Vector3 vel_i = scenario_.velocityAtTime(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; - const PoseVelocityBias prediction = integrated.predict( - pose_i, vel_i, zeroBias, gravity, omegaCoriolis, use2ndOrderCoriolis); + const PoseVelocityBias prediction = + integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), + omegaCoriolis, use2ndOrderCoriolis); return prediction.pose; } @@ -87,6 +87,17 @@ using namespace gtsam; static const double degree = M_PI / 180.0; +/* ************************************************************************* */ +TEST(ScenarioRunner, Forward) { + const double v = 2; // m/s + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + + ScenarioRunner runner(forward); + const double T = 10; // seconds + ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); + EXPECT(assert_equal(forward.poseAtTime(T), runner.mean(integrated), 1e-9)); +} + /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec From e16d798bd495f5ef1a7e73d13483606e923be89b Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 21 Dec 2015 15:54:52 -0800 Subject: [PATCH 07/40] Deal w new Rot3 operators --- gtsam_unstable/slam/Mechanization_bRn2.cpp | 2 +- gtsam_unstable/slam/Mechanization_bRn2.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index d2dd02dd3..0f3f2feae 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -101,7 +101,7 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u, // convert to navigation frame Rot3 nRb = bRn_.inverse(); - Vector3 n_omega_bn = (nRb*b_omega_bn).vector(); + Vector3 n_omega_bn = nRb*b_omega_bn; // integrate bRn using exponential map, assuming constant over dt Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index fa33ce5b9..4c85614d4 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -42,7 +42,7 @@ public: /// gravity in the body frame Vector3 b_g(double g_e) const { Vector3 n_g(0, 0, g_e); - return (bRn_ * n_g).vector(); + return bRn_ * n_g; } const Rot3& bRn() const {return bRn_; } From 5f9053ae39c5591ab4c892390197d2c6ff5cc6e6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 21 Dec 2015 16:07:40 -0800 Subject: [PATCH 08/40] Get rid of warnings w develop changes --- gtsam/nonlinear/Values-inl.h | 81 ++++++++++++++++++++++-------------- 1 file changed, 50 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index fe2c3f3ca..05e58accb 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -49,8 +49,12 @@ namespace gtsam { const Key key; ///< The key const ValueType& value; ///< The value - _ValuesConstKeyValuePair(Key _key, const ValueType& _value) : key(_key), value(_value) {} - _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} + _ValuesConstKeyValuePair(Key _key, const ValueType& _value) : + key(_key), value(_value) { + } + _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : + key(rhs.key), value(rhs.value) { + } }; /* ************************************************************************* */ @@ -59,17 +63,20 @@ namespace gtsam { // need to use a struct here for later partial specialization template struct ValuesCastHelper { - static CastedKeyValuePairType cast(KeyValuePairType key_value) { - // Static cast because we already checked the type during filtering - return CastedKeyValuePairType(key_value.key, const_cast&>(static_cast&>(key_value.value)).value()); - } + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + return CastedKeyValuePairType(key_value.key, + const_cast&>(static_cast&>(key_value.value)).value()); + } }; // partial specialized version for ValueType == Value template struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type + // (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; @@ -78,7 +85,8 @@ namespace gtsam { struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type + // (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; @@ -126,23 +134,29 @@ namespace gtsam { } private: - Filtered(const boost::function& filter, Values& values) : - begin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.begin(), values.end()), - &ValuesCastHelper::cast)), - end_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, values.end(), values.end()), - &ValuesCastHelper::cast)), - constBegin_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).begin(), ((const Values&)values).end()), - &ValuesCastHelper::cast)), - constEnd_(boost::make_transform_iterator( - boost::make_filter_iterator( - filter, ((const Values&)values).end(), ((const Values&)values).end()), - &ValuesCastHelper::cast)) {} + Filtered( + const boost::function& filter, + Values& values) : + begin_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, values.begin(), values.end()), + &ValuesCastHelper::cast)), end_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, values.end(), values.end()), + &ValuesCastHelper::cast)), constBegin_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, + ((const Values&) values).begin(), + ((const Values&) values).end()), + &ValuesCastHelper::cast)), constEnd_( + boost::make_transform_iterator( + boost::make_filter_iterator(filter, + ((const Values&) values).end(), + ((const Values&) values).end()), + &ValuesCastHelper::cast)) { + } friend class Values; iterator begin_; @@ -191,7 +205,9 @@ namespace gtsam { friend class Values; const_iterator begin_; const_iterator end_; - ConstFiltered(const boost::function& filter, const Values& values) { + ConstFiltered( + const boost::function& filter, + const Values& values) { // We remove the const from values to create a non-const Filtered // view, then pull the const_iterators out of it. const Filtered filtered(filter, const_cast(values)); @@ -247,7 +263,8 @@ namespace gtsam { /* ************************************************************************* */ template<> - inline bool Values::filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + inline bool Values::filterHelper(const boost::function filter, + const ConstKeyValuePair& key_value) { // Filter and check the type return filter(key_value.key); } @@ -263,10 +280,11 @@ namespace gtsam { throw ValuesKeyDoesNotExist("retrieve", j); // Check the type and throw exception if incorrect + const Value& value = *item->second; try { - return dynamic_cast&>(*item->second).value(); + return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); + throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); } } @@ -278,10 +296,11 @@ namespace gtsam { if(item != values_.end()) { // dynamic cast the type and throw exception if incorrect + const Value& value = *item->second; try { - return dynamic_cast&>(*item->second).value(); + return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); + throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); } } else { return boost::none; From 699ba32c9e399a834d944cff5b3d6dfff07c603f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 10:02:12 -0800 Subject: [PATCH 09/40] Further examining a circular trajectory --- gtsam/navigation/Scenario.h | 40 +++++++++++++------ gtsam/navigation/tests/testScenarioRunner.cpp | 22 ++++++---- 2 files changed, 43 insertions(+), 19 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 7731e33df..ea9cba6a7 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -20,33 +20,49 @@ namespace gtsam { -/// Simple class with constant twist 3D trajectory +/** + * Simple class with constant twist 3D trajectory. + * It is also assumed that gravity is magically counteracted and has no effect + * on trajectory. Hence, a simulated IMU yields the actual body angular + * velocity, and negative G acceleration plus the acceleration created by the + * rotating body frame. + */ class Scenario { public: /// Construct scenario with constant twist [w,v] - Scenario(const Vector3& w, const Vector3& v, double imuSampleTime = 1e-2) + Scenario(const Vector3& w, const Vector3& v, + double imuSampleTime = 1.0 / 100.0) : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {} + const double& imuSampleTime() const { return imuSampleTime_; } + // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) // also, uses g=10 for easy debugging Vector3 gravity() const { return Vector3(0, 0, -10.0); } - Vector3 groundTruthGyroInBody() const { return twist_.head<3>(); } - Vector3 groundTruthVelocityInBody() const { return twist_.tail<3>(); } + Vector3 angularVelocityInBody() const { return twist_.head<3>(); } + Vector3 linearVelocityInBody() const { return twist_.tail<3>(); } - // All constant twist scenarios have zero acceleration - Vector3 groundTruthAccInBody() const { return Vector3::Zero(); } - - const double& imuSampleTime() const { return imuSampleTime_; } + /// Rotation of body in nav frame at time t + Rot3 rotAtTime(double t) const { + return Rot3::Expmap(angularVelocityInBody() * t); + } /// Pose of body in nav frame at time t - Pose3 poseAtTime(double t) { return Pose3::Expmap(twist_ * t); } + Pose3 poseAtTime(double t) const { return Pose3::Expmap(twist_ * t); } /// Velocity in nav frame at time t Vector3 velocityAtTime(double t) { - const Pose3 pose = poseAtTime(t); - const Rot3& nRb = pose.rotation(); - return nRb * groundTruthVelocityInBody(); + const Rot3 nRb = rotAtTime(t); + return nRb * linearVelocityInBody(); + } + + // acceleration in nav frame + Vector3 accelerationAtTime(double t) const { + const Rot3 nRb = rotAtTime(t); + const Vector3 centripetalAcceleration = + angularVelocityInBody().cross(linearVelocityInBody()); + return nRb * centripetalAcceleration - gravity(); } private: diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index cd1bc2e35..a00dfe7fa 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -18,6 +18,8 @@ #include #include +#include + namespace gtsam { double accNoiseVar = 0.01; @@ -42,11 +44,17 @@ class ScenarioRunner { zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, use2ndOrderCoriolis); - const Vector3 measuredAcc = scenario_.groundTruthAccInBody(); - const Vector3 measuredOmega = scenario_.groundTruthGyroInBody(); - double deltaT = scenario_.imuSampleTime(); - for (double t = 0; t <= T; t += deltaT) { + const Vector3 measuredOmega = scenario_.angularVelocityInBody(); + const double deltaT = scenario_.imuSampleTime(); + const size_t nrSteps = T / deltaT; + double t = 0; + for (size_t k = 0; k < nrSteps; k++, t += deltaT) { + std::cout << t << ", " << deltaT << ": "; + const Vector3 measuredAcc = scenario_.accelerationAtTime(t); result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // std::cout << result.deltaRij() << std::endl; + std::cout << " a:" << measuredAcc.transpose(); + std::cout << " P:" << result.deltaVij().transpose() << std::endl; } return result; @@ -87,13 +95,13 @@ using namespace gtsam; static const double degree = M_PI / 180.0; -/* ************************************************************************* */ +/* ************************************************************************* * TEST(ScenarioRunner, Forward) { const double v = 2; // m/s Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); ScenarioRunner runner(forward); - const double T = 10; // seconds + const double T = 1; // seconds ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); EXPECT(assert_equal(forward.poseAtTime(T), runner.mean(integrated), 1e-9)); } @@ -102,7 +110,7 @@ TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec const double v = 2, omega = 6 * degree; - Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); + Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.1); ScenarioRunner runner(circle); const double T = 15; // seconds From ef5031e33e7d24c84e5d338a8a6c75f0d0952725 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 22 Dec 2015 11:09:44 -0800 Subject: [PATCH 10/40] Avoid some warnings by copying from develop --- .../examples/SmartProjectionFactorExample.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 1423ef113..de1d3f77b 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -54,7 +54,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b cout << "Reading calibration info" << endl; @@ -67,17 +67,17 @@ int main(int argc, char** argv){ cout << "Reading camera poses" << endl; ifstream pose_file(pose_loc.c_str()); - int i; + int pose_index; MatrixRowMajor m(4,4); //read camera pose parameters and use to make initial estimates of camera poses - while (pose_file >> i) { + while (pose_file >> pose_index) { for (int i = 0; i < 16; i++) pose_file >> m.data()[i]; - initial_estimate.insert(i, Pose3(m)); + initial_estimate.insert(pose_index, Pose3(m)); } - + // landmark keys - size_t l; + size_t landmark_key; // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation @@ -90,14 +90,14 @@ int main(int argc, char** argv){ SmartFactor::shared_ptr factor(new SmartFactor(model, K)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> pose_index >> landmark_key >> uL >> uR >> v >> X >> Y >> Z) { - if(current_l != l) { + if(current_l != landmark_key) { graph.push_back(factor); factor = SmartFactor::shared_ptr(new SmartFactor(model, K)); - current_l = l; + current_l = landmark_key; } - factor->add(Point2(uL,v), i); + factor->add(Point2(uL,v), pose_index); } Pose3 firstPose = initial_estimate.at(1); From d3534b2d2b2b73b73192074a9a843a7bea81a06a Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 22 Dec 2015 11:37:04 -0800 Subject: [PATCH 11/40] Fixed circle example --- gtsam/navigation/Scenario.h | 16 +++++++++---- gtsam/navigation/tests/testScenario.cpp | 4 ++-- gtsam/navigation/tests/testScenarioRunner.cpp | 24 ++++++++++++------- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index ea9cba6a7..8872d536d 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -49,22 +49,30 @@ class Scenario { } /// Pose of body in nav frame at time t - Pose3 poseAtTime(double t) const { return Pose3::Expmap(twist_ * t); } + Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } /// Velocity in nav frame at time t - Vector3 velocityAtTime(double t) { + Vector3 velocity(double t) { const Rot3 nRb = rotAtTime(t); return nRb * linearVelocityInBody(); } // acceleration in nav frame - Vector3 accelerationAtTime(double t) const { - const Rot3 nRb = rotAtTime(t); + Vector3 acceleration(double t) const { const Vector3 centripetalAcceleration = angularVelocityInBody().cross(linearVelocityInBody()); + const Rot3 nRb = rotAtTime(t); return nRb * centripetalAcceleration - gravity(); } + // acceleration in body frame frame + Vector3 accelerationInBody(double t) const { + const Vector3 centripetalAcceleration = + angularVelocityInBody().cross(linearVelocityInBody()); + const Rot3 nRb = rotAtTime(t); + return centripetalAcceleration - nRb.transpose() * gravity(); + } + private: Vector6 twist_; double imuSampleTime_; diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index a4176be12..25c3dfdc8 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -29,7 +29,7 @@ TEST(Scenario, Forward) { const double v = 2; // m/s Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); - const Pose3 T15 = forward.poseAtTime(15); + const Pose3 T15 = forward.pose(15); EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9)); } @@ -42,7 +42,7 @@ TEST(Scenario, Circle) { // R = v/omega, so test if circle is of right size const double R = v / omega; - const Pose3 T15 = circle.poseAtTime(15); + const Pose3 T15 = circle.pose(15); EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); } diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index a00dfe7fa..30d7fbd14 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -48,13 +48,19 @@ class ScenarioRunner { const double deltaT = scenario_.imuSampleTime(); const size_t nrSteps = T / deltaT; double t = 0; + Vector3 v0 = scenario_.velocity(0); + Vector3 v = Vector3::Zero(); + Vector3 p = Vector3::Zero(); for (size_t k = 0; k < nrSteps; k++, t += deltaT) { std::cout << t << ", " << deltaT << ": "; - const Vector3 measuredAcc = scenario_.accelerationAtTime(t); + p += deltaT * v; + v += deltaT * scenario_.acceleration(t); + const Vector3 measuredAcc = scenario_.accelerationInBody(t); result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // std::cout << result.deltaRij() << std::endl; - std::cout << " a:" << measuredAcc.transpose(); - std::cout << " P:" << result.deltaVij().transpose() << std::endl; + std::cout << " P:" << result.deltaPij().transpose(); + std::cout << " p:" << p.transpose(); + std::cout << " p0:" << (p + v0 * t).transpose(); + std::cout << std::endl; } return result; @@ -65,7 +71,7 @@ class ScenarioRunner { // TODO(frank): allow non-standard const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); - const Vector3 vel_i = scenario_.velocityAtTime(0); + const Vector3 vel_i = scenario_.velocity(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; const PoseVelocityBias prediction = @@ -95,7 +101,7 @@ using namespace gtsam; static const double degree = M_PI / 180.0; -/* ************************************************************************* * +/* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); @@ -103,19 +109,19 @@ TEST(ScenarioRunner, Forward) { ScenarioRunner runner(forward); const double T = 1; // seconds ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(forward.poseAtTime(T), runner.mean(integrated), 1e-9)); + EXPECT(assert_equal(forward.pose(T), runner.mean(integrated), 1e-9)); } /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec const double v = 2, omega = 6 * degree; - Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.1); + Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.01); ScenarioRunner runner(circle); const double T = 15; // seconds ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(circle.poseAtTime(T), runner.mean(integrated), 1e-9)); + EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1)); } /* ************************************************************************* */ From f1fa66e9c1485e98c45b2e6ce62244c15918cb66 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 22 Dec 2015 11:39:20 -0800 Subject: [PATCH 12/40] Removed debug code --- gtsam/navigation/tests/testScenarioRunner.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 30d7fbd14..e7502778e 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -48,19 +48,9 @@ class ScenarioRunner { const double deltaT = scenario_.imuSampleTime(); const size_t nrSteps = T / deltaT; double t = 0; - Vector3 v0 = scenario_.velocity(0); - Vector3 v = Vector3::Zero(); - Vector3 p = Vector3::Zero(); for (size_t k = 0; k < nrSteps; k++, t += deltaT) { - std::cout << t << ", " << deltaT << ": "; - p += deltaT * v; - v += deltaT * scenario_.acceleration(t); const Vector3 measuredAcc = scenario_.accelerationInBody(t); result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - std::cout << " P:" << result.deltaPij().transpose(); - std::cout << " p:" << p.transpose(); - std::cout << " p0:" << (p + v0 * t).transpose(); - std::cout << std::endl; } return result; From 40bc3149ad0c2a571122e0d65769ef896e658dc8 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 22 Dec 2015 11:47:37 -0800 Subject: [PATCH 13/40] Added loop --- gtsam/navigation/tests/testScenario.cpp | 24 +++++++++++++++---- gtsam/navigation/tests/testScenarioRunner.cpp | 15 +++++++++++- 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index 25c3dfdc8..ae406f953 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -36,17 +36,31 @@ TEST(Scenario, Forward) { /* ************************************************************************* */ TEST(Scenario, Circle) { - // Forward velocity 2m/s, angular velocity 6 degree/sec - const double v = 2, omega = 6 * degree; - Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); + // Forward velocity 2m/s, angular velocity 6 degree/sec around Z + const double v = 2, w = 6 * degree; + Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); - // R = v/omega, so test if circle is of right size - const double R = v / omega; + // R = v/w, so test if circle is of right size + const double R = v / w; const Pose3 T15 = circle.pose(15); EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); } +/* ************************************************************************* */ +TEST(Scenario, Loop) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 degree/sec (negative in FLU) + const double v = 2, w = 6 * degree; + Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + + // R = v/w, so test if loop crests at 2*R + const double R = v / w; + const Pose3 T30 = loop.pose(30); + EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9)); + EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index e7502778e..df5ebbfd4 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -106,7 +106,7 @@ TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec const double v = 2, omega = 6 * degree; - Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0), 0.01); + Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); ScenarioRunner runner(circle); const double T = 15; // seconds @@ -114,6 +114,19 @@ TEST(ScenarioRunner, Circle) { EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1)); } +/* ************************************************************************* */ +TEST(ScenarioRunner, Loop) { + // Forward velocity 2m/s + // Pitch up with angular velocity 6 degree/sec (negative in FLU) + const double v = 2, w = 6 * degree; + Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + + ScenarioRunner runner(loop); + const double T = 30; // seconds + ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); + EXPECT(assert_equal(loop.pose(T), runner.mean(integrated), 0.1)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 95745015e03784fb8ad1914cd9428cb7028b9bfd Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 22 Dec 2015 11:49:14 -0800 Subject: [PATCH 14/40] Moved to header file --- gtsam/navigation/ScenarioRunner.h | 79 +++++++++++++++++++ gtsam/navigation/tests/testScenarioRunner.cpp | 71 +---------------- 2 files changed, 81 insertions(+), 69 deletions(-) create mode 100644 gtsam/navigation/ScenarioRunner.h diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h new file mode 100644 index 000000000..45ec74978 --- /dev/null +++ b/gtsam/navigation/ScenarioRunner.h @@ -0,0 +1,79 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ScenarioRunner.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#pragma once +#include +#include + +#include + +namespace gtsam { + +double accNoiseVar = 0.01; +double omegaNoiseVar = 0.03; +double intNoiseVar = 0.0001; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; + +/// Simple class to test navigation scenarios +class ScenarioRunner { + public: + ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} + + // Integrate measurements for T seconds + ImuFactor::PreintegratedMeasurements integrate(double T) { + // TODO(frank): allow non-zero + const imuBias::ConstantBias zeroBias; + const bool use2ndOrderCoriolis = true; + + ImuFactor::PreintegratedMeasurements result( + zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, use2ndOrderCoriolis); + + const Vector3 measuredOmega = scenario_.angularVelocityInBody(); + const double deltaT = scenario_.imuSampleTime(); + const size_t nrSteps = T / deltaT; + double t = 0; + for (size_t k = 0; k < nrSteps; k++, t += deltaT) { + const Vector3 measuredAcc = scenario_.accelerationInBody(t); + result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + } + + return result; + } + + // Predict mean + Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) { + // TODO(frank): allow non-standard + const imuBias::ConstantBias zeroBias; + const Pose3 pose_i = Pose3::identity(); + const Vector3 vel_i = scenario_.velocity(0); + const Vector3 omegaCoriolis = Vector3::Zero(); + const bool use2ndOrderCoriolis = true; + const PoseVelocityBias prediction = + integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), + omegaCoriolis, use2ndOrderCoriolis); + return prediction.pose; + } + + private: + Scenario scenario_; +}; + +} // namespace gtsam + diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index df5ebbfd4..fc92bc416 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -10,79 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file ScenarioRunner.h - * @brief Simple class to test navigation scenarios - * @author Frank Dellaert - */ - -#include -#include - -#include - -namespace gtsam { - -double accNoiseVar = 0.01; -double omegaNoiseVar = 0.03; -double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; - -/// Simple class to test navigation scenarios -class ScenarioRunner { - public: - ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} - - // Integrate measurements for T seconds - ImuFactor::PreintegratedMeasurements integrate(double T) { - // TODO(frank): allow non-zero - const imuBias::ConstantBias zeroBias; - const bool use2ndOrderCoriolis = true; - - ImuFactor::PreintegratedMeasurements result( - zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, use2ndOrderCoriolis); - - const Vector3 measuredOmega = scenario_.angularVelocityInBody(); - const double deltaT = scenario_.imuSampleTime(); - const size_t nrSteps = T / deltaT; - double t = 0; - for (size_t k = 0; k < nrSteps; k++, t += deltaT) { - const Vector3 measuredAcc = scenario_.accelerationInBody(t); - result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - } - - return result; - } - - // Predict mean - Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) { - // TODO(frank): allow non-standard - const imuBias::ConstantBias zeroBias; - const Pose3 pose_i = Pose3::identity(); - const Vector3 vel_i = scenario_.velocity(0); - const Vector3 omegaCoriolis = Vector3::Zero(); - const bool use2ndOrderCoriolis = true; - const PoseVelocityBias prediction = - integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), - omegaCoriolis, use2ndOrderCoriolis); - return prediction.pose; - } - - private: - Scenario scenario_; -}; - -} // namespace gtsam - -/** - * @file testScenario.cpp + * @file testScenarioRunner.cpp * @brief test ImuFacor with ScenarioRunner class * @author Frank Dellaert */ -#include +#include #include #include From 69fa553495a92c5907c127d8800ae6de578da148 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 22 Dec 2015 14:01:16 -0800 Subject: [PATCH 15/40] Monte Carlo analysis --- gtsam/navigation/Scenario.h | 23 ++++- gtsam/navigation/ScenarioRunner.h | 92 +++++++++++++------ gtsam/navigation/tests/testScenarioRunner.cpp | 18 ++-- 3 files changed, 97 insertions(+), 36 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 8872d536d..ce5631e21 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -16,12 +16,13 @@ */ #pragma once +#include #include namespace gtsam { /** - * Simple class with constant twist 3D trajectory. + * Simple IMU simulator with constant twist 3D trajectory. * It is also assumed that gravity is magically counteracted and has no effect * on trajectory. Hence, a simulated IMU yields the actual body angular * velocity, and negative G acceleration plus the acceleration created by the @@ -31,8 +32,12 @@ class Scenario { public: /// Construct scenario with constant twist [w,v] Scenario(const Vector3& w, const Vector3& v, - double imuSampleTime = 1.0 / 100.0) - : twist_((Vector6() << w, v).finished()), imuSampleTime_(imuSampleTime) {} + double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, + double accSigma = 0.01) + : twist_((Vector6() << w, v).finished()), + imuSampleTime_(imuSampleTime), + gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), + accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} const double& imuSampleTime() const { return imuSampleTime_; } @@ -40,6 +45,17 @@ class Scenario { // also, uses g=10 for easy debugging Vector3 gravity() const { return Vector3(0, 0, -10.0); } + const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { + return gyroNoiseModel_; + } + + const noiseModel::Diagonal::shared_ptr& accNoiseModel() const { + return accNoiseModel_; + } + + Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } + Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } + Vector3 angularVelocityInBody() const { return twist_.head<3>(); } Vector3 linearVelocityInBody() const { return twist_.tail<3>(); } @@ -76,6 +92,7 @@ class Scenario { private: Vector6 twist_; double imuSampleTime_; + noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 45ec74978..67b9d0b0c 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -16,59 +16,100 @@ */ #pragma once +#include #include #include -#include +#include namespace gtsam { -double accNoiseVar = 0.01; -double omegaNoiseVar = 0.03; -double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; +static double intNoiseVar = 0.0001; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; /// Simple class to test navigation scenarios class ScenarioRunner { public: ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} - // Integrate measurements for T seconds - ImuFactor::PreintegratedMeasurements integrate(double T) { + /// Integrate measurements for T seconds into a PIM + ImuFactor::PreintegratedMeasurements integrate( + double T, boost::optional gyroSampler = boost::none, + boost::optional accSampler = boost::none) { // TODO(frank): allow non-zero const imuBias::ConstantBias zeroBias; const bool use2ndOrderCoriolis = true; - ImuFactor::PreintegratedMeasurements result( - zeroBias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, + ImuFactor::PreintegratedMeasurements pim( + zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(), kIntegrationErrorCovariance, use2ndOrderCoriolis); - const Vector3 measuredOmega = scenario_.angularVelocityInBody(); - const double deltaT = scenario_.imuSampleTime(); - const size_t nrSteps = T / deltaT; + const double dt = scenario_.imuSampleTime(); + const double sqrt_dt = std::sqrt(dt); + const size_t nrSteps = T / dt; double t = 0; - for (size_t k = 0; k < nrSteps; k++, t += deltaT) { - const Vector3 measuredAcc = scenario_.accelerationInBody(t); - result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (size_t k = 0; k < nrSteps; k++, t += dt) { + Vector3 measuredOmega = scenario_.angularVelocityInBody(); + if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; + Vector3 measuredAcc = scenario_.accelerationInBody(t); + if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } - return result; + return pim; } - // Predict mean - Pose3 mean(const ImuFactor::PreintegratedMeasurements& integrated) { - // TODO(frank): allow non-standard + /// Predict predict given a PIM + PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim) { + // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); const Vector3 vel_i = scenario_.velocity(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; - const PoseVelocityBias prediction = - integrated.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), - omegaCoriolis, use2ndOrderCoriolis); - return prediction.pose; + return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), + omegaCoriolis, use2ndOrderCoriolis); + } + + /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately + Matrix6 poseCovariance(const ImuFactor::PreintegratedMeasurements& pim) { + Matrix9 cov = pim.preintMeasCov(); // _ position rotation + Matrix6 poseCov; + poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), // + cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3); + return poseCov; + } + + /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples + Matrix6 estimatePoseCovariance(double T, size_t N = 1000) { + // Get predict prediction from ground truth measurements + Pose3 prediction = predict(integrate(T)).pose; + + // Create two samplers for acceleration and omega noise + Sampler gyroSampler(scenario_.gyroNoiseModel(), 29285); + Sampler accSampler(scenario_.accNoiseModel(), 29284); + + // Sample ! + Matrix samples(9, N); + Vector6 sum = Vector6::Zero(); + for (size_t i = 0; i < N; i++) { + Pose3 sampled = predict(integrate(T, gyroSampler, accSampler)).pose; + Vector6 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + + // Compute MC covariance + Vector6 sampleMean = sum / N; + Matrix6 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector6 xi = samples.col(i); + xi -= sampleMean; + Q += xi * (xi.transpose() / (N - 1)); + } + + return Q; } private: @@ -76,4 +117,3 @@ class ScenarioRunner { }; } // namespace gtsam - diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index fc92bc416..c38b9a20a 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -27,12 +27,15 @@ static const double degree = M_PI / 180.0; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.1, 0.00001); ScenarioRunner runner(forward); const double T = 1; // seconds - ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(forward.pose(T), runner.mean(integrated), 1e-9)); + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-9)); } /* ************************************************************************* */ @@ -43,8 +46,9 @@ TEST(ScenarioRunner, Circle) { ScenarioRunner runner(circle); const double T = 15; // seconds - ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(circle.pose(T), runner.mean(integrated), 0.1)); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1)); } /* ************************************************************************* */ @@ -56,8 +60,8 @@ TEST(ScenarioRunner, Loop) { ScenarioRunner runner(loop); const double T = 30; // seconds - ImuFactor::PreintegratedMeasurements integrated = runner.integrate(T); - EXPECT(assert_equal(loop.pose(T), runner.mean(integrated), 0.1)); + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); } /* ************************************************************************* */ From 75385d009bd32b1c12a0ddac3427dbd25c8a16bb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 18:45:38 -0800 Subject: [PATCH 16/40] Small improvements --- gtsam/navigation/ScenarioRunner.h | 20 +++++++++---------- gtsam/navigation/tests/testScenarioRunner.cpp | 8 +++++--- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 67b9d0b0c..9cd5c0d41 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -24,7 +24,7 @@ namespace gtsam { -static double intNoiseVar = 0.0001; +static double intNoiseVar = 0.0000001; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; /// Simple class to test navigation scenarios @@ -33,16 +33,16 @@ class ScenarioRunner { ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} /// Integrate measurements for T seconds into a PIM - ImuFactor::PreintegratedMeasurements integrate( - double T, boost::optional gyroSampler = boost::none, - boost::optional accSampler = boost::none) { + ImuFactor::PreintegratedMeasurements integrate(double T, + Sampler* gyroSampler = 0, + Sampler* accSampler = 0) { // TODO(frank): allow non-zero const imuBias::ConstantBias zeroBias; - const bool use2ndOrderCoriolis = true; + const bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pim( zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(), - kIntegrationErrorCovariance, use2ndOrderCoriolis); + kIntegrationErrorCovariance, use2ndOrderIntegration); const double dt = scenario_.imuSampleTime(); const double sqrt_dt = std::sqrt(dt); @@ -86,14 +86,14 @@ class ScenarioRunner { Pose3 prediction = predict(integrate(T)).pose; // Create two samplers for acceleration and omega noise - Sampler gyroSampler(scenario_.gyroNoiseModel(), 29285); + Sampler gyroSampler(scenario_.gyroNoiseModel(), 10); Sampler accSampler(scenario_.accNoiseModel(), 29284); // Sample ! Matrix samples(9, N); Vector6 sum = Vector6::Zero(); for (size_t i = 0; i < N; i++) { - Pose3 sampled = predict(integrate(T, gyroSampler, accSampler)).pose; + Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose; Vector6 xi = sampled.localCoordinates(prediction); samples.col(i) = xi; sum += xi; @@ -106,10 +106,10 @@ class ScenarioRunner { for (size_t i = 0; i < N; i++) { Vector6 xi = samples.col(i); xi -= sampleMean; - Q += xi * (xi.transpose() / (N - 1)); + Q += xi * xi.transpose(); } - return Q; + return Q / (N - 1); } private: diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index c38b9a20a..517e488dd 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -27,10 +27,11 @@ static const double degree = M_PI / 180.0; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.1, 0.00001); + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.000001, 1); ScenarioRunner runner(forward); const double T = 1; // seconds + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); @@ -41,8 +42,8 @@ TEST(ScenarioRunner, Forward) { /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec - const double v = 2, omega = 6 * degree; - Scenario circle(Vector3(0, 0, omega), Vector3(v, 0, 0)); + const double v = 2, w = 6 * degree; + Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); ScenarioRunner runner(circle); const double T = 15; // seconds @@ -60,6 +61,7 @@ TEST(ScenarioRunner, Loop) { ScenarioRunner runner(loop); const double T = 30; // seconds + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); } From 320823303cbcb8fa8e542820240d195bcb1b72e0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 19:00:09 -0800 Subject: [PATCH 17/40] const correctness --- gtsam/navigation/Scenario.h | 2 +- gtsam/navigation/ScenarioRunner.h | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index ce5631e21..2fae5bf74 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -68,7 +68,7 @@ class Scenario { Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } /// Velocity in nav frame at time t - Vector3 velocity(double t) { + Vector3 velocity(double t) const { const Rot3 nRb = rotAtTime(t); return nRb * linearVelocityInBody(); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 9cd5c0d41..fa07b290f 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -33,9 +33,8 @@ class ScenarioRunner { ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} /// Integrate measurements for T seconds into a PIM - ImuFactor::PreintegratedMeasurements integrate(double T, - Sampler* gyroSampler = 0, - Sampler* accSampler = 0) { + ImuFactor::PreintegratedMeasurements integrate( + double T, Sampler* gyroSampler = 0, Sampler* accSampler = 0) const { // TODO(frank): allow non-zero const imuBias::ConstantBias zeroBias; const bool use2ndOrderIntegration = true; @@ -60,7 +59,8 @@ class ScenarioRunner { } /// Predict predict given a PIM - PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim) { + PoseVelocityBias predict( + const ImuFactor::PreintegratedMeasurements& pim) const { // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); @@ -72,7 +72,8 @@ class ScenarioRunner { } /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately - Matrix6 poseCovariance(const ImuFactor::PreintegratedMeasurements& pim) { + Matrix6 poseCovariance( + const ImuFactor::PreintegratedMeasurements& pim) const { Matrix9 cov = pim.preintMeasCov(); // _ position rotation Matrix6 poseCov; poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), // @@ -81,7 +82,7 @@ class ScenarioRunner { } /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples - Matrix6 estimatePoseCovariance(double T, size_t N = 1000) { + Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const { // Get predict prediction from ground truth measurements Pose3 prediction = predict(integrate(T)).pose; From 380d0dc989564c595308e05b887aa1e7f780a5dc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 19:08:46 -0800 Subject: [PATCH 18/40] const correctness --- gtsam/navigation/PreintegrationBase.cpp | 2 +- gtsam/navigation/PreintegrationBase.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index bb01971e6..b550af134 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -297,7 +297,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { + const bool use2ndOrderCoriolis) const { // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility boost::shared_ptr q = boost::make_shared(p()); q->n_gravity = n_gravity; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 07225dd9a..26b8aca2a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -109,8 +109,8 @@ protected: */ NavState deltaXij_; - /// Parameters - boost::shared_ptr p_; + /// Parameters. Declared mutable only for deprecated predict method. + mutable boost::shared_ptr p_; /// Acceleration and gyro bias used for preintegration imuBias::ConstantBias biasHat_; @@ -239,7 +239,7 @@ public: /// @deprecated predict PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const; private: /** Serialization function */ From 9b559b362009e9a99c0cba42922cd7463b19aa3d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 19:09:05 -0800 Subject: [PATCH 19/40] Pick out correct blocks --- gtsam/navigation/ScenarioRunner.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index fa07b290f..4e15e00a1 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -74,10 +74,10 @@ class ScenarioRunner { /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately Matrix6 poseCovariance( const ImuFactor::PreintegratedMeasurements& pim) const { - Matrix9 cov = pim.preintMeasCov(); // _ position rotation + Matrix9 cov = pim.preintMeasCov(); Matrix6 poseCov; - poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), // - cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3); + poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), // + cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3); return poseCov; } From bcdfea37d9d06ddfba2f66d820cf2e444cc43aac Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 19:28:49 -0800 Subject: [PATCH 20/40] pick out correct blocks --- gtsam/navigation/ScenarioRunner.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index fa07b290f..310d96d6f 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -74,10 +74,10 @@ class ScenarioRunner { /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately Matrix6 poseCovariance( const ImuFactor::PreintegratedMeasurements& pim) const { - Matrix9 cov = pim.preintMeasCov(); // _ position rotation + Matrix9 cov = pim.preintMeasCov(); Matrix6 poseCov; - poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3), // - cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3); + poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 0), // + cov.block<3, 3>(0, 6), cov.block<3, 3>(0, 0); return poseCov; } From 4129c9651a1526ee6568f16dd02740fe36fcc2af Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 19:29:27 -0800 Subject: [PATCH 21/40] Set up tests that pass --- gtsam/navigation/tests/testScenarioRunner.cpp | 36 ++++++++++++------- 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 517e488dd..42bffa1a5 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -22,48 +22,60 @@ using namespace std; using namespace gtsam; -static const double degree = M_PI / 180.0; +static const double kDegree = M_PI / 180.0; +static const double kDeltaT = 1e-2; +static const double kGyroSigma = 0.02; +static const double kAccelerometerSigma = 0.1; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.000001, 1); + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, kGyroSigma, + kAccelerometerSigma); ScenarioRunner runner(forward); - const double T = 1; // seconds + const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-9)); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { - // Forward velocity 2m/s, angular velocity 6 degree/sec - const double v = 2, w = 6 * degree; - Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); + // Forward velocity 2m/s, angular velocity 6 kDegree/sec + const double v = 2, w = 6 * kDegree; + Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, kGyroSigma, + kAccelerometerSigma); ScenarioRunner runner(circle); - const double T = 15; // seconds + const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, Loop) { // Forward velocity 2m/s - // Pitch up with angular velocity 6 degree/sec (negative in FLU) - const double v = 2, w = 6 * degree; - Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, kGyroSigma, + kAccelerometerSigma); ScenarioRunner runner(loop); - const double T = 30; // seconds + const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } /* ************************************************************************* */ From dfdac8c4ca6fb37379c92d1b2e8da9dd758ce349 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Dec 2015 19:30:48 -0800 Subject: [PATCH 22/40] Set up tests that pass --- gtsam/navigation/tests/testScenarioRunner.cpp | 36 ++++++++++++------- 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 517e488dd..42bffa1a5 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -22,48 +22,60 @@ using namespace std; using namespace gtsam; -static const double degree = M_PI / 180.0; +static const double kDegree = M_PI / 180.0; +static const double kDeltaT = 1e-2; +static const double kGyroSigma = 0.02; +static const double kAccelerometerSigma = 0.1; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), 1e-2, 0.000001, 1); + Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, kGyroSigma, + kAccelerometerSigma); ScenarioRunner runner(forward); - const double T = 1; // seconds + const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-9)); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, Circle) { - // Forward velocity 2m/s, angular velocity 6 degree/sec - const double v = 2, w = 6 * degree; - Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); + // Forward velocity 2m/s, angular velocity 6 kDegree/sec + const double v = 2, w = 6 * kDegree; + Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, kGyroSigma, + kAccelerometerSigma); ScenarioRunner runner(circle); - const double T = 15; // seconds + const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } /* ************************************************************************* */ TEST(ScenarioRunner, Loop) { // Forward velocity 2m/s - // Pitch up with angular velocity 6 degree/sec (negative in FLU) - const double v = 2, w = 6 * degree; - Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + // Pitch up with angular velocity 6 kDegree/sec (negative in FLU) + const double v = 2, w = 6 * kDegree; + Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, kGyroSigma, + kAccelerometerSigma); ScenarioRunner runner(loop); - const double T = 30; // seconds + const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } /* ************************************************************************* */ From 21ed3ec44154f07c829ff2becb8a1f6a5bc1c7c6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 08:59:53 -0800 Subject: [PATCH 23/40] Set up acceleration test --- gtsam/navigation/tests/testImuFactor.cpp | 38 ++++++++++++++---------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 92cb92e70..6d9c9cf5e 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -165,17 +166,30 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim, /* ************************************************************************* */ TEST(ImuFactor, StraightLine) { - // Set up IMU measurements - static const double g = 10; // make gravity 10 to make this easy to check - static const double v = 50.0, a = 0.2, dt = 3.0; - const double dt22 = dt * dt / 2; - // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X // The body itself has Z axis pointing down - static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - static const Point3 initial_position(10, 20, 0); - static const Vector3 initial_velocity(v, 0, 0); - static const NavState state1(nRb, initial_position, initial_velocity); + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 initial_position(10, 20, 0); + const Vector3 initial_velocity(50, 0, 0); + const NavState state1(nRb, initial_position, initial_velocity); + + const double a = 0.2, dt = 3.0; + AcceleratingScenario scenario(nRb, inititial_position, initial_velocity, + Vector3(a, 0, 0), dt / 10, sqrt(omegaNoiseVar), + sqrt(accNoiseVar)); + + ScenarioRunner runner(scenario); + const double T = 3; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); + + // Set up IMU measurements + const double g = 10; // make gravity 10 to make this easy to check + const double dt22 = dt * dt / 2; // set up acceleration in X direction, no angular velocity. // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z @@ -188,12 +202,6 @@ TEST(ImuFactor, StraightLine) { p->gyroscopeCovariance = kMeasuredOmegaCovariance; // Check G1 and G2 derivatives of pim.update - - // Now, preintegrate for 3 seconds, in 10 steps - PreintegratedImuMeasurements pim(p, kZeroBiasHat); - for (size_t i = 0; i < 10; i++) - pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); - Matrix93 aG1,aG2; boost::function f = boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10, From dfe3f3a34804fdbd7dc56146ccba47609d320d75 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 09:21:54 -0800 Subject: [PATCH 24/40] Split off Scenario abstract base class --- gtsam/navigation/Scenario.h | 67 +++++++++++-------- gtsam/navigation/ScenarioRunner.h | 20 +++--- gtsam/navigation/tests/testScenario.cpp | 6 +- gtsam/navigation/tests/testScenarioRunner.cpp | 24 +++---- 4 files changed, 65 insertions(+), 52 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 2fae5bf74..58d6057b3 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -22,7 +22,7 @@ namespace gtsam { /** - * Simple IMU simulator with constant twist 3D trajectory. + * Simple IMU simulator. * It is also assumed that gravity is magically counteracted and has no effect * on trajectory. Hence, a simulated IMU yields the actual body angular * velocity, and negative G acceleration plus the acceleration created by the @@ -31,11 +31,9 @@ namespace gtsam { class Scenario { public: /// Construct scenario with constant twist [w,v] - Scenario(const Vector3& w, const Vector3& v, - double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, + Scenario(double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, double accSigma = 0.01) - : twist_((Vector6() << w, v).finished()), - imuSampleTime_(imuSampleTime), + : imuSampleTime_(imuSampleTime), gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} @@ -56,43 +54,58 @@ class Scenario { Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } - Vector3 angularVelocityInBody() const { return twist_.head<3>(); } - Vector3 linearVelocityInBody() const { return twist_.tail<3>(); } + /// Pose of body in nav frame at time t + virtual Pose3 pose(double t) const = 0; /// Rotation of body in nav frame at time t - Rot3 rotAtTime(double t) const { - return Rot3::Expmap(angularVelocityInBody() * t); - } + virtual Rot3 rotation(double t) const { return pose(t).rotation(); } - /// Pose of body in nav frame at time t - Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } + virtual Vector3 angularVelocityInBody(double t) const = 0; + virtual Vector3 linearVelocityInBody(double t) const = 0; + virtual Vector3 accelerationInBody(double t) const = 0; /// Velocity in nav frame at time t Vector3 velocity(double t) const { - const Rot3 nRb = rotAtTime(t); - return nRb * linearVelocityInBody(); + return rotation(t) * linearVelocityInBody(t); } // acceleration in nav frame + // TODO(frank): correct for rotating frames? Vector3 acceleration(double t) const { - const Vector3 centripetalAcceleration = - angularVelocityInBody().cross(linearVelocityInBody()); - const Rot3 nRb = rotAtTime(t); - return nRb * centripetalAcceleration - gravity(); - } - - // acceleration in body frame frame - Vector3 accelerationInBody(double t) const { - const Vector3 centripetalAcceleration = - angularVelocityInBody().cross(linearVelocityInBody()); - const Rot3 nRb = rotAtTime(t); - return centripetalAcceleration - nRb.transpose() * gravity(); + return rotation(t) * accelerationInBody(t); } private: - Vector6 twist_; double imuSampleTime_; noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; +/** + * Simple IMU simulator with constant twist 3D trajectory. + */ +class ExpmapScenario : public Scenario { + public: + /// Construct scenario with constant twist [w,v] + ExpmapScenario(const Vector3& w, const Vector3& v, + double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, + double accSigma = 0.01) + : Scenario(imuSampleTime, gyroSigma, accSigma), + twist_((Vector6() << w, v).finished()), + centripetalAcceleration_(twist_.head<3>().cross(twist_.tail<3>())) {} + + Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } + + Vector3 angularVelocityInBody(double t) const { return twist_.head<3>(); } + Vector3 linearVelocityInBody(double t) const { return twist_.tail<3>(); } + + Vector3 accelerationInBody(double t) const { + const Rot3 nRb = rotation(t); + return centripetalAcceleration_ - nRb.transpose() * gravity(); + } + + private: + const Vector6 twist_; + const Vector3 centripetalAcceleration_; +}; + } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 310d96d6f..048692a37 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -30,7 +30,7 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; /// Simple class to test navigation scenarios class ScenarioRunner { public: - ScenarioRunner(const Scenario& scenario) : scenario_(scenario) {} + ScenarioRunner(const Scenario* scenario) : scenario_(scenario) {} /// Integrate measurements for T seconds into a PIM ImuFactor::PreintegratedMeasurements integrate( @@ -40,17 +40,17 @@ class ScenarioRunner { const bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pim( - zeroBias, scenario_.accCovariance(), scenario_.gyroCovariance(), + zeroBias, scenario_->accCovariance(), scenario_->gyroCovariance(), kIntegrationErrorCovariance, use2ndOrderIntegration); - const double dt = scenario_.imuSampleTime(); + const double dt = scenario_->imuSampleTime(); const double sqrt_dt = std::sqrt(dt); const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Vector3 measuredOmega = scenario_.angularVelocityInBody(); + Vector3 measuredOmega = scenario_->angularVelocityInBody(t); if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_.accelerationInBody(t); + Vector3 measuredAcc = scenario_->accelerationInBody(t); if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -64,10 +64,10 @@ class ScenarioRunner { // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); - const Vector3 vel_i = scenario_.velocity(0); + const Vector3 vel_i = scenario_->velocity(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; - return pim.predict(pose_i, vel_i, zeroBias, scenario_.gravity(), + return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(), omegaCoriolis, use2ndOrderCoriolis); } @@ -87,8 +87,8 @@ class ScenarioRunner { Pose3 prediction = predict(integrate(T)).pose; // Create two samplers for acceleration and omega noise - Sampler gyroSampler(scenario_.gyroNoiseModel(), 10); - Sampler accSampler(scenario_.accNoiseModel(), 29284); + Sampler gyroSampler(scenario_->gyroNoiseModel(), 10); + Sampler accSampler(scenario_->accNoiseModel(), 29284); // Sample ! Matrix samples(9, N); @@ -114,7 +114,7 @@ class ScenarioRunner { } private: - Scenario scenario_; + const Scenario* scenario_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index ae406f953..ffac96902 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -27,7 +27,7 @@ static const double degree = M_PI / 180.0; /* ************************************************************************* */ TEST(Scenario, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + ExpmapScenario forward(Vector3::Zero(), Vector3(v, 0, 0)); const Pose3 T15 = forward.pose(15); EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); @@ -38,7 +38,7 @@ TEST(Scenario, Forward) { TEST(Scenario, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec around Z const double v = 2, w = 6 * degree; - Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); + ExpmapScenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); // R = v/w, so test if circle is of right size const double R = v / w; @@ -52,7 +52,7 @@ TEST(Scenario, Loop) { // Forward velocity 2m/s // Pitch up with angular velocity 6 degree/sec (negative in FLU) const double v = 2, w = 6 * degree; - Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + ExpmapScenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); // R = v/w, so test if loop crests at 2*R const double R = v / w; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 42bffa1a5..3a6b63b92 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -30,14 +30,14 @@ static const double kAccelerometerSigma = 0.1; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - Scenario forward(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, kGyroSigma, - kAccelerometerSigma); + ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, + kGyroSigma, kAccelerometerSigma); - ScenarioRunner runner(forward); + ScenarioRunner runner(&scenario); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(forward.pose(T), runner.predict(pim).pose, 1e-9)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); @@ -47,14 +47,14 @@ TEST(ScenarioRunner, Forward) { TEST(ScenarioRunner, Circle) { // Forward velocity 2m/s, angular velocity 6 kDegree/sec const double v = 2, w = 6 * kDegree; - Scenario circle(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, kGyroSigma, - kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0), kDeltaT, + kGyroSigma, kAccelerometerSigma); - ScenarioRunner runner(circle); + ScenarioRunner runner(&scenario); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(circle.pose(T), runner.predict(pim).pose, 0.1)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); @@ -65,14 +65,14 @@ 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; - Scenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, kGyroSigma, - kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0), kDeltaT, + kGyroSigma, kAccelerometerSigma); - ScenarioRunner runner(loop); + ScenarioRunner runner(&scenario); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); - EXPECT(assert_equal(loop.pose(T), runner.predict(pim).pose, 0.1)); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 0.1)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); From 00b83ced7aabd41cfe895fdc046792d356f20a8a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 11:15:42 -0800 Subject: [PATCH 25/40] AcceleratingScenario + some refactoring (v and a specified in nav frame) --- gtsam/navigation/Scenario.h | 71 ++++++++++++------- gtsam/navigation/ScenarioRunner.h | 6 +- gtsam/navigation/tests/testScenario.cpp | 53 ++++++++++++-- gtsam/navigation/tests/testScenarioRunner.cpp | 24 +++++++ 4 files changed, 118 insertions(+), 36 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 58d6057b3..69b5dfa00 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -54,25 +54,25 @@ class Scenario { Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } - /// Pose of body in nav frame at time t - virtual Pose3 pose(double t) const = 0; + // Quantities a Scenario needs to specify: + + virtual Pose3 pose(double t) const = 0; + virtual Vector3 omega_b(double t) const = 0; + virtual Vector3 velocity_n(double t) const = 0; + virtual Vector3 acceleration_n(double t) const = 0; + + // Derived quantities: - /// Rotation of body in nav frame at time t virtual Rot3 rotation(double t) const { return pose(t).rotation(); } - virtual Vector3 angularVelocityInBody(double t) const = 0; - virtual Vector3 linearVelocityInBody(double t) const = 0; - virtual Vector3 accelerationInBody(double t) const = 0; - - /// Velocity in nav frame at time t - Vector3 velocity(double t) const { - return rotation(t) * linearVelocityInBody(t); + Vector3 velocity_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * velocity_n(t); } - // acceleration in nav frame - // TODO(frank): correct for rotating frames? - Vector3 acceleration(double t) const { - return rotation(t) * accelerationInBody(t); + Vector3 acceleration_b(double t) const { + const Rot3 nRb = rotation(t); + return nRb.transpose() * acceleration_n(t); } private: @@ -80,9 +80,7 @@ class Scenario { noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; -/** - * Simple IMU simulator with constant twist 3D trajectory. - */ +/// Scenario with constant twist 3D trajectory. class ExpmapScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] @@ -91,21 +89,40 @@ class ExpmapScenario : public Scenario { double accSigma = 0.01) : Scenario(imuSampleTime, gyroSigma, accSigma), twist_((Vector6() << w, v).finished()), - centripetalAcceleration_(twist_.head<3>().cross(twist_.tail<3>())) {} + a_b_(twist_.head<3>().cross(twist_.tail<3>())) {} Pose3 pose(double t) const { return Pose3::Expmap(twist_ * t); } - - Vector3 angularVelocityInBody(double t) const { return twist_.head<3>(); } - Vector3 linearVelocityInBody(double t) const { return twist_.tail<3>(); } - - Vector3 accelerationInBody(double t) const { - const Rot3 nRb = rotation(t); - return centripetalAcceleration_ - nRb.transpose() * gravity(); - } + Vector3 omega_b(double t) const { return twist_.head<3>(); } + Vector3 velocity_n(double t) const { return rotation(t) * twist_.tail<3>(); } + Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; } private: const Vector6 twist_; - const Vector3 centripetalAcceleration_; + const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b +}; + +/// Accelerating from an arbitrary initial state +class AcceleratingScenario : public Scenario { + public: + /// Construct scenario with constant twist [w,v] + AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, + const Vector3& accelerationInBody, + double imuSampleTime = 1.0 / 100.0, + double gyroSigma = 0.17, double accSigma = 0.01) + : Scenario(imuSampleTime, gyroSigma, accSigma), + nRb_(nRb), + p0_(p0.vector()), + v0_(v0), + a_n_(nRb_ * accelerationInBody) {} + + Pose3 pose(double t) const { return Pose3(nRb_, p0_ + a_n_ * t * t / 2.0); } + Vector3 omega_b(double t) const { return Vector3::Zero(); } + Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; } + Vector3 acceleration_n(double t) const { return a_n_; } + + private: + const Rot3 nRb_; + const Vector3 p0_, v0_, a_n_; }; } // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 048692a37..40f5f8585 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -48,9 +48,9 @@ class ScenarioRunner { const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Vector3 measuredOmega = scenario_->angularVelocityInBody(t); + Vector3 measuredOmega = scenario_->omega_b(t); if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_->accelerationInBody(t); + Vector3 measuredAcc = scenario_->acceleration_b(t); if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -64,7 +64,7 @@ class ScenarioRunner { // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; const Pose3 pose_i = Pose3::identity(); - const Vector3 vel_i = scenario_->velocity(0); + const Vector3 vel_i = scenario_->velocity_n(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(), diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index ffac96902..b975440c7 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -27,9 +27,15 @@ static const double degree = M_PI / 180.0; /* ************************************************************************* */ TEST(Scenario, Forward) { const double v = 2; // m/s - ExpmapScenario forward(Vector3::Zero(), Vector3(v, 0, 0)); + const Vector3 W(0, 0, 0), V(v, 0, 0); + const ExpmapScenario scenario(W, V); - const Pose3 T15 = forward.pose(15); + const double T = 15; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); + + const Pose3 T15 = scenario.pose(T); EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9)); } @@ -38,11 +44,17 @@ TEST(Scenario, Forward) { TEST(Scenario, Circle) { // Forward velocity 2m/s, angular velocity 6 degree/sec around Z const double v = 2, w = 6 * degree; - ExpmapScenario circle(Vector3(0, 0, w), Vector3(v, 0, 0)); + const Vector3 W(0, 0, w), V(v, 0, 0); + const ExpmapScenario scenario(W, V); + + const double T = 15; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); // R = v/w, so test if circle is of right size const double R = v / w; - const Pose3 T15 = circle.pose(15); + const Pose3 T15 = scenario.pose(T); EXPECT(assert_equal(Vector3(0, 0, 90 * degree), T15.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9)); } @@ -52,15 +64,44 @@ TEST(Scenario, Loop) { // Forward velocity 2m/s // Pitch up with angular velocity 6 degree/sec (negative in FLU) const double v = 2, w = 6 * degree; - ExpmapScenario loop(Vector3(0, -w, 0), Vector3(v, 0, 0)); + const Vector3 W(0, -w, 0), V(v, 0, 0); + const ExpmapScenario scenario(W, V); + + const double T = 30; + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9)); + EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9)); // R = v/w, so test if loop crests at 2*R const double R = v / w; - const Pose3 T30 = loop.pose(30); + const Pose3 T30 = scenario.pose(30); EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz(), 1e-9)); EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); } +/* ************************************************************************* */ +TEST(Scenario, Accelerating) { + // Set up body pointing towards y axis, and start at 10,20,0 with velocity + // going in X. The body itself has Z axis pointing down + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 P0(10, 20, 0); + const Vector3 V0(50, 0, 0); + + const double a_b = 0.2; // m/s^2 + const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); + + const double T = 3; + const Vector3 A = nRb * Vector3(a_b, 0, 0); + EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9)); + EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9)); + + const Pose3 T3 = scenario.pose(3); + EXPECT(assert_equal(nRb, T3.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(P0.vector() + T * T * A / 2.0), T3.translation(), + 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 3a6b63b92..15c2456b6 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -78,6 +78,30 @@ TEST(ScenarioRunner, Loop) { EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + // Set up body pointing towards y axis, and start at 10,20,0 with velocity + // going in X + // The body itself has Z axis pointing down + const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + const Point3 initial_position(10, 20, 0); + const Vector3 initial_velocity(50, 0, 0); + + const double a = 0.2, dt = 3.0; + AcceleratingScenario scenario(nRb, initial_velocity, initial_velocity, + Vector3(a, 0, 0), dt / 10, kGyroSigma, + kAccelerometerSigma); + + ScenarioRunner runner(&scenario); + const double T = 3; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; From dc2bac5a9e8330b42590e66afc9d9feb5bcbe829 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 11:33:52 -0800 Subject: [PATCH 26/40] Moved all noise/sampling of IMU to ScenarioRunner --- gtsam/navigation/Scenario.h | 54 +++---------------- gtsam/navigation/ScenarioRunner.h | 43 +++++++++++---- gtsam/navigation/tests/testScenarioRunner.cpp | 30 +++++------ 3 files changed, 51 insertions(+), 76 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 69b5dfa00..421507d92 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -21,39 +21,9 @@ namespace gtsam { -/** - * Simple IMU simulator. - * It is also assumed that gravity is magically counteracted and has no effect - * on trajectory. Hence, a simulated IMU yields the actual body angular - * velocity, and negative G acceleration plus the acceleration created by the - * rotating body frame. - */ +/// Simple trajectory simulator. class Scenario { public: - /// Construct scenario with constant twist [w,v] - Scenario(double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, - double accSigma = 0.01) - : imuSampleTime_(imuSampleTime), - gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), - accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} - - const double& imuSampleTime() const { return imuSampleTime_; } - - // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) - // also, uses g=10 for easy debugging - Vector3 gravity() const { return Vector3(0, 0, -10.0); } - - const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { - return gyroNoiseModel_; - } - - const noiseModel::Diagonal::shared_ptr& accNoiseModel() const { - return accNoiseModel_; - } - - Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } - Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } - // Quantities a Scenario needs to specify: virtual Pose3 pose(double t) const = 0; @@ -74,24 +44,18 @@ class Scenario { const Rot3 nRb = rotation(t); return nRb.transpose() * acceleration_n(t); } - - private: - double imuSampleTime_; - noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; /// Scenario with constant twist 3D trajectory. class ExpmapScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] - ExpmapScenario(const Vector3& w, const Vector3& v, - double imuSampleTime = 1.0 / 100.0, double gyroSigma = 0.17, - double accSigma = 0.01) - : Scenario(imuSampleTime, gyroSigma, accSigma), - twist_((Vector6() << w, v).finished()), + ExpmapScenario(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 { return rotation(t) * twist_.tail<3>(); } Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; } @@ -106,14 +70,8 @@ class AcceleratingScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, - const Vector3& accelerationInBody, - double imuSampleTime = 1.0 / 100.0, - double gyroSigma = 0.17, double accSigma = 0.01) - : Scenario(imuSampleTime, gyroSigma, accSigma), - nRb_(nRb), - p0_(p0.vector()), - v0_(v0), - a_n_(nRb_ * accelerationInBody) {} + const Vector3& accelerationInBody) + : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {} Pose3 pose(double t) const { return Pose3(nRb_, p0_ + a_n_ * t * t / 2.0); } Vector3 omega_b(double t) const { return Vector3::Zero(); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 40f5f8585..60cc9a751 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -30,7 +30,29 @@ static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; /// Simple class to test navigation scenarios class ScenarioRunner { public: - ScenarioRunner(const Scenario* scenario) : scenario_(scenario) {} + ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, + double gyroSigma = 0.17, double accSigma = 0.01) + : scenario_(scenario), + imuSampleTime_(imuSampleTime), + gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), + accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} + + const double& imuSampleTime() const { return imuSampleTime_; } + + // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) + // also, uses g=10 for easy debugging + Vector3 gravity_n() const { return Vector3(0, 0, -10.0); } + + const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { + return gyroNoiseModel_; + } + + const noiseModel::Diagonal::shared_ptr& accNoiseModel() const { + return accNoiseModel_; + } + + Matrix3 gyroCovariance() const { return gyroNoiseModel_->covariance(); } + Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } /// Integrate measurements for T seconds into a PIM ImuFactor::PreintegratedMeasurements integrate( @@ -40,17 +62,18 @@ class ScenarioRunner { const bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pim( - zeroBias, scenario_->accCovariance(), scenario_->gyroCovariance(), + zeroBias, accCovariance(), gyroCovariance(), kIntegrationErrorCovariance, use2ndOrderIntegration); - const double dt = scenario_->imuSampleTime(); + const double dt = imuSampleTime(); const double sqrt_dt = std::sqrt(dt); const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { + Rot3 bRn = scenario_->rotation(t).transpose(); Vector3 measuredOmega = scenario_->omega_b(t); if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_->acceleration_b(t); + Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n(); if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -63,12 +86,10 @@ class ScenarioRunner { const ImuFactor::PreintegratedMeasurements& pim) const { // TODO(frank): allow non-zero bias, omegaCoriolis const imuBias::ConstantBias zeroBias; - const Pose3 pose_i = Pose3::identity(); - const Vector3 vel_i = scenario_->velocity_n(0); const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; - return pim.predict(pose_i, vel_i, zeroBias, scenario_->gravity(), - omegaCoriolis, use2ndOrderCoriolis); + return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias, + gravity_n(), omegaCoriolis, use2ndOrderCoriolis); } /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately @@ -87,8 +108,8 @@ class ScenarioRunner { Pose3 prediction = predict(integrate(T)).pose; // Create two samplers for acceleration and omega noise - Sampler gyroSampler(scenario_->gyroNoiseModel(), 10); - Sampler accSampler(scenario_->accNoiseModel(), 29284); + Sampler gyroSampler(gyroNoiseModel(), 10); + Sampler accSampler(accNoiseModel(), 29284); // Sample ! Matrix samples(9, N); @@ -115,6 +136,8 @@ class ScenarioRunner { private: const Scenario* scenario_; + double imuSampleTime_; + noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 15c2456b6..c7a3216a9 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -30,10 +30,9 @@ static const double kAccelerometerSigma = 0.1; /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { const double v = 2; // m/s - ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0), kDeltaT, - kGyroSigma, kAccelerometerSigma); + ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -47,10 +46,9 @@ TEST(ScenarioRunner, Forward) { 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), kDeltaT, - kGyroSigma, kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -65,10 +63,9 @@ 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), kDeltaT, - kGyroSigma, kAccelerometerSigma); + ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -81,19 +78,16 @@ TEST(ScenarioRunner, Loop) { /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating) { // Set up body pointing towards y axis, and start at 10,20,0 with velocity - // going in X - // The body itself has Z axis pointing down + // going in X. The body itself has Z axis pointing down const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - const Point3 initial_position(10, 20, 0); - const Vector3 initial_velocity(50, 0, 0); + const Point3 P0(10, 20, 0); + const Vector3 V0(50, 0, 0); - const double a = 0.2, dt = 3.0; - AcceleratingScenario scenario(nRb, initial_velocity, initial_velocity, - Vector3(a, 0, 0), dt / 10, kGyroSigma, - kAccelerometerSigma); + const double a_b = 0.2; // m/s^2 + const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); - ScenarioRunner runner(&scenario); const double T = 3; // seconds + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); From ccef2faa95a345c79f3f988c9b585c6c4a9274be Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 11:58:41 -0800 Subject: [PATCH 27/40] Fixed pose of accelerating trajectory --- gtsam/navigation/Scenario.h | 4 +++- gtsam/navigation/tests/testScenario.cpp | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 421507d92..7badd6d4e 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -73,7 +73,9 @@ class AcceleratingScenario : public Scenario { const Vector3& accelerationInBody) : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {} - Pose3 pose(double t) const { return Pose3(nRb_, p0_ + a_n_ * t * t / 2.0); } + Pose3 pose(double t) const { + return Pose3(nRb_, p0_ + v0_ * t + a_n_ * t * t / 2.0); + } Vector3 omega_b(double t) const { return Vector3::Zero(); } Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; } Vector3 acceleration_n(double t) const { return a_n_; } diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index b975440c7..c029ecc03 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -98,8 +98,8 @@ TEST(Scenario, Accelerating) { const Pose3 T3 = scenario.pose(3); EXPECT(assert_equal(nRb, T3.rotation(), 1e-9)); - EXPECT(assert_equal(Point3(P0.vector() + T * T * A / 2.0), T3.translation(), - 1e-9)); + EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0), + T3.translation(), 1e-9)); } /* ************************************************************************* */ From 30946af98162e7e8c34e4fb57c3fe24ec87cd1ac Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 11:59:56 -0800 Subject: [PATCH 28/40] Acceleration scenario tested --- gtsam/navigation/tests/testScenarioRunner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index c7a3216a9..b7a864016 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -93,7 +93,7 @@ TEST(ScenarioRunner, Accelerating) { EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); } /* ************************************************************************* */ From 16789c09ead0d1e290d4fdffee18981cd4d760e5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:09:16 -0800 Subject: [PATCH 29/40] compilation issue --- gtsam/navigation/Scenario.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 7badd6d4e..3324abaab 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -57,7 +57,9 @@ class ExpmapScenario : public Scenario { 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 { return rotation(t) * twist_.tail<3>(); } + Vector3 velocity_n(double t) const { + return rotation(t).matrix() * twist_.tail<3>(); + } Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; } private: From 27dcf8d4a263ed9b49e75cab77f2b5430773c3a4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:09:28 -0800 Subject: [PATCH 30/40] Covariance convention --- gtsam/navigation/ScenarioRunner.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 60cc9a751..88041b6f6 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -97,8 +97,8 @@ class ScenarioRunner { const ImuFactor::PreintegratedMeasurements& pim) const { Matrix9 cov = pim.preintMeasCov(); Matrix6 poseCov; - poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 0), // - cov.block<3, 3>(0, 6), cov.block<3, 3>(0, 0); + poseCov << cov.block<3, 3>(0, 0), cov.block<3, 3>(0, 3), // + cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3); return poseCov; } From 9ecb6ed5f3eaa377c41f346dbe5fdf4a513e7f20 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:12:12 -0800 Subject: [PATCH 31/40] Now using ScenarioRunner --- gtsam/navigation/tests/testImuFactor.cpp | 33 ++++++++++++------------ 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 6d9c9cf5e..059be543c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -166,30 +166,30 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim, /* ************************************************************************* */ TEST(ImuFactor, StraightLine) { + const double a = 0.2, v=50; + // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X // The body itself has Z axis pointing down const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); const Point3 initial_position(10, 20, 0); - const Vector3 initial_velocity(50, 0, 0); - const NavState state1(nRb, initial_position, initial_velocity); + const Vector3 initial_velocity(v, 0, 0); - const double a = 0.2, dt = 3.0; - AcceleratingScenario scenario(nRb, inititial_position, initial_velocity, - Vector3(a, 0, 0), dt / 10, sqrt(omegaNoiseVar), - sqrt(accNoiseVar)); + const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, + Vector3(a, 0, 0)); - ScenarioRunner runner(scenario); - const double T = 3; // seconds + const double T = 3.0; // seconds + ScenarioRunner runner(&scenario, T / 10, sqrt(omegaNoiseVar), + sqrt(accNoiseVar)); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); Matrix6 estimatedCov = runner.estimatePoseCovariance(T); - EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // Set up IMU measurements const double g = 10; // make gravity 10 to make this easy to check - const double dt22 = dt * dt / 2; + const double dt22 = T * T / 2; // set up acceleration in X direction, no angular velocity. // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z @@ -204,9 +204,9 @@ TEST(ImuFactor, StraightLine) { // Check G1 and G2 derivatives of pim.update Matrix93 aG1,aG2; boost::function f = - boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10, + boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10, boost::none, boost::none, boost::none); - pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2); + pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2); EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); @@ -214,12 +214,12 @@ TEST(ImuFactor, StraightLine) { PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise EXPECT( - MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc, + MonteCarlo(pimMC, state1, kZeroBias, T / 10, boost::none, measuredAcc, measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); // Check integrated quantities in body frame: gravity measured by IMU is integrated! Vector3 b_deltaP(a * dt22, 0, -g * dt22); - Vector3 b_deltaV(a * dt, 0, -g * dt); + Vector3 b_deltaV(a * T, 0, -g * T); EXPECT(assert_equal(Rot3(), pim.deltaRij())); EXPECT(assert_equal(b_deltaP, pim.deltaPij())); EXPECT(assert_equal(b_deltaV, pim.deltaVij())); @@ -230,9 +230,10 @@ TEST(ImuFactor, StraightLine) { EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); // Check prediction in nav, note we move along x in body, along y in nav - Point3 expected_position(10 + v * dt, 20 + a * dt22, 0); - Velocity3 expected_velocity(v, a * dt, 0); + Point3 expected_position(10 + v * T, 20 + a * dt22, 0); + Velocity3 expected_velocity(v, a * T, 0); NavState expected(nRb, expected_position, expected_velocity); + const NavState state1(nRb, initial_position, initial_velocity); EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); } From d74e00ab2ae33c86413fe8194d59ffd004b2b59c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:12:43 -0800 Subject: [PATCH 32/40] compilation issue --- gtsam/navigation/Scenario.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 7badd6d4e..3324abaab 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -57,7 +57,9 @@ class ExpmapScenario : public Scenario { 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 { return rotation(t) * twist_.tail<3>(); } + Vector3 velocity_n(double t) const { + return rotation(t).matrix() * twist_.tail<3>(); + } Vector3 acceleration_n(double t) const { return rotation(t) * a_b_; } private: From 26ae74e1fb848a9e9d1d13fccbbe6529c47c9c87 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:19:15 -0800 Subject: [PATCH 33/40] Split off cpp file --- gtsam/navigation/ScenarioRunner.cpp | 95 +++++++++++++++++++++++++++++ gtsam/navigation/ScenarioRunner.h | 74 ++-------------------- 2 files changed, 101 insertions(+), 68 deletions(-) create mode 100644 gtsam/navigation/ScenarioRunner.cpp diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp new file mode 100644 index 000000000..6e713598f --- /dev/null +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ScenarioRunner.h + * @brief Simple class to test navigation scenarios + * @author Frank Dellaert + */ + +#include +#include + +#include + +namespace gtsam { + +static double intNoiseVar = 0.0000001; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; + +ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( + double T, Sampler* gyroSampler, Sampler* accSampler) const { + // TODO(frank): allow non-zero + const imuBias::ConstantBias zeroBias; + const bool use2ndOrderIntegration = true; + + ImuFactor::PreintegratedMeasurements pim( + zeroBias, accCovariance(), gyroCovariance(), kIntegrationErrorCovariance, + use2ndOrderIntegration); + + const double dt = imuSampleTime(); + const double sqrt_dt = std::sqrt(dt); + const size_t nrSteps = T / dt; + double t = 0; + for (size_t k = 0; k < nrSteps; k++, t += dt) { + Rot3 bRn = scenario_->rotation(t).transpose(); + Vector3 measuredOmega = scenario_->omega_b(t); + if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; + Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n(); + if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + } + + return pim; +} + +PoseVelocityBias ScenarioRunner::predict( + const ImuFactor::PreintegratedMeasurements& pim) const { + // TODO(frank): allow non-zero bias, omegaCoriolis + const imuBias::ConstantBias zeroBias; + const Vector3 omegaCoriolis = Vector3::Zero(); + const bool use2ndOrderCoriolis = true; + return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias, + gravity_n(), omegaCoriolis, use2ndOrderCoriolis); +} + +Matrix6 ScenarioRunner::estimatePoseCovariance(double T, size_t N) const { + // Get predict prediction from ground truth measurements + Pose3 prediction = predict(integrate(T)).pose; + + // Create two samplers for acceleration and omega noise + Sampler gyroSampler(gyroNoiseModel(), 10); + Sampler accSampler(accNoiseModel(), 29284); + + // Sample ! + Matrix samples(9, N); + Vector6 sum = Vector6::Zero(); + for (size_t i = 0; i < N; i++) { + Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose; + Vector6 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + + // Compute MC covariance + Vector6 sampleMean = sum / N; + Matrix6 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector6 xi = samples.col(i); + xi -= sampleMean; + Q += xi * xi.transpose(); + } + + return Q / (N - 1); +} + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 60cc9a751..16bde9d69 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -16,16 +16,12 @@ */ #pragma once -#include #include #include -#include - namespace gtsam { -static double intNoiseVar = 0.0000001; -static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; +class Sampler; /// Simple class to test navigation scenarios class ScenarioRunner { @@ -55,42 +51,13 @@ class ScenarioRunner { Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } /// Integrate measurements for T seconds into a PIM - ImuFactor::PreintegratedMeasurements integrate( - double T, Sampler* gyroSampler = 0, Sampler* accSampler = 0) const { - // TODO(frank): allow non-zero - const imuBias::ConstantBias zeroBias; - const bool use2ndOrderIntegration = true; - - ImuFactor::PreintegratedMeasurements pim( - zeroBias, accCovariance(), gyroCovariance(), - kIntegrationErrorCovariance, use2ndOrderIntegration); - - const double dt = imuSampleTime(); - const double sqrt_dt = std::sqrt(dt); - const size_t nrSteps = T / dt; - double t = 0; - for (size_t k = 0; k < nrSteps; k++, t += dt) { - Rot3 bRn = scenario_->rotation(t).transpose(); - Vector3 measuredOmega = scenario_->omega_b(t); - if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n(); - if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; - pim.integrateMeasurement(measuredAcc, measuredOmega, dt); - } - - return pim; - } + ImuFactor::PreintegratedMeasurements integrate(double T, + Sampler* gyroSampler = 0, + Sampler* accSampler = 0) const; /// Predict predict given a PIM PoseVelocityBias predict( - const ImuFactor::PreintegratedMeasurements& pim) const { - // TODO(frank): allow non-zero bias, omegaCoriolis - const imuBias::ConstantBias zeroBias; - const Vector3 omegaCoriolis = Vector3::Zero(); - const bool use2ndOrderCoriolis = true; - return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias, - gravity_n(), omegaCoriolis, use2ndOrderCoriolis); - } + const ImuFactor::PreintegratedMeasurements& pim) const; /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately Matrix6 poseCovariance( @@ -103,36 +70,7 @@ class ScenarioRunner { } /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples - Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const { - // Get predict prediction from ground truth measurements - Pose3 prediction = predict(integrate(T)).pose; - - // Create two samplers for acceleration and omega noise - Sampler gyroSampler(gyroNoiseModel(), 10); - Sampler accSampler(accNoiseModel(), 29284); - - // Sample ! - Matrix samples(9, N); - Vector6 sum = Vector6::Zero(); - for (size_t i = 0; i < N; i++) { - Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose; - Vector6 xi = sampled.localCoordinates(prediction); - samples.col(i) = xi; - sum += xi; - } - - // Compute MC covariance - Vector6 sampleMean = sum / N; - Matrix6 Q; - Q.setZero(); - for (size_t i = 0; i < N; i++) { - Vector6 xi = samples.col(i); - xi -= sampleMean; - Q += xi * xi.transpose(); - } - - return Q / (N - 1); - } + Matrix6 estimatePoseCovariance(double T, size_t N = 100) const; private: const Scenario* scenario_; From 630c2a7a1896e401de1b4f959d5526102e42f762 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:34:30 -0800 Subject: [PATCH 34/40] Now uses Runner --- gtsam/navigation/tests/testImuFactor.cpp | 65 +++++------------------- 1 file changed, 13 insertions(+), 52 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 059be543c..a54143bd0 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -57,15 +57,17 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, // Define covariance matrices /* ************************************************************************* */ -double accNoiseVar = 0.01; -double omegaNoiseVar = 0.03; -double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; -const Vector3 accNoiseVar2(0.01, 0.02, 0.03); -const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); -int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10; +static const double kGyroSigma = 0.02; +static const double kAccelerometerSigma = 0.1; +static double omegaNoiseVar = kGyroSigma * kGyroSigma; +static double accNoiseVar = kAccelerometerSigma * kAccelerometerSigma; +static double intNoiseVar = 0.0001; +static const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +static const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; +static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; +static const Vector3 accNoiseVar2(0.01, 0.02, 0.03); +static const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); +static int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10; // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ @@ -165,7 +167,7 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim, } /* ************************************************************************* */ -TEST(ImuFactor, StraightLine) { +TEST(ImuFactor, Accelerating) { const double a = 0.2, v=50; // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X @@ -178,8 +180,7 @@ TEST(ImuFactor, StraightLine) { Vector3(a, 0, 0)); const double T = 3.0; // seconds - ScenarioRunner runner(&scenario, T / 10, sqrt(omegaNoiseVar), - sqrt(accNoiseVar)); + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); @@ -187,20 +188,6 @@ TEST(ImuFactor, StraightLine) { Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); - // Set up IMU measurements - const double g = 10; // make gravity 10 to make this easy to check - const double dt22 = T * T / 2; - - // set up acceleration in X direction, no angular velocity. - // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z - Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); - - // Create parameters assuming nav frame has Z up - boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedU(g); - p->accelerometerCovariance = kMeasuredAccCovariance; - p->gyroscopeCovariance = kMeasuredOmegaCovariance; - // Check G1 and G2 derivatives of pim.update Matrix93 aG1,aG2; boost::function f = @@ -209,32 +196,6 @@ TEST(ImuFactor, StraightLine) { pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2); EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); - - // Do Monte-Carlo analysis - PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, - p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise - EXPECT( - MonteCarlo(pimMC, state1, kZeroBias, T / 10, boost::none, measuredAcc, - measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); - - // Check integrated quantities in body frame: gravity measured by IMU is integrated! - Vector3 b_deltaP(a * dt22, 0, -g * dt22); - Vector3 b_deltaV(a * T, 0, -g * T); - EXPECT(assert_equal(Rot3(), pim.deltaRij())); - EXPECT(assert_equal(b_deltaP, pim.deltaPij())); - EXPECT(assert_equal(b_deltaV, pim.deltaVij())); - - // Check bias-corrected delta: also in body frame - Vector9 expectedBC; - expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; - EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); - - // Check prediction in nav, note we move along x in body, along y in nav - Point3 expected_position(10 + v * T, 20 + a * dt22, 0); - Velocity3 expected_velocity(v, a * T, 0); - NavState expected(nRb, expected_position, expected_velocity); - const NavState state1(nRb, initial_position, initial_velocity); - EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); } /* ************************************************************************* */ From 9eb7e38cb8963388c7e6db9327d48cb890a19b71 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:48:07 -0800 Subject: [PATCH 35/40] measured quantities --- gtsam/navigation/ScenarioRunner.cpp | 5 ++--- gtsam/navigation/ScenarioRunner.h | 24 +++++++++++++++++++----- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 6e713598f..d1cf71eb9 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -40,10 +40,9 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Rot3 bRn = scenario_->rotation(t).transpose(); - Vector3 measuredOmega = scenario_->omega_b(t); + Vector3 measuredOmega = measured_angular_velocity(t); if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = scenario_->acceleration_b(t) - bRn * gravity_n(); + Vector3 measuredAcc = measured_acceleration(t); if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 16bde9d69..3107b90a8 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -23,7 +23,10 @@ namespace gtsam { class Sampler; -/// Simple class to test navigation scenarios +/* + * Simple class to test navigation scenarios. + * Takes a trajectory scenario as input, and can generate IMU measurements + */ class ScenarioRunner { public: ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, @@ -33,11 +36,22 @@ class ScenarioRunner { gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} - const double& imuSampleTime() const { return imuSampleTime_; } - // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) // also, uses g=10 for easy debugging - Vector3 gravity_n() const { return Vector3(0, 0, -10.0); } + static Vector3 gravity_n() { return Vector3(0, 0, -10.0); } + + // A gyro simly measures angular velocity in body frame + Vector3 measured_angular_velocity(double t) const { + return scenario_->omega_b(t); + } + + // An accelerometer measures acceleration in body, but not gravity + Vector3 measured_acceleration(double t) const { + Rot3 bRn = scenario_->rotation(t).transpose(); + return scenario_->acceleration_b(t) - bRn * gravity_n(); + } + + const double& imuSampleTime() const { return imuSampleTime_; } const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { return gyroNoiseModel_; @@ -70,7 +84,7 @@ class ScenarioRunner { } /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples - Matrix6 estimatePoseCovariance(double T, size_t N = 100) const; + Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const; private: const Scenario* scenario_; From e7f3f1cd2989ddb233d58c8a8421ee1bccecb01d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 12:51:28 -0800 Subject: [PATCH 36/40] Derivative tested again --- gtsam/navigation/tests/testImuFactor.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index a54143bd0..0d16ba444 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -193,9 +193,13 @@ TEST(ImuFactor, Accelerating) { boost::function f = boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10, boost::none, boost::none, boost::none); + const Vector3 measuredAcc = runner.measured_acceleration(T); + const Vector3 measuredOmega = runner.measured_angular_velocity(T); pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2); - EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); - EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); + EXPECT(assert_equal( + numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal( + numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); } /* ************************************************************************* */ From 25db851a0b6e5355a0008511f2f21897e690b6d1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 13:44:07 -0800 Subject: [PATCH 37/40] Getting rid of old MonteCarlo - in progress --- gtsam/navigation/tests/testImuFactor.cpp | 144 +++++++---------------- 1 file changed, 45 insertions(+), 99 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 0d16ba444..55b828d7f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -120,52 +120,6 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { } // namespace -/* ************************************************************************* */ -bool MonteCarlo(const PreintegratedImuMeasurements& pim, - const NavState& state, const imuBias::ConstantBias& bias, double dt, - boost::optional body_P_sensor, const Vector3& measuredAcc, - const Vector3& measuredOmega, const Vector3& accNoiseVar, - const Vector3& omegaNoiseVar, size_t N = 10000, - size_t M = 1) { - // Get mean prediction from "ground truth" measurements - PreintegratedImuMeasurements pim1 = pim; - for (size_t k = 0; k < M; k++) - pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); - NavState prediction = pim1.predict(state, bias); - Matrix9 actual = pim1.preintMeasCov(); - - // Do a Monte Carlo analysis to determine empirical density on the predicted state - Sampler sampleAccelerationNoise((accNoiseVar/dt).cwiseSqrt(), accSamplerSeed); - Sampler sampleOmegaNoise((omegaNoiseVar/dt).cwiseSqrt(), omegaSamplerSeed); - Matrix samples(9, N); - Vector9 sum = Vector9::Zero(); - for (size_t i = 0; i < N; i++) { - PreintegratedImuMeasurements pim2 = pim; - for (size_t k = 0; k < M; k++) { - Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); - Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); - pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, - body_P_sensor); - } - NavState sampled = pim2.predict(state, bias); - Vector9 xi = sampled.localCoordinates(prediction); - samples.col(i) = xi; - sum += xi; - } - - Vector9 sampleMean = sum / N; - Matrix9 Q; - Q.setZero(); - for (size_t i = 0; i < N; i++) { - Vector9 xi = samples.col(i); - xi -= sampleMean; - Q += xi * (xi.transpose() / (N - 1)); - } - - // Compare MonteCarlo value with actual (computed) value - return assert_equal(Matrix(Q), actual, 1e-4); -} - /* ************************************************************************* */ TEST(ImuFactor, Accelerating) { const double a = 0.2, v=50; @@ -589,12 +543,25 @@ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& mea } TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), - Point3(5.5, 1.0, -50.0)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); + const Rot3 nRb = Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)); + const Point3 initial_position(5.0, 1.0, -50.0); + const Vector3 initial_velocity(0.5, 0.0, 0.0); + + const double a = 0.2; + const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, + Vector3(a, 0, 0)); + + const double T = 3.0; // seconds + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); + + /////////////////////////////////////////////////////////////////////////////////////////// + Pose3 x1(nRb, initial_position); // Measurements Vector3 measuredOmega; @@ -633,55 +600,10 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { 1e-6); EXPECT(assert_equal(expectedG2, G2, 1e-5)); -#if 0 - /* - * This code is to verify the quality of the generated samples - * by checking if the covariance of the generated noises matches - * with the input covariance, and visualizing the nonlinearity of - * the sample set using the following matlab script: - * - noises = dlmread('noises.txt'); - cov(noises) - - samples = dlmread('noises.txt'); - figure(1); - for i=1:9 - subplot(3,3,i) - hist(samples(:,i), 500) - end - */ - size_t N = 100000; - Matrix samples(9,N); - Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284); - Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10); - ofstream samplesOut("preintSamples.txt"); - ofstream noiseOut("noises.txt"); - for (size_t i = 0; i Date: Wed, 23 Dec 2015 14:29:42 -0800 Subject: [PATCH 38/40] Acceleration now specified in nav frame, allow angular velocity --- gtsam/navigation/Scenario.h | 16 ++++++++------ gtsam/navigation/tests/testScenario.cpp | 22 ++++++++++++++----- gtsam/navigation/tests/testScenarioRunner.cpp | 5 +++-- 3 files changed, 28 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index 3324abaab..bc9dfe8eb 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -67,24 +67,26 @@ class ExpmapScenario : public Scenario { const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b }; -/// Accelerating from an arbitrary initial state +/// Accelerating from an arbitrary initial state, with optional rotation class AcceleratingScenario : public Scenario { public: - /// Construct scenario with constant twist [w,v] + /// Construct scenario with constant acceleration in navigation frame and + /// optional angular velocity in body: rotating while translating... AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, - const Vector3& accelerationInBody) - : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {} + const Vector3& a_n, + 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 { - return Pose3(nRb_, p0_ + v0_ * t + a_n_ * t * t / 2.0); + return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); } - Vector3 omega_b(double t) const { return Vector3::Zero(); } + 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_; } private: const Rot3 nRb_; - const Vector3 p0_, v0_, a_n_; + const Vector3 p0_, v0_, a_n_, omega_b_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenario.cpp b/gtsam/navigation/tests/testScenario.cpp index c029ecc03..ab538e02a 100644 --- a/gtsam/navigation/tests/testScenario.cpp +++ b/gtsam/navigation/tests/testScenario.cpp @@ -16,7 +16,10 @@ */ #include +#include + #include +#include #include using namespace std; @@ -87,18 +90,25 @@ TEST(Scenario, Accelerating) { const Point3 P0(10, 20, 0); const Vector3 V0(50, 0, 0); - const double a_b = 0.2; // m/s^2 - const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 3; - const Vector3 A = nRb * Vector3(a_b, 0, 0); - EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9)); + EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9)); EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9)); EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9)); + { + // Check acceleration in nav + Matrix expected = numericalDerivative11( + boost::bind(&Scenario::velocity_n, scenario, _1), T); + EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9)); + } + const Pose3 T3 = scenario.pose(3); - EXPECT(assert_equal(nRb, T3.rotation(), 1e-9)); - EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0), + EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9)); + EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0), T3.translation(), 1e-9)); } diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index b7a864016..8d01f5572 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -83,8 +83,9 @@ TEST(ScenarioRunner, Accelerating) { const Point3 P0(10, 20, 0); const Vector3 V0(50, 0, 0); - const double a_b = 0.2; // m/s^2 - const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A); const double T = 3; // seconds ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); From f79a9b8d3a04a11ad0b29b3002263933c9d2c729 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Dec 2015 15:04:36 -0800 Subject: [PATCH 39/40] Make two acceleration scenarios --- gtsam/navigation/tests/testScenarioRunner.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index 8d01f5572..bf3e3836a 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -76,13 +76,17 @@ TEST(ScenarioRunner, Loop) { } /* ************************************************************************* */ -TEST(ScenarioRunner, Accelerating) { - // Set up body pointing towards y axis, and start at 10,20,0 with velocity - // going in X. The body itself has Z axis pointing down - const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - const Point3 P0(10, 20, 0); - const Vector3 V0(50, 0, 0); +namespace initial { +// Set up body pointing towards y axis, and start at 10,20,0 with velocity +// going in X. The body itself has Z axis pointing down +const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); +const Point3 P0(10, 20, 0); +const Vector3 V0(50, 0, 0); +} +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace initial; const double a = 0.2; // m/s^2 const Vector3 A(0, a, 0); const AcceleratingScenario scenario(nRb, P0, V0, A); @@ -93,7 +97,26 @@ TEST(ScenarioRunner, Accelerating) { ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); + cout << estimatedCov << endl << endl; + cout << runner.poseCovariance(pim) << endl; +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingAndRotating) { + using namespace initial; + const double a = 0.2; // m/s^2 + const Vector3 A(0, a, 0), W(0, 0.1, 0); + const AcceleratingScenario scenario(nRb, P0, V0, A, W); + + const double T = 3; // seconds + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); + EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); + + Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); } From 1245e899d6aa5023ab83617fe30829ec5a11d787 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 23 Dec 2015 16:09:36 -0800 Subject: [PATCH 40/40] Allow for bias --- gtsam/navigation/ScenarioRunner.cpp | 38 +++++----- gtsam/navigation/ScenarioRunner.h | 56 ++++++++++----- gtsam/navigation/tests/testScenarioRunner.cpp | 69 ++++++++++++++----- 3 files changed, 105 insertions(+), 58 deletions(-) diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index d1cf71eb9..3b0a763d8 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -16,7 +16,6 @@ */ #include -#include #include @@ -26,24 +25,21 @@ static double intNoiseVar = 0.0000001; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( - double T, Sampler* gyroSampler, Sampler* accSampler) const { - // TODO(frank): allow non-zero - const imuBias::ConstantBias zeroBias; + double T, const imuBias::ConstantBias& estimatedBias, + bool corrupted) const { const bool use2ndOrderIntegration = true; ImuFactor::PreintegratedMeasurements pim( - zeroBias, accCovariance(), gyroCovariance(), kIntegrationErrorCovariance, - use2ndOrderIntegration); + estimatedBias, accCovariance(), gyroCovariance(), + kIntegrationErrorCovariance, use2ndOrderIntegration); const double dt = imuSampleTime(); - const double sqrt_dt = std::sqrt(dt); const size_t nrSteps = T / dt; double t = 0; for (size_t k = 0; k < nrSteps; k++, t += dt) { - Vector3 measuredOmega = measured_angular_velocity(t); - if (gyroSampler) measuredOmega += gyroSampler->sample() / sqrt_dt; - Vector3 measuredAcc = measured_acceleration(t); - if (accSampler) measuredAcc += accSampler->sample() / sqrt_dt; + Vector3 measuredOmega = corrupted ? measured_omega_b(t) : actual_omega_b(t); + Vector3 measuredAcc = + corrupted ? measured_specific_force_b(t) : actual_specific_force_b(t); pim.integrateMeasurement(measuredAcc, measuredOmega, dt); } @@ -51,28 +47,26 @@ ImuFactor::PreintegratedMeasurements ScenarioRunner::integrate( } PoseVelocityBias ScenarioRunner::predict( - const ImuFactor::PreintegratedMeasurements& pim) const { - // TODO(frank): allow non-zero bias, omegaCoriolis - const imuBias::ConstantBias zeroBias; + const ImuFactor::PreintegratedMeasurements& pim, + const imuBias::ConstantBias& estimatedBias) const { + // TODO(frank): allow non-zero omegaCoriolis const Vector3 omegaCoriolis = Vector3::Zero(); const bool use2ndOrderCoriolis = true; - return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), zeroBias, - gravity_n(), omegaCoriolis, use2ndOrderCoriolis); + return pim.predict(scenario_->pose(0), scenario_->velocity_n(0), + estimatedBias, gravity_n(), omegaCoriolis, + use2ndOrderCoriolis); } -Matrix6 ScenarioRunner::estimatePoseCovariance(double T, size_t N) const { +Matrix6 ScenarioRunner::estimatePoseCovariance( + double T, size_t N, const imuBias::ConstantBias& estimatedBias) const { // Get predict prediction from ground truth measurements Pose3 prediction = predict(integrate(T)).pose; - // Create two samplers for acceleration and omega noise - Sampler gyroSampler(gyroNoiseModel(), 10); - Sampler accSampler(accNoiseModel(), 29284); - // Sample ! Matrix samples(9, N); Vector6 sum = Vector6::Zero(); for (size_t i = 0; i < N; i++) { - Pose3 sampled = predict(integrate(T, &gyroSampler, &accSampler)).pose; + Pose3 sampled = predict(integrate(T, estimatedBias, true)).pose; Vector6 xi = sampled.localCoordinates(prediction); samples.col(i) = xi; sum += xi; diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 3107b90a8..2c84d3d97 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -18,11 +18,10 @@ #pragma once #include #include +#include namespace gtsam { -class Sampler; - /* * Simple class to test navigation scenarios. * Takes a trajectory scenario as input, and can generate IMU measurements @@ -30,27 +29,42 @@ class Sampler; class ScenarioRunner { public: ScenarioRunner(const Scenario* scenario, double imuSampleTime = 1.0 / 100.0, - double gyroSigma = 0.17, double accSigma = 0.01) + double gyroSigma = 0.17, double accSigma = 0.01, + const imuBias::ConstantBias& bias = imuBias::ConstantBias()) : scenario_(scenario), imuSampleTime_(imuSampleTime), gyroNoiseModel_(noiseModel::Isotropic::Sigma(3, gyroSigma)), - accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)) {} + accNoiseModel_(noiseModel::Isotropic::Sigma(3, accSigma)), + bias_(bias), + // NOTE(duy): random seeds that work well: + gyroSampler_(gyroNoiseModel_, 10), + accSampler_(accNoiseModel_, 29284) + + {} // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) // also, uses g=10 for easy debugging static Vector3 gravity_n() { return Vector3(0, 0, -10.0); } - // A gyro simly measures angular velocity in body frame - Vector3 measured_angular_velocity(double t) const { - return scenario_->omega_b(t); - } + // A gyro simply measures angular velocity in body frame + Vector3 actual_omega_b(double t) const { return scenario_->omega_b(t); } // An accelerometer measures acceleration in body, but not gravity - Vector3 measured_acceleration(double t) const { + Vector3 actual_specific_force_b(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() + + gyroSampler_.sample() / std::sqrt(imuSampleTime_); + } + Vector3 measured_specific_force_b(double t) const { + return actual_specific_force_b(t) + bias_.accelerometer() + + accSampler_.sample() / std::sqrt(imuSampleTime_); + } + const double& imuSampleTime() const { return imuSampleTime_; } const noiseModel::Diagonal::shared_ptr& gyroNoiseModel() const { @@ -65,13 +79,15 @@ class ScenarioRunner { Matrix3 accCovariance() const { return accNoiseModel_->covariance(); } /// Integrate measurements for T seconds into a PIM - ImuFactor::PreintegratedMeasurements integrate(double T, - Sampler* gyroSampler = 0, - Sampler* accSampler = 0) const; + ImuFactor::PreintegratedMeasurements integrate( + double T, + const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(), + bool corrupted = false) const; /// Predict predict given a PIM - PoseVelocityBias predict( - const ImuFactor::PreintegratedMeasurements& pim) const; + PoseVelocityBias predict(const ImuFactor::PreintegratedMeasurements& pim, + const imuBias::ConstantBias& estimatedBias = + imuBias::ConstantBias()) const; /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately Matrix6 poseCovariance( @@ -84,12 +100,18 @@ class ScenarioRunner { } /// Compute a Monte Carlo estimate of the PIM pose covariance using N samples - Matrix6 estimatePoseCovariance(double T, size_t N = 1000) const; + Matrix6 estimatePoseCovariance(double T, size_t N = 1000, + const imuBias::ConstantBias& estimatedBias = + imuBias::ConstantBias()) const; private: const Scenario* scenario_; - double imuSampleTime_; - noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; + const double imuSampleTime_; + const noiseModel::Diagonal::shared_ptr gyroNoiseModel_, accNoiseModel_; + const imuBias::ConstantBias bias_; + + // Create two samplers for acceleration and omega noise + mutable Sampler gyroSampler_, accSampler_; }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index bf3e3836a..019d60f91 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -25,14 +25,20 @@ using namespace gtsam; static const double kDegree = M_PI / 180.0; static const double kDeltaT = 1e-2; static const double kGyroSigma = 0.02; -static const double kAccelerometerSigma = 0.1; +static const double kAccelSigma = 0.1; + +static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); +static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); +/* ************************************************************************* */ +namespace forward { +const double v = 2; // m/s +ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +} /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { - const double v = 2; // m/s - ExpmapScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); - - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); + using namespace forward; + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -42,13 +48,24 @@ TEST(ScenarioRunner, Forward) { EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 1e-5)); } +/* ************************************************************************* */ +TEST(ScenarioRunner, ForwardWithBias) { + using namespace forward; + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); + const double T = 0.1; // seconds + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); +} + /* ************************************************************************* */ 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)); - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -65,7 +82,7 @@ TEST(ScenarioRunner, Loop) { const double v = 2, w = 6 * kDegree; ExpmapScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelerometerSigma); + ScenarioRunner runner(&scenario, kDeltaT, kGyroSigma, kAccelSigma); const double T = 0.1; // seconds ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); @@ -85,22 +102,36 @@ const Vector3 V0(50, 0, 0); } /* ************************************************************************* */ -TEST(ScenarioRunner, Accelerating) { - using namespace initial; - const double a = 0.2; // m/s^2 - const Vector3 A(0, a, 0); - const AcceleratingScenario scenario(nRb, P0, V0, A); +namespace accelerating { +using namespace initial; +const double a = 0.2; // m/s^2 +const Vector3 A(0, a, 0); +const AcceleratingScenario scenario(nRb, P0, V0, A); - const double T = 3; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); +const double T = 3; // seconds +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, Accelerating) { + using namespace accelerating; + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); + EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); +} + +/* ************************************************************************* */ +TEST(ScenarioRunner, AcceleratingWithBias) { + using namespace accelerating; + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma, + kNonZeroBias); + + ImuFactor::PreintegratedMeasurements pim = runner.integrate(T, kNonZeroBias); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T, 1000, kNonZeroBias); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); - cout << estimatedCov << endl << endl; - cout << runner.poseCovariance(pim) << endl; } /* ************************************************************************* */ @@ -111,12 +142,12 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 3; // seconds - ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); + ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelSigma); ImuFactor::PreintegratedMeasurements pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9)); - Matrix6 estimatedCov = runner.estimatePoseCovariance(T,10000); + Matrix6 estimatedCov = runner.estimatePoseCovariance(T); EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); }