diff --git a/gtsam.h b/gtsam.h index 34c3fb6d3..9a44c7aa2 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2414,7 +2414,7 @@ class ImuFactorPreintegratedMeasurements { Matrix delVdelBiasAcc() const; Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; - Matrix PreintMeasCov() const; + Matrix preintMeasCov() const; // Standard Interface diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index c47316e40..cbda02ecc 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -37,7 +37,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( biasHat_(bias), deltaTij_(0.0) { measurementCovariance_ << measuredOmegaCovariance; delRdelBiasOmega_.setZero(); - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -46,7 +46,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : measurementCovariance_.setZero(); delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero(); - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -56,18 +56,18 @@ void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { deltaRij_.print(" deltaRij "); std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; + std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl; } //------------------------------------------------------------------------------ bool AHRSFactor::PreintegratedMeasurements::equals( - const PreintegratedMeasurements& expected, double tol) const { - return biasHat_.equals(expected.biasHat_, tol) + const PreintegratedMeasurements& other, double tol) const { + return biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(measurementCovariance_, - expected.measurementCovariance_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && std::fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + other.measurementCovariance_, tol) + && deltaRij_.equals(other.deltaRij_, tol) + && std::fabs(deltaTij_ - other.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } //------------------------------------------------------------------------------ @@ -75,7 +75,7 @@ void AHRSFactor::PreintegratedMeasurements::resetIntegration() { deltaRij_ = Rot3(); deltaTij_ = 0.0; delRdelBiasOmega_.setZero(); - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -131,7 +131,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT; // Update preintegrated measurements diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e3c96b091..1b8cdc45d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -32,11 +32,13 @@ namespace gtsam { class AHRSFactor: public NoiseModelFactor3 { public: - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ - - /** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/ + /** + * CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope + * measurements (rotation rates) and the corresponding covariance matrix. + * The measurements are then used to build the Preintegrated AHRS factor. + * Can be built incrementally so as to avoid costly integration at time of + * factor construction. + */ class PreintegratedMeasurements { friend class AHRSFactor; @@ -48,48 +50,54 @@ public: Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) double deltaTij_; ///< Time interval from i to j Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix3 PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: /// Default constructor PreintegratedMeasurements(); - /// Default constructor, initialize with no measurements - PreintegratedMeasurements(const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate - ); + /** + * Default constructor, initialize with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param measuredOmegaCovariance Covariance matrix of measured angular rate + */ + PreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredOmegaCovariance); - /** print */ + /// print void print(const std::string& s = "Preintegrated Measurements: ") const; - /** equals */ - bool equals(const PreintegratedMeasurements& expected, - double tol = 1e-9) const; + /// equals + bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const {return deltaTij_;} - Vector biasHat() const {return biasHat_.vector();} - Matrix3 delRdelBiasOmega() const {return delRdelBiasOmega_;} - Matrix PreintMeasCov() const {return PreintMeasCov_;} + const Matrix3& measurementCovariance() const {return measurementCovariance_;} + Matrix3 deltaRij() const {return deltaRij_.matrix();} + double deltaTij() const {return deltaTij_;} + Vector6 biasHat() const {return biasHat_.vector();} + const Matrix3& delRdelBiasOmega() const {return delRdelBiasOmega_;} + const Matrix3& preintMeasCov() const {return preintMeasCov_;} /// TODO: Document void resetIntegration(); - /** Add a single Gyroscope measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ); + /** + * Add a single Gyroscope measurement to the preintegration. + * @param measureOmedga Measured angular velocity (in body frame) + * @param deltaT Time step + * @param body_P_sensor Optional sensor frame + */ + void integrateMeasurement(const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none); - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + // This function is only used for test purposes + // (compare numerical derivatives wrt analytic ones) static Vector PreIntegrateIMUObservations_delta_angles( const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles); private: + /** Serialization function */ friend class boost::serialization::access; template @@ -134,7 +142,7 @@ public: ) : Base( noiseModel::Gaussian::Covariance( - preintegratedMeasurements.PreintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( body_P_sensor) { } diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 096e6ded3..faf8dce32 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -48,8 +48,7 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, } AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredOmegas, + const imuBias::ConstantBias& bias, const list& measuredOmegas, const list& deltaTs, const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); @@ -64,12 +63,12 @@ AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredOmegas, + const imuBias::ConstantBias& bias, const list& measuredOmegas, const list& deltaTs, const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { - return Rot3(evaluatePreintegratedMeasurements(bias, measuredOmegas, - deltaTs, initialRotationRate).deltaRij()); + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, + initialRotationRate).deltaRij()); } Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, @@ -82,8 +81,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { } } -/* ************************************************************************* */ -TEST( AHRSFactor, PreintegratedMeasurements ) { +/* ************************************************************************* */TEST( AHRSFactor, PreintegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases @@ -114,8 +112,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } -/* ************************************************************************* */ -TEST( ImuFactor, Error ) { +/* ************************************************************************* */TEST( ImuFactor, Error ) { // Linearization point imuBias::ConstantBias bias; // Bias Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); @@ -133,8 +130,7 @@ TEST( ImuFactor, Error ) { pre_int_data.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, - false); + AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false); Vector errorActual = factor.evaluateError(x1, x2, bias); @@ -162,18 +158,20 @@ TEST( ImuFactor, Error ) { (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); // rotations - EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(RH1e, H1a, 1e-5)); + // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H2e, H2a, 1e-5)); // rotations - EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(RH2e, H2a, 1e-5)); + // 1e-5 needs to be added only when using quaternions for rotations - EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions. + EXPECT(assert_equal(H3e, H3a, 1e-5)); + // FIXME!! DOes not work. Different matrix dimensions. } -/* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiases ) { +/* ************************************************************************* */TEST( ImuFactor, ErrorWithBiases ) { // Linearization point imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) @@ -188,7 +186,8 @@ TEST( ImuFactor, ErrorWithBiases ) { 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)), Matrix3::Zero()); + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Zero()); pre_int_data.integrateMeasurement(measuredOmega, deltaT); // Create factor @@ -201,7 +200,7 @@ TEST( ImuFactor, ErrorWithBiases ) { // Expected error Vector errorExpected(3); errorExpected << 0, 0, 0; - EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians Matrix H1e = numericalDerivative11( @@ -225,11 +224,10 @@ TEST( ImuFactor, ErrorWithBiases ) { EXPECT(assert_equal(H1e, H1a)); EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H3e, H3a)); } -/* ************************************************************************* */ -TEST( AHRSFactor, PartialDerivativeExpmap ) { +/* ************************************************************************* */TEST( AHRSFactor, PartialDerivativeExpmap ) { // Linearization point Vector3 biasOmega; biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias @@ -241,8 +239,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11( - boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), - biasOmega); + boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( (measuredOmega - biasOmega) * deltaT); @@ -250,12 +247,12 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); + // 1e-3 needs to be added only when using quaternions for rotations } -/* ************************************************************************* */ -TEST( AHRSFactor, PartialDerivativeLogmap ) { +/* ************************************************************************* */TEST( AHRSFactor, PartialDerivativeLogmap ) { // Linearization point Vector3 thetahat; thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias @@ -280,8 +277,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { } -/* ************************************************************************* */ -TEST( AHRSFactor, fistOrderExponential ) { +/* ************************************************************************* */TEST( AHRSFactor, fistOrderExponential ) { // Linearization point Vector3 biasOmega; biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias @@ -313,8 +309,7 @@ TEST( AHRSFactor, fistOrderExponential ) { EXPECT(assert_equal(expectedRot, actualRot)); } -/* ************************************************************************* */ -TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { +/* ************************************************************************* */TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases @@ -335,30 +330,28 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Actual preintegrated values AHRSFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredOmegas, - deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)); + evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, + Vector3(M_PI / 100.0, 0.0, 0.0)); // Compute numerical derivatives Matrix expectedDelRdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredOmegas, deltaTs, - Vector3(M_PI / 100.0, 0.0, 0.0)), bias); + 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 + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); + // 1e-3 needs to be added only when using quaternions for rotations } #include #include -/* ************************************************************************* */ -TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { +/* ************************************************************************* */TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); @@ -415,23 +408,25 @@ TEST (AHRSFactor, predictTest) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.0, 0.0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0.0, 0.0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.001; - AHRSFactor::PreintegratedMeasurements pre_int_data(bias,Matrix3::Zero()); - for (int i = 0; i < 1000; ++i){ - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero()); + for (int i = 0; i < 1000; ++i) { + pre_int_data.integrateMeasurement(measuredOmega, deltaT); } AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); // Predict Rot3 x; - Rot3 expectedRot = Rot3().ypr(M_PI/10,0,0); - Rot3 actualRot = factor.predict(x,bias, pre_int_data, omegaCoriolis); + Rot3 expectedRot = Rot3().ypr(M_PI / 10, 0, 0); + Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); - } /* ************************************************************************* */ #include @@ -439,7 +434,7 @@ TEST (AHRSFactor, predictTest) { TEST (AHRSFactor, graphTest) { // linearization point Rot3 x1(Rot3::RzRyRx(0, 0, 0)); - Rot3 x2(Rot3::RzRyRx(0, M_PI/4, 0)); + Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0)); imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // PreIntegrator @@ -453,17 +448,19 @@ TEST (AHRSFactor, graphTest) { // Pre-integrate measurements Vector3 measuredAcc(0.0, 0.0, 0.0); - Vector3 measuredOmega(0, M_PI/20, 0); + Vector3 measuredOmega(0, M_PI / 20, 0); double deltaT = 1; // Create Factor - noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov()); + noiseModel::Base::shared_ptr model = // + noiseModel::Gaussian::Covariance(pre_int_data.preintMeasCov()); NonlinearFactorGraph graph; Values values; - for(size_t i = 0; i < 5; ++i) { + for (size_t i = 0; i < 5; ++i) { pre_int_data.integrateMeasurement(measuredOmega, deltaT); } -// pre_int_data.print("Pre integrated measurementes"); + + // pre_int_data.print("Pre integrated measurementes"); AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); values.insert(X(1), x1); values.insert(X(2), x2); @@ -471,7 +468,7 @@ TEST (AHRSFactor, graphTest) { graph.push_back(factor); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); - Rot3 expectedRot(Rot3::RzRyRx(0, M_PI/4, 0)); + Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0)); EXPECT(assert_equal(expectedRot, result.at(X(2)))); }