From c4bd02c3fabf320e54297e5572c1b8bcc2d0c2b6 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 19:54:42 -0500 Subject: [PATCH] split of measurement covariance into diagonal blocks. --- gtsam.h | 3 --- gtsam/navigation/AHRSFactor.cpp | 15 +++++------ gtsam/navigation/AHRSFactor.h | 6 ----- gtsam/navigation/CombinedImuFactor.cpp | 32 ++++++++++++------------ gtsam/navigation/CombinedImuFactor.h | 7 +++--- gtsam/navigation/ImuFactor.cpp | 18 ++++++------- gtsam/navigation/ImuFactor.h | 5 ---- gtsam/navigation/PreintegratedRotation.h | 12 ++++++--- gtsam/navigation/PreintegrationBase.h | 22 +++++++++++++--- 9 files changed, 60 insertions(+), 60 deletions(-) diff --git a/gtsam.h b/gtsam.h index 63345d289..f0ad16096 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2417,7 +2417,6 @@ class ImuFactorPreintegratedMeasurements { Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - Matrix measurementCovariance() const; // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); @@ -2473,7 +2472,6 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - Matrix measurementCovariance() const; // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); @@ -2501,7 +2499,6 @@ class AHRSFactorPreintegratedMeasurements { bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); // get Data - Matrix measurementCovariance() const; Matrix deltaRij() const; double deltaTij() const; Vector biasHat() const; diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 2bf49d9df..68c0afccb 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -29,15 +29,15 @@ namespace gtsam { //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - biasHat_(bias) { - measurementCovariance_ << measuredOmegaCovariance; + PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(Vector3()) { - measurementCovariance_.setZero(); + PreintegratedRotation(I_3x3), biasHat_(Vector3()) +{ preintMeasCov_.setZero(); } @@ -45,7 +45,6 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { PreintegratedRotation::print(s); cout << "biasHat [" << biasHat_.transpose() << "]" << endl; - cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl; cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } @@ -53,9 +52,7 @@ void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { return PreintegratedRotation::equals(other, tol) - && equal_with_abs_tol(biasHat_, other.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, - other.measurementCovariance_, tol); + && equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ @@ -96,7 +93,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() - + measurementCovariance_ * deltaT; + + gyroscopeCovariance() * deltaT; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index d6a6c8dd9..fd4b9be87 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -42,8 +42,6 @@ public: protected: 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) - Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: @@ -59,9 +57,6 @@ public: PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); - const Matrix3& measurementCovariance() const { - return measurementCovariance_; - } Vector3 biasHat() const { return biasHat_; } @@ -105,7 +100,6 @@ public: 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_); } }; diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 55caa32ee..914bbe1ca 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -36,28 +36,28 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, use2ndOrderIntegration) + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), + biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), + biasAccOmegaInit_(biasAccOmegaInit) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; - measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; - measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ PreintegrationBase::print(s); - cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; + cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; + cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; + cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl; cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ - return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol) + && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol) + &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } @@ -120,17 +120,17 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Matrix G_measCov_Gt = Matrix::Zero(15,15); // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); + G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) * + (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * (H_vel_biasacc.transpose()); G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * + (gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * measurementCovariance_.block<3,3>(9,9); - G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * measurementCovariance_.block<3,3>(12,12); + G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; + G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; // OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); + Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose(); G_measCov_Gt.block<3,3>(3,6) = block23; G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index f7041fdeb..a7c6ecd39 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -82,8 +82,9 @@ public: protected: - Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + 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 Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] @@ -130,7 +131,6 @@ public: boost::optional F_test = boost::none, boost::optional G_test = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} Matrix preintMeasCov() const { return preintMeasCov_;} private: @@ -140,7 +140,6 @@ public: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 2d8b94618..0aaa0122e 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -35,26 +35,22 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, use2ndOrderIntegration) + PreintegrationBase(bias, + measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { PreintegrationBase::print(s); - cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { - return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) + return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } @@ -94,9 +90,9 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, // as G and measurementCovariance are blockdiagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - preintMeasCov_.block<3,3>(0,0) += measurementCovariance_.block<3,3>(0,0) * deltaT; - preintMeasCov_.block<3,3>(3,3) += R_i * measurementCovariance_.block<3,3>(3,3) * R_i.transpose() * deltaT; - preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * measurementCovariance_.block<3,3>(6,6) * D_Rincr_integratedOmega.transpose() * deltaT; + preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; + preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; + preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; // F_test and G_test are given as output for testing purposes and are not needed by the factor if(F_test){ // This in only for testing diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 125c81040..b91c76ade 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -75,9 +75,6 @@ public: protected: - Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of - ///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). @@ -118,7 +115,6 @@ public: OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} Matrix preintMeasCov() const { return preintMeasCov_;} private: @@ -128,7 +124,6 @@ public: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index aee6b6738..731e11f2e 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -37,33 +37,39 @@ class PreintegratedRotation { /// Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements public: /** * Default constructor, initializes the variables in the base class */ - PreintegratedRotation() : + PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : deltaRij_(Rot3()), deltaTij_(0.0), - delRdelBiasOmega_(Z_3x3) {} + delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} /// methods to access class variables Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive Vector3 thetaRij(boost::optional H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive const double& deltaTij() const{return deltaTij_;} const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} + const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} /// Needed for testable void print(const std::string& s) const { std::cout << s << std::endl; + std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; deltaRij_.print(" deltaRij "); + std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; + std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; } /// 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); + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) + && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); } /// Re-initialize PreintegratedMeasurements diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index a38743c0e..8214c1930 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -59,6 +59,10 @@ class PreintegrationBase : public PreintegratedRotation { Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + /// (to compensate errors in Euler integration) + public: /** @@ -67,11 +71,16 @@ public: * @param use2ndOrderIntegration Controls the order of integration * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ - PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) : + PreintegrationBase(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) : + PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3) {} + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + accelerometerCovariance_(measuredAccCovariance), + integrationCovariance_(integrationErrorCovariance) {} /// methods to access class variables const Vector3& deltaPij() const {return deltaPij_;} @@ -83,9 +92,14 @@ public: const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} + const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} + const Matrix3& integrationCovariance() const { return integrationCovariance_;} + /// Needed for testable void print(const std::string& s) const { PreintegratedRotation::print(s); + std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; + std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; biasHat_.print(" biasHat"); @@ -100,7 +114,9 @@ public: && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol); + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) + && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); } /// Re-initialize PreintegratedMeasurements