From ebba015227357381911f808ee89994e8c830f695 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jan 2016 18:29:04 -0800 Subject: [PATCH] Rename key method --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/PreintegrationBase.cpp | 82 +++++++++++++------ gtsam/navigation/PreintegrationBase.h | 12 ++- .../tests/testPreintegrationBase.cpp | 52 ++++++++++++ 5 files changed, 124 insertions(+), 26 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index db0e6d34a..a961a79bd 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -70,7 +70,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // Update preintegrated measurements. Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 523809b1d..2f8e42917 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -55,7 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - updatedPreintegrated(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 751dd3fa3..2536e1993 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -178,7 +178,7 @@ Vector9 PreintegrationBase::UpdatePreintegrated(const Vector3& a_body, } //------------------------------------------------------------------------------ -void PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc, +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, Matrix9* A, Matrix93* B, Matrix93* C) { @@ -211,6 +211,15 @@ void PreintegrationBase::updatedPreintegrated(const Vector3& measuredAcc, preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); } +void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt) { + // NOTE(frank): integrateMeasuremtn always needs to compute the derivatives, + // even when not of interest to the caller. Provide scratch space here. + Matrix9 A; + Matrix93 B, C; + integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); +} //------------------------------------------------------------------------------ Vector9 PreintegrationBase::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { @@ -320,6 +329,53 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, #define D_v_t(H) (H)->block<3,3>(6,3) #define D_v_v(H) (H)->block<3,3>(6,6) +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::Compose(const Vector9& zeta01, + const Vector9& zeta12, double deltaT12, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { + const auto t01 = zeta01.segment<3>(0); + const auto p01 = zeta01.segment<3>(3); + const auto v01 = zeta01.segment<3>(6); + + const auto t12 = zeta12.segment<3>(0); + const auto p12 = zeta12.segment<3>(3); + const auto v12 = zeta12.segment<3>(6); + + Matrix3 R01_H_t01, R12_H_t12; + const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); + const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); + + Matrix3 R02_H_R01, R02_H_R12; + const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); + + Matrix3 t02_H_R02; + Vector9 zeta02; + const Matrix3 R = R01.matrix(); + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity + + if (H1) { + H1->setZero(); + D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; + D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; + D_t_t(H1) = I_3x3; + D_t_v(H1) = I_3x3 * deltaT12; + D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; + D_v_v(H1) = I_3x3; + } + + if (H2) { + H2->setZero(); + D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; + D_t_t(H2) = R; + D_v_v(H2) = R; + } + + return zeta02; +} + //------------------------------------------------------------------------------ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, Matrix9* H2) { @@ -335,28 +391,8 @@ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, const double& t12 = pim12.deltaTij(); deltaTij_ = t01 + t12; - Matrix3 R01_H_theta01, R12_H_theta12; - const Rot3 R01 = Rot3::Expmap(theta(), R01_H_theta01); - const Rot3 R12 = Rot3::Expmap(pim12.theta(), R12_H_theta12); - - Matrix3 R02_H_R01, R02_H_R12; - const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); - - Matrix3 theta02_H_R02; - preintegrated_ << Rot3::Logmap(R02, theta02_H_R02), - deltaPij() + deltaVij() * t12 + R01 * pim12.deltaPij(), - deltaVij() + R01 * pim12.deltaVij(); - - H1->setZero(); - D_R_R(H1) = theta02_H_R02 * R02_H_R01 * R01_H_theta01; - D_t_t(H1) = I_3x3; - D_t_v(H1) = I_3x3 * t12; - D_v_v(H1) = I_3x3; - - H2->setZero(); - D_R_R(H2) = theta02_H_R02 * R02_H_R12 * R12_H_theta12; // I_3x3 ?? - D_t_t(H2) = R01.matrix(); // + rotated_H_theta1 ?? - D_v_v(H2) = R01.matrix(); // + rotated_H_theta1 ?? + preintegrated_ << PreintegrationBase::Compose( + preintegrated(), pim12.preintegrated(), t12, H1, H2); preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 97e1d76f4..f8639cf79 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -183,10 +183,14 @@ public: /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose - void updatedPreintegrated(const Vector3& measuredAcc, + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double deltaT, Matrix9* A, Matrix93* B, Matrix93* C); + // Version without derivatives + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double deltaT); + /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, @@ -211,6 +215,12 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; + // Compose the two preintegrated vectors + static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, + double deltaT12, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none); + /// Merge in a different set of measurements and update bias derivatives accordingly /// The derivatives apply to the preintegrated Vector9 void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index 17b34c0d3..ad1aae3db 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -87,6 +87,58 @@ TEST(PreintegrationBase, computeError) { EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); } +/* ************************************************************************* */ +TEST(PreintegrationBase, Compose) { + testing::SomeMeasurements measurements; + PreintegrationBase pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + boost::function f = + [pim](const Vector9& zeta01, const Vector9& zeta12) { + return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij()); + }; + + // Expected merge result + PreintegrationBase expected_pim02(testing::Params()); + testing::integrateMeasurements(measurements, &expected_pim02); + testing::integrateMeasurements(measurements, &expected_pim02); + + // Actual result + Matrix9 H1, H2; + PreintegrationBase actual_pim02 = pim; + actual_pim02.mergeWith(pim, &H1, &H2); + + const Vector9 zeta = pim.preintegrated(); + const Vector9 actual_zeta = + PreintegrationBase::Compose(zeta, zeta, pim.deltaTij()); + EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7)); + EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7)); + EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7)); +} + +/* ************************************************************************* */ + TEST(PreintegrationBase, MergedBiasDerivatives) { + testing::SomeMeasurements measurements; + + boost::function f = + [=](const Vector3& a, const Vector3& w) { + PreintegrationBase pim02(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim02); + testing::integrateMeasurements(measurements, &pim02); + return pim02.preintegrated(); + }; + + // Expected merge result + PreintegrationBase expected_pim02(testing::Params()); + testing::integrateMeasurements(measurements, &expected_pim02); + testing::integrateMeasurements(measurements, &expected_pim02); + + EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1), + expected_pim02.preintegrated_H_biasAcc())); + EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), + expected_pim02.preintegrated_H_biasOmega(), 1e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr;