From d0f911139dcf016b2f49c97432dbf2c2bf76d6aa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Dec 2015 13:57:26 -0800 Subject: [PATCH 01/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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/17] 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;