diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 6c3cf1607..e731249bd 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -155,8 +155,29 @@ Vector9 PreintegrationBase::biasCorrectedDelta( } //------------------------------------------------------------------------------ -Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const { +static Vector3 rotate(const Matrix3& R, const Vector3& p, + OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) { + if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = R; + return R * p; +} + +//------------------------------------------------------------------------------ +static Vector3 unrotate(const Matrix3& R, const Vector3& p, + OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) { + const Matrix3 Rt = R.transpose(); + Vector3 q = Rt * p; + const double wx = q.x(), wy = q.y(), wz = q.z(); + if (H1) *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + if (H2) *H2 = Rt; + return q; +} + +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i, + OptionalJacobian<9, 9> H) const { Vector9 result = Vector9::Zero(); + if (H) H->setZero(); if (p().omegaCoriolis) { const Pose3& pose_i = state_i.pose(); const Vector3& vel_i = state_i.velocity(); @@ -164,22 +185,29 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i) const { const double dt = deltaTij(), dt2 = dt * dt; const Vector3& omegaCoriolis = *p().omegaCoriolis; - NavState::dR(result) -= Ri.transpose() * omegaCoriolis * dt; + Matrix3 D_dP_Ri; + NavState::dR(result) -= unrotate(Ri, omegaCoriolis * dt, H ? &D_dP_Ri : 0); NavState::dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper - NavState::dV(result) -= 2 * omegaCoriolis.cross(vel_i) * dt; + NavState::dV(result) -= 2.0 * omegaCoriolis.cross(vel_i) * dt; if (p().use2ndOrderCoriolis) { Vector3 temp = omegaCoriolis.cross( omegaCoriolis.cross(pose_i.translation().vector())); NavState::dP(result) -= 0.5 * temp * dt2; NavState::dV(result) -= temp * dt; } + if (H) { + const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); + H->block<3,3>(0,0) = -D_dP_Ri; + H->block<3,3>(3,6) = - omegaCoriolisHat * dt2; + H->block<3,3>(6,6) = - 2.0 * omegaCoriolisHat * dt; + } } return result; } //------------------------------------------------------------------------------ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i, - Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1, + const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { const Pose3& pose_i = state_i.pose(); @@ -189,11 +217,29 @@ Vector9 PreintegrationBase::recombinedPrediction(const NavState& state_i, // Rotation, translation, and velocity: Vector9 delta; + Matrix3 D_dP_Ri, D_dP_bc, D_dV_Ri, D_dV_bc; NavState::dR(delta) = NavState::dR(biasCorrectedDelta); - NavState::dP(delta) = Ri * NavState::dP(biasCorrectedDelta) + vel_i * dt + 0.5 * p().gravity * dt2; - NavState::dV(delta) = Ri * NavState::dV(biasCorrectedDelta) + p().gravity * dt; + NavState::dP(delta) = rotate(Ri, NavState::dP(biasCorrectedDelta), D_dP_Ri, D_dP_bc) + vel_i * dt + 0.5 * p().gravity * dt2; + NavState::dV(delta) = rotate(Ri, NavState::dV(biasCorrectedDelta), D_dV_Ri, D_dV_bc) + p().gravity * dt; + + Matrix9 Hcoriolis; + if (p().omegaCoriolis) { + delta += integrateCoriolis(state_i, H1 ? &Hcoriolis : 0); + } + if (H1) { + H1->setZero(); + H1->block<3,3>(3,0) = D_dP_Ri; + H1->block<3,3>(3,6) = I_3x3 * dt; + H1->block<3,3>(6,0) = D_dV_Ri; + if (p().omegaCoriolis) *H1 += Hcoriolis; + } + if (H2) { + H2->setZero(); + H2->block<3,3>(0,0) = I_3x3; + H2->block<3,3>(3,3) = Ri; + H2->block<3,3>(6,6) = Ri; + } - if (p().omegaCoriolis) delta += integrateCoriolis(state_i); return delta; } @@ -299,11 +345,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Ri.transpose(); // dfV/dVj } if (H5) { - const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_biasCorrected_bias.block<3,3>(0,3); + const Matrix36 JbiasOmega = D_cDeltaRij_cOmega * D_biasCorrected_bias.middleRows<3>(0); (*H5) << - Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega), // dfR/dBias - -delPdelBiasAcc(), -delPdelBiasOmega(), // dfP/dBias - -delVdelBiasAcc(), -delVdelBiasOmega(); // dfV/dBias + -D_fR_fRrot * fRrot.inverse().matrix() * JbiasOmega, // dfR/dBias + -D_biasCorrected_bias.middleRows<3>(3), // dfP/dBias + -D_biasCorrected_bias.middleRows<3>(6); // dfV/dBias } // TODO(frank): Vector9 r = state_i.localCoordinates(predictedState_j); does not work ??? Vector9 r; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index b2fa130be..2b61b95bb 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -63,10 +63,13 @@ public: static Eigen::Block dR(Vector9& v) { return v.segment<3>(0); } static Eigen::Block dP(Vector9& v) { return v.segment<3>(3); } static Eigen::Block dV(Vector9& v) { return v.segment<3>(6); } + static Eigen::Block dR(const Vector9& v) { return v.segment<3>(0); } + static Eigen::Block dP(const Vector9& v) { return v.segment<3>(3); } + static Eigen::Block dV(const Vector9& v) { return v.segment<3>(6); } // Specialize Retract/Local that agrees with IMUFactors // TODO(frank): This is a very specific retract. Talk to Luca about implications. - NavState retract(Vector9& v, // + NavState retract(const Vector9& v, // ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { if (H1||H2) throw std::runtime_error("NavState::retract derivatives not implemented yet"); return NavState(rotation().expmap(dR(v)), translation() + Point3(dP(v)), velocity() + dV(v)); @@ -213,11 +216,12 @@ class PreintegrationBase : public PreintegratedRotation { OptionalJacobian<9, 6> H = boost::none) const; /// Integrate coriolis correction in body frame state_i - Vector9 integrateCoriolis(const NavState& state_i) const; + Vector9 integrateCoriolis(const NavState& state_i, + OptionalJacobian<9, 9> H = boost::none) const; /// Recombine the preintegration, gravity, and coriolis in a single NavState tangent vector Vector9 recombinedPrediction(const NavState& state_i, - Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none, + const Vector9& biasCorrectedDelta, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = boost::none) const; /// Predict state at time j diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index e1df186cb..cd64c2212 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -200,6 +200,7 @@ Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0), Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); +NavState state1(x1, v1); // Measurements Vector3 measuredOmega(M_PI / 100, 0, 0); @@ -208,22 +209,59 @@ double deltaT = 1.0; } // namespace common /* ************************************************************************* */ -TEST(ImuFactor, BiasCorrectedDelta) { +TEST(ImuFactor, PreintegrationBaseMethods) { using namespace common; boost::shared_ptr p = - PreintegratedImuMeasurements::MakeParams(kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + boost::make_shared(); + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); + p->accelerometerCovariance = kMeasuredAccCovariance; + p->integrationCovariance = kIntegrationErrorCovariance; + p->use2ndOrderIntegration = false; + p->use2ndOrderCoriolis = false; + PreintegratedImuMeasurements pim(p, bias); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - Vector9 expected; // TODO(frank): taken from output. Should really verify. - expected << 0.0628318531, 0.0, 0.0, 4.905, -2.19885135, -8.20622494, 9.81, -4.13885394, -16.4774682; - Matrix96 actualH; - EXPECT(assert_equal(expected, pim.biasCorrectedDelta(bias,actualH), 1e-5)); - Matrix expectedH = numericalDerivative11( - boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, boost::none), bias); - EXPECT(assert_equal(expectedH, actualH)); + { // biasCorrectedDelta + Vector9 expected; // TODO(frank): taken from output. Should really verify. + expected << 0.0628318531, 0.0, 0.0, 4.905, -2.19885135, -8.20622494, 9.81, -4.13885394, -16.4774682; + Matrix96 actualH; + EXPECT(assert_equal(expected, pim.biasCorrectedDelta(bias, actualH), 1e-5)); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), bias); + EXPECT(assert_equal(expectedH, actualH)); + } + { + Vector9 expected; // TODO(frank): taken from output. Should really verify. + expected << -0.0212372436, -0.0407423986, -0.0974116854, 0.0, -0.08, 0.06, 0.0, -0.08, 0.06; + Matrix99 actualH; + EXPECT(assert_equal(expected, pim.integrateCoriolis(state1, actualH), 1e-5)); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::integrateCoriolis, pim, _1, + boost::none), state1); + EXPECT(assert_equal(expectedH, actualH)); + } + { + Vector9 expected; // TODO(frank): taken from output. Should really verify. + expected << 0.0415946095, -0.0407423986, -0.0974116854, 1, -0.08, 9.85, -0.187214027, 0.110178303, 0.0436304821; + Matrix99 actualH1, actualH2; + Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias); + EXPECT( + assert_equal(expected, + pim.recombinedPrediction(state1, biasCorrectedDelta, actualH1, + actualH2), 1e-5)); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&PreintegrationBase::recombinedPrediction, pim, _1, + biasCorrectedDelta, boost::none, boost::none), state1); + EXPECT(assert_equal(expectedH1, actualH1)); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&PreintegrationBase::recombinedPrediction, pim, state1, _1, + boost::none, boost::none), biasCorrectedDelta); + EXPECT(assert_equal(expectedH2, actualH2)); + } } /* ************************************************************************* */