Minor cleanup, moved reset method closer to constructor

gtsam/navigation/ImuFactor.cpp

@@ -69,9 +69,11 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
   correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
       correctedAcc, correctedOmega, body_P_sensor);
 
-  const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
+  // rotation increment computed from the current rotation rate measurement
+  const Vector3 integratedOmega = correctedOmega * deltaT;
   Matrix3 D_Rincr_integratedOmega;  // Right jacobian computed at theta_incr
-  const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
+  // rotation increment computed from the current rotation rate measurement
+  const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega);
 
   // Update Jacobians
   updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
@@ -82,12 +84,12 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
   updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
 
   // first order covariance propagation:
-  // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
+  // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
-  /* ----------------------------------------------------------------------------------------------------------------------- */
+  /* --------------------------------------------------------------------------------------------*/
-  // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
+  // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
   // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
   // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
-  // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise,
+  // NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise,
   // as G and measurementCovariance are block-diagonal matrices
   preintMeasCov_ = F * preintMeasCov_ * F.transpose();
   preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT;
@@ -101,16 +103,17 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
     F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
     preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
         * accelerometerCovariance() * F_pos_noiseacc.transpose();
-    Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
+    Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose();  // has 1/deltaT
     preintMeasCov_.block<3, 3>(0, 3) += temp;
     preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
   }
 
   // F_test and G_test are given as output for testing purposes and are not needed by the factor
-  if (F_test) { // This in only for testing
+  if (F_test) {
     (*F_test) << F;
   }
-  if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise
+  if (G_test) {
+    // This in only for testing & documentation, while the actual computation is done block-wise
     if (!use2ndOrderIntegration())
       F_pos_noiseacc = Z_3x3;
 
@@ -176,4 +179,4 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
       gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
 }
 
-} /// namespace gtsam
+}  // namespace gtsam

gtsam/navigation/PreintegratedRotation.h

@@ -31,31 +31,40 @@ namespace gtsam {
  * It includes the definitions of the preintegrated rotation.
  */
 class PreintegratedRotation {
-
   Rot3 deltaRij_;  ///< Preintegrated relative orientation (in frame i)
   double deltaTij_;  ///< Time interval from i to j
 
   /// Jacobian of preintegrated rotation w.r.t. angular rate bias
   Matrix3 delRdelBiasOmega_;
-  Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements
 
-public:
+  ///< continuous-time "Covariance" of gyroscope measurements
+  const Matrix3 gyroscopeCovariance_;
 
-  /**
+ public:
-   *  Default constructor, initializes the variables in the base class
+  /// Default constructor, initializes the variables in the base class
-   */
+  explicit PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
-  PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
       deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(
           measuredOmegaCovariance) {
   }
 
-  /// methods to access class variables
+  /// Re-initialize PreintegratedMeasurements
+  void resetIntegration() {
+    deltaRij_ = Rot3();
+    deltaTij_ = 0.0;
+    delRdelBiasOmega_ = Z_3x3;
+  }
+
+  /// @name Access instance variables
+  /// @{
+
+  // Return 3*3 matrix of rotation from time i to time j. Expensive in quaternion case
   Matrix3 deltaRij() const {
     return deltaRij_.matrix();
-  } // expensive
+  }
+  // Return log(rotation) from time i to time j. Expensive in both Rot3M and quaternion case.
   Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const {
     return Rot3::Logmap(deltaRij_, H);
-  } // super-expensive
+  }
   const double& deltaTij() const {
     return deltaTij_;
   }
@@ -65,15 +74,17 @@ public:
   const Matrix3& gyroscopeCovariance() const {
     return gyroscopeCovariance_;
   }
+  /// @}
+
+  /// @name Testable
+  /// @{
 
-  /// Needed for testable
   void print(const std::string& s) const {
     std::cout << s << std::endl;
     std::cout << "    deltaTij [" << deltaTij_ << "]" << std::endl;
     std::cout << "    deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl;
   }
 
-  /// Needed for testable
   bool equals(const PreintegratedRotation& expected, double tol) const {
     return deltaRij_.equals(expected.deltaRij_, tol)
         && fabs(deltaTij_ - expected.deltaTij_) < tol
@@ -83,12 +94,7 @@ public:
             expected.gyroscopeCovariance_, tol);
   }
 
-  /// Re-initialize PreintegratedMeasurements
+  /// @}
-  void resetIntegration() {
-    deltaRij_ = Rot3();
-    deltaTij_ = 0.0;
-    delRdelBiasOmega_ = Z_3x3;
-  }
 
   /// Update preintegrated measurements
   void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
@@ -104,8 +110,7 @@ public:
   void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega,
       const Rot3& incrR, double deltaT) {
     const Matrix3 incrRt = incrR.transpose();
-    delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+    delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT;
-        - D_Rincr_integratedOmega * deltaT;
   }
 
   /// Return a bias corrected version of the integrated rotation - expensive
@@ -125,12 +130,11 @@ public:
       // This was done via an expmap, now we go *back* to so<3>
       const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected,
           Jrinv_theta_bc);
-      const Matrix3 Jr_JbiasOmegaIncr = //
+      const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
-          Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
       (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
       return biascorrectedOmega;
     }
-    //else
+    //  else
     return Rot3::Logmap(deltaRij_biascorrected);
   }
 
@@ -140,15 +144,15 @@ public:
     return rot_i.transpose() * omegaCoriolis * deltaTij();
   }
 
-private:
+ private:
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {  // NOLINT
     ar & BOOST_SERIALIZATION_NVP(deltaRij_);
     ar & BOOST_SERIALIZATION_NVP(deltaTij_);
     ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
   }
 };
 
-} /// namespace gtsam
+}  // namespace gtsam

gtsam/navigation/PreintegrationBase.cpp

@@ -41,6 +41,17 @@ PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias,
       integrationCovariance_(integrationErrorCovariance) {
 }
 
+/// Re-initialize PreintegratedMeasurements
+void PreintegrationBase::resetIntegration() {
+  PreintegratedRotation::resetIntegration();
+  deltaPij_ = Vector3::Zero();
+  deltaVij_ = Vector3::Zero();
+  delPdelBiasAcc_ = Z_3x3;
+  delPdelBiasOmega_ = Z_3x3;
+  delVdelBiasAcc_ = Z_3x3;
+  delVdelBiasOmega_ = Z_3x3;
+}
+
 /// Needed for testable
 void PreintegrationBase::print(const std::string& s) const {
   PreintegratedRotation::print(s);
@@ -62,17 +73,6 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) con
       && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol);
 }
 
-/// Re-initialize PreintegratedMeasurements
-void PreintegrationBase::resetIntegration() {
-  PreintegratedRotation::resetIntegration();
-  deltaPij_ = Vector3::Zero();
-  deltaVij_ = Vector3::Zero();
-  delPdelBiasAcc_ = Z_3x3;
-  delPdelBiasOmega_ = Z_3x3;
-  delVdelBiasAcc_ = Z_3x3;
-  delVdelBiasOmega_ = Z_3x3;
-}
-
 /// Update preintegrated measurements
 void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc,
                                                          const Rot3& incrR, const double deltaT,
@@ -161,7 +161,7 @@ PoseVelocityBias PreintegrationBase::predict(
   const Vector3 pos_i = pose_i.translation().vector();
 
   // Predict state at time j
-  /* ---------------------------------------------------------------------------------------------------- */
+  /* ---------------------------------------------------------------------------------------------*/
   const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
       + delPdelBiasOmega() * biasOmegaIncr;
   if (deltaPij_biascorrected_out)  // if desired, store this
@@ -177,13 +177,13 @@ PoseVelocityBias PreintegrationBase::predict(
   if (deltaVij_biascorrected_out)  // if desired, store this
     *deltaVij_biascorrected_out = deltaVij_biascorrected;
 
-  Vector3 vel_j = Vector3(
+  Vector3 vel_j = Vector3(vel_i + Rot_i_matrix * deltaVij_biascorrected -
-      vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt  // Coriolis term
+                          2 * omegaCoriolis.cross(vel_i) * dt  // Coriolis term
                           + gravity * dt);
 
   if (use2ndOrderCoriolis) {
-    pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2;  // 2nd order coriolis term for position
+    pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2;  // for position
-    vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt;  // 2nd order term for velocity
+    vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt;  // for velocity
   }
 
   const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);

gtsam/navigation/PreintegrationBase.h

@@ -52,8 +52,8 @@ struct PoseVelocityBias {
  */
 class PreintegrationBase : public PreintegratedRotation {
 
-  imuBias::ConstantBias biasHat_;  ///< Acceleration and angular rate bias values used during preintegration
+  const imuBias::ConstantBias biasHat_;  ///< Acceleration and gyro bias used for preintegration
-  bool use2ndOrderIntegration_;  ///< Controls the order of integration
+  const bool use2ndOrderIntegration_;  ///< Controls the order of integration
 
   Vector3 deltaPij_;  ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
   Vector3 deltaVij_;  ///< Preintegrated relative velocity (in global frame)
@@ -79,6 +79,9 @@ class PreintegrationBase : public PreintegratedRotation {
                      const Matrix3& measuredOmegaCovariance,
                      const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration);
 
+  /// Re-initialize PreintegratedMeasurements
+  void resetIntegration();
+
   /// methods to access class variables
   bool use2ndOrderIntegration() const {
     return use2ndOrderIntegration_;
@@ -121,9 +124,6 @@ class PreintegrationBase : public PreintegratedRotation {
   /// check equality
   bool equals(const PreintegrationBase& other, double tol) const;
 
-  /// Re-initialize PreintegratedMeasurements
-  void resetIntegration();
-
   /// Update preintegrated measurements
   void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR,
                                        const double deltaT, OptionalJacobian<9, 9> F);