diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c826f2010..269e16390 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -20,9 +20,8 @@ **/ #include "PreintegrationBase.h" -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +#include #include -#endif using namespace std; @@ -78,14 +77,14 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, //------------------------------------------------------------------------------ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + const Vector3& measuredAcc, const Vector3& measuredOmega, OptionalJacobian<3, 3> D_correctedAcc_measuredAcc, OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { // Correct for bias in the sensor frame - Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc); - Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega); + Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); // Compensate for sensor-body displacement if needed: we express the quantities // (originally in the IMU frame) into the body frame @@ -95,8 +94,8 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); // Convert angular velocity and acceleration from sensor to body frame - j_correctedOmega = bRs * j_correctedOmega; - j_correctedAcc = bRs * j_correctedAcc; + correctedOmega = bRs * correctedOmega; + correctedAcc = bRs * correctedAcc; // Jacobians if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs; @@ -108,21 +107,21 @@ pair PreintegrationBase::correctMeasurementsByBiasAndSensorPos if (!b_arm.isZero()) { // Subtract out the the centripetal acceleration from the measured one // to get linear acceleration vector in the body frame: - const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega); + const Matrix3 body_Omega_body = skewSymmetric(correctedOmega); const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm - j_correctedAcc -= body_Omega_body * b_velocity_bs; + correctedAcc -= body_Omega_body * b_velocity_bs; // Update derivative: centrifugal causes the correlation between acc and omega!!! if (D_correctedAcc_measuredOmega) { - double wdp = j_correctedOmega.dot(b_arm); + double wdp = correctedOmega.dot(b_arm); *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp)) - + j_correctedOmega * b_arm.transpose()) * bRs.matrix() - + 2 * b_arm * j_measuredOmega.transpose(); + + correctedOmega * b_arm.transpose()) * bRs.matrix() + + 2 * b_arm * measuredOmega.transpose(); } } } - return make_pair(j_correctedAcc, j_correctedOmega); + return make_pair(correctedAcc, correctedOmega); } //------------------------------------------------------------------------------ @@ -144,15 +143,24 @@ PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate( if (A) { // First order (small angle) approximation of derivative of invH*w: - const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body); +// const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body); + // NOTE(frank): Rot3::ExpmapDerivative(w_body) is also an approximation (but less accurate): + // If we replace approximation with numerical derivative we get better accuracy: + auto f = [w_body](const Vector3& theta) { + return Rot3::ExpmapDerivative(theta).inverse() * w_body; + }; + const Matrix3 invHw_H_theta = numericalDerivative11(f, zeta.theta()); // Exact derivative of R*a with respect to theta: const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H; A->setIdentity(); + // theta: A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt; + // position: A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; A->block<3, 3>(3, 6) = I_3x3 * dt; + // velocity: A->block<3, 3>(6, 0) = a_nav_H_theta * dt; } if (B) { @@ -176,8 +184,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij( OptionalJacobian<9, 3> C) const { if (!p().body_P_sensor) { // Correct for bias in the sensor frame - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + const Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + const Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); // Do update in one fell swoop return UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, B, C); @@ -209,8 +217,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij( } //------------------------------------------------------------------------------ -void PreintegrationBase::update(const Vector3& j_measuredAcc, - const Vector3& j_measuredOmega, double dt, +void PreintegrationBase::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt, Matrix3* D_incrR_integratedOmega, Matrix9* A, Matrix93* B, Matrix93* C) { // Save current rotation for updating Jacobians @@ -218,44 +226,41 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc, // Do update deltaTij_ += dt; - deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, A, B, C); + deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C); - // Update Jacobians - // TODO(frank): we are repeating some computation here: accessible in A ? - // Possibly: derivatives are just -B and -C ?? - Vector3 j_correctedAcc, j_correctedOmega; - boost::tie(j_correctedAcc, j_correctedOmega) = - correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega); + // D_plus_abias = D_plus_zeta * D_zeta_abias + D_plus_a * D_a_abias + Matrix93 D_zeta_abias, D_plus_abias; + D_zeta_abias << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_; + D_plus_abias = (*A) * D_zeta_abias - (*B); + delPdelBiasAcc_ = D_plus_abias.middleRows<3>(3); + delVdelBiasAcc_ = D_plus_abias.middleRows<3>(6); - double dt22 = 0.5 * dt * dt; - const Matrix3 dRij = oldRij.matrix(); // expensive - delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; - delVdelBiasAcc_ += -dRij * dt; - - cout << "B:" << endl; - cout << -(*B) << endl; - cout << "update:" << endl; - cout << - dt22 * dRij << endl; - cout << -dRij * dt << endl; +#ifdef USE_NEW_WAY_FOR_OMEGA + // D_plus_wbias = D_plus_zeta * D_zeta_wbias + D_plus_w * D_w_wbias + Matrix93 D_zeta_wbias, D_plus_wbias; + D_zeta_wbias << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_; + D_plus_wbias = (*A) * D_zeta_wbias - (*C); + delRdelBiasOmega_ = D_plus_wbias.middleRows<3>(0); + delPdelBiasOmega_ = D_plus_wbias.middleRows<3>(3); + delVdelBiasOmega_ = D_plus_wbias.middleRows<3>(6); +#else + // The old way matches the derivatives of deltaRij() + Vector3 correctedAcc, correctedOmega; + boost::tie(correctedAcc, correctedOmega) = + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega); Matrix3 D_acc_R; - oldRij.rotate(j_correctedAcc, D_acc_R); + double dt22 = 0.5 * dt * dt; + oldRij.rotate(correctedAcc, D_acc_R); const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; - const Vector3 integratedOmega = j_correctedOmega * dt; - const Rot3 incrR = - Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Vector3 integratedOmega = correctedOmega * dt; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = - incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt; delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; delVdelBiasOmega_ += D_acc_biasOmega * dt; - cout << "C:" << endl; - cout << -(*C) << endl; - cout << "update:" << endl; - cout << - *D_incrR_integratedOmega* dt << endl; - cout << dt22 * D_acc_biasOmega << endl; - cout << D_acc_biasOmega * dt << endl; +#endif } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 97e12c7ad..177c9b8f3 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -226,7 +226,7 @@ public: /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) /// Ignore D_correctedOmega_measuredAcc as it is trivially zero std::pair correctMeasurementsByBiasAndSensorPose( - const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + const Vector3& measuredAcc, const Vector3& measuredOmega, OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none, OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; @@ -243,14 +243,14 @@ public: /// Calculate the updated preintegrated measurement, does not modify /// It takes measured quantities in the j frame PreintegrationBase::TangentVector updatedDeltaXij( - const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, double dt, + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, OptionalJacobian<9, 9> A = boost::none, OptionalJacobian<9, 3> B = boost::none, OptionalJacobian<9, 3> C = boost::none) const; /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame - void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* A, Matrix93* B, Matrix93* C); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 6f6cde59f..853860516 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -34,6 +34,8 @@ using namespace std; using namespace gtsam; +typedef imuBias::ConstantBias Bias; + // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::V; @@ -43,7 +45,8 @@ static const double kGravity = 10; static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); static const Vector3 kZeroOmegaCoriolis(0, 0, 0); static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); -static const imuBias::ConstantBias kZeroBiasHat, kZeroBias; +static const Bias kZeroBiasHat, kZeroBias; +static const Vector3 Z_3x1 = Vector3::Zero(); /* ************************************************************************* */ namespace { @@ -51,7 +54,7 @@ namespace { /* ************************************************************************* */ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) { + const Bias& bias) { return Rot3::Expmap( factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); } @@ -75,7 +78,7 @@ static boost::shared_ptr defaultParams() { // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ PreintegratedImuMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, const list& measuredAccs, + const Bias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs) { PreintegratedImuMeasurements result(defaultParams(), bias); @@ -89,27 +92,19 @@ PreintegratedImuMeasurements evaluatePreintegratedMeasurements( } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, const list& measuredAccs, + const Bias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs) { return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, const list& measuredAccs, + const Bias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs) { return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaVij(); } -Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs) { - return Rot3( - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs).deltaRij()); -} - Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) { return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); @@ -168,7 +163,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); // Check derivatives of computeError - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) NavState x1, x2 = actual1.predict(x1, bias); { @@ -176,7 +171,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { Matrix96 aH3; actual1.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = + const Bias&)> f = boost::bind(&PreintegrationBase::computeError, actual1, _1, _2, _3, boost::none, boost::none, boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 @@ -237,7 +232,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) { // biasCorrectedDelta Matrix96 actualH; pim.biasCorrectedDelta(kZeroBias, actualH); - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, boost::none), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); @@ -249,7 +244,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) { boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, boost::none), state1); EXPECT(assert_equal(eH1, aH1)); - Matrix eH2 = numericalDerivative11( + Matrix eH2 = numericalDerivative11( boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); @@ -328,7 +323,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { using common::x1; using common::v1; using common::v2; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); @@ -342,14 +337,14 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { auto p = defaultParams(); p->omegaCoriolis = kNonZeroOmegaCoriolis; - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); + Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); PreintegratedImuMeasurements pim(p, biasHat); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Make sure of biasCorrectedDelta Matrix96 actualH; pim.biasCorrectedDelta(bias, actualH); - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, boost::none), bias); EXPECT(assert_equal(expectedH, actualH)); @@ -374,7 +369,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { using common::x1; using common::v1; using common::v2; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); @@ -390,7 +385,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { p->use2ndOrderCoriolis = true; PreintegratedImuMeasurements pim(p, - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1))); + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1))); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -502,40 +497,40 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { // Actual pre-integrated values PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, - deltaTs); + deltaTs); - // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( + // Check derivative of rotation + boost::function theta = [=]( + const Vector3& a, const Vector3& w) { + return evaluatePreintegratedMeasurements( + Bias(a, w), measuredAccs, measuredOmegas, deltaTs).deltaRij(); + }; + EXPECT( + assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3))); + EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1), + preintegrated.delRdelBiasOmega(), 1e-7)); + + // Check derivative of translation + Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), kZeroBias); + measuredOmegas, deltaTs), + kZeroBias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); + EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); + EXPECT(assert_equal(expectedDelPdelBiasOmega, + preintegrated.delPdelBiasOmega(), 1e-8)); - Matrix expectedDelVdelBias = numericalDerivative11( + // Check derivative of velocity + Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), kZeroBias); + measuredOmegas, deltaTs), + kZeroBias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - - Matrix expectedDelRdelBias = - numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredAccs, measuredOmegas, deltaTs), kZeroBias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); - Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); - - // Compare Jacobians - EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT( - assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega(),1e-8)); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT( - assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega(),1e-8)); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); - EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),1e-7)); + EXPECT(assert_equal(expectedDelVdelBiasOmega, + preintegrated.delVdelBiasOmega(), 1e-8)); } /* ************************************************************************* */ @@ -558,7 +553,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { 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)); + Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); const double T = 3.0; // seconds ScenarioRunner runner(&scenario, p, T / 10); @@ -608,7 +603,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // 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) + Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) // integrate at least twice to get position information // otherwise factor cov noise from preint_cov is not positive definite @@ -636,7 +631,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { /* ************************************************************************* */ TEST(ImuFactor, PredictPositionAndVelocity) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements Vector3 measuredOmega; @@ -646,7 +641,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) { double deltaT = 0.001; PreintegratedImuMeasurements pim(defaultParams(), - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -664,7 +659,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) { /* ************************************************************************* */ TEST(ImuFactor, PredictRotation) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements Vector3 measuredOmega; @@ -674,7 +669,7 @@ TEST(ImuFactor, PredictRotation) { double deltaT = 0.001; PreintegratedImuMeasurements pim(defaultParams(), - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); for (int i = 0; i < 1000; ++i) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -684,7 +679,7 @@ TEST(ImuFactor, PredictRotation) { // Predict NavState actual = pim.predict(NavState(), bias); - NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Vector3::Zero()); + NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1); EXPECT(assert_equal(expected, actual)); } @@ -706,7 +701,7 @@ TEST(ImuFactor, PredictArbitrary) { // EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1)); ////////////////////////////////////////////////////////////////////////////////// - imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); // Measurements Vector3 measuredOmega = runner.actualAngularVelocity(0); @@ -715,7 +710,7 @@ TEST(ImuFactor, PredictArbitrary) { auto p = defaultParams(); 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)); + Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, // Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); @@ -742,7 +737,7 @@ TEST(ImuFactor, PredictArbitrary) { /* ************************************************************************* */ TEST(ImuFactor, bodyPSensorNoBias) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Rotate sensor (z-down) to body (same as navigation) i.e. z-up auto p = defaultParams(); @@ -784,7 +779,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { TEST(ImuFactor, bodyPSensorWithBias) { using noiseModel::Diagonal; - typedef imuBias::ConstantBias Bias; + typedef Bias Bias; int numFactors = 80; Vector6 noiseBetweenBiasSigma; @@ -890,7 +885,7 @@ TEST(ImuFactor, serialization) { 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) + Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) PreintegratedImuMeasurements pim(p, priorBias); diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index 5363f6b9f..bc67091ae 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -69,7 +69,7 @@ TEST(PreintegrationBase, UpdateEstimate2) { pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3)); - EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7)); + EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8)); EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9)); }