From a3032fe367c33c646719ad45fb70ade9bdfac685 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 9 Aug 2015 11:23:34 -0700
Subject: [PATCH] Params::print, and comments

---
 gtsam/navigation/PreintegrationBase.cpp | 38 +++++++++++++++++++------
 gtsam/navigation/PreintegrationBase.h   |  6 ++--
 2 files changed, 32 insertions(+), 12 deletions(-)

diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 5533a1f9b..dd385897e 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -26,7 +26,21 @@ using namespace std;
 
 namespace gtsam {
 
-/// Re-initialize PreintegratedMeasurements
+//------------------------------------------------------------------------------
+void PreintegrationBase::Params::print(const string& s) const {
+  PreintegratedRotation::Params::print(s);
+  cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
+      << endl;
+  cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]"
+      << endl;
+  if (omegaCoriolis && use2ndOrderCoriolis)
+    cout << "Using 2nd-order Coriolis" << endl;
+  if (body_P_sensor)
+    body_P_sensor->print("    ");
+  cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
+}
+
+//------------------------------------------------------------------------------
 void PreintegrationBase::resetIntegration() {
   deltaTij_ = 0.0;
   deltaXij_ = NavState();
@@ -37,7 +51,7 @@ void PreintegrationBase::resetIntegration() {
   delVdelBiasOmega_ = Z_3x3;
 }
 
-/// Needed for testable
+//------------------------------------------------------------------------------
 void PreintegrationBase::print(const string& s) const {
   cout << s << endl;
   cout << "    deltaTij [" << deltaTij_ << "]" << endl;
@@ -47,7 +61,7 @@ void PreintegrationBase::print(const string& s) const {
   biasHat_.print("    biasHat");
 }
 
-/// Needed for testable
+//------------------------------------------------------------------------------
 bool PreintegrationBase::equals(const PreintegrationBase& other,
     double tol) const {
   return fabs(deltaTij_ - other.deltaTij_) < tol
@@ -61,7 +75,7 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
 }
 
 //------------------------------------------------------------------------------
-std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
+pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
     const Vector3& j_measuredAcc, const Vector3& j_measuredOmega) const {
 
   // Correct for bias in the sensor frame
@@ -106,8 +120,8 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
 }
 
 //------------------------------------------------------------------------------
-void PreintegrationBase::update(
-    const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, const double dt,
+void PreintegrationBase::update(const Vector3& j_measuredAcc,
+    const Vector3& j_measuredOmega, const double dt,
     Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) {
 
   // Save current rotation for updating Jacobians
@@ -130,7 +144,8 @@ void PreintegrationBase::update(
   const Vector3 integratedOmega = j_correctedOmega * dt;
   const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+      - *D_incrR_integratedOmega * dt;
 
   double dt22 = 0.5 * dt * dt;
   const Matrix3 dRij = oldRij.matrix(); // expensive
@@ -227,7 +242,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
   if (H1)
     *H1 << D_error_predict * D_predict_state_i.leftCols<6>();
   if (H2)
-    *H2 << D_error_predict * D_predict_state_i.rightCols<3>() * state_i.R().transpose();
+    *H2
+        << D_error_predict * D_predict_state_i.rightCols<3>()
+            * state_i.R().transpose();
   if (H3)
     *H3 << D_error_state_j.leftCols<6>();
   if (H4)
@@ -251,4 +268,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
   p_ = q;
   return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
 }
-} /// namespace gtsam
+
+//------------------------------------------------------------------------------
+
+}/// namespace gtsam
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index f479b0b1e..6eca295b2 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -60,10 +60,8 @@ public:
   struct Params: PreintegratedRotation::Params {
     Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
     Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
-    /// (to compensate errors in Euler integration)
-    ///  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
     bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
-    Vector3 n_gravity; ///< Gravity constant in body frame
+    Vector3 n_gravity; ///< Gravity vector in nav frame
 
     /// The Params constructor insists on getting the navigation frame gravity vector
     /// For convenience, two commonly used conventions are provided by named constructors below
@@ -82,6 +80,8 @@ public:
       return boost::make_shared<Params>(Vector3(0, 0, -g));
     }
 
+    void print(const std::string& s) const;
+
   protected:
     /// Default constructor for serialization only: uninitialized!
     Params();