diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index c4309e16f..137f102b3 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -28,7 +28,7 @@ namespace gtsam { // Inner class PreintegratedMeasurements //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( - const imuBias::ConstantBias& bias, const Matrix3& measuredOmegaCovariance) : + const Vector3& bias, const Matrix3& measuredOmegaCovariance) : biasHat_(bias), deltaTij_(0.0) { measurementCovariance_ << measuredOmegaCovariance; delRdelBiasOmega_.setZero(); @@ -37,7 +37,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(imuBias::ConstantBias()), deltaTij_(0.0) { + biasHat_(Vector3()), deltaTij_(0.0) { measurementCovariance_.setZero(); delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero(); @@ -47,7 +47,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : //------------------------------------------------------------------------------ void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { std::cout << s << std::endl; - biasHat_.print(" biasHat"); + std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl; deltaRij_.print(" deltaRij "); std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" << std::endl; @@ -57,7 +57,7 @@ void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { //------------------------------------------------------------------------------ bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { - return biasHat_.equals(other.biasHat_, tol) + return equal_with_abs_tol(biasHat_, other.biasHat_, tol) && equal_with_abs_tol(measurementCovariance_, other.measurementCovariance_, tol) && deltaRij_.equals(other.deltaRij_, tol) @@ -80,7 +80,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // NOTE: order is important here because each update uses old values. // First we compensate the measurements for the bias - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + Vector3 correctedOmega = measuredOmega - biasHat_; // Then compensate for sensor-body displacement: we express the quantities // (originally in the IMU frame) into the body frame @@ -136,9 +136,9 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( } //------------------------------------------------------------------------------ -Vector3 AHRSFactor::PreintegratedMeasurements::predict( - const imuBias::ConstantBias& bias, boost::optional H) const { - const Vector3 biasOmegaIncr = bias.gyroscope() - biasHat_.gyroscope(); +Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, + boost::optional H) const { + const Vector3 biasOmegaIncr = bias - biasHat_; Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; const Rot3 deltaRij_biascorrected = deltaRij_.retract( delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); @@ -172,7 +172,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( // AHRSFactor methods //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) { + preintegratedMeasurements_(Vector3(), Matrix3::Zero()) { } AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, @@ -217,7 +217,7 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { //------------------------------------------------------------------------------ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, - const imuBias::ConstantBias& bias, boost::optional H1, + const Vector3& bias, boost::optional H1, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below @@ -258,8 +258,8 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, if (H3) { // dfR/dBias, note H3 contains derivative of predict const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); - H3->resize(3, 6); - (*H3) << Matrix::Zero(3, 3), Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); + H3->resize(3, 3); + (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); } Vector error(3); @@ -268,7 +268,7 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, } //------------------------------------------------------------------------------ -Rot3 AHRSFactor::predict(const Rot3& rot_i, const imuBias::ConstantBias& bias, +Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, boost::optional body_P_sensor) { diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index b61a8bfe7..3050f82e6 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -20,11 +20,11 @@ /* GTSAM includes */ #include -#include +#include namespace gtsam { -class AHRSFactor: public NoiseModelFactor3 { +class AHRSFactor: public NoiseModelFactor3 { public: /** @@ -39,7 +39,7 @@ public: friend class AHRSFactor; protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer + Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) @@ -57,7 +57,7 @@ public: * @param bias Current estimate of acceleration and rotation rate biases * @param measuredOmegaCovariance Covariance matrix of measured angular rate */ - PreintegratedMeasurements(const imuBias::ConstantBias& bias, + PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); /// print @@ -75,8 +75,8 @@ public: double deltaTij() const { return deltaTij_; } - Vector6 biasHat() const { - return biasHat_.vector(); + Vector3 biasHat() const { + return biasHat_; } const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; @@ -99,8 +99,8 @@ public: /// Predict bias-corrected incremental rotation /// TODO: The matrix Hbias is the derivative of predict? Unit-test? - Vector3 predict(const imuBias::ConstantBias& bias, - boost::optional H = boost::none) const; + Vector3 predict(const Vector3& bias, boost::optional H = + boost::none) const; /// Integrate coriolis correction in body frame rot_i Vector3 integrateCoriolis(const Rot3& rot_i, @@ -129,7 +129,7 @@ public: private: typedef AHRSFactor This; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; PreintegratedMeasurements preintegratedMeasurements_; Vector3 gravity_; @@ -188,12 +188,12 @@ public: /// vector of errors Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, - const imuBias::ConstantBias& bias, boost::optional H1 = - boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const; + const Vector3& bias, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, boost::optional H3 = + boost::none) const; /// predicted states from IMU - static Rot3 predict(const Rot3& rot_i, const imuBias::ConstantBias& bias, + static Rot3 predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 200b4cc59..a05ed32f7 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -39,19 +39,19 @@ using symbol_shorthand::B; //****************************************************************************** namespace { Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i, - const Rot3 rot_j, const imuBias::ConstantBias& bias) { + const Rot3 rot_j, const Vector3& bias) { return factor.evaluateError(rot_i, rot_j, bias); } Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, - const Rot3 rot_j, const imuBias::ConstantBias& bias) { + const Rot3 rot_j, const Vector3& bias) { return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3)); } AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, const list& measuredOmegas, + const Vector3& bias, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { + const Vector3& initialRotationRate = Vector3()) { AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); list::const_iterator itOmega = measuredOmegas.begin(); @@ -64,9 +64,9 @@ AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, const list& measuredOmegas, + const Vector3& bias, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { + const Vector3& initialRotationRate = Vector3::Zero()) { return Rot3( evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, initialRotationRate).deltaRij()); @@ -85,7 +85,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { //****************************************************************************** TEST( AHRSFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases + Vector3 bias(0,0,0); ///< Current estimate of angular rate bias // Measurements Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); @@ -117,7 +117,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { //****************************************************************************** TEST( ImuFactor, Error ) { // Linearization point - imuBias::ConstantBias bias; // Bias + Vector3 bias; // Bias Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); @@ -147,7 +147,7 @@ TEST( ImuFactor, Error ) { boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); // Check rotation Jacobians @@ -178,7 +178,7 @@ TEST( ImuFactor, Error ) { TEST( ImuFactor, ErrorWithBiases ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Vector3 bias(0, 0, 0.3); Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); @@ -189,8 +189,7 @@ TEST( ImuFactor, ErrorWithBiases ) { measuredOmega << 0, 0, M_PI / 10.0 + 0.3; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3(0,0,0), Matrix3::Zero()); pre_int_data.integrateMeasurement(measuredOmega, deltaT); @@ -211,7 +210,7 @@ TEST( ImuFactor, ErrorWithBiases ) { boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); // Check rotation Jacobians @@ -219,7 +218,7 @@ TEST( ImuFactor, ErrorWithBiases ) { boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); Matrix RH2e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); // Actual Jacobians @@ -234,8 +233,7 @@ TEST( ImuFactor, ErrorWithBiases ) { //****************************************************************************** TEST( AHRSFactor, PartialDerivativeExpmap ) { // Linearization point - Vector3 biasOmega; - biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias + Vector3 biasOmega(0,0,0); // Measurements Vector3 measuredOmega; @@ -286,8 +284,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { //****************************************************************************** TEST( AHRSFactor, fistOrderExponential ) { // Linearization point - Vector3 biasOmega; - biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias + Vector3 biasOmega(0,0,0); // Measurements Vector3 measuredOmega; @@ -319,7 +316,7 @@ TEST( AHRSFactor, fistOrderExponential ) { //****************************************************************************** TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + Vector3 bias; ///< Current estimate of rotation rate bias Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); @@ -343,14 +340,12 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Compute numerical derivatives Matrix expectedDelRdelBias = - numericalDerivative11( + numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); EXPECT( assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations @@ -362,7 +357,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { //****************************************************************************** TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Vector3 bias(0, 0, 0.3); Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); @@ -378,8 +373,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0)); - AHRSFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3::Zero(), Matrix3::Zero()); pre_int_data.integrateMeasurement(measuredOmega, deltaT); @@ -392,7 +386,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); // Check rotation Jacobians @@ -400,7 +394,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); Matrix RH2e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); // Actual Jacobians @@ -413,7 +407,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { } //****************************************************************************** TEST (AHRSFactor, predictTest) { - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) + Vector3 bias(0,0,0); // Measurements Vector3 gravity; @@ -435,6 +429,15 @@ TEST (AHRSFactor, predictTest) { Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); + // AHRSFactor::PreintegratedMeasurements::predict + Matrix expectedH = numericalDerivative11( + boost::bind(&AHRSFactor::PreintegratedMeasurements::predict, + &pre_int_data, _1, boost::none), bias); + + // Actual Jacobians + Matrix H; + (void) pre_int_data.predict(bias,H); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** #include @@ -444,10 +447,10 @@ TEST (AHRSFactor, graphTest) { // linearization point Rot3 x1(Rot3::RzRyRx(0, 0, 0)); Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0)); - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + Vector3 bias(0,0,0); // PreIntegrator - imuBias::ConstantBias biasHat(Vector3(0, 0, 0), Vector3(0, 0, 0)); + Vector3 biasHat(0, 0, 0); Vector3 gravity; gravity << 0, 0, 9.81; Vector3 omegaCoriolis; @@ -456,7 +459,6 @@ TEST (AHRSFactor, graphTest) { Matrix3::Identity()); // Pre-integrate measurements - Vector3 measuredAcc(0.0, 0.0, 0.0); Vector3 measuredOmega(0, M_PI / 20, 0); double deltaT = 1;