diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 07781a2b6..9f76c30f4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -67,7 +67,8 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor) { + boost::optional body_P_sensor, + boost::optional Fout, boost::optional Gout) { // NOTE: order is important here because each update uses old values (i.e., we have to update // jacobians and covariances before updating preintegrated measurements). @@ -87,6 +88,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) + const Matrix3 R_i = deltaRij(); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); // Update preintegrated measurements. TODO Frank moved from end of this function !!! @@ -102,7 +104,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( Matrix H_vel_pos = Z_3x3; Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); @@ -124,15 +126,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // This in only kept for documentation. - // - // Matrix G(9,9); - // G << I_3x3 * deltaT, Z_3x3, Z_3x3, - // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, - // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - // - // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // Fout and Gout are used for testing purposes and are not needed by the factor + if(Fout){ + Fout->resize(9,9); + (*Fout) << F; + } + if(Gout){ + // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT + // This in only kept for testing. + Gout->resize(9,9); + (*Gout) << I_3x3 * deltaT, Z_3x3, Z_3x3, + Z_3x3, R_i * deltaT, Z_3x3, + Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; + //preintMeasCov = F * preintMeasCov * F.transpose() + Gout * (1/deltaT) * measurementCovariance * Gout.transpose(); + } } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 999fda80f..e0a6b861a 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -111,9 +111,11 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param deltaT Time interval between two consecutive IMU measurements * @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) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + boost::optional Fout = boost::none, boost::optional Gout = boost::none); /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 0468b77e2..df5553b38 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -343,28 +343,6 @@ public: return r; } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 57becd86c..fbc4bee05 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -49,6 +49,23 @@ Rot3 evaluateRotationError(const ImuFactor& factor, return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } +Vector PreIntegrateIMUObservations_delta_vel(const Vector3& msr_acc_t, + const double msr_dt, const Vector3& delta_angles, const Vector3& delta_vel_in_t0){ + // Note: all delta terms refer to an IMU\sensor system at t0 + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; +} + +Vector PreIntegrateIMUObservations_delta_angles(const Vector3& msr_gyro_t, + const double msr_dt, const Vector3& delta_angles){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Calculate the corrected measurements using the Bias object + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles) * Rot3::Expmap( msr_gyro_t * msr_dt ); + Vector result = Rot3::Logmap(R_t_to_t0); + return result; +} + ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, @@ -423,6 +440,75 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) +{ + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + 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 theta_i = preintegrated.thetaRij(); // before adding new measurement + const Vector3 deltaVij = preintegrated.deltaVij();// before adding new measurement + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, + body_P_sensor, Factual, Gactual); + + // Compute expected F + Matrix H_vel_angles_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, + newMeasuredAcc, newDeltaT, _1, deltaVij), theta_i); + Matrix H_angles_angles_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, + newMeasuredOmega, newDeltaT, _1), theta_i); + Matrix Fexpected(9,9); + Fexpected << I_3x3, I_3x3 * newDeltaT, Z_3x3, + Z_3x3, I_3x3, H_vel_angles_expected, + Z_3x3, Z_3x3, H_angles_angles_expected; + + // verify F + EXPECT(assert_equal(Fexpected, Factual)); + + // Compute expected G + Matrix H_vel_noiseAcc_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, + _1, newDeltaT, theta_i, deltaVij), newMeasuredAcc); + Matrix H_angles_noiseOmega_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, + _1, newDeltaT, theta_i), newMeasuredOmega); + Matrix Gexpected(9,9); + // [integrationError measuredAcc measuredOmega] + Gexpected << I_3x3 * newDeltaT, Z_3x3, Z_3x3, + Z_3x3, H_vel_noiseAcc_expected, Z_3x3, + Z_3x3, Z_3x3, H_angles_noiseOmega_expected; + // verify G + EXPECT(assert_equal(Gexpected, Gactual,1e-7)); +} + //#include ///* ************************************************************************* */ //TEST( ImuFactor, LinearizeTiming)