diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp index d5273263f..3289e74f2 100644 --- a/gtsam/navigation/AggregateImuReadings.cpp +++ b/gtsam/navigation/AggregateImuReadings.cpp @@ -27,45 +27,24 @@ namespace gtsam { AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr& p, const Bias& estimatedBias) : p_(p), estimatedBias_(estimatedBias), deltaTij_(0.0) { - zeta_.setZero(); cov_.setZero(); } -// Tangent space sugar. -namespace sugar { -static Eigen::Block dR(Vector9& v) { return v.segment<3>(0); } -static Eigen::Block dP(Vector9& v) { return v.segment<3>(3); } -static Eigen::Block dV(Vector9& v) { return v.segment<3>(6); } - -typedef const Vector9 constV9; -static Eigen::Block dR(constV9& v) { return v.segment<3>(0); } -static Eigen::Block dP(constV9& v) { return v.segment<3>(3); } -static Eigen::Block dV(constV9& v) { return v.segment<3>(6); } -} // namespace sugar - // See extensive discussion in ImuFactor.lyx -Vector9 AggregateImuReadings::UpdateEstimate(const Vector3& a_body, - const Vector3& w_body, double dt, - const Vector9& zeta, - OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, - OptionalJacobian<9, 3> C) { - using namespace sugar; - - const Vector3 theta = dR(zeta); - const Vector3 v = dV(zeta); - +AggregateImuReadings::TangentVector AggregateImuReadings::UpdateEstimate( + const Vector3& a_body, const Vector3& w_body, double dt, + const TangentVector& zeta, OptionalJacobian<9, 9> A, + OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { // Calculate exact mean propagation Matrix3 H; - const Matrix3 R = Rot3::Expmap(theta, H).matrix(); + const Matrix3 R = Rot3::Expmap(zeta.theta(), H).matrix(); const Matrix3 invH = H.inverse(); const Vector3 a_nav = R * a_body; const double dt22 = 0.5 * dt * dt; - Vector9 zetaPlus; - dR(zetaPlus) = dR(zeta) + invH * w_body * dt; // theta - dP(zetaPlus) = dP(zeta) + v * dt + a_nav * dt22; // position - dV(zetaPlus) = dV(zeta) + a_nav * dt; // velocity + TangentVector zetaPlus(zeta.theta() + invH * w_body * dt, + zeta.position() + zeta.velocity() * dt + a_nav * dt22, + zeta.velocity() + a_nav * dt); if (A) { // First order (small angle) approximation of derivative of invH*w: @@ -122,17 +101,17 @@ NavState AggregateImuReadings::predict(const NavState& state_i, const Bias& bias_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { - using namespace sugar; - Vector9 zeta = zeta_; + TangentVector zeta = zeta_; // Correct for initial velocity and gravity Rot3 Ri = state_i.attitude(); Matrix3 Rit = Ri.transpose(); Vector3 gt = deltaTij_ * p_->n_gravity; - dP(zeta) += Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt); - dV(zeta) += Rit * gt; + zeta.position() += + Rit * (state_i.velocity() * deltaTij_ + 0.5 * deltaTij_ * gt); + zeta.velocity() += Rit * gt; - return state_i.retract(zeta); + return state_i.retract(zeta.vector()); } SharedGaussian AggregateImuReadings::noiseModel() const { diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h index 5c77f6104..698e1f130 100644 --- a/gtsam/navigation/AggregateImuReadings.h +++ b/gtsam/navigation/AggregateImuReadings.h @@ -17,6 +17,7 @@ #pragma once +#include #include #include @@ -32,6 +33,35 @@ class AggregateImuReadings { typedef imuBias::ConstantBias Bias; typedef PreintegrationBase::Params Params; + /// The IMU is integrated in the tangent space, represented by a Vector9 + /// This small inner class provides some convenient constructors and efficient + /// access to the orientation, position, and velocity components + class TangentVector { + Vector9 v_; + typedef const Vector9 constV9; + + public: + TangentVector() { v_.setZero(); } + TangentVector(const Vector9& v) : v_(v) {} + TangentVector(const Vector3& theta, const Vector3& pos, + const Vector3& vel) { + this->theta() = theta; + this->position() = pos; + this->velocity() = vel; + } + + const Vector9& vector() const { return v_; } + + Eigen::Block theta() { return v_.segment<3>(0); } + Eigen::Block theta() const { return v_.segment<3>(0); } + + Eigen::Block position() { return v_.segment<3>(3); } + Eigen::Block position() const { return v_.segment<3>(3); } + + Eigen::Block velocity() { return v_.segment<3>(6); } + Eigen::Block velocity() const { return v_.segment<3>(6); } + }; + private: const boost::shared_ptr p_; const Bias estimatedBias_; @@ -39,14 +69,15 @@ class AggregateImuReadings { double deltaTij_; ///< sum of time increments /// Current estimate of zeta_k - Vector9 zeta_; + TangentVector zeta_; Matrix9 cov_; public: AggregateImuReadings(const boost::shared_ptr& p, const Bias& estimatedBias = Bias()); - const Vector9& zeta() const { return zeta_; } + Vector3 theta() const { return zeta_.theta(); } + const Vector9& zeta() const { return zeta_.vector(); } const Matrix9& zetaCov() const { return cov_; } /** @@ -70,15 +101,14 @@ class AggregateImuReadings { /// @deprecated: Explicitly calculate covariance Matrix9 preintMeasCov() const; - Vector3 theta() const { return zeta_.head<3>(); } - // Update integrated vector on tangent manifold zeta with acceleration // readings a_body and gyro readings w_body, bias-corrected in body frame. - static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body, - double dt, const Vector9& zeta, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); + static TangentVector UpdateEstimate(const Vector3& a_body, + const Vector3& w_body, double dt, + const TangentVector& zeta, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); }; } // namespace gtsam diff --git a/gtsam/navigation/tests/testAggregateImuReadings.cpp b/gtsam/navigation/tests/testAggregateImuReadings.cpp index 63a127ee4..2bfe63dc0 100644 --- a/gtsam/navigation/tests/testAggregateImuReadings.cpp +++ b/gtsam/navigation/tests/testAggregateImuReadings.cpp @@ -39,8 +39,9 @@ static boost::shared_ptr defaultParams() { } Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - Vector9 zeta_plus = AggregateImuReadings::UpdateEstimate(a, w, kDt, zeta); - return zeta_plus; + AggregateImuReadings::TangentVector zeta_plus = + AggregateImuReadings::UpdateEstimate(a, w, kDt, zeta); + return zeta_plus.vector(); } /* ************************************************************************* */