12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Minor formatting changes.

release/4.3a0
krunalchande 2015-03-10 22:42:20 -04:00
parent 3d1b24ea01
commit 9ae797a88c
2 changed files with 19 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -151,7 +151,12 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
G_test->resize(15, 21); G_test->resize(15, 21);
// This is for testing & documentation // This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) ///< 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;
} }
} }

27
gtsam/navigation/PreintegrationBase.h
View File

@ -133,21 +133,19 @@ public:
} }
/// Needed for testable /// Needed for testable
bool equals(const PreintegrationBase& expected, double tol) const { bool equals(const PreintegrationBase& other, double tol) const {
return PreintegratedRotation::equals(expected, tol) return PreintegratedRotation::equals(other, tol)
&& biasHat_.equals(expected.biasHat_, tol) && biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
tol) && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_,
tol)
&& equal_with_abs_tol(accelerometerCovariance_, && equal_with_abs_tol(accelerometerCovariance_,
expected.accelerometerCovariance_, tol) other.accelerometerCovariance_, tol)
&& equal_with_abs_tol(integrationCovariance_, && equal_with_abs_tol(integrationCovariance_,
expected.integrationCovariance_, tol); other.integrationCovariance_, tol);
} }
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
@ -188,7 +186,8 @@ public:
F_pos_angles = Z_3x3; F_pos_angles = Z_3x3;
// pos vel angle // 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, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles; // angle Z_3x3, Z_3x3, F_angles_angles; // angle
} }