diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3a2b78754..5a4e300e8 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -178,7 +178,9 @@ public: //------------------------------------------------------------------------------ PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const { + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) const { const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); @@ -188,16 +190,20 @@ public: // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() - + delPdelBiasAcc() * biasAccIncr - + delPdelBiasOmega() * biasOmegaIncr) + Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; + if(deltaPij_biascorrected_out)// if desired, store this + *deltaPij_biascorrected_out = deltaPij_biascorrected; + + Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected + vel_i * deltaTij() - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij()*deltaTij(); - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() - + delVdelBiasAcc() * biasAccIncr - + delVdelBiasOmega() * biasOmegaIncr) + Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; + if(deltaVij_biascorrected_out)// if desired, store this + *deltaVij_biascorrected_out = deltaVij_biascorrected; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term + gravity * deltaTij()); @@ -231,15 +237,28 @@ public: boost::optional H5) const { // We need the mismatch w.r.t. the biases used for preintegration - const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); // we give some shorter name to rotations and translations const Rot3& Ri = pose_i.rotation(); const Rot3& Rj = pose_j.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); const Vector3& pos_j = pose_j.translation().vector(); + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected, deltaVij_biascorrected; + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); + + // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) + // Jacobian computation /* ---------------------------------------------------------------------------------------------------- */ // Get Get so<3> version of bias corrected rotation @@ -248,7 +267,7 @@ public: Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); + const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); Vector3 correctedOmega = biascorrectedOmega - coriolis; @@ -271,27 +290,23 @@ public: fRrot = correctedDeltaRij.between(Ri.between(Rj)); fR = Rot3::Logmap(fRrot); } - if(H1) { H1->resize(9,6); - Matrix3 dfPdPi = -I_3x3; Matrix3 dfVdPi = Z_3x3; if(use2ndOrderCoriolis){ - dfPdPi += 0.5 * Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij(); - dfVdPi += Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij(); + // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() + Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); + dfVdPi += temp * deltaTij(); } (*H1) << // dfP/dRi - skewSymmetric( Ri.inverse().matrix() * (pos_j - pos_i - vel_i * deltaTij() - + omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity *deltaTij()*deltaTij())), + skewSymmetric(fp + deltaPij_biascorrected), // dfP/dPi dfPdPi, // dfV/dRi - skewSymmetric( Ri.inverse().matrix() * - (vel_j - vel_i - gravity * deltaTij() + 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term - ) ), + skewSymmetric(fv + deltaVij_biascorrected), // dfV/dPi dfVdPi, // dfR/dRi @@ -304,10 +319,10 @@ public: (*H2) << // dfP/dVi - Ri.transpose() * deltaTij() - + Ri.transpose() * omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Ri.transpose() - + 2 * Ri.transpose() * omegaCoriolisHat * deltaTij(), // Coriolis term + + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term // dfR/dVi Z_3x3; } @@ -343,20 +358,6 @@ public: // dfR/dBias Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, use2ndOrderCoriolis); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); - - // fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) - Vector r(9); r << fp, fv, fR; return r; } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 36571ab43..ce7689b82 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -315,6 +315,71 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) EXPECT(assert_equal(H5e, H5a)); } +/* ************************************************************************* */ +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) +{ + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + Pose3 bodyPsensor = Pose3(); + bool use2ndOrderCoriolis = true; + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); +} + /* ************************************************************************* */ TEST( ImuFactor, PartialDerivative_wrt_Bias ) {