diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp new file mode 100644 index 000000000..65d1b9889 --- /dev/null +++ b/gtsam/navigation/AggregateImuReadings.cpp @@ -0,0 +1,209 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file AggregateImuReadings.cpp + * @brief Integrates IMU readings on the NavState tangent space + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace std; +using namespace boost::assign; + +namespace gtsam { + +using symbol_shorthand::T; // for theta +using symbol_shorthand::P; // for position +using symbol_shorthand::V; // for velocity + +static const Symbol kBiasKey('B', 0); + +SharedDiagonal AggregateImuReadings::discreteAccelerometerNoiseModel( + double dt) const { + return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() / + std::sqrt(dt)); +} + +SharedDiagonal AggregateImuReadings::discreteGyroscopeNoiseModel( + double dt) const { + return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() / + std::sqrt(dt)); +} + +NonlinearFactorGraph AggregateImuReadings::createGraph( + const Vector3_& theta_, const Vector3_& pos_, const Vector3_& vel_, + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) const { + NonlinearFactorGraph graph; + Expression bias_(kBiasKey); + Vector3_ theta_plus_(T(k_ + 1)), pos_plus_(P(k_ + 1)), vel_plus_(V(k_ + 1)); + + Vector3_ omega_(PredictAngularVelocity(dt), theta_, theta_plus_); + Vector3_ measuredOmega_(boost::bind(&Bias::correctGyroscope, _1, _2, _3, _4), + bias_, omega_); + auto gyroModel = discreteGyroscopeNoiseModel(dt); + graph.addExpressionFactor(gyroModel, measuredOmega, measuredOmega_); + + Vector3_ averageVelocity_(averageVelocity, vel_, vel_plus_); + Vector3_ defect_(PositionDefect(dt), pos_, pos_plus_, averageVelocity_); + static const auto constrModel = noiseModel::Constrained::All(3); + static const Vector3 kZero(Vector3::Zero()); + graph.addExpressionFactor(constrModel, kZero, defect_); + + Vector3_ acc_(PredictAcceleration(dt), vel_, vel_plus_, theta_); + Vector3_ measuredAcc_( + boost::bind(&Bias::correctAccelerometer, _1, _2, _3, _4), bias_, acc_); + auto accModel = discreteAccelerometerNoiseModel(dt); + graph.addExpressionFactor(accModel, measuredAcc, measuredAcc_); + + return graph; +} + +AggregateImuReadings::SharedBayesNet AggregateImuReadings::initPosterior( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { + static const Vector3 kZero(Vector3::Zero()); + static const Vector3_ zero_(kZero); + + // We create a factor graph and then compute P(zeta|bias) + auto graph = createGraph(zero_, zero_, zero_, measuredAcc, measuredOmega, dt); + + // These values are exact the first time + values.insert(T(k_ + 1), measuredOmega * dt); + values.insert(P(k_ + 1), measuredAcc * (0.5 * dt * dt)); + values.insert(V(k_ + 1), measuredAcc * dt); + values.insert(kBiasKey, estimatedBias_); + auto linear_graph = graph.linearize(values); + + // eliminate all but biases + // NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias) + Ordering keys = list_of(T(k_ + 1))(P(k_ + 1))(V(k_ + 1)); + return linear_graph->eliminatePartialSequential(keys, EliminateQR).first; +} + +AggregateImuReadings::SharedBayesNet AggregateImuReadings::integrateCorrected( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { + static const Vector3 kZero(Vector3::Zero()); + static const auto constrModel = noiseModel::Constrained::All(3); + + // We create a factor graph and then compute P(zeta|bias) + auto graph = createGraph(Vector3_(T(k_)), Vector3_(P(k_)), Vector3_(V(k_)), + measuredAcc, measuredOmega, dt); + + // Get current estimates + const Vector3 theta = values.at(T(k_)); + const Vector3 pos = values.at(P(k_)); + const Vector3 vel = values.at(V(k_)); + + // Calculate exact solution: means we do not have to update values + // TODO(frank): Expmap and ExpmapDerivative are called again :-( + const Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); + const Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); + Matrix3 H; + const Rot3 R = Rot3::Expmap(theta, H); + const Vector3 theta_plus = theta + H.inverse() * correctedOmega * dt; + const Vector3 vel_plus = vel + R.rotate(correctedAcc) * dt; + const Vector3 vel_avg = 0.5 * (vel + vel_plus); + const Vector3 pos_plus = pos + vel_avg * dt; + + // Add those values to estimate and linearize around them + values.insert(T(k_ + 1), theta_plus); + values.insert(P(k_ + 1), pos_plus); + values.insert(V(k_ + 1), vel_plus); + auto linear_graph = graph.linearize(values); + + // add previous posterior + for (const auto& conditional : *posterior_k_) + linear_graph->add(boost::static_pointer_cast(conditional)); + + // eliminate all but biases + // TODO(frank): does not seem to eliminate in order I want. What gives? + Ordering keys = list_of(T(k_))(P(k_))(V(k_))(T(k_ + 1))(P(k_ + 1))(V(k_ + 1)); + SharedBayesNet bayesNet = + linear_graph->eliminatePartialSequential(keys, EliminateQR).first; + + // The Bayes net now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias) + // We marginalize zeta(k) by removing the conditionals on zeta(k) + // TODO(frank): could use erase(begin, begin+3) if order above was correct + SharedBayesNet marginal = boost::make_shared(); + for (const auto& conditional : *bayesNet) { + Symbol symbol(conditional->front()); + if (symbol.index() > k_) marginal->push_back(conditional); + } + + return marginal; +} + +void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt) { + typedef map Terms; + + // Handle first time differently + if (k_ == 0) + posterior_k_ = initPosterior(measuredAcc, measuredOmega, dt); + else + posterior_k_ = integrateCorrected(measuredAcc, measuredOmega, dt); + + // increment counter and time + k_ += 1; + deltaTij_ += dt; +} + +NavState AggregateImuReadings::predict(const NavState& state_i, + const Bias& bias_i, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { + // TODO(frank): handle bias + + // Get current estimates + Vector3 theta = values.at(T(k_)); + Vector3 pos = values.at(P(k_)); + Vector3 vel = values.at(V(k_)); + + // Correct for initial velocity and gravity + Rot3 Ri = state_i.attitude(); + Matrix3 Rit = Ri.transpose(); + Vector3 gt = deltaTij_ * p_->n_gravity; + pos += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt); + vel += Rit * gt; + + // Convert local coordinates to manifold near state_i + Vector9 zeta; + zeta << theta, pos, vel; + return state_i.retract(zeta); +} + +SharedGaussian AggregateImuReadings::noiseModel() const { + Matrix RS; + Vector d; + boost::tie(RS, d) = posterior_k_->matrix(); + + // R'*R = A'*A = inv(Cov) + // TODO(frank): think of a faster way - implement in noiseModel + return noiseModel::Gaussian::SqrtInformation(RS.block<9, 9>(0, 0), false); +} + +Matrix9 AggregateImuReadings::preintMeasCov() const { + return noiseModel()->covariance(); +} + +} // namespace gtsam diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h new file mode 100644 index 000000000..9cb34849f --- /dev/null +++ b/gtsam/navigation/AggregateImuReadings.h @@ -0,0 +1,120 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file AggregateImuReadings.h + * @brief Integrates IMU readings on the NavState tangent space + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class NonlinearFactorGraph; +template +class Expression; +typedef Expression Vector3_; + +// Convert covariance to diagonal noise model, if possible, otherwise throw +static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { + bool smart = true; + auto model = noiseModel::Gaussian::Covariance(covariance, smart); + auto diagonal = boost::dynamic_pointer_cast(model); + if (!diagonal) + throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); + return diagonal; +} + +class GaussianBayesNet; + +/** + * Class that integrates state estimate on the manifold. + * We integrate zeta = [theta, position, velocity] + * See ImuFactor.lyx for the relevant math. + */ +class AggregateImuReadings { + public: + typedef imuBias::ConstantBias Bias; + typedef ImuFactor::PreintegratedMeasurements::Params Params; + typedef boost::shared_ptr SharedBayesNet; + + private: + const boost::shared_ptr p_; + const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; + const Bias estimatedBias_; + + size_t k_; ///< index/count of measurements integrated + double deltaTij_; ///< sum of time increments + + /// posterior on current iterate, stored as a Bayes net + /// P(delta_zeta|estimatedBias_delta): + SharedBayesNet posterior_k_; + + /// Current estimate of zeta_k + Values values; + + public: + AggregateImuReadings(const boost::shared_ptr& p, + const Bias& estimatedBias = Bias()) + : p_(p), + accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), + gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)), + estimatedBias_(estimatedBias), + k_(0), + deltaTij_(0.0) {} + + // We obtain discrete-time noise models by dividing the continuous-time + // covariances by dt: + + SharedDiagonal discreteAccelerometerNoiseModel(double dt) const; + SharedDiagonal discreteGyroscopeNoiseModel(double dt) const; + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame) + * @param measuredOmega Measured angular velocity (in body frame) + * @param dt Time interval between this and the last IMU measurement + */ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt); + + /// Predict state at time j + NavState predict(const NavState& state_i, const Bias& estimatedBias_i, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 6> H2 = boost::none) const; + + /// Return Gaussian noise model on prediction + SharedGaussian noiseModel() const; + + /// @deprecated: Explicitly calculate covariance + Matrix9 preintMeasCov() const; + + private: + NonlinearFactorGraph createGraph(const Vector3_& theta_, + const Vector3_& pose_, const Vector3_& vel_, + const Vector3& measuredAcc, + const Vector3& measuredOmega, + double dt) const; + + // initialize posterior with first (corrected) IMU measurement + SharedBayesNet initPosterior(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt); + + // integrate + SharedBayesNet integrateCorrected(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt); +}; + +} // namespace gtsam diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 047f24e8f..4acccb578 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -78,19 +78,19 @@ public: /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ Vector3 correctAccelerometer(const Vector3& measurement, - OptionalJacobian<3, 6> H = boost::none) const { - if (H) { - (*H) << -I_3x3, Z_3x3; - } + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << -I_3x3, Z_3x3; + if (H2) (*H2) << I_3x3; return measurement - biasAcc_; } /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ Vector3 correctGyroscope(const Vector3& measurement, - OptionalJacobian<3, 6> H = boost::none) const { - if (H) { - (*H) << Z_3x3, -I_3x3; - } + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << Z_3x3, -I_3x3; + if (H2) (*H2) << I_3x3; return measurement - biasGyro_; } diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index da757fdb6..7068e64c6 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -16,12 +16,6 @@ */ #include -#include -#include -#include - -#include - #include using namespace std; @@ -29,194 +23,13 @@ using namespace boost::assign; namespace gtsam { -using symbol_shorthand::T; // for theta -using symbol_shorthand::P; // for position -using symbol_shorthand::V; // for velocity - -static const Symbol kBiasKey('B', 0); -static const Matrix36 acc_H_bias = (Matrix36() << I_3x3, Z_3x3).finished(); -static const Matrix36 omega_H_bias = (Matrix36() << Z_3x3, I_3x3).finished(); - -Vector9 PreintegratedMeasurements2::currentEstimate() const { - VectorValues biasValues; - biasValues.insert(kBiasKey, estimatedBias_.vector()); - VectorValues zetaValues = posterior_k_->optimize(biasValues); - Vector9 zeta; - zeta << zetaValues.at(T(k_)), zetaValues.at(P(k_)), zetaValues.at(V(k_)); - return zeta; -} - -Vector3 PreintegratedMeasurements2::currentTheta() const { - // TODO(frank): make faster version theta = inv(R)*d - VectorValues biasValues; - biasValues.insert(kBiasKey, estimatedBias_.vector()); - VectorValues zetaValues = posterior_k_->optimize(biasValues); - return zetaValues.at(T(k_)); -} - -SharedDiagonal PreintegratedMeasurements2::discreteAccelerometerNoiseModel( - double dt) const { - return noiseModel::Diagonal::Sigmas(accelerometerNoiseModel_->sigmas() / - std::sqrt(dt)); -} - -SharedDiagonal PreintegratedMeasurements2::discreteGyroscopeNoiseModel( - double dt) const { - return noiseModel::Diagonal::Sigmas(gyroscopeNoiseModel_->sigmas() / - std::sqrt(dt)); -} - -PreintegratedMeasurements2::SharedBayesNet -PreintegratedMeasurements2::initPosterior(const Vector3& correctedAcc, - const Vector3& correctedOmega, - double dt) const { - typedef map Terms; - - // We create a factor graph and then compute P(zeta|bias) - GaussianFactorGraph graph; - - // theta(1) = (correctedOmega - bias_delta) * dt - // => theta(1)/dt + bias_delta = correctedOmega - auto I_dt = I_3x3 / dt; - graph.add({{T(k_ + 1), I_dt}, {kBiasKey, omega_H_bias}}, - correctedOmega, discreteGyroscopeNoiseModel(dt)); - - // pose(1) = (correctedAcc - bias_delta) * dt22 - // => pose(1) / dt22 + bias_delta = correctedAcc - auto accModel = discreteAccelerometerNoiseModel(dt); - graph.add({{P(k_ + 1), I_dt * (2.0 / dt)}, {kBiasKey, acc_H_bias}}, - correctedAcc, accModel); - - // vel(1) = (correctedAcc - bias_delta) * dt - // => vel(1)/dt + bias_delta = correctedAcc - graph.add({{V(k_ + 1), I_dt}, {kBiasKey, acc_H_bias}}, correctedAcc, - accModel); - - // eliminate all but biases - // NOTE(frank): After this, posterior_k_ contains P(zeta(1)|bias) - Ordering keys = list_of(T(k_ + 1))(P(k_ + 1))(V(k_ + 1)); - return graph.eliminatePartialSequential(keys, EliminateQR).first; -} - -PreintegratedMeasurements2::SharedBayesNet -PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc, - const Vector3& correctedOmega, - double dt) const { - typedef map Terms; - - GaussianFactorGraph graph; - - // estimate current theta from posterior - Vector3 theta_k = currentTheta(); - Rot3 Rk = Rot3::Expmap(theta_k); - Matrix3 Rkt = Rk.transpose(); - - // add previous posterior - for (const auto& conditional : *posterior_k_) - graph.add(boost::static_pointer_cast(conditional)); - - // theta(k+1) = theta(k) + inverse(H)*(correctedOmega - bias_delta) dt - // => H*theta(k+1)/dt - H*theta(k)/dt + bias_delta = (measuredOmega - bias) - Matrix3 H = Rot3::ExpmapDerivative(theta_k) / dt; - graph.add({{T(k_ + 1), H}, {T(k_), -H}, {kBiasKey, omega_H_bias}}, - correctedOmega, discreteGyroscopeNoiseModel(dt)); - - double dt22 = 0.5 * dt * dt; - // pos(k+1) = pos(k) + vel(k) dt + Rk*(correctedAcc - bias_delta) dt22 - // => Rkt*pos(k+1)/dt22 - Rkt*pos(k)/dt22 - Rkt*vel(k) dt/dt22 + bias_delta - // = correctedAcc - auto accModel = discreteAccelerometerNoiseModel(dt); - graph.add({{P(k_ + 1), Rkt / dt22}, - {P(k_), -Rkt / dt22}, - {V(k_), -Rkt * (2.0 / dt)}, - {kBiasKey, acc_H_bias}}, - correctedAcc, accModel); - - // vel(k+1) = vel(k) + Rk*(correctedAcc - bias_delta) dt - // => Rkt*vel(k+1)/dt - Rkt*vel(k)/dt + bias_delta = correctedAcc - graph.add( - {{V(k_ + 1), Rkt / dt}, {V(k_), -Rkt / dt}, {kBiasKey, acc_H_bias}}, - correctedAcc, accModel); - - // eliminate all but biases - // TODO(frank): does not seem to eliminate in order I want. What gives? - Ordering keys = list_of(T(k_))(P(k_))(V(k_))(T(k_ + 1))(P(k_ + 1))(V(k_ + 1)); - SharedBayesNet bayesNet = - graph.eliminatePartialSequential(keys, EliminateQR).first; - - // The Bayes net now contains P(zeta(k)|zeta(k+1),bias) P(zeta(k+1)|bias) - // We marginalize zeta(k) by removing the conditionals on zeta(k) - // TODO(frank): could use erase(begin, begin+3) if order above was correct - SharedBayesNet marginal = boost::make_shared(); - for (const auto& conditional : *bayesNet) { - Symbol symbol(conditional->front()); - if (symbol.index() > k_) marginal->push_back(conditional); - } - - return marginal; -} - -void PreintegratedMeasurements2::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - typedef map Terms; - - // Correct measurements by subtracting bias - Vector3 correctedAcc = measuredAcc - estimatedBias_.accelerometer(); - Vector3 correctedOmega = measuredOmega - estimatedBias_.gyroscope(); - - // Handle first time differently - if (k_ == 0) - posterior_k_ = initPosterior(correctedAcc, correctedOmega, dt); - else - posterior_k_ = integrateCorrected(correctedAcc, correctedOmega, dt); - - // increment counter and time - k_ += 1; - deltaTij_ += dt; -} - -NavState PreintegratedMeasurements2::predict( - const NavState& state_i, const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { - // Get mean of current posterior on zeta - // TODO(frank): handle bias - Vector9 zeta = currentEstimate(); - - // Correct for initial velocity and gravity - Rot3 Ri = state_i.attitude(); - Matrix3 Rit = Ri.transpose(); - Vector3 gt = deltaTij_ * p_->n_gravity; - zeta.segment<3>(3) += - Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt); - zeta.segment<3>(6) += Rit * gt; - - // Convert local coordinates to manifold near state_i - return state_i.retract(zeta); -} - -SharedGaussian PreintegratedMeasurements2::noiseModel() const { - Matrix RS; - Vector d; - boost::tie(RS, d) = posterior_k_->matrix(); - - // R'*R = A'*A = inv(Cov) - // TODO(frank): think of a faster way - implement in noiseModel - return noiseModel::Gaussian::SqrtInformation(RS.block<9, 9>(0, 0), false); -} - -Matrix9 PreintegratedMeasurements2::preintMeasCov() const { - return noiseModel()->covariance(); -} - -//////////////////////////////////////////////////////////////////////////////////////////// - static double intNoiseVar = 0.0000001; static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; -PreintegratedMeasurements2 ScenarioRunner::integrate( - double T, const imuBias::ConstantBias& estimatedBias, - bool corrupted) const { - PreintegratedMeasurements2 pim(p_, estimatedBias); +AggregateImuReadings ScenarioRunner::integrate(double T, + const Bias& estimatedBias, + bool corrupted) const { + AggregateImuReadings pim(p_, estimatedBias); const double dt = imuSampleTime(); const size_t nrSteps = T / dt; @@ -231,15 +44,14 @@ PreintegratedMeasurements2 ScenarioRunner::integrate( return pim; } -NavState ScenarioRunner::predict( - const PreintegratedMeasurements2& pim, - const imuBias::ConstantBias& estimatedBias) const { +NavState ScenarioRunner::predict(const AggregateImuReadings& pim, + const Bias& estimatedBias) const { const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); return pim.predict(state_i, estimatedBias); } -Matrix9 ScenarioRunner::estimateCovariance( - double T, size_t N, const imuBias::ConstantBias& estimatedBias) const { +Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N, + const Bias& estimatedBias) const { // Get predict prediction from ground truth measurements NavState prediction = predict(integrate(T)); diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index f942a4f31..c8b5efc15 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -16,126 +16,38 @@ */ #pragma once -#include +#include #include #include namespace gtsam { -// Convert covariance to diagonal noise model, if possible, otherwise throw -static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { - bool smart = true; - auto model = noiseModel::Gaussian::Covariance(covariance, smart); - auto diagonal = boost::dynamic_pointer_cast(model); - if (!diagonal) - throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); - return diagonal; -} - -class GaussianBayesNet; - -/** - * Class that integrates state estimate on the manifold. - * We integrate zeta = [theta, position, velocity] - * See ImuFactor.lyx for the relevant math. - */ -class PreintegratedMeasurements2 { - public: - typedef ImuFactor::PreintegratedMeasurements::Params Params; - typedef boost::shared_ptr SharedBayesNet; - - private: - const boost::shared_ptr p_; - const SharedDiagonal accelerometerNoiseModel_, gyroscopeNoiseModel_; - const imuBias::ConstantBias estimatedBias_; - - size_t k_; ///< index/count of measurements integrated - double deltaTij_; ///< sum of time increments - - /// posterior on current iterate, stored as a Bayes net P(zeta|bias_delta): - SharedBayesNet posterior_k_; - - public: - PreintegratedMeasurements2( - const boost::shared_ptr& p, - const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias()) - : p_(p), - accelerometerNoiseModel_(Diagonal(p->accelerometerCovariance)), - gyroscopeNoiseModel_(Diagonal(p->gyroscopeCovariance)), - estimatedBias_(estimatedBias), - k_(0), - deltaTij_(0.0) {} - - SharedDiagonal discreteAccelerometerNoiseModel(double dt) const; - SharedDiagonal discreteGyroscopeNoiseModel(double dt) const; - - /** - * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame) - * @param measuredOmega Measured angular velocity (in body frame) - * @param dt Time interval between this and the last IMU measurement - */ - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt); - - /// Predict state at time j - NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 6> H2 = boost::none) const; - - /// Return Gaussian noise model on prediction - SharedGaussian noiseModel() const; - - /// @deprecated: Explicitly calculate covariance - Matrix9 preintMeasCov() const; - - private: - // estimate zeta given estimated biases - // calculates conditional mean of P(zeta|bias_delta) - Vector9 currentEstimate() const; - - // estimate theta given estimated biases - Vector3 currentTheta() const; - - // We obtain discrete-time noise models by dividing the continuous-time - // covariances by dt: - - // initialize posterior with first (corrected) IMU measurement - SharedBayesNet initPosterior(const Vector3& correctedAcc, - const Vector3& correctedOmega, double dt) const; - - // integrate - SharedBayesNet integrateCorrected(const Vector3& correctedAcc, - const Vector3& correctedOmega, - double dt) const; -}; - /* * Simple class to test navigation scenarios. * Takes a trajectory scenario as input, and can generate IMU measurements */ class ScenarioRunner { public: - typedef boost::shared_ptr SharedParams; + typedef imuBias::ConstantBias Bias; + typedef boost::shared_ptr SharedParams; private: const Scenario* scenario_; const SharedParams p_; const double imuSampleTime_, sqrt_dt_; - const imuBias::ConstantBias bias_; + const Bias estimatedBias_; // Create two samplers for acceleration and omega noise mutable Sampler gyroSampler_, accSampler_; public: ScenarioRunner(const Scenario* scenario, const SharedParams& p, - double imuSampleTime = 1.0 / 100.0, - const imuBias::ConstantBias& bias = imuBias::ConstantBias()) + double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) : scenario_(scenario), p_(p), imuSampleTime_(imuSampleTime), sqrt_dt_(std::sqrt(imuSampleTime)), - bias_(bias), + estimatedBias_(bias), // NOTE(duy): random seeds that work well: gyroSampler_(Diagonal(p->gyroscopeCovariance), 10), accSampler_(Diagonal(p->accelerometerCovariance), 29284) {} @@ -155,31 +67,27 @@ class ScenarioRunner { // versions corrupted by bias and noise Vector3 measured_omega_b(double t) const { - return actual_omega_b(t) + bias_.gyroscope() + + return actual_omega_b(t) + estimatedBias_.gyroscope() + gyroSampler_.sample() / sqrt_dt_; } Vector3 measured_specific_force_b(double t) const { - return actual_specific_force_b(t) + bias_.accelerometer() + + return actual_specific_force_b(t) + estimatedBias_.accelerometer() + accSampler_.sample() / sqrt_dt_; } const double& imuSampleTime() const { return imuSampleTime_; } /// Integrate measurements for T seconds into a PIM - PreintegratedMeasurements2 integrate( - double T, - const imuBias::ConstantBias& estimatedBias = imuBias::ConstantBias(), - bool corrupted = false) const; + AggregateImuReadings integrate(double T, const Bias& estimatedBias = Bias(), + bool corrupted = false) const; /// Predict predict given a PIM - NavState predict(const PreintegratedMeasurements2& pim, - const imuBias::ConstantBias& estimatedBias = - imuBias::ConstantBias()) const; + NavState predict(const AggregateImuReadings& pim, + const Bias& estimatedBias = Bias()) const; /// Compute a Monte Carlo estimate of the predict covariance using N samples Matrix9 estimateCovariance(double T, size_t N = 1000, - const imuBias::ConstantBias& estimatedBias = - imuBias::ConstantBias()) const; + const Bias& estimatedBias = Bias()) const; /// Estimate covariance of sampled noise for sanity-check Matrix6 estimateNoiseCovariance(size_t N = 1000) const; diff --git a/gtsam/navigation/functors.h b/gtsam/navigation/functors.h new file mode 100644 index 000000000..36d87ac7b --- /dev/null +++ b/gtsam/navigation/functors.h @@ -0,0 +1,154 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file functors.h + * @brief Functors for use in Navigation factors + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +namespace gtsam { + +// Implement Rot3::ExpmapDerivative(omega) * theta, with derivatives +static Vector3 correctWithExpmapDerivative( + const Vector3& omega, const Vector3& theta, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) { + using std::sin; + const double angle2 = omega.dot(omega); // rotation angle, squared + if (angle2 <= std::numeric_limits::epsilon()) { + if (H1) *H1 = 0.5 * skewSymmetric(theta); + if (H2) *H2 = I_3x3; + return theta; + } + + const double angle = std::sqrt(angle2); // rotation angle + const double s1 = sin(angle) / angle; + const double s2 = sin(angle / 2.0); + const double a = 2.0 * s2 * s2 / angle2; + const double b = (1.0 - s1) / angle2; + + const Vector3 omega_x_theta = omega.cross(theta); + const Vector3 yt = a * omega_x_theta; + + const Matrix3 W = skewSymmetric(omega); + const Vector3 omega_x_omega_x_theta = omega.cross(omega_x_theta); + const Vector3 yyt = b * omega_x_omega_x_theta; + + if (H1) { + Matrix13 omega_t = omega.transpose(); + const Matrix3 T = skewSymmetric(theta); + const double Da = (s1 - 2.0 * a) / angle2; + const double Db = (3.0 * s1 - cos(angle) - 2.0) / angle2 / angle2; + *H1 = (-Da * omega_x_theta + Db * omega_x_omega_x_theta) * omega_t + a * T - + b * skewSymmetric(omega_x_theta) - b * W * T; + } + if (H2) *H2 = I_3x3 - a* W + b* W* W; + + return theta - yt + yyt; +} + +// theta(k+1) = theta(k) + inverse(H)*omega dt +// omega = (H/dt_)*(theta(k+1) - H*theta(k)) +// TODO(frank): make linear expression +class PredictAngularVelocity { + private: + double dt_; + + public: + typedef Vector3 result_type; + + PredictAngularVelocity(double dt) : dt_(dt) {} + + Vector3 operator()(const Vector3& theta, const Vector3& theta_plus, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + // TODO(frank): take into account derivative of ExpmapDerivative + const Vector3 predicted = (theta_plus - theta) / dt_; + Matrix3 D_c_t, D_c_p; + const Vector3 corrected = + correctWithExpmapDerivative(theta, predicted, D_c_t, D_c_p); + if (H1) *H1 = D_c_t - D_c_p / dt_; + if (H2) *H2 = D_c_p / dt_; + return corrected; + } +}; + +// TODO(frank): make linear expression +static Vector3 averageVelocity(const Vector3& vel, const Vector3& vel_plus, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) { + // TODO(frank): take into account derivative of ExpmapDerivative + if (H1) *H1 = 0.5 * I_3x3; + if (H2) *H2 = 0.5 * I_3x3; + return 0.5 * (vel + vel_plus); +} + +// pos(k+1) = pos(k) + average_velocity * dt +// TODO(frank): make linear expression +class PositionDefect { + private: + double dt_; + + public: + typedef Vector3 result_type; + + PositionDefect(double dt) : dt_(dt) {} + + Vector3 operator()(const Vector3& pos, const Vector3& pos_plus, + const Vector3& average_velocity, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none, + OptionalJacobian<3, 3> H3 = boost::none) const { + // TODO(frank): take into account derivative of ExpmapDerivative + if (H1) *H1 = I_3x3; + if (H2) *H2 = -I_3x3; + if (H3) *H3 = I_3x3* dt_; + return (pos + average_velocity * dt_) - pos_plus; + } +}; + +// vel(k+1) = vel(k) + Rk * acc * dt +// acc = Rkt * [vel(k+1) - vel(k)]/dt +// TODO(frank): take in Rot3 +class PredictAcceleration { + private: + double dt_; + + public: + typedef Vector3 result_type; + + PredictAcceleration(double dt) : dt_(dt) {} + + Vector3 operator()(const Vector3& vel, const Vector3& vel_plus, + const Vector3& theta, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none, + OptionalJacobian<3, 3> H3 = boost::none) const { + Matrix3 D_R_theta; + // TODO(frank): D_R_theta is ExpmapDerivative (computed again) + Rot3 nRb = Rot3::Expmap(theta, D_R_theta); + Vector3 n_acc = (vel_plus - vel) / dt_; + Matrix3 D_b_R, D_b_n; + Vector3 b_acc = nRb.unrotate(n_acc, D_b_R, D_b_n); + if (H1) *H1 = -D_b_n / dt_; + if (H2) *H2 = D_b_n / dt_; + if (H3) *H3 = D_b_R* D_R_theta; + return b_acc; + } +}; + +} // namespace gtsam diff --git a/gtsam/navigation/tests/testAggregateImuReadings.cpp b/gtsam/navigation/tests/testAggregateImuReadings.cpp new file mode 100644 index 000000000..91b01d320 --- /dev/null +++ b/gtsam/navigation/tests/testAggregateImuReadings.cpp @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testInertialNavFactor.cpp + * @brief Unit test for the InertialNavFactor + * @author Frank Dellaert + */ + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +static const double kDt = 0.1; + +/* ************************************************************************* */ +TEST(AggregateImuReadings, CorrectWithExpmapDerivative1) { + Matrix aH1, aH2; + boost::function f = boost::bind( + correctWithExpmapDerivative, _1, _2, boost::none, boost::none); + for (Vector3 omega : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { + for (Vector3 theta : + {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { + Vector3 expected = Rot3::ExpmapDerivative(omega) * theta; + EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta))); + EXPECT(assert_equal(expected, + correctWithExpmapDerivative(omega, theta, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); + } + } +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, CorrectWithExpmapDerivative2) { + Matrix aH1, aH2; + boost::function f = boost::bind( + correctWithExpmapDerivative, _1, _2, boost::none, boost::none); + const Vector3 omega(0, 0, 0); + for (Vector3 theta : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { + Vector3 expected = Rot3::ExpmapDerivative(omega) * theta; + EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta))); + EXPECT(assert_equal(expected, + correctWithExpmapDerivative(omega, theta, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); + } +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, CorrectWithExpmapDerivative3) { + Matrix aH1, aH2; + boost::function f = boost::bind( + correctWithExpmapDerivative, _1, _2, boost::none, boost::none); + const Vector3 omega(0.1, 0.2, 0.3), theta(0.4, 0.3, 0.2); + Vector3 expected = Rot3::ExpmapDerivative(omega) * theta; + EXPECT(assert_equal(expected, correctWithExpmapDerivative(omega, theta))); + EXPECT(assert_equal(expected, + correctWithExpmapDerivative(omega, theta, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, PredictAngularVelocity1) { + Matrix aH1, aH2; + PredictAngularVelocity functor(kDt); + boost::function f = + boost::bind(functor, _1, _2, boost::none, boost::none); + const Vector3 theta(0, 0, 0), theta_plus(0.4, 0.3, 0.2); + EXPECT(assert_equal(Vector3(4, 3, 2), functor(theta, theta_plus, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2)); +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, PredictAngularVelocity2) { + Matrix aH1, aH2; + PredictAngularVelocity functor(kDt); + boost::function f = + boost::bind(functor, _1, _2, boost::none, boost::none); + const Vector3 theta(0.1, 0.2, 0.3), theta_plus(0.4, 0.3, 0.2); + EXPECT( + assert_equal(Vector3(Rot3::ExpmapDerivative(theta) * Vector3(3, 1, -1)), + functor(theta, theta_plus, aH1, aH2), 1e-5)); + EXPECT(assert_equal(numericalDerivative21(f, theta, theta_plus), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, theta, theta_plus), aH2)); +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, AverageVelocity) { + Matrix aH1, aH2; + boost::function f = + boost::bind(averageVelocity, _1, _2, boost::none, boost::none); + const Vector3 v(1, 2, 3), v_plus(3, 2, 1); + EXPECT(assert_equal(Vector3(2, 2, 2), averageVelocity(v, v_plus, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, v, v_plus), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, v, v_plus), aH2)); +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, PositionDefect) { + Matrix aH1, aH2, aH3; + PositionDefect functor(kDt); + boost::function f = + boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none); + const Vector3 pos(1, 2, 3), pos_plus(2, 4, 6); + const Vector3 avg(10, 20, 30); + EXPECT(assert_equal(Vector3(0, 0, 0), + functor(pos, pos_plus, avg, aH1, aH2, aH3))); + EXPECT(assert_equal(numericalDerivative31(f, pos, pos_plus, avg), aH1)); + EXPECT(assert_equal(numericalDerivative32(f, pos, pos_plus, avg), aH2)); + EXPECT(assert_equal(numericalDerivative33(f, pos, pos_plus, avg), aH3)); +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, PredictAcceleration1) { + Matrix aH1, aH2, aH3; + PredictAcceleration functor(kDt); + boost::function f = + boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none); + const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6); + const Vector3 theta(0, 0, 0); + EXPECT(assert_equal(Vector3(10, 20, 30), + functor(vel, vel_plus, theta, aH1, aH2, aH3))); + EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1)); + EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2)); + EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3)); +} + +/* ************************************************************************* */ +TEST(AggregateImuReadings, PredictAcceleration2) { + Matrix aH1, aH2, aH3; + PredictAcceleration functor(kDt); + boost::function f = + boost::bind(functor, _1, _2, _3, boost::none, boost::none, boost::none); + const Vector3 vel(1, 2, 3), vel_plus(2, 4, 6); + const Vector3 theta(0.1, 0.2, 0.3); + EXPECT(assert_equal(Vector3(10, 20, 30), + functor(vel, vel_plus, theta, aH1, aH2, aH3))); + EXPECT(assert_equal(numericalDerivative31(f, vel, vel_plus, theta), aH1)); + EXPECT(assert_equal(numericalDerivative32(f, vel, vel_plus, theta), aH2)); + EXPECT(assert_equal(numericalDerivative33(f, vel, vel_plus, theta), aH3)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 4d5df3f05..596b76f6a 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -16,113 +16,135 @@ */ #include +#include + #include +#include using namespace std; using namespace gtsam; +typedef imuBias::ConstantBias Bias; + Vector biasAcc1(Vector3(0.2, -0.1, 0)); Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); -imuBias::ConstantBias bias1(biasAcc1, biasGyro1); +Bias bias1(biasAcc1, biasGyro1); Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); -imuBias::ConstantBias bias2(biasAcc2, biasGyro2); +Bias bias2(biasAcc2, biasGyro2); /* ************************************************************************* */ -TEST( ImuBias, Constructor) { +TEST(ImuBias, Constructor) { // Default Constructor - imuBias::ConstantBias bias1; + Bias bias1; // Acc + Gyro Constructor - imuBias::ConstantBias bias2(biasAcc2, biasGyro2); + Bias bias2(biasAcc2, biasGyro2); // Copy Constructor - imuBias::ConstantBias bias3(bias2); + Bias bias3(bias2); } /* ************************************************************************* */ -TEST( ImuBias, inverse) { - imuBias::ConstantBias biasActual = bias1.inverse(); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, - -biasGyro1); +TEST(ImuBias, inverse) { + Bias biasActual = bias1.inverse(); + Bias biasExpected = Bias(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, compose) { - imuBias::ConstantBias biasActual = bias2.compose(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); +TEST(ImuBias, compose) { + Bias biasActual = bias2.compose(bias1); + Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, between) { +TEST(ImuBias, between) { // p.between(q) == q - p - imuBias::ConstantBias biasActual = bias2.between(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + Bias biasActual = bias2.between(bias1); + Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, localCoordinates) { +TEST(ImuBias, localCoordinates) { Vector deltaActual = Vector(bias2.localCoordinates(bias1)); - Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, - biasGyro1 - biasGyro2)).vector(); + Vector deltaExpected = + (Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, retract) { +TEST(ImuBias, retract) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.retract(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); + Bias biasActual = bias2.retract(delta); + Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0), + biasGyro2 + delta.block<3, 1>(3, 0)); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, Logmap) { +TEST(ImuBias, Logmap) { Vector deltaActual = bias2.Logmap(bias1); Vector deltaExpected = bias1.vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, Expmap) { +TEST(ImuBias, Expmap) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.Expmap(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); + Bias biasActual = bias2.Expmap(delta); + Bias biasExpected = Bias(delta); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSub) { - imuBias::ConstantBias biasActual = -bias1; - imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); +TEST(ImuBias, operatorSub) { + Bias biasActual = -bias1; + Bias biasExpected(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorAdd) { - imuBias::ConstantBias biasActual = bias2 + bias1; - imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, - biasGyro2 + biasGyro1); +TEST(ImuBias, operatorAdd) { + Bias biasActual = bias2 + bias1; + Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSubB) { - imuBias::ConstantBias biasActual = bias2 - bias1; - imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, - biasGyro2 - biasGyro1); +TEST(ImuBias, operatorSubB) { + Bias biasActual = bias2 - bias1; + Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } +/* ************************************************************************* */ +TEST(ImuBias, Correct1) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = boost::bind( + &Bias::correctAccelerometer, _1, _2, boost::none, boost::none); + bias1.correctAccelerometer(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + +/* ************************************************************************* */ +TEST(ImuBias, Correct2) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = + boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none); + bias1.correctGyroscope(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index a987245d0..21a8c8190 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -31,7 +31,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr defaultParams() { +static boost::shared_ptr defaultParams() { auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;