From 9ae797a88c6273f4eff7ff747d4b1dc9157a8a43 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Tue, 10 Mar 2015 22:42:20 -0400 Subject: [PATCH] Minor formatting changes. --- gtsam/navigation/CombinedImuFactor.cpp | 7 ++++++- gtsam/navigation/PreintegrationBase.h | 27 +++++++++++++------------- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 075d16022..3547719ac 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -151,7 +151,12 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( G_test->resize(15, 21); // This is for testing & documentation ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + (*G_test) << // + I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // + Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; } } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index b8cd08a6d..0c5d7522d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -133,21 +133,19 @@ public: } /// Needed for testable - bool equals(const PreintegrationBase& expected, double tol) const { - return PreintegratedRotation::equals(expected, tol) - && biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && 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) + bool equals(const PreintegrationBase& other, double tol) const { + return PreintegratedRotation::equals(other, tol) + && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol) && equal_with_abs_tol(accelerometerCovariance_, - expected.accelerometerCovariance_, tol) + other.accelerometerCovariance_, tol) && equal_with_abs_tol(integrationCovariance_, - expected.integrationCovariance_, tol); + other.integrationCovariance_, tol); } /// Re-initialize PreintegratedMeasurements @@ -188,7 +186,8 @@ public: F_pos_angles = Z_3x3; // pos vel angle - *F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos + *F << // + I_3x3, I_3x3 * deltaT, F_pos_angles, // pos Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, Z_3x3, F_angles_angles; // angle }