From 438da30dceebdea6df2d9c716e1d3e52926f443f Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 25 Jan 2016 14:16:21 -0800 Subject: [PATCH] Isolated PreintegrationParams in separate class --- gtsam/navigation/CombinedImuFactor.h | 6 +- gtsam/navigation/ImuFactor.h | 6 +- gtsam/navigation/PreintegrationBase.cpp | 30 +------- gtsam/navigation/PreintegrationBase.h | 65 +++-------------- gtsam/navigation/PreintegrationParams.cpp | 54 ++++++++++++++ gtsam/navigation/PreintegrationParams.h | 71 +++++++++++++++++++ gtsam/navigation/ScenarioRunner.cpp | 1 + gtsam/navigation/ScenarioRunner.h | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 +- gtsam/navigation/tests/testScenarioRunner.cpp | 4 +- 10 files changed, 148 insertions(+), 95 deletions(-) create mode 100644 gtsam/navigation/PreintegrationParams.cpp create mode 100644 gtsam/navigation/PreintegrationParams.h diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index f5ce1f3db..bc353c7d9 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -63,14 +63,14 @@ public: /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params : PreintegrationBase::Params { + struct Params : PreintegrationParams { Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration /// See two named constructors below for good values of n_gravity in body frame Params(const Vector3& n_gravity) : - PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( + PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( I_3x3), biasAccOmegaInit(I_6x6) { } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index f52d95b26..d94789d4a 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -82,7 +82,7 @@ public: * @param bias Current estimate of acceleration and rotation rate biases * @param p Parameters, typically fixed in a single application */ - PreintegratedImuMeasurements(const boost::shared_ptr& p, + PreintegratedImuMeasurements(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationBase(p, biasHat) { preintMeasCov_.setZero(); @@ -230,7 +230,7 @@ public: /// @deprecated typename typedef PreintegratedImuMeasurements PreintegratedMeasurements; - /// @deprecated constructor, in the new one gravity, coriolis settings are in Params + /// @deprecated constructor, in the new one gravity, coriolis settings are in PreintegrationParams ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& n_gravity, const Vector3& omegaCoriolis, @@ -238,7 +238,7 @@ public: const bool use2ndOrderCoriolis = false); /// @deprecated use PreintegrationBase::predict, - /// in the new one gravity, coriolis settings are in Params + /// in the new one gravity, coriolis settings are in PreintegrationParams static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, PreintegratedMeasurements& pim, const Vector3& n_gravity, diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 626dcdf70..5552a952e 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -20,38 +20,14 @@ **/ #include "PreintegrationBase.h" +#ifdef ALLOW_DEPRECATED_IN_GTSAM4 #include +#endif using namespace std; namespace gtsam { -//------------------------------------------------------------------------------ -void PreintegrationBase::Params::print(const string& s) const { - PreintegratedRotation::Params::print(s); - cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" - << endl; - cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]" - << endl; - if (omegaCoriolis && use2ndOrderCoriolis) - cout << "Using 2nd-order Coriolis" << endl; - if (body_P_sensor) - body_P_sensor->print(" "); - cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; -} - -//------------------------------------------------------------------------------ -bool PreintegrationBase::Params::equals( - const PreintegratedRotation::Params& other, double tol) const { - auto e = dynamic_cast(&other); - return e != nullptr && PreintegratedRotation::Params::equals(other, tol) - && use2ndOrderCoriolis == e->use2ndOrderCoriolis - && equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance, - tol) - && equal_with_abs_tol(integrationCovariance, e->integrationCovariance, - tol) && equal_with_abs_tol(n_gravity, e->n_gravity, tol); -} - //------------------------------------------------------------------------------ void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f5e8c7183..857157114 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -21,10 +21,9 @@ #pragma once -#include +#include #include #include -#include namespace gtsam { @@ -55,54 +54,6 @@ struct PoseVelocityBias { */ class PreintegrationBase { -public: - - /// Parameters for pre-integration: - /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params: PreintegratedRotation::Params { - Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer - Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty - bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration - Vector3 n_gravity; ///< Gravity vector in nav frame - - /// The Params constructor insists on getting the navigation frame gravity vector - /// For convenience, two commonly used conventions are provided by named constructors below - Params(const Vector3& n_gravity) : - accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis( - false), n_gravity(n_gravity) { - } - - // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis - static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, g)); - } - - // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::make_shared(Vector3(0, 0, -g)); - } - - void print(const std::string& s) const; - bool equals(const PreintegratedRotation::Params& other, double tol) const; - - protected: - /// Default constructor for serialization only: uninitialized! - Params() {} - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & boost::serialization::make_nvp("PreintegratedRotation_Params", - boost::serialization::base_object(*this)); - ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); - ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); - ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); - ar & BOOST_SERIALIZATION_NVP(n_gravity); - } - }; - protected: /// Parameters. Declared mutable only for deprecated predict method. @@ -110,7 +61,7 @@ protected: #ifdef ALLOW_DEPRECATED_IN_GTSAM4 mutable #endif - boost::shared_ptr p_; + boost::shared_ptr p_; /// Acceleration and gyro bias used for preintegration imuBias::ConstantBias biasHat_; @@ -146,7 +97,7 @@ public: * @param p Parameters, typically fixed in a single application * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegrationBase(const boost::shared_ptr& p, + PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : p_(p), biasHat_(biasHat) { resetIntegration(); @@ -155,7 +106,7 @@ public: /** * Constructor which takes in all members for generic construction */ - PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat, + PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat, double deltaTij, const NavState& deltaXij, const Matrix3& delPdelBiasAcc, const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc, const Matrix3& delVdelBiasOmega) @@ -174,19 +125,19 @@ public: /// Re-initialize PreintegratedMeasurements void resetIntegration(); - /// check parameters equality: checks whether shared pointer points to same Params object. + /// check parameters equality: checks whether shared pointer points to same PreintegrationParams object. bool matchesParamsWith(const PreintegrationBase& other) const { return p_ == other.p_; } /// shared pointer to params - const boost::shared_ptr& params() const { + const boost::shared_ptr& params() const { return p_; } /// const reference to params - const Params& p() const { - return *boost::static_pointer_cast(p_); + const PreintegrationParams& p() const { + return *boost::static_pointer_cast(p_); } void set_body_P_sensor(const Pose3& body_P_sensor) { diff --git a/gtsam/navigation/PreintegrationParams.cpp b/gtsam/navigation/PreintegrationParams.cpp new file mode 100644 index 000000000..61cd1617c --- /dev/null +++ b/gtsam/navigation/PreintegrationParams.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationParams.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "PreintegrationParams.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +void PreintegrationParams::print(const string& s) const { + PreintegratedRotation::Params::print(s); + cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" + << endl; + cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]" + << endl; + if (omegaCoriolis && use2ndOrderCoriolis) + cout << "Using 2nd-order Coriolis" << endl; + if (body_P_sensor) body_P_sensor->print(" "); + cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; +} + +//------------------------------------------------------------------------------ +bool PreintegrationParams::equals(const PreintegratedRotation::Params& other, + double tol) const { + auto e = dynamic_cast(&other); + return e != nullptr && PreintegratedRotation::Params::equals(other, tol) && + use2ndOrderCoriolis == e->use2ndOrderCoriolis && + equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance, + tol) && + equal_with_abs_tol(integrationCovariance, e->integrationCovariance, + tol) && + equal_with_abs_tol(n_gravity, e->n_gravity, tol); +} + +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h new file mode 100644 index 000000000..f68f711f1 --- /dev/null +++ b/gtsam/navigation/PreintegrationParams.h @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationParams.h + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/// Parameters for pre-integration: +/// Usage: Create just a single Params and pass a shared pointer to the constructor +struct PreintegrationParams: PreintegratedRotation::Params { + Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer + Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty + bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration + Vector3 n_gravity; ///< Gravity vector in nav frame + + /// The Params constructor insists on getting the navigation frame gravity vector + /// For convenience, two commonly used conventions are provided by named constructors below + PreintegrationParams(const Vector3& n_gravity) + : accelerometerCovariance(I_3x3), + integrationCovariance(I_3x3), + use2ndOrderCoriolis(false), + n_gravity(n_gravity) {} + + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } + + void print(const std::string& s) const; + bool equals(const PreintegratedRotation::Params& other, double tol) const; + +protected: + /// Default constructor for serialization only: uninitialized! + PreintegrationParams() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & boost::serialization::make_nvp("PreintegratedRotation_Params", + boost::serialization::base_object(*this)); + ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); + ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); + ar & BOOST_SERIALIZATION_NVP(n_gravity); + } +}; + +} // namespace gtsam diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 48f649b07..e485e37a3 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace gtsam { diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index c2e62384f..7cb98379a 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -28,7 +28,7 @@ namespace gtsam { */ class ScenarioRunner { public: - typedef boost::shared_ptr SharedParams; + typedef boost::shared_ptr SharedParams; ScenarioRunner(const Scenario* scenario, const SharedParams& p, double imuSampleTime = 1.0 / 100.0, const imuBias::ConstantBias& bias = imuBias::ConstantBias()) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index a92d0737f..99f5ec056 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -62,8 +62,8 @@ static const double kGyroSigma = 0.02; static const double kAccelerometerSigma = 0.1; // Create default parameters with Z-down and above noise paramaters -static boost::shared_ptr defaultParams() { - auto p = PreintegratedImuMeasurements::Params::MakeSharedD(kGravity); +static boost::shared_ptr defaultParams() { + auto p = PreintegrationParams::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma * I_3x3; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index a6aa80b71..7fd5b6637 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -32,8 +32,8 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr defaultParams() { - auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10); +static boost::shared_ptr defaultParams() { + auto p = PreintegrationParams::MakeSharedD(10); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; p->integrationCovariance = 0.0000001 * I_3x3;