From c20bacf025485301117791311a63b6195d9d954c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 17 Jan 2016 21:31:29 -0800 Subject: [PATCH] Fixed equals --- gtsam/navigation/ImuFactor.cpp | 20 ++-- gtsam/navigation/ImuFactor.h | 4 +- gtsam/navigation/PreintegratedRotation.cpp | 34 ++++-- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/PreintegrationBase.cpp | 99 ++++++++++------- gtsam/navigation/PreintegrationBase.h | 2 + gtsam/navigation/tests/testImuFactor.cpp | 120 +++++++++++---------- 7 files changed, 167 insertions(+), 114 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 673706d58..692154c9d 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -73,8 +73,8 @@ void PreintegratedImuMeasurements::integrateMeasurement( G << G1, Gi, G2; Matrix9 Cov; Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3, - Z_3x3, p().integrationCovariance * dt, Z_3x3, - Z_3x3, Z_3x3, p().gyroscopeCovariance / dt; + Z_3x3, p().integrationCovariance * dt, Z_3x3, + Z_3x3, Z_3x3, p().gyroscopeCovariance / dt; preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose(); #else preintMeasCov_ = F * preintMeasCov_ * F.transpose() @@ -91,7 +91,7 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements( const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) { if (!use2ndOrderIntegration) - throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; @@ -141,8 +141,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { //------------------------------------------------------------------------------ bool ImuFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) - && Base::equals(*e, tol); + const bool base = Base::equals(*e, tol); + const bool pim = _PIM_.equals(e->_PIM_, tol); + return e != nullptr && base && pim; } //------------------------------------------------------------------------------ @@ -161,10 +162,10 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedImuMeasurements& 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) { +Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { boost::shared_ptr p = boost::make_shared< - PreintegratedImuMeasurements::Params>(pim.p()); + PreintegratedImuMeasurements::Params>(pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; @@ -185,4 +186,5 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, #endif //------------------------------------------------------------------------------ -} // namespace gtsam +} + // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 97756b0b9..f52d95b26 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -71,7 +71,9 @@ protected: ///< (first-order propagation from *measurementCovariance*). /// Default constructor for serialization - PreintegratedImuMeasurements() {} + PreintegratedImuMeasurements() { + preintMeasCov_.setZero(); + } public: diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 9d623bf38..df38df0b8 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -29,12 +29,26 @@ void PreintegratedRotation::Params::print(const string& s) const { cout << s << endl; cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; if (omegaCoriolis) - cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" - << endl; + cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl; if (body_P_sensor) body_P_sensor->print("body_P_sensor"); } +bool PreintegratedRotation::Params::equals( + const PreintegratedRotation::Params& other, double tol) const { + if (body_P_sensor) { + if (!other.body_P_sensor + || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol)) + return false; + } + if (omegaCoriolis) { + if (!other.omegaCoriolis + || !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol)) + return false; + } + return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol); +} + void PreintegratedRotation::resetIntegration() { deltaTij_ = 0.0; deltaRij_ = Rot3(); @@ -49,8 +63,7 @@ void PreintegratedRotation::print(const string& s) const { bool PreintegratedRotation::equals(const PreintegratedRotation& other, double tol) const { - return this->matchesParamsWith(other) - && deltaRij_.equals(other.deltaRij_, tol) + return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } @@ -76,11 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! } -void PreintegratedRotation::integrateMeasurement( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { +void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, + OptionalJacobian<3, 3> F) { Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, D_incrR_integratedOmega); + const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, + D_incrR_integratedOmega); // If asked, pass first derivative as well if (optional_D_incrR_integratedOmega) { @@ -93,7 +108,8 @@ void PreintegratedRotation::integrateMeasurement( // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - D_incrR_integratedOmega * deltaT; } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index afedaaa89..b170ea863 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -43,6 +43,7 @@ class PreintegratedRotation { Params() : gyroscopeCovariance(I_3x3) {} virtual void print(const std::string& s) const; + virtual bool equals(const Params& other, double tol=1e-9) const; private: /** Serialization function */ @@ -53,7 +54,6 @@ class PreintegratedRotation { ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); - ar& BOOST_SERIALIZATION_NVP(body_P_sensor); } }; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 2372b2ee2..626dcdf70 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -31,7 +31,7 @@ void PreintegrationBase::Params::print(const string& s) const { PreintegratedRotation::Params::print(s); cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" << endl; - cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]" + cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]" << endl; if (omegaCoriolis && use2ndOrderCoriolis) cout << "Using 2nd-order Coriolis" << endl; @@ -40,6 +40,18 @@ void PreintegrationBase::Params::print(const string& s) const { cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; } +//------------------------------------------------------------------------------ +bool PreintegrationBase::Params::equals( + const PreintegratedRotation::Params& other, double tol) const { + auto e = dynamic_cast(&other); + return e != nullptr && PreintegratedRotation::Params::equals(other, tol) + && use2ndOrderCoriolis == e->use2ndOrderCoriolis + && equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance, + tol) + && equal_with_abs_tol(integrationCovariance, e->integrationCovariance, + tol) && equal_with_abs_tol(n_gravity, e->n_gravity, tol); +} + //------------------------------------------------------------------------------ void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; @@ -64,8 +76,8 @@ void PreintegrationBase::print(const string& s) const { //------------------------------------------------------------------------------ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - return this->matchesParamsWith(other) - && fabs(deltaTij_ - other.deltaTij_) < tol + const bool params_match = p_->equals(*other.p_, tol); + return params_match && fabs(deltaTij_ - other.deltaTij_) < tol && deltaXij_.equals(other.deltaXij_, tol) && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) @@ -82,13 +94,13 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { - // Correct for bias in the sensor frame +// Correct for bias in the sensor frame Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc); Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega); - // Compensate for sensor-body displacement if needed: we express the quantities - // (originally in the IMU frame) into the body frame - // Equations below assume the "body" frame is the CG +// Compensate for sensor-body displacement if needed: we express the quantities +// (originally in the IMU frame) into the body frame +// Equations below assume the "body" frame is the CG if (p().body_P_sensor) { // Correct omega to rotation rate vector in the body frame const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); @@ -98,9 +110,12 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos j_correctedAcc = bRs * j_correctedAcc; // Jacobians - if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs; - if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero(); - if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs; + if (D_correctedAcc_measuredAcc) + *D_correctedAcc_measuredAcc = bRs; + if (D_correctedAcc_measuredOmega) + *D_correctedAcc_measuredOmega = Matrix3::Zero(); + if (D_correctedOmega_measuredOmega) + *D_correctedOmega_measuredOmega = bRs; // Centrifugal acceleration const Vector3 b_arm = p().body_P_sensor->translation().vector(); @@ -120,7 +135,7 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos } } - // Do update in one fell swoop +// Do update in one fell swoop return make_pair(j_correctedAcc, j_correctedOmega); } @@ -135,22 +150,27 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc, Matrix3 D_correctedAcc_measuredAcc, // D_correctedAcc_measuredOmega, // D_correctedOmega_measuredOmega; - bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor; + bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega + && p().body_P_sensor; boost::tie(j_correctedAcc, j_correctedOmega) = correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega, (needDerivs ? &D_correctedAcc_measuredAcc : 0), (needDerivs ? &D_correctedAcc_measuredOmega : 0), (needDerivs ? &D_correctedOmega_measuredOmega : 0)); - // Do update in one fell swoop +// Do update in one fell swoop Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; - NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current, - (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), - (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); + NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, + D_updated_current, + (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), + (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); if (needDerivs) { - *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc; - *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega; + *D_updated_measuredAcc = D_updated_correctedAcc + * D_correctedAcc_measuredAcc; + *D_updated_measuredOmega = D_updated_correctedOmega + * D_correctedOmega_measuredOmega; if (!p().body_P_sensor->translation().vector().isZero()) { - *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega; + *D_updated_measuredOmega += D_updated_correctedAcc + * D_correctedAcc_measuredOmega; } } return updated; @@ -162,16 +182,16 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { - // Save current rotation for updating Jacobians +// Save current rotation for updating Jacobians const Rot3 oldRij = deltaXij_.attitude(); - // Do update +// Do update deltaTij_ += dt; deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional - // Update Jacobians - // TODO(frank): we are repeating some computation here: accessible in F ? +// Update Jacobians +// TODO(frank): we are repeating some computation here: accessible in F ? Vector3 j_correctedAcc, j_correctedOmega; boost::tie(j_correctedAcc, j_correctedOmega) = correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega); @@ -197,7 +217,7 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc, //------------------------------------------------------------------------------ Vector9 PreintegrationBase::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // Correct deltaRij, derivative is delRdelBiasOmega_ +// Correct deltaRij, derivative is delRdelBiasOmega_ const imuBias::ConstantBias biasIncr = bias_i - biasHat_; Matrix3 D_correctedRij_bias; const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); @@ -208,8 +228,8 @@ Vector9 PreintegrationBase::biasCorrectedDelta( Vector9 xi; Matrix3 D_dR_correctedRij; - // TODO(frank): could line below be simplified? It is equivalent to - // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) +// TODO(frank): could line below be simplified? It is equivalent to +// LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + delPdelBiasOmega_ * biasIncr.gyroscope(); @@ -230,18 +250,18 @@ Vector9 PreintegrationBase::biasCorrectedDelta( NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { - // correct for bias +// correct for bias Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); - // integrate on tangent space +// integrate on tangent space Matrix9 D_delta_state, D_delta_biasCorrected; Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0); - // Use retract to get back to NavState manifold +// 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); if (H1) @@ -258,12 +278,12 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { - // Note that derivative of constructors below is not identity for velocity, but - // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() +// Note that derivative of constructors below is not identity for velocity, but +// a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() NavState state_i(pose_i, vel_i); NavState state_j(pose_j, vel_j); - /// Predict state at time j +/// Predict state at time j Matrix99 D_predict_state_i; Matrix96 D_predict_bias_i; NavState predictedState_j = predict(state_i, bias_i, @@ -273,11 +293,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, Vector9 error = state_j.localCoordinates(predictedState_j, H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0); - // Separate out derivatives in terms of 5 arguments - // Note that doing so requires special treatment of velocities, as when treated as - // separate variables the retract applied will not be the semi-direct product in NavState - // Instead, the velocities in nav are updated using a straight addition - // This is difference is accounted for by the R().transpose calls below +// Separate out derivatives in terms of 5 arguments +// Note that doing so requires special treatment of velocities, as when treated as +// separate variables the retract applied will not be the semi-direct product in NavState +// Instead, the velocities in nav are updated using a straight addition +// This is difference is accounted for by the R().transpose calls below if (H1) *H1 << D_error_predict * D_predict_state_i.leftCols<6>(); if (H2) @@ -300,7 +320,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) const { - // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility +// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility boost::shared_ptr q = boost::make_shared(p()); q->n_gravity = n_gravity; q->omegaCoriolis = omegaCoriolis; @@ -311,4 +331,5 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, #endif //------------------------------------------------------------------------------ -}/// namespace gtsam +} + /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 74f22f8d5..f5e8c7183 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -83,6 +83,7 @@ public: } void print(const std::string& s) const; + bool equals(const PreintegratedRotation::Params& other, double tol) const; protected: /// Default constructor for serialization only: uninitialized! @@ -132,6 +133,7 @@ protected: /// Default constructor for serialization PreintegrationBase() { + resetIntegration(); } public: diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 6d7c84e54..a92d0737f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -65,12 +65,12 @@ static const double kAccelerometerSigma = 0.1; static boost::shared_ptr defaultParams() { auto p = PreintegratedImuMeasurements::Params::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; - p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma * I_3x3; + p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma + * I_3x3; p->integrationCovariance = 0.0001 * I_3x3; return p; } - // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ @@ -123,7 +123,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { /* ************************************************************************* */ TEST(ImuFactor, Accelerating) { - const double a = 0.2, v=50; + const double a = 0.2, v = 50; // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X // The body itself has Z axis pointing down @@ -132,9 +132,9 @@ TEST(ImuFactor, Accelerating) { const Vector3 initial_velocity(v, 0, 0); const AcceleratingScenario scenario(nRb, initial_position, initial_velocity, - Vector3(a, 0, 0)); + Vector3(a, 0, 0)); - const double T = 3.0; // seconds + const double T = 3.0; // seconds ScenarioRunner runner(&scenario, defaultParams(), T / 10); PreintegratedImuMeasurements pim = runner.integrate(T); @@ -144,17 +144,20 @@ TEST(ImuFactor, Accelerating) { EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); // Check G1 and G2 derivatives of pim.update - Matrix93 aG1,aG2; - boost::function f = - boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10, - boost::none, boost::none, boost::none); + Matrix93 aG1, aG2; + boost::function f = boost::bind( + &PreintegrationBase::updatedDeltaXij, pim, _1, _2, T / 10, boost::none, + boost::none, boost::none); const Vector3 measuredAcc = runner.actualSpecificForce(T); const Vector3 measuredOmega = runner.actualAngularVelocity(T); - pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2); - EXPECT(assert_equal( - numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); - EXPECT(assert_equal( - numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); + pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, + aG2); + EXPECT( + assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), + aG1, 1e-7)); + EXPECT( + assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), + aG2, 1e-7)); } /* ************************************************************************* */ @@ -268,7 +271,8 @@ TEST(ImuFactor, ErrorAndJacobians) { Vector expectedError(9); expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT( - assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias))); + assert_equal(expectedError, + factor.evaluateError(x1, v1, x2, v2, kZeroBias))); Values values; values.insert(X(1), x1); @@ -284,16 +288,19 @@ TEST(ImuFactor, ErrorAndJacobians) { // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a); + (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, + H5a); // Make sure rotation part is correct when error is interpreted as axis-angle // Jacobians are around zero, so the rotation part is the same as: Matrix H1Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1); + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), + x1); EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2); + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), + x2); EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); // Evaluate error with wrong values @@ -386,7 +393,6 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - bool use2ndOrderCoriolis = true; ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); Values values; @@ -533,7 +539,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { /* ************************************************************************* */ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, - const Vector3& measuredAcc, const Vector3& measuredOmega) { + const Vector3& measuredAcc, const Vector3& measuredOmega) { return pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega).first; } @@ -544,15 +550,16 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { const Vector3 a = nRb * Vector3(0.2, 0.0, 0.0); const AcceleratingScenario scenario(nRb, p1, v1, a, - Vector3(0, 0, M_PI / 10.0 + 0.3)); + Vector3(0, 0, M_PI / 10.0 + 0.3)); auto p = defaultParams(); - p->body_P_sensor =Pose3(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01)); + p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)), + Point3(0.1, 0.05, 0.01)); p->omegaCoriolis = kNonZeroOmegaCoriolis; imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); - const double T = 3.0; // seconds + const double T = 3.0; // seconds ScenarioRunner runner(&scenario, p, T / 10); // PreintegratedImuMeasurements pim = runner.integrate(T); @@ -575,32 +582,34 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // Check updatedDeltaXij derivatives Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero(); - pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); - Matrix3 expectedD = numericalDerivative11(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); + pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + boost::none, D_correctedAcc_measuredOmega, boost::none); + Matrix3 expectedD = numericalDerivative11( + boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); Matrix93 G1, G2; double dt = 0.1; - NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); + NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, + boost::none, G1, G2); // Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose(); Matrix93 expectedG1 = numericalDerivative21( boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, - dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, - 1e-6); + dt, boost::none, boost::none, boost::none), measuredAcc, + measuredOmega, 1e-6); EXPECT(assert_equal(expectedG1, G1, 1e-5)); Matrix93 expectedG2 = numericalDerivative22( boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, - dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, - 1e-6); + dt, boost::none, boost::none, boost::none), measuredAcc, + measuredOmega, 1e-6); EXPECT(assert_equal(expectedG2, G2, 1e-5)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) // EXPECT(MonteCarlo(pim, NavState(x1, initial_velocity), bias, dt, body_P_sensor, // measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000)); - // integrate at least twice to get position information // otherwise factor cov noise from preint_cov is not positive definite pim.integrateMeasurement(measuredAcc, measuredOmega, dt); @@ -687,10 +696,9 @@ TEST(ImuFactor, PredictArbitrary) { const Vector3 v1(0, 0, 0); const AcceleratingScenario scenario(x1.rotation(), x1.translation(), v1, - Vector3(0.1, 0.2, 0), - Vector3(M_PI / 10, M_PI / 10, M_PI / 10)); + Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10)); - const double T = 3.0; // seconds + const double T = 3.0; // seconds ScenarioRunner runner(&scenario, defaultParams(), T / 10); // // PreintegratedImuMeasurements pim = runner.integrate(T); @@ -707,7 +715,7 @@ TEST(ImuFactor, PredictArbitrary) { Vector3 measuredAcc = runner.actualSpecificForce(0); auto p = defaultParams(); - p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise + p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise PreintegratedImuMeasurements pim(p, biasHat); imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, @@ -724,9 +732,9 @@ TEST(ImuFactor, PredictArbitrary) { NavState actual = pim.predict(NavState(x1, v1), bias); // Regression test for Imu Refactor - Rot3 expectedR( // - +0.903715275, -0.250741668, 0.347026393, // - +0.347026393, 0.903715275, -0.250741668, // + Rot3 expectedR( // + +0.903715275, -0.250741668, 0.347026393, // + +0.347026393, 0.903715275, -0.250741668, // -0.250741668, 0.347026393, 0.903715275); Point3 expectedT(-0.516077031, 0.57842919, 0.0876478403); Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571); @@ -740,7 +748,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { // Rotate sensor (z-down) to body (same as navigation) i.e. z-up auto p = defaultParams(); - p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame + p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); // Measurements @@ -836,8 +844,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { // Now add IMU factors and bias noise models Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); for (int i = 1; i < numFactors; i++) { - PreintegratedImuMeasurements pim = - PreintegratedImuMeasurements(p, priorBias); + PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p, + priorBias); for (int j = 0; j < 200; ++j) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -862,28 +870,31 @@ TEST(ImuFactor, bodyPSensorWithBias) { /* ************************************************************************** */ #include -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, + "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, + "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, + "gtsam_noiseModel_Gaussian"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, + "gtsam_noiseModel_Isotropic"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); TEST(ImuFactor, serialization) { using namespace gtsam::serializationTestHelpers; - Vector3 n_gravity(0, 0, -9.81); - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); - Matrix3 accCov = 1e-7 * I_3x3; - Matrix3 gyroCov = 1e-8 * I_3x3; - Matrix3 integrationCov = 1e-9 * I_3x3; + auto p = defaultParams(); + p->n_gravity = Vector3(0, 0, -9.81); + p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3()); + p->accelerometerCovariance = 1e-7 * I_3x3; + p->gyroscopeCovariance = 1e-8 * I_3x3; + p->integrationCovariance = 1e-9 * I_3x3; double deltaT = 0.005; imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) - ImuFactor::PreintegratedMeasurements pim = - ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, - integrationCov, true); + PreintegratedImuMeasurements pim(p, priorBias); // measurements are needed for non-inf noise model, otherwise will throw err when deserialize @@ -895,8 +906,7 @@ TEST(ImuFactor, serialization) { Vector3 measuredAcc(0, 0, -9.81); for (int j = 0; j < 200; ++j) - pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, - body_P_sensor); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);