From dc13912ce2a0e965d31cb0fee99672d4c8c400db Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 18:41:39 -0500 Subject: [PATCH] moved jacobian computation to updateMeasurement functions, and fixed noise propagation. Luca&Christian: insight is that preintegration noise acts on rotations as R * expmap(noise), while before it was expmap( logmap(R) + noise) --- gtsam/navigation/AHRSFactor.cpp | 35 ++----- gtsam/navigation/ImuFactor.cpp | 39 ++----- gtsam/navigation/PreintegratedRotation.h | 7 +- gtsam/navigation/PreintegrationBase.h | 15 ++- gtsam/navigation/tests/testImuFactor.cpp | 123 ++++++++++++++++------- 5 files changed, 120 insertions(+), 99 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 2859429db..9c684658c 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -69,7 +69,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor) { - // NOTE: order is important here because each update uses old values. // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - biasHat_; @@ -84,42 +83,20 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; - - // rotation increment computed from the current rotation rate measurement - const Rot3 incrR = Rot3::Expmap(theta_incr); // expensive !! - const Matrix3 incrRt = incrR.transpose(); - - // Right Jacobian computed at theta_incr - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); + Matrix3 Jr_theta_incr; + const Rot3 incrR = Rot3::Expmap(theta_incr, Jr_theta_incr); // expensive !! // Update Jacobian update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT); - // Update preintegrated measurements covariance - const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - - // Update rotation and deltaTij. TODO Frank moved from end of this function !!! - updateIntegratedRotationAndDeltaT(incrR, deltaT); - - const Vector3 theta_j = thetaRij(); // super-expensive, , Parameterization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first - // order propagation that can be seen as a prediction phase in an EKF framework - Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11 - // (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrpt preintegrated measurements (df/dx) - const Matrix3& F = H_angles_angles; + // Update rotation and deltaTij. + Matrix3 Fr; // Jacobian of the update + updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + measurementCovariance_ * deltaT; - } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index f25268350..862d081f2 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -70,9 +70,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( boost::optional body_P_sensor, boost::optional F_test, boost::optional G_test) { - // NOTE: order is important here because each update uses old values (i.e., we have to update - // jacobians and covariances before updating preintegrated measurements). - Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); @@ -81,42 +78,22 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); - // Update preintegrated measurements covariance + // Update preintegrated measurements (also get Jacobian) + const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test + Matrix F; // overall Jacobian wrt preintegrated measurements (df/dx) + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); + + // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ -// Matrix3 Jr_theta_i; -// const Vector3 theta_i = thetaRij(Jr_theta_i); // super-expensive parametrization of so(3) -// const Matrix3 R_i = deltaRij(); - - const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - const Matrix3 R_i = deltaRij(); - - // Update preintegrated measurements - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); - - Matrix3 Jrinv_theta_j; thetaRij(Jrinv_theta_j); // computation of rightJacobianInverse - - Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - // pos vel angle - F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos - Z_3x3, I_3x3, H_vel_angles, // vel - Z_3x3, Z_3x3, H_angles_angles;// angle - - // first order uncertainty propagation: // the deltaT allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - // F_test and Gout are used for testing purposes and are not needed by the factor + // F_test and G_test are used for testing purposes and are not needed by the factor if(F_test){ // This in only for testing F_test->resize(9,9); @@ -129,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle + Z_3x3, Z_3x3, Jr_theta_incr * deltaT; // angle // Propagation with no approximation: // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b93360fab..b5dea4a6c 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -74,9 +74,14 @@ public: } /// Update preintegrated measurements - void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT){ + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, + boost::optional H = boost::none){ deltaRij_ = deltaRij_ * incrR; deltaTij_ += deltaT; + if(H){ + H->resize(3,3); + *H = incrR.transpose(); + } } /** diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f91a889f2..200219fb3 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -116,7 +116,8 @@ public: /// Update preintegrated measurements void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT) { + const Rot3& incrR, const double deltaT, boost::optional F = boost::none) { + Matrix3 dRij = deltaRij(); // expensive Vector3 temp = dRij * correctedAcc * deltaT; if(!use2ndOrderIntegration_){ @@ -125,7 +126,17 @@ public: deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; } deltaVij_ += temp; - updateIntegratedRotationAndDeltaT(incrR,deltaT); + Matrix3 F_angles_angles; + const Matrix3 R_i = deltaRij(); + updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); + Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; + if(F){ + F->resize(9,9); + // pos vel angle + *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos + Z_3x3, I_3x3, F_vel_angles, // vel + Z_3x3, Z_3x3, F_angles_angles;// angle + } } /// Update Jacobians to be used during preintegration diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index b68d4ea5d..29a2a13de 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -37,6 +37,8 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { +// Auxiliary functions to test evaluate error in ImuFactor +/* ************************************************************************* */ Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias){ @@ -49,13 +51,14 @@ Rot3 evaluateRotationError(const ImuFactor& factor, return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } -// Correspond to updatePreintegratedMeasurements, but has a different syntax to test numerical derivatives -Vector updatePreintegratedMeasurementsTest( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Vector3& logDeltaRij_old, +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedPosVel( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, const bool use2ndOrderIntegration_) { - Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * correctedAcc * deltaT; Vector3 deltaPij_new; @@ -65,12 +68,20 @@ Vector updatePreintegratedMeasurementsTest( deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; - Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); - Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); - Vector result(9); result << deltaPij_new, deltaVij_new, logDeltaRij_new; + + Vector result(6); result << deltaPij_new, deltaVij_new; return result; } +Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, + const Vector3& correctedOmega, const double deltaT) { + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + return deltaRij_new; +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, @@ -356,6 +367,24 @@ TEST( ImuFactor, PartialDerivativeLogmap ) EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); } +Rot3 constRot = Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0); +Rot3 testRot(const Rot3& Rk){ + return Rk * constRot; +} +/* ************************************************************************* */ +TEST( ImuFactor, understandRot ) +{ + Rot3 Rbar = Rot3::RzRyRx( M_PI, M_PI/6.0, -M_PI/4.0 ); + + Matrix Jexpected = numericalDerivative11(boost::bind( + &testRot, _1), Rbar); + + Matrix3 Jactual = constRot.transpose(); + + // Compare Jacobians + EXPECT(assert_equal(Jexpected, Jactual)); +} + /* ************************************************************************* */ TEST( ImuFactor, fistOrderExponential ) { @@ -469,7 +498,6 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement 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 @@ -480,44 +508,67 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) bool use2ndOrderIntegration = false; - // Compute expected F wrt positions - Matrix df_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - _1, deltaVij_old, logDeltaRij_old, + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected f_pos_vel wrt positions + Matrix dfpv_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + _1, deltaVij_old, deltaRij_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); - // Compute expected F wrt velocities - Matrix df_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, _1, logDeltaRij_old, + // Compute expected f_pos_vel wrt velocities + Matrix dfpv_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, _1, deltaRij_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); - // Compute expected F wrt angles - Matrix df_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + // Compute expected f_pos_vel wrt angles + Matrix dfpv_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); - Matrix Fexpected(9,9); + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix FexpectedTop6(6,9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; - Fexpected << df_dpos, df_dvel, df_dangle; - EXPECT(assert_equal(Fexpected, Factual)); + EXPECT(assert_equal(FexpectedTop6, Factual.topRows(6))); - // Compute expected G wrt integration noise - Matrix df_dintNoise(9,3); - df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3; + // Compute expected f_rot wrt angles + Matrix dfr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + _1, newMeasuredOmega, newDeltaT), deltaRij_old); - // Compute expected F wrt acc noise - Matrix df_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, + Matrix FexpectedBottom3(3,9); + FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; + EXPECT(assert_equal(FexpectedBottom3, Factual.bottomRows(3))); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute jacobian wrt integration noise + Matrix dgpv_dintNoise(6,3); + dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; + + // Compute jacobian wrt acc noise + Matrix dgpv_daccNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix df_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, + Matrix dgpv_domegaNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix Gexpected(9,9); + Matrix GexpectedTop6(6,9); + GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; + EXPECT(assert_equal(GexpectedTop6, Gactual.topRows(6))); - Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise; - EXPECT(assert_equal(Gexpected, Gactual)); + // Compute expected f_rot wrt gyro noise + Matrix dgr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + deltaRij_old, _1, newDeltaT), newMeasuredOmega); + + Matrix GexpectedBottom3(3,9); + GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; + EXPECT(assert_equal(GexpectedBottom3, Gactual.bottomRows(3))); } //#include