Inner class

gtsam/navigation/AggregateImuReadings.cpp

@@ -27,45 +27,24 @@ namespace gtsam {
 AggregateImuReadings::AggregateImuReadings(const boost::shared_ptr<Params>& p,
                                            const Bias& estimatedBias)
     : p_(p), estimatedBias_(estimatedBias), deltaTij_(0.0) {
-  zeta_.setZero();
   cov_.setZero();
 }
 
-// Tangent space sugar.
-namespace sugar {
-static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { return v.segment<3>(0); }
-static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) { return v.segment<3>(3); }
-static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) { return v.segment<3>(6); }
-
-typedef const Vector9 constV9;
-static Eigen::Block<constV9, 3, 1> dR(constV9& v) { return v.segment<3>(0); }
-static Eigen::Block<constV9, 3, 1> dP(constV9& v) { return v.segment<3>(3); }
-static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
-}  // namespace sugar
-
 // See extensive discussion in ImuFactor.lyx
-Vector9 AggregateImuReadings::UpdateEstimate(const Vector3& a_body,
+AggregateImuReadings::TangentVector AggregateImuReadings::UpdateEstimate(
-                                             const Vector3& w_body, double dt,
+    const Vector3& a_body, const Vector3& w_body, double dt,
-                                             const Vector9& zeta,
+    const TangentVector& zeta, OptionalJacobian<9, 9> A,
-                                             OptionalJacobian<9, 9> A,
+    OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
-                                             OptionalJacobian<9, 3> B,
-                                             OptionalJacobian<9, 3> C) {
-  using namespace sugar;
-
-  const Vector3 theta = dR(zeta);
-  const Vector3 v = dV(zeta);
-
   // 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;
+  TangentVector zetaPlus(zeta.theta() + invH * w_body * dt,
-  dR(zetaPlus) = dR(zeta) + invH * w_body * dt;     // theta
+                         zeta.position() + zeta.velocity() * dt + a_nav * dt22,
-  dP(zetaPlus) = dP(zeta) + v * dt + a_nav * dt22;  // position
+                         zeta.velocity() + a_nav * dt);
-  dV(zetaPlus) = dV(zeta) + a_nav * dt;             // velocity
 
   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;
+  TangentVector zeta = zeta_;
-  Vector9 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);
+  zeta.position() +=
-  dV(zeta) += Rit * gt;
+      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 {

gtsam/navigation/AggregateImuReadings.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include <Eigen/Dense>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 
@@ -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<Vector9, 3, 1> theta() { return v_.segment<3>(0); }
+    Eigen::Block<constV9, 3, 1> theta() const { return v_.segment<3>(0); }
+
+    Eigen::Block<Vector9, 3, 1> position() { return v_.segment<3>(3); }
+    Eigen::Block<constV9, 3, 1> position() const { return v_.segment<3>(3); }
+
+    Eigen::Block<Vector9, 3, 1> velocity() { return v_.segment<3>(6); }
+    Eigen::Block<constV9, 3, 1> velocity() const { return v_.segment<3>(6); }
+  };
+
  private:
   const boost::shared_ptr<Params> 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<Params>& 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,12 +101,11 @@ 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,
+  static TangentVector UpdateEstimate(const Vector3& a_body,
-                                double dt, const Vector9& zeta,
+                                      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);

gtsam/navigation/tests/testAggregateImuReadings.cpp

@@ -39,8 +39,9 @@ static boost::shared_ptr<AggregateImuReadings::Params> defaultParams() {
 }
 
 Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
-  Vector9 zeta_plus = AggregateImuReadings::UpdateEstimate(a, w, kDt, zeta);
+  AggregateImuReadings::TangentVector zeta_plus =
-  return zeta_plus;
+      AggregateImuReadings::UpdateEstimate(a, w, kDt, zeta);
+  return zeta_plus.vector();
 }
 
 /* ************************************************************************* */