From f8df938b30c384f406ee46f865e2a645e8cc3f94 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 1 Aug 2015 17:17:14 -0700 Subject: [PATCH] New naming, old derivative code --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 12 +++++++++++- gtsam/navigation/PreintegrationBase.cpp | 12 ++++++------ gtsam/navigation/PreintegrationBase.h | 17 +++++++++++------ 4 files changed, 29 insertions(+), 14 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 4ee8f17de..a697a5e4c 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -74,7 +74,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 G1,G2; - updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT, + PreintegrationBase::update(measuredAcc, measuredOmega, deltaT, &D_incrR_integratedOmega, &F_9x9, &G1, &G2); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 90a369bb1..9a0d3d94a 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -58,7 +58,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 G1, G2; Matrix3 D_incrR_integratedOmega; - updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt, + PreintegrationBase::update(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &F, &G1, &G2); // first order covariance propagation: @@ -67,10 +67,20 @@ void PreintegratedImuMeasurements::integrateMeasurement( // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G' // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) +#ifdef OLD_JACOBIAN_CALCULATION + Matrix9 G; + G << G1, Gi, G2; + Matrix9 Cov; + Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3, + Z_3x3, p().integrationCovariance * dt, Z_3x3, + Z_3x3, Z_3x3, p().gyroscopeCovariance / dt; + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose(); +#else preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G1 * (p().accelerometerCovariance / dt) * G1.transpose() + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) + G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); +#endif } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 0d51e59f9..0e40c3183 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -61,9 +61,9 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, } //------------------------------------------------------------------------------ -NavState PreintegrationBase::update(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double dt, Matrix9* F, Matrix93* G1, - Matrix93* G2) const { +NavState PreintegrationBase::updatedDeltaXij(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, OptionalJacobian<9, 9> F, + OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { // Correct for bias in the sensor frame Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); @@ -90,7 +90,7 @@ NavState PreintegrationBase::update(const Vector3& measuredAcc, } //------------------------------------------------------------------------------ -void PreintegrationBase::updatePreintegratedMeasurements( +void PreintegrationBase::update( const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) { @@ -99,7 +99,7 @@ void PreintegrationBase::updatePreintegratedMeasurements( // Do update deltaTij_ += dt; - deltaXij_ = update(measuredAcc, measuredOmega, dt, F, G1, G2); // functional + deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, F, G1, G2); // functional // Update Jacobians // TODO(frank): we are repeating some computation here: accessible in F ? @@ -139,7 +139,7 @@ Vector9 PreintegrationBase::biasCorrectedDelta( Vector9 xi; Matrix3 D_dR_correctedRij; // TODO(frank): could line below be simplified? It is equivalent to - // LogMap(deltaRij_.compose(Expmap(delRdelBiasOmega_ * biasIncr.gyroscope()))) + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + delPdelBiasOmega_ * biasIncr.gyroscope(); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 95de5e935..af83bb0e0 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -146,6 +146,9 @@ public: } /// getters + const NavState& deltaXij() const { + return deltaXij_; + } const double& deltaTij() const { return deltaTij_; } @@ -189,13 +192,15 @@ public: bool equals(const PreintegrationBase& other, double tol) const; /// Calculate the updated preintegrated measurement, does not modify - NavState update(const Vector3& measuredAcc, const Vector3& measuredOmega, - const double dt, Matrix9* F, Matrix93* G1, Matrix93* G2) const; + NavState updatedDeltaXij(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, OptionalJacobian<9, 9> F = + boost::none, OptionalJacobian<9, 3> G1 = boost::none, + OptionalJacobian<9, 3> G2 = boost::none) const; - /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT, - Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2); + /// Update preintegrated measurements and get derivatives + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F, + Matrix93* G1, Matrix93* G2); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far