diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 8ed695622..02384e23d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -55,6 +55,26 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation resetIntegration(); } + /** + * Non-Default constructor, initialize with measurements + * @param p: Parameters for AHRS pre-integration + * @param bias_hat: Current estimate of acceleration and rotation rate biases + * @param deltaTij: Delta time in pre-integration + * @param deltaRij: Delta rotation in pre-integration + * @param delRdelBiasOmega: Jacobian of rotation wrt. to gyro bias + * @param preint_meas_cov: Pre-integration covariance + */ + PreintegratedAhrsMeasurements( + const boost::shared_ptr& p, + const Vector3& bias_hat, + double deltaTij, + const Rot3& deltaRij, + const Matrix3& delRdelBiasOmega, + const Matrix3& preint_meas_cov) : + biasHat_(bias_hat), + PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega), + preintMeasCov_(preint_meas_cov) {} + const Params& p() const { return *boost::static_pointer_cast(p_);} const Vector3& biasHat() const { return biasHat_; } const Matrix3& preintMeasCov() const { return preintMeasCov_; } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index bf2f5c0c8..71ef5d08f 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -35,6 +35,13 @@ struct GTSAM_EXPORT PreintegratedRotationParams { PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + PreintegratedRotationParams(const Matrix3& gyroscope_covariance, + boost::optional omega_coriolis) + : gyroscopeCovariance(gyroscope_covariance) { + if (omega_coriolis) + omegaCoriolis.reset(omega_coriolis.get()); + } + virtual ~PreintegratedRotationParams() {} virtual void print(const std::string& s) const; diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 208d0e709..d32553b94 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -119,6 +119,34 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } +//****************************************************************************** +TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { + Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4; + Vector3 omegaCoriolis(0.1, 0.5, 0.9); + PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis); + Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias + Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); + double deltaTij = 0.02; + Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; + Matrix3 preintMeasCov = Matrix3::Ones()*0.2; + PreintegratedAhrsMeasurements actualPim( + boost::make_shared(params), + bias, + deltaTij, + deltaRij, + delRdelBiasOmega, + preintMeasCov); + EXPECT(assert_equal(gyroscopeCovariance, + actualPim.p().getGyroscopeCovariance(), 1e-6)); + EXPECT(assert_equal(omegaCoriolis, + actualPim.p().getOmegaCoriolis().get(), 1e-6)); + EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6)); + DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6); + EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6)); + EXPECT(assert_equal(delRdelBiasOmega, actualPim.delRdelBiasOmega(), 1e-6)); + EXPECT(assert_equal(preintMeasCov, actualPim.preintMeasCov(), 1e-6)); +} + /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point