diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index bdad84e6b..5a46c5da3 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -81,10 +81,10 @@ void PreintegratedImuMeasurements::integrateMeasurement( // NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise, // as G and measurementCovariance are block-diagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT; - D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT; D_R_R(&preintMeasCov_) += D_incrR_integratedOmega * p().gyroscopeCovariance * D_incrR_integratedOmega.transpose() * deltaT; + D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT; + D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT; Matrix3 F_pos_noiseacc; F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 3a5d6d559..6353eb16c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -28,7 +28,9 @@ namespace gtsam { /// Re-initialize PreintegratedMeasurements void PreintegrationBase::resetIntegration() { - PreintegratedRotation::resetIntegration(); + deltaTij_ = 0.0; + deltaRij_ = Rot3(); + delRdelBiasOmega_ = Z_3x3; deltaPij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero(); delPdelBiasAcc_ = Z_3x3; @@ -39,7 +41,9 @@ void PreintegrationBase::resetIntegration() { /// Needed for testable void PreintegrationBase::print(const string& s) const { - PreintegratedRotation::print(s); + cout << s << endl; + cout << " deltaTij [" << deltaTij_ << "]" << endl; + cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl; cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; biasHat_.print(" biasHat"); @@ -48,7 +52,9 @@ void PreintegrationBase::print(const string& s) const { /// Needed for testable bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - return PreintegratedRotation::equals(other, tol) + return deltaRij_.equals(other.deltaRij_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) @@ -130,7 +136,9 @@ Vector9 PreintegrationBase::biasCorrectedDelta( // Correct deltaRij, derivative is delRdelBiasOmega_ const imuBias::ConstantBias biasIncr = bias_i - biasHat_; Matrix3 D_deltaRij_bias; - Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0); + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 deltaRij = deltaRij_.expmap(biasInducedOmega, boost::none, H ? &D_deltaRij_bias : 0); + if (H) D_deltaRij_bias *= delRdelBiasOmega_; Vector9 xi; Matrix3 D_dR_deltaRij; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f3d8a3ff2..bf01db00a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -51,7 +51,7 @@ struct PoseVelocityBias { * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase: public PreintegratedRotation { +class PreintegrationBase { public: @@ -100,6 +100,13 @@ public: protected: + double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + /// Parameters + boost::shared_ptr p_; + /// Acceleration and gyro bias used for preintegration imuBias::ConstantBias biasHat_; @@ -124,7 +131,7 @@ public: */ PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat) : - PreintegratedRotation(p), biasHat_(biasHat) { + p_(p), biasHat_(biasHat) { resetIntegration(); } @@ -136,6 +143,15 @@ public: } /// getters + const double& deltaTij() const { + return deltaTij_; + } + const Rot3& deltaRij() const { + return deltaRij_; + } + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; + } const imuBias::ConstantBias& biasHat() const { return biasHat_; } @@ -202,8 +218,10 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(deltaPij_); ar & BOOST_SERIALIZATION_NVP(deltaVij_); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index ca01de3ec..176379da4 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -364,15 +364,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Make sure of biascorrectedDeltaRij with this example... - Matrix3 aH; - pim.biascorrectedDeltaRij(bias.gyroscope(), aH); - Matrix3 eH = numericalDerivative11( - boost::bind(&PreintegrationBase::biascorrectedDeltaRij, pim, _1, - boost::none), bias.gyroscope()); - EXPECT(assert_equal(eH, aH)); - - // ... and of biasCorrectedDelta + // Make sure of biasCorrectedDelta Matrix96 actualH; pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( @@ -803,11 +795,14 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim, const NavState& state, const imuBias::ConstantBias& bias, double dt, const Pose3& body_P_sensor, const Vector3& measuredAcc, - const Vector3& measuredOmega, size_t N = 1000) { + const Vector3& measuredOmega, size_t N = 100) { // Get mean prediction from "ground truth" measurements PreintegratedImuMeasurements pim1 = pim; - pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); + for (size_t k=0;k<10;k++) + pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); NavState mean = pim1.predict(state, bias); + cout << "pim1.preintMeasCov():" << endl; + cout << pim1.preintMeasCov() << endl; // Do a Monte Carlo analysis to determine empirical density on the predicted state Sampler sampleAccelerationNoise(Vector3::Constant(sqrt(accNoiseVar))); @@ -818,18 +813,23 @@ Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim, PreintegratedImuMeasurements pim2 = pim; Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); - pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor); + for (size_t k=0;k<10;k++) + pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, body_P_sensor); NavState prediction = pim2.predict(state, bias); samples.col(i) = mean.localCoordinates(prediction); sum += samples.col(i); } Vector9 sampleMean = sum / N; + cout << ":" << endl; + cout << sampleMean << endl; Matrix9 Q; Q.setZero(); for (size_t i = 0; i < N; i++) { Vector9 xi = samples.col(i) - sampleMean; Q += xi * xi.transpose() / (N - 1); } + cout << "Q:" << endl; + cout << Q << endl; return Q; } @@ -857,8 +857,6 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, dt, body_P_sensor, measuredAcc, measuredOmega, 1000); - cout << Q << endl; - Matrix expected(9,9); expected << @@ -993,14 +991,10 @@ TEST(ImuFactor, PredictArbitrary) { kIntegrationErrorCovariance, true); Pose3 x1; Vector3 v1 = Vector3(0, 0, 0); - Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, dt, Pose3(), + Matrix9 Q = MonteCarlo(pim, NavState(x1, v1), biasHat, 0.1, Pose3(), measuredAcc, measuredOmega, 1000); - cout << Q << endl; - pim.integrateMeasurement(measuredAcc, measuredOmega, dt); - cout << pim.preintMeasCov() << endl; - - for (int i = 0; i < 999; ++i) + for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, dt); Matrix expected(9,9);