diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index f3647fcc0..9c2dedea1 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -148,7 +148,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( if (!use2ndOrderIntegration) throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; - boost::shared_ptr p = Params::MakeSharedFRD(); + boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; p->accelerometerCovariance = measuredAccCovariance; p->integrationCovariance = integrationErrorCovariance; @@ -260,7 +260,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor( Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& pim, const Vector3& b_gravity, + const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, @@ -268,7 +268,7 @@ CombinedImuFactor::CombinedImuFactor( _PIM_(pim) { boost::shared_ptr p = boost::make_shared(pim.p()); - p->b_gravity = b_gravity; + p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; p->use2ndOrderCoriolis = use2ndOrderCoriolis; @@ -279,11 +279,11 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, CombinedPreintegratedMeasurements& pim, - const Vector3& b_gravity, + const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // use deprecated predict - PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity, + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, omegaCoriolis, use2ndOrderCoriolis); pose_j = pvb.pose; vel_j = pvb.velocity; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 142e8706f..192cc3d99 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -66,19 +66,19 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration - /// See two named constructors below for good values of b_gravity in body frame - Params(const Vector3& b_gravity) : - PreintegrationBase::Params(b_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( + /// See two named constructors below for good values of n_gravity in body frame + Params(const Vector3& n_gravity) : + PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( I_3x3), biasAccOmegaInit(I_6x6) { } - // Default Params for Front-Right-Down convention: b_gravity points along positive Z-axis - static boost::shared_ptr MakeSharedFRD(double g = 9.81) { + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { return boost::make_shared(Vector3(0, 0, g)); } - // Default Params for Front-Left-Up convention: b_gravity points along negative Z-axis - static boost::shared_ptr MakeSharedFLU(double g = 9.81) { + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { return boost::make_shared(Vector3(0, 0, -g)); } @@ -151,7 +151,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { Matrix preintMeasCov() const { return preintMeasCov_; } /// deprecated constructor - /// NOTE(frank): assumes FRD convention, only second order integration supported + /// NOTE(frank): assumes Z-Down convention, only second order integration supported PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, @@ -263,7 +263,7 @@ public: /// @deprecated constructor CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& pim, - const Vector3& b_gravity, const Vector3& omegaCoriolis, + const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); @@ -271,7 +271,7 @@ public: static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, CombinedPreintegratedMeasurements& pim, - const Vector3& b_gravity, const Vector3& omegaCoriolis, + const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 1b3736bec..1589f1a1b 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -116,7 +116,7 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements( if (!use2ndOrderIntegration) throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; - boost::shared_ptr p = Params::MakeSharedFRD(); + boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; p->accelerometerCovariance = measuredAccCovariance; p->integrationCovariance = integrationErrorCovariance; @@ -171,14 +171,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, //------------------------------------------------------------------------------ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& pim, const Vector3& b_gravity, + const PreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), _PIM_(pim) { boost::shared_ptr p = boost::make_shared< PreintegratedMeasurements::Params>(pim.p()); - p->b_gravity = b_gravity; + p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; p->use2ndOrderCoriolis = use2ndOrderCoriolis; @@ -188,10 +188,10 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, //------------------------------------------------------------------------------ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - PreintegratedMeasurements& pim, const Vector3& b_gravity, + PreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // use deprecated predict - PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity, + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, omegaCoriolis, use2ndOrderCoriolis); pose_j = pvb.pose; vel_j = pvb.velocity; diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c739120d3..f66d28828 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -108,7 +108,7 @@ public: Matrix preintMeasCov() const { return preintMeasCov_; } /// @deprecated constructor - /// NOTE(frank): assumes FRD convention, only second order integration supported + /// NOTE(frank): assumes Z-Down convention, only second order integration supported PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, @@ -210,14 +210,14 @@ public: /// @deprecated constructor ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& b_gravity, const Vector3& omegaCoriolis, + const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); /// @deprecated use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - PreintegratedMeasurements& pim, const Vector3& b_gravity, + PreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 8ce8a200b..989a300fe 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -248,7 +248,8 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, } //------------------------------------------------------------------------------ -Vector9 NavState::integrateTangent(const Vector9& pim, double dt, +Vector9 NavState::correctPIM(const Vector9& pim, double dt, + const Vector3& n_gravity, const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; @@ -256,11 +257,12 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt, const double dt2 = dt * dt; Vector9 delta; - Matrix3 D_dP_Ri, D_dP_nv; + Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; dR(delta) = dR(pim); dP(delta) = dP(pim) - + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri : 0, H2 ? &D_dP_nv : 0); - dV(delta) = dV(pim); + + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) + + (0.5 * dt2) * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); + dV(delta) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); if (omegaCoriolis) { delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); @@ -272,8 +274,9 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt, if (H1) { if (!omegaCoriolis) H1->setZero(); // if coriolis H1 is already initialized - D_t_R(H1) += dt * D_dP_Ri; + D_t_R(H1) += dt * D_dP_Ri1 + (0.5 * dt2) * D_dP_Ri2; D_t_v(H1) += dt * D_dP_nv * Ri; + D_v_R(H1) += dt * D_dV_Ri; } if (H2) { H2->setIdentity(); @@ -283,24 +286,5 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt, return delta; } //------------------------------------------------------------------------------ -NavState NavState::predict(const Vector9& pim, double dt, - const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis, - OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - - Matrix9 D_delta_state, D_delta_pim; - Vector9 delta = integrateTangent(pim, dt, omegaCoriolis, use2ndOrderCoriolis, - H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0); - - Matrix9 D_predicted_state, D_predicted_delta; - NavState predicted = retract(delta, H1 ? &D_predicted_state : 0, - H1 || H2 ? &D_predicted_delta : 0); - // TODO(frank): the derivatives of retract are very sparse: inline below: - if (H1) - *H1 = D_predicted_state + D_predicted_delta * D_delta_state; - if (H2) - *H2 = D_predicted_delta * D_delta_pim; - return predicted; -} -//------------------------------------------------------------------------------ }/// namespace gtsam diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 7326b8df7..8eb8c54d7 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -210,21 +210,13 @@ public: Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, OptionalJacobian<9, 9> H = boost::none) const; - /// Integrate a tangent vector forwards on tangent space, taking into account - /// Coriolis forces if omegaCoriolis is given. - Vector9 integrateTangent(const Vector9& pim, double dt, + /// Correct preintegrated tangent vector with our velocity and rotated gravity, + /// taking into account Coriolis forces if omegaCoriolis is given. + Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = false, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = boost::none) const; - /// Integrate a tangent vector forwards on tangent space, taking into account - /// Coriolis forces if omegaCoriolis is given. Calls retract after to yield a NavState - NavState predict(const Vector9& pim, double dt, - const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = - false, OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none) const; - /// @} - private: /// @{ /// serialization diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 2c4694e41..f45b0f49c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -157,15 +157,10 @@ NavState PreintegrationBase::predict(const NavState& state_i, // integrate on tangent space Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = state_i.integrateTangent(biasCorrected, deltaTij_, + Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0); - // Correct for gravity - double dt = deltaTij_, dt2 = dt * dt; - NavState::dP(xi) += 0.5 * p().b_gravity * dt2; - NavState::dV(xi) += p().b_gravity * dt; - // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); @@ -312,11 +307,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, //------------------------------------------------------------------------------ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& b_gravity, const Vector3& omegaCoriolis, + const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility boost::shared_ptr q = boost::make_shared(p()); - q->b_gravity = b_gravity; + q->n_gravity = n_gravity; q->omegaCoriolis = omegaCoriolis; q->use2ndOrderCoriolis = use2ndOrderCoriolis; p_ = q; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3079cd5c8..8182693ed 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -63,22 +63,22 @@ public: /// (to compensate errors in Euler integration) /// (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration - Vector3 b_gravity; ///< Gravity constant in body frame + Vector3 n_gravity; ///< Gravity constant in body frame - /// The Params constructor insists on the user passing in gravity in the *body* frame. + /// The Params constructor insists on getting the navigation frame gravity vector /// For convenience, two commonly used conventions are provided by named constructors below - Params(const Vector3& b_gravity) : + Params(const Vector3& n_gravity) : accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis( - false), b_gravity(b_gravity) { + false), n_gravity(n_gravity) { } - // Default Params for Front-Right-Down convention: gravity points along positive Z-axis - static boost::shared_ptr MakeSharedFRD(double g = 9.81) { + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { return boost::make_shared(Vector3(0, 0, g)); } - // Default Params for Front-Left-Up convention: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedFLU(double g = 9.81) { + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { return boost::make_shared(Vector3(0, 0, -g)); } @@ -94,7 +94,7 @@ public: ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); ar & BOOST_SERIALIZATION_NVP(integrationCovariance); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); - ar & BOOST_SERIALIZATION_NVP(b_gravity); + ar & BOOST_SERIALIZATION_NVP(n_gravity); } }; @@ -201,7 +201,7 @@ public: /// @deprecated predict PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, const Vector3& b_gravity, + const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index a2746f1ea..f433c2a9e 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -36,7 +36,7 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; -static const Vector3 kGravity(0, 0, 9.81); +static const Vector3 kGravityAlongNavZDown(0, 0, 9.81); static const Vector3 kZeroOmegaCoriolis(0, 0, 0); static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); @@ -151,8 +151,7 @@ TEST(ImuFactor, StraightLine) { imuBias::ConstantBias biasHat, bias; boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedFRD(); - p->b_gravity = Vector3(0, 0, g); // FRD convention + PreintegratedImuMeasurements::Params::MakeSharedU(g); // Up convention! PreintegratedImuMeasurements pim(p, biasHat); for (size_t i = 0; i < 10; i++) @@ -171,7 +170,6 @@ TEST(ImuFactor, StraightLine) { // Check prediction, note we move along x in body, along y in nav NavState expected(nRb, Point3(10, 20 + a * exact, 0), Velocity3(0, a * dt, 0)); - GTSAM_PRINT(pim); EXPECT(assert_equal(expected, pim.predict(state1, bias))); } @@ -197,12 +195,10 @@ TEST(ImuFactor, PreintegratedMeasurements) { kMeasuredOmegaCovariance, kIntegrationErrorCovariance); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT( - assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); - EXPECT( - assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); - EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); + EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()))); + EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()))); + EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()))); + DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); // Integrate again Vector3 expectedDeltaP2; @@ -216,39 +212,38 @@ TEST(ImuFactor, PreintegratedMeasurements) { PreintegratedImuMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT( - assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); - EXPECT( - assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); - EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); + EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()))); + EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()))); + EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()))); + DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9); } /* ************************************************************************* */ // Common linearization point and measurements for tests namespace common { -imuBias::ConstantBias bias; // Bias -Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), - Point3(5.0, 1.0, -50.0)); -Vector3 v1(Vector3(0.5, 0.0, 0.0)); - -NavState state1(x1, v1); -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 state2(x2, v2); +static const imuBias::ConstantBias biasHat, bias; // Bias +static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, 0)); +static const Vector3 v1(Vector3(0.5, 0.0, 0.0)); +static const NavState state1(x1, v1); // Measurements -Vector3 measuredOmega(M_PI / 100, 0, 0); -Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector(); -double deltaT = 1.0; +static const double w = M_PI / 100; +static const Vector3 measuredOmega(w, 0, 0); +static const Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown); +static const double deltaT = 1.0; + +static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0), + Point3(5.5, 1.0, 0)); +static const Vector3 v2(Vector3(0.5, 0.0, 0.0)); +static const NavState state2(x2, v2); } // namespace common /* ************************************************************************* */ TEST(ImuFactor, PreintegrationBaseMethods) { using namespace common; boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedFRD(); + PreintegratedImuMeasurements::Params::MakeSharedD(); p->gyroscopeCovariance = kMeasuredOmegaCovariance; p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); p->accelerometerCovariance = kMeasuredAccCovariance; @@ -285,22 +280,21 @@ TEST(ImuFactor, PreintegrationBaseMethods) { /* ************************************************************************* */ TEST(ImuFactor, ErrorAndJacobians) { using namespace common; - PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance, + PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6)); + EXPECT(assert_equal(state2, pim.predict(state1, bias))); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); // Expected error - Vector errorExpected(9); - errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + Vector expectedError(9); + expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT( - assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2, bias), - 1e-6)); + assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, bias))); Values values; values.insert(X(1), x1); @@ -308,7 +302,7 @@ TEST(ImuFactor, ErrorAndJacobians) { values.insert(X(2), x2); values.insert(V(2), v2); values.insert(B(1), bias); - EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6)); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); // Make sure linearization is correct double diffDelta = 1e-5; @@ -331,17 +325,17 @@ TEST(ImuFactor, ErrorAndJacobians) { // Evaluate error with wrong values Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); values.update(V(2), v2_wrong); - errorExpected << 0, 0, 0, 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901; + expectedError << 0, 0, 0, 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901; EXPECT( - assert_equal(errorExpected, - factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6)); - EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6)); + assert_equal(expectedError, + factor.evaluateError(x1, v1, x2, v2_wrong, bias))); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); // Make sure the whitening is done correctly Matrix cov = pim.preintMeasCov(); Matrix R = RtR(cov.inverse()); - Vector whitened = R * errorExpected; - EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6)); + Vector whitened = R * expectedError; + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values),1e-5)); // Make sure linearization is correct EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-5); @@ -359,7 +353,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { // Measurements Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; @@ -370,7 +364,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, kNonZeroOmegaCoriolis); Values values; @@ -397,7 +391,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { // Measurements Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; @@ -410,7 +404,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { // Create factor Pose3 bodyPsensor = Pose3(); bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, kNonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis); Values values; @@ -808,7 +802,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // Measurements Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector() + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; @@ -823,7 +817,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, kNonZeroOmegaCoriolis); Values values; @@ -858,13 +852,13 @@ TEST(ImuFactor, PredictPositionAndVelocity) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravity, + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravityAlongNavZDown, kZeroOmegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; @@ -893,14 +887,14 @@ TEST(ImuFactor, PredictRotation) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); // Predict Pose3 x1, x2; Vector3 v1 = Vector3(0, 0.0, 0.0); Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravity, kZeroOmegaCoriolis); + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 0, 0; @@ -926,14 +920,14 @@ TEST(ImuFactor, PredictArbitrary) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity, + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); // Predict Pose3 x1, x2; Vector3 v1 = Vector3(0, 0.0, 0.0); Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravity, kZeroOmegaCoriolis); + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); // Regression test for Imu Refactor Rot3 expectedR( // diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index e5f25ffa5..ca6c2ffc1 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -149,55 +149,41 @@ boost::function coriolis = boost::bind( &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); TEST(NavState, Coriolis) { - Matrix9 actualH; + Matrix9 aH; // first-order - kState1.coriolis(dt, kOmegaCoriolis, false, actualH); - EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), actualH)); + kState1.coriolis(dt, kOmegaCoriolis, false, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH)); // second-order - kState1.coriolis(dt, kOmegaCoriolis, true, actualH); - EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), actualH)); + kState1.coriolis(dt, kOmegaCoriolis, true, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH)); } TEST(NavState, Coriolis2) { - Matrix9 actualH; + Matrix9 aH; NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); // first-order - state2.coriolis(dt, kOmegaCoriolis, false, actualH); - EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), actualH)); + state2.coriolis(dt, kOmegaCoriolis, false, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH)); // second-order - state2.coriolis(dt, kOmegaCoriolis, true, actualH); - EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), actualH)); + state2.coriolis(dt, kOmegaCoriolis, true, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH)); } /* ************************************************************************* */ -TEST(NavState, PredictXi) { +TEST(NavState, correctPIM) { Vector9 xi; xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; double dt = 0.5; - Matrix9 actualH1, actualH2; - boost::function integrateTangent = - boost::bind(&NavState::integrateTangent, _1, _2, dt, kGravity, kOmegaCoriolis, + Matrix9 aH1, aH2; + boost::function correctPIM = + boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis, false, boost::none, boost::none); - kState1.integrateTangent(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2); - EXPECT(assert_equal(numericalDerivative21(integrateTangent, kState1, xi), actualH1)); - EXPECT(assert_equal(numericalDerivative22(integrateTangent, kState1, xi), actualH2)); -} - -/* ************************************************************************* */ -TEST(NavState, Predict) { - Vector9 xi; - xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; - double dt = 0.5; - Matrix9 actualH1, actualH2; - boost::function predict = - boost::bind(&NavState::predict, _1, _2, dt, kGravity, kOmegaCoriolis, - false, boost::none, boost::none); - kState1.predict(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2); - EXPECT(assert_equal(numericalDerivative21(predict, kState1, xi), actualH1)); - EXPECT(assert_equal(numericalDerivative22(predict, kState1, xi), actualH2)); + kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); + EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); } /* ************************************************************************* */