From 507979c5262fee3970d8e7082f4bc4e3295aed10 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 12:12:54 +0100 Subject: [PATCH] PreintegratedRotation works for AHRS --- gtsam/navigation/AHRSFactor.cpp | 75 +++++--------- gtsam/navigation/AHRSFactor.h | 41 +++----- gtsam/navigation/PreintegratedRotation.h | 126 +++++++++++++++++++++++ 3 files changed, 165 insertions(+), 77 deletions(-) create mode 100644 gtsam/navigation/PreintegratedRotation.h diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 137f102b3..090b0c980 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -18,9 +18,9 @@ **/ #include +#include -/* External or standard includes */ -#include +using namespace std; namespace gtsam { @@ -29,47 +29,38 @@ namespace gtsam { //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - biasHat_(bias), deltaTij_(0.0) { + biasHat_(bias) { measurementCovariance_ << measuredOmegaCovariance; - delRdelBiasOmega_.setZero(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(Vector3()), deltaTij_(0.0) { + biasHat_(Vector3()) { measurementCovariance_.setZero(); - delRdelBiasOmega_.setZero(); - delRdelBiasOmega_.setZero(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { - std::cout << s << std::endl; - std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" - << std::endl; - std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl; +void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { + PreintegratedRotation::print(s); + cout << "biasHat [" << biasHat_.transpose() << "]" << endl; + cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl; + cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { - return equal_with_abs_tol(biasHat_, other.biasHat_, tol) + return PreintegratedRotation::equals(other, tol) + && equal_with_abs_tol(biasHat_, other.biasHat_, tol) && equal_with_abs_tol(measurementCovariance_, - other.measurementCovariance_, tol) - && deltaRij_.equals(other.deltaRij_, tol) - && std::fabs(deltaTij_ - other.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); + other.measurementCovariance_, tol); } //------------------------------------------------------------------------------ void AHRSFactor::PreintegratedMeasurements::resetIntegration() { - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_.setZero(); + PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } @@ -95,23 +86,23 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3 theta_incr = correctedOmega * deltaT; // rotation increment computed from the current rotation rate measurement - const Rot3 incrR = Rot3::Expmap(theta_incr); + const Rot3 incrR = Rot3::Expmap(theta_incr); // expensive !! const Matrix3 incrRt = incrR.transpose(); // Right Jacobian computed at theta_incr const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); - // Update Jacobians - // --------------------------------------------------------------------------- - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + // Update Jacobian + update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT); // Update preintegrated measurements covariance - // --------------------------------------------------------------------------- - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) + const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - Rot3 Rot_j = deltaRij_ * incrR; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) + // Update rotation and deltaTij. TODO Frank moved from end of this function !!! + updateIntegratedRotationAndDeltaT(incrR, deltaT); + + const Vector3 theta_j = thetaRij(); // super-expensive, , Parameterization of so(3) const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); // Update preintegrated measurements covariance: as in [2] we consider a first @@ -129,28 +120,13 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT; - // Update preintegrated measurements - // --------------------------------------------------------------------------- - deltaRij_ = deltaRij_ * incrR; - deltaTij_ += deltaT; } //------------------------------------------------------------------------------ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, boost::optional H) const { const Vector3 biasOmegaIncr = bias - biasHat_; - Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; - const Rot3 deltaRij_biascorrected = deltaRij_.retract( - delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - if (H) { - const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr); - (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - } - return theta_biascorrected; + return biascorrectedThetaRij(biasOmegaIncr, H); } //------------------------------------------------------------------------------ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( @@ -192,13 +168,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { } //------------------------------------------------------------------------------ -void AHRSFactor::print(const std::string& s, +void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," + cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" - << std::endl; + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); if (body_P_sensor_) body_P_sensor_->print(" sensor pose in body frame: "); diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e62a24cca..8b065dd7e 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -20,6 +20,7 @@ #pragma once /* GTSAM includes */ +#include #include #include @@ -35,7 +36,7 @@ public: * Can be built incrementally so as to avoid costly integration at time of * factor construction. */ - class PreintegratedMeasurements { + class PreintegratedMeasurements : public PreintegratedRotation { friend class AHRSFactor; @@ -43,9 +44,6 @@ public: Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: @@ -61,31 +59,22 @@ public: PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); + const Matrix3& measurementCovariance() const { + return measurementCovariance_; + } + Vector3 biasHat() const { + return biasHat_; + } + const Matrix3& preintMeasCov() const { + return preintMeasCov_; + } + /// print void print(const std::string& s = "Preintegrated Measurements: ") const; /// equals bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - const Matrix3& measurementCovariance() const { - return measurementCovariance_; - } - Matrix3 deltaRij() const { - return deltaRij_.matrix(); - } - double deltaTij() const { - return deltaTij_; - } - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } - /// TODO: Document void resetIntegration(); @@ -106,7 +95,7 @@ public: /// Integrate coriolis correction in body frame rot_i Vector3 integrateCoriolis(const Rot3& rot_i, const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij_; + return rot_i.transpose() * omegaCoriolis * deltaTij(); } // This function is only used for test purposes @@ -120,11 +109,9 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h new file mode 100644 index 000000000..1b21978fb --- /dev/null +++ b/gtsam/navigation/PreintegratedRotation.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegratedRotation.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * PreintegratedRotation is the base class for all PreintegratedMeasurements + * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). + * It includes the definitions of the preintegrated rotation. + */ +class PreintegratedRotation { + + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + /// Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; + +public: + + /** + * Default constructor, initializes the variables in the base class + */ + PreintegratedRotation() : + deltaRij_(Rot3()), deltaTij_(0.0), + delRdelBiasOmega_(Z_3x3) {} + + /// methods to access class variables + Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive + Vector3 thetaRij() const {return Rot3::Logmap(deltaRij_);} // super-expensive + const double& deltaTij() const{return deltaTij_;} + const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} + + /// Needed for testable + void print(const std::string& s) const { + std::cout << s << std::endl; + deltaRij_.print(" deltaRij "); + } + + /// Needed for testable + bool equals(const PreintegratedRotation& expected, double tol) const { + return deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(){ + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delRdelBiasOmega_ = Z_3x3; + } + + /// Update preintegrated measurements + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, double deltaT){ + deltaRij_ = deltaRij_ * incrR; + deltaTij_ += deltaT; + } + + /** + * Update Jacobians to be used during preintegration + * TODO: explain arguments + */ + void update_delRdelBiasOmega(const Matrix3& Jr_theta_incr, const Rot3& incrR, + double deltaT) { + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + } + + /// Return a bias corrected version of the integrated rotation - expensive + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { + return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + } + + /// Get so<3> version of bias corrected rotation, with optional Jacobian + Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, + boost::optional H) const { + // First, we correct deltaRij using the biasOmegaIncr, rotated + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // This was done via an expmap, now we go *back* to so<3> + const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + if (H) { + // We then do a very expensive Jacobian calculation. TODO Right Duy ? + const Matrix3 Jrinv_theta_bc = // + Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = // + Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); + (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; + } + return theta_biascorrected; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + } +}; + +} /// namespace gtsam