From 325ede23fe8bc98bcffe41b916c6886a08fae1f2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 31 Jul 2015 15:08:12 -0700 Subject: [PATCH] BIG: switch to NavState delta pose --- gtsam/navigation/ImuFactor.cpp | 45 +++------ gtsam/navigation/ImuFactor.h | 10 +- gtsam/navigation/NavState.cpp | 6 +- gtsam/navigation/PreintegrationBase.cpp | 90 ++++++++---------- gtsam/navigation/PreintegrationBase.h | 43 ++++----- gtsam/navigation/tests/testImuFactor.cpp | 115 +++++++++++------------ 6 files changed, 135 insertions(+), 174 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 5a46c5da3..2f5861de5 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -61,16 +61,17 @@ void PreintegratedImuMeasurements::resetIntegration() { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, + OptionalJacobian<9, 9> outF, OptionalJacobian<9, 9> G) { - const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test + static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); // Update preintegrated measurements (also get Jacobian) Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) - updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT, - &D_incrR_integratedOmega, &F); + Matrix93 G1, G2; + updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt, + &D_incrR_integratedOmega, &F, &G1, &G2); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF @@ -78,35 +79,13 @@ void PreintegratedImuMeasurements::integrateMeasurement( // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G' // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // 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_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; + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + + G1 * (p().accelerometerCovariance / dt) * G1.transpose() + + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) + + G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); - Matrix3 F_pos_noiseacc; - F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; - D_t_t(&preintMeasCov_) += (1 / deltaT) * F_pos_noiseacc - * p().accelerometerCovariance * F_pos_noiseacc.transpose(); - Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance - * dRij.transpose(); // has 1/deltaT - D_t_v(&preintMeasCov_) += temp; - D_v_t(&preintMeasCov_) += temp.transpose(); - - // F_test and G_test are given as output for testing purposes and are not needed by the factor - if (F_test) { - (*F_test) << F; - } - if (G_test) { - // This in only for testing & documentation, while the actual computation is done block-wise - // omegaNoise intNoise accNoise - (*G_test) << - D_incrR_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle - Z_3x3, I_3x3 * deltaT, F_pos_noiseacc, // pos - Z_3x3, Z_3x3, dRij * deltaT; // vel - } + if (outF) *outF = F; + if (G) *G << G2, Gi*dt, G1; // NOTE(frank): order here is R,P,V } //------------------------------------------------------------------------------ PreintegratedImuMeasurements::PreintegratedImuMeasurements( diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 32ee4185e..1d32623bd 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -96,13 +96,13 @@ public: * Add a single IMU measurement to the preintegration. * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor) - * @param deltaT Time interval between this and the last IMU measurement - * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) - * @param Fout, Gout Jacobians used internally (only needed for testing) + * @param dt Time interval between this and the last IMU measurement + * @param F, F Jacobians used internally (only needed for testing) */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT, - OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); + const Vector3& measuredOmega, double dt, // + OptionalJacobian<9, 9> F = boost::none, // + OptionalJacobian<9, 9> G = boost::none); /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 2e7917688..6266c328f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -322,14 +322,14 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; const Velocity3& n_v = v_; // derivative is Ri ! - const double dt2 = dt * dt; + const double dt22 = 0.5 * dt * dt; Vector9 xi; Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; dR(xi) = dR(pim); dP(xi) = dP(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); + + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); if (omegaCoriolis) { @@ -342,7 +342,7 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, if (H1) { if (!omegaCoriolis) H1->setZero(); // if coriolis H1 is already initialized - D_t_R(H1) += dt * D_dP_Ri1 + (0.5 * dt2) * D_dP_Ri2; + D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2; D_t_v(H1) += dt * D_dP_nv * Ri; D_v_R(H1) += dt * D_dV_Ri; } diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 6353eb16c..2373c474e 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -29,10 +29,8 @@ namespace gtsam { /// Re-initialize PreintegratedMeasurements void PreintegrationBase::resetIntegration() { deltaTij_ = 0.0; - deltaRij_ = Rot3(); + deltaXij_ = NavState(); delRdelBiasOmega_ = Z_3x3; - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); delPdelBiasAcc_ = Z_3x3; delPdelBiasOmega_ = Z_3x3; delVdelBiasAcc_ = Z_3x3; @@ -43,21 +41,19 @@ void PreintegrationBase::resetIntegration() { void PreintegrationBase::print(const string& s) const { cout << s << endl; cout << " deltaTij [" << deltaTij_ << "]" << endl; - cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl; - cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; + cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl; + cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl; biasHat_.print(" biasHat"); } /// Needed for testable bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - return deltaRij_.equals(other.deltaRij_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + return fabs(deltaTij_ - other.deltaTij_) < tol + && deltaXij_.equals(other.deltaXij_, tol) && biasHat_.equals(other.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) @@ -65,8 +61,8 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, } void PreintegrationBase::updatePreintegratedMeasurements( - const Vector3& measuredAcc, const Vector3& measuredOmega, - const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F) { + const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) { // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). @@ -91,43 +87,31 @@ void PreintegrationBase::updatePreintegratedMeasurements( correctedAcc = bRs * correctedAcc - correctedOmega.cross(b_velocity_bs); } - // Calculate acceleration in *current* i frame, i.e., before rotation update below + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update in one fell swoop + deltaTij_ += dt; + deltaXij_ = deltaXij_.update(correctedAcc, correctedOmega, dt, F, G1, G2); + + // Update Jacobians + // TODO(frank): we are repeating some computation here: accessible in F ? Matrix3 D_acc_R; - const Matrix3 dRij = deltaRij_.matrix(); // expensive - const Vector3 i_acc = deltaRij_.rotate(correctedAcc, D_acc_R); + oldRij.rotate(correctedAcc, D_acc_R); const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; -// NavState iTj(deltaRij_, deltaPij_, deltaVij_); -// iTj = iTj.update(); - - // rotation vector describing rotation increment computed from the - // current rotation rate measurement - const Vector3 integratedOmega = correctedOmega * deltaT; - const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! - - // Update deltaTij and rotation - deltaTij_ += deltaT; - Matrix3 D_Rij_incrR; - deltaRij_ = deltaRij_.compose(incrR, D_Rij_incrR); - - // Update Jacobian + const Vector3 integratedOmega = correctedOmega * dt; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! const Matrix3 incrRt = incrR.transpose(); delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - *D_incrR_integratedOmega * deltaT; + - *D_incrR_integratedOmega * dt; - double dt22 = 0.5 * deltaT * deltaT; - deltaPij_ += dt22 * i_acc + deltaT * deltaVij_; - deltaVij_ += deltaT * i_acc; - - *F << // angle pos vel - D_Rij_incrR, Z_3x3, Z_3x3, // angle - dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos - deltaT * D_acc_R, Z_3x3, I_3x3; // vel - - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - dt22 * dRij; - delPdelBiasOmega_ += deltaT * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; - delVdelBiasAcc_ += -dRij * deltaT; - delVdelBiasOmega_ += D_acc_biasOmega * deltaT; + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; } //------------------------------------------------------------------------------ @@ -135,24 +119,26 @@ Vector9 PreintegrationBase::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { // Correct deltaRij, derivative is delRdelBiasOmega_ const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - Matrix3 D_deltaRij_bias; + Matrix3 D_correctedRij_bias; 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_; + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; Vector9 xi; - Matrix3 D_dR_deltaRij; + Matrix3 D_dR_correctedRij; // TODO(frank): could line below be simplified? It is equivalent to // LogMap(deltaRij_.compose(Expmap(delRdelBiasOmega_ * biasIncr.gyroscope()))) - NavState::dR(xi) = Rot3::Logmap(deltaRij, H ? &D_dR_deltaRij : 0); - NavState::dP(xi) = deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer() + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + delPdelBiasOmega_ * biasIncr.gyroscope(); - NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer() + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + delVdelBiasOmega_ * biasIncr.gyroscope(); if (H) { Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; - D_dR_bias << Z_3x3, D_dR_deltaRij * D_deltaRij_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; (*H) << D_dR_bias, D_dP_bias, D_dV_bias; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index bf01db00a..6b4626654 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -100,9 +100,14 @@ 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 + double deltaTij_; ///< Time interval from i to j + + /** + * Preintegrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; /// Parameters boost::shared_ptr p_; @@ -110,12 +115,10 @@ protected: /// Acceleration and gyro bias used for preintegration imuBias::ConstantBias biasHat_; - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias /// Default constructor for serialization @@ -147,19 +150,19 @@ public: return deltaTij_; } const Rot3& deltaRij() const { - return deltaRij_; + return deltaXij_.attitude(); } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; + Vector3 deltaPij() const { + return deltaXij_.position().vector(); + } + Vector3 deltaVij() const { + return deltaXij_.velocity(); } const imuBias::ConstantBias& biasHat() const { return biasHat_; } - const Vector3& deltaPij() const { - return deltaPij_; - } - const Vector3& deltaVij() const { - return deltaVij_; + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; } const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; @@ -188,7 +191,7 @@ public: /// Update preintegrated measurements void updatePreintegratedMeasurements(const Vector3& measuredAcc, const Vector3& measuredOmega, const double deltaT, - Matrix3* D_incrR_integratedOmega, Matrix9* F); + Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far @@ -220,11 +223,9 @@ private: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 176379da4..fdf9fd04c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -40,6 +40,7 @@ using symbol_shorthand::B; static const Vector3 kGravityAlongNavZDown(0, 0, 9.81); static const Vector3 kZeroOmegaCoriolis(0, 0, 0); static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); +static const imuBias::ConstantBias kZeroBiasHat, kZeroBias; /* ************************************************************************* */ namespace { @@ -139,46 +140,51 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { /* ************************************************************************* */ TEST(ImuFactor, StraightLine) { // Set up IMU measurements - static const double g = 10; // make this easy to check - static const double a = 0.2, dt = 3.0; - const double exact = dt * dt / 2; - Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); + static const double g = 10; // make gravity 10 to make this easy to check + static const double v = 5.0, a = 0.2, dt = 3.0; + const double dt22 = dt * dt / 2; - // Set up body pointing towards y axis, and start at 10,20,0 with zero velocity - // TODO(frank): clean up Rot3 mess + // 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 static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); static const Point3 initial_position(10, 20, 0); - static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0)); + static const Vector3 initial_velocity(v,0,0); + static const NavState state1(nRb, initial_position, initial_velocity); - imuBias::ConstantBias biasHat, bias; + // set up acceleration in X direction, no angular velocity. + // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z + Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); + + // Create parameters assuming nav frame has Z up boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedU(g); // Up convention! + PreintegratedImuMeasurements::Params::MakeSharedU(g); - PreintegratedImuMeasurements pim(p, biasHat); + // Now, preintegrate for 3 seconds, in 10 steps + PreintegratedImuMeasurements pim(p, kZeroBiasHat); for (size_t i = 0; i < 10; i++) pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); // Check integrated quantities in body frame: gravity measured by IMU is integrated! + Vector3 b_deltaP(a * dt22, 0, -g * dt22); + Vector3 b_deltaV(a * dt, 0, -g * dt); EXPECT(assert_equal(Rot3(), pim.deltaRij())); - EXPECT(assert_equal(Vector3(a * exact, 0, -g * exact), pim.deltaPij())); - EXPECT(assert_equal(Vector3(a * dt, 0, -g * dt), pim.deltaVij())); + EXPECT(assert_equal(b_deltaP, pim.deltaPij())); + EXPECT(assert_equal(b_deltaV, pim.deltaVij())); // Check bias-corrected delta: also in body frame Vector9 expectedBC; - expectedBC << 0, 0, 0, a * exact, 0, -g * exact, a * dt, 0, -g * dt; - EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(bias))); + expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; + EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); - // 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)); - EXPECT(assert_equal(expected, pim.predict(state1, bias))); + // Check prediction in nav, note we move along x in body, along y in nav + Point3 expected_position(10 + v*dt, 20 + a * dt22, 0); + Velocity3 expected_velocity(v, a * dt, 0); + NavState expected(nRb, expected_position, expected_velocity); + EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); } /* ************************************************************************* */ TEST(ImuFactor, PreintegratedMeasurements) { - // Linearization point - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); @@ -192,7 +198,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { double expectedDeltaT1(0.5); // Actual preintegrated values - PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance, + PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -222,7 +228,6 @@ TEST(ImuFactor, PreintegratedMeasurements) { /* ************************************************************************* */ // Common linearization point and measurements for tests namespace common { -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)); @@ -252,28 +257,28 @@ TEST(ImuFactor, PreintegrationBaseMethods) { p->integrationCovariance = kIntegrationErrorCovariance; p->use2ndOrderCoriolis = true; - PreintegratedImuMeasurements pim(p, bias); + PreintegratedImuMeasurements pim(p, kZeroBiasHat); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // biasCorrectedDelta Matrix96 actualH; - pim.biasCorrectedDelta(bias, actualH); + pim.biasCorrectedDelta(kZeroBias, actualH); Matrix expectedH = numericalDerivative11( boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, - boost::none), bias); + boost::none), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); Matrix9 aH1; Matrix96 aH2; - NavState predictedState = pim.predict(state1, bias, aH1, aH2); + NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( - boost::bind(&PreintegrationBase::predict, pim, _1, bias, boost::none, + boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, boost::none), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, - boost::none), bias); + boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); return; @@ -282,11 +287,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) { /* ************************************************************************* */ TEST(ImuFactor, ErrorAndJacobians) { using namespace common; - PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, + PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(state2, pim.predict(state1, bias))); + EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias))); // Create factor ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, @@ -296,14 +301,14 @@ 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, bias))); + assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias))); Values values; values.insert(X(1), x1); values.insert(V(1), v1); values.insert(X(2), x2); values.insert(V(2), v2); - values.insert(B(1), bias); + values.insert(B(1), kZeroBias); EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); // Make sure linearization is correct @@ -312,16 +317,16 @@ TEST(ImuFactor, ErrorAndJacobians) { // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, 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, bias), 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, bias), x2); + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2); EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); // Evaluate error with wrong values @@ -330,7 +335,7 @@ TEST(ImuFactor, ErrorAndJacobians) { expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901; EXPECT( assert_equal(expectedError, - factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-2)); + factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2)); EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2)); // Make sure the whitening is done correctly @@ -503,9 +508,6 @@ TEST(ImuFactor, fistOrderExponential) { /* ************************************************************************* */ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - // Linearization point - imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements @@ -526,28 +528,28 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { // Actual preintegrated values PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, deltaTs); // Compute numerical derivatives Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), bias); + measuredOmegas, deltaTs), kZeroBias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), bias); + measuredOmegas, deltaTs), kZeroBias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelRdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredAccs, measuredOmegas, deltaTs), bias); + measuredAccs, measuredOmegas, deltaTs), kZeroBias); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -565,9 +567,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { /* ************************************************************************* */ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { - // Linearization point - imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases - // Measurements const Vector3 measuredAcc(0.1, 0.0, 0.0); const Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); @@ -587,20 +586,19 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { deltaTs.push_back(deltaT); } // Actual preintegrated values - PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs); + PreintegratedImuMeasurements pim = evaluatePreintegratedMeasurements( + kZeroBias, measuredAccs, measuredOmegas, deltaTs); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + const Rot3 deltaRij_old = pim.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = pim.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = pim.deltaPij(); // before adding new measurement - Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + Matrix oldPreintCovariance = pim.preintMeasCov(); Matrix Factual, Gactual; - preintegrated.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// @@ -673,15 +671,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { * Factual.transpose() + (1 / deltaT) * Gactual * measurementCovariance * Gactual.transpose(); - Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + Matrix newPreintCovarianceActual = pim.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } /* ************************************************************************* */ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { - // Linearization point - imuBias::ConstantBias bias; // Current estimate of acceleration and rotation rate biases - // Measurements list measuredAccs, measuredOmegas; list deltaTs; @@ -699,7 +694,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { } // Actual preintegrated values PreintegratedImuMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, deltaTs); // so far we only created a nontrivial linearization point for the preintegrated measurements