From 659ab595f304c120793734683b258cc8fc51e941 Mon Sep 17 00:00:00 2001
From: Xiaohan Fei <hzhsfxh@gmail.com>
Date: Fri, 13 Sep 2019 23:17:22 -0700
Subject: [PATCH] comments: gravity alignment

---
 src/core.h        | 24 +++++++++++++++---------
 src/estimator.cpp | 19 ++++++++-----------
 2 files changed, 23 insertions(+), 20 deletions(-)

diff --git a/src/core.h b/src/core.h
index 3c935b18..161a5ed0 100644
--- a/src/core.h
+++ b/src/core.h
@@ -31,6 +31,11 @@ using timestamp_t = nanoseconds;
 ////////////////////////////////////////
 // STATE DIMENSION
 ////////////////////////////////////////
+// NOTE: in the implementation, the spatial frame is actually the world frame, which is arbitrary.
+// Gravity of the form g=[0, 0, -9.8] is brought from the inertial frame to the spatial (world) frame .
+// by the rotation matrix Rg, i.e., Rg * g is the gravity in the spatial (world) frame.
+// Since the rotation around z-axis of the inertial frame is not observable, the z-component of Wg=Log(Rg)
+// is not included in the model.
 enum Index : int {
   W = 0, // Wsb, rotation
   Wsb = 0,
@@ -42,17 +47,17 @@ enum Index : int {
   ba = 12,  // alpha bias
   Wbc = 15, // alignment rotation
   Tbc = 18, // alignment translation
-  Wg = 21,  // gravity
+  Wg = 21,  // alignment of gravity from [0, 0, -9.8] to the spatial frame
 #ifdef USE_ONLINE_TEMPORAL_CALIB
-  td = 24, // temporal offset
+  td = Wg + 2, // temporal offset
 #endif
 
 #ifdef USE_ONLINE_IMU_CALIB
 
 #ifdef USE_ONLINE_TEMPORAL_CALIB
-  Cg = 25, // gyro calibration, 9 numbers
+  Cg = td + 1, // gyro calibration, 9 numbers
 #else
-  Cg = 24, // gyro calibration, 9 numbers
+  Cg = Wg + 2, // gyro calibration, 9 numbers
 #endif
   Ca = Cg + 9, // accel calibration, 6 numbers
   End = Ca + 6,
@@ -111,7 +116,7 @@ struct State {
 
   SO3 Rbc;
   Vec3 Tbc;
-  SO3 Rg;
+  SO3 Rg;  // gravity -> spatial
 
   number_t td;
 
@@ -125,8 +130,8 @@ struct State {
     ba += dX.segment<3>(Index::ba);
     Rbc *= SO3::exp(dX.segment<3>(Index::Wbc));
     Tbc += dX.segment<3>(Index::Tbc);
-    // Rg *= SO3::exp(Vec3{dX(Index::Wg), dX(Index::Wg + 1), 0.0});
-    Rg *= SO3::exp(dX.segment<3>(Index::Wg));
+    Rg *= SO3::exp(Vec3{dX(Index::Wg), dX(Index::Wg + 1), 0.0});
+    // Rg *= SO3::exp(dX.segment<3>(Index::Wg));
 // std::cout << "Wg=" << dX.segment<3>(Index::Wg).transpose() << std::endl;
 #ifdef USE_ONLINE_TEMPORAL_CALIB
     td += dX(Index::td);
@@ -136,7 +141,9 @@ struct State {
       if (++counter % kEnforceSO3Freq == 0) {
         Rsb = SO3::project(Rsb.matrix());
         Rbc = SO3::project(Rbc.matrix());
-        Rg = SO3::project(Rg.matrix());
+        auto Wg = SO3::log(Rg);
+        Wg(2) = 0;
+        Rg = SO3::exp(Wg);
       }
     }
 
@@ -156,7 +163,6 @@ struct State {
     os << "\n=====\n";
     return os;
   }
-
 };
 
 ////////////////////////////////////////
diff --git a/src/estimator.cpp b/src/estimator.cpp
index 58145fec..0b52d437 100644
--- a/src/estimator.cpp
+++ b/src/estimator.cpp
@@ -180,8 +180,7 @@ Estimator::Estimator(const Json::Value &cfg)
   }
   X_.Tbc = GetVectorFromJson<number_t, 3>(X, "Tbc");
   Vec3 Wg;
-  // Wg.head<2>() = GetVectorFromJson<number_t, 2>(X, "Wg");
-  Wg = GetVectorFromJson<number_t, 3>(X, "Wg");
+  Wg.head<2>() = GetVectorFromJson<number_t, 2>(X, "Wg");
   X_.Rg = SO3::exp(Wg);
 // temporal offset
 #ifdef USE_ONLINE_TEMPORAL_CALIB
@@ -374,9 +373,8 @@ bool Estimator::InitializeGravity() {
     Eigen::AngleAxis<number_t> AAg(
         Eigen::Quaternion<number_t>::FromTwoVectors(-g_, accel_calib));
     Vec3 Wg(AAg.axis() * AAg.angle());
-    // Wg(2) = 0;
-    auto Rg = SO3::exp(Wg);
-    X_.Rg = Rg;
+    Wg(2) = 0;
+    X_.Rg = SO3::exp(Wg);
 
     LOG(INFO) << "===== Wg initialization =====";
     LOG(INFO) << "stationary accel samples=" << gravity_init_buf_.size();
@@ -541,10 +539,7 @@ void Estimator::ComputeMotionJacobianAt(
   Mat3 dV_dW = -R * hat(accel_calib);
   Mat3 dV_dba = -R;
 
-  // static Mat3 tmp = -R * hat(g_); // 3x3
-  // Mat32 dV_dWg = tmp.block<3, 3>(0, 0); // 3x2
-  
-  Mat3 dV_dWg = -R * hat(g_);
+  Mat3 dV_dWg = -R * hat(g_); // effective dimension: 3x2, since Wg is 2-dim
   // Mat2 dWg_dWg = Mat2::Identity();
 
   F_.setZero(); // wipe out the delta added to F in the previous step
@@ -564,8 +559,10 @@ void Estimator::ComputeMotionJacobianAt(
       F_.coeffRef(Index::V + i, Index::W + j) = dV_dW(i, j);
       F_.coeffRef(Index::V + i, Index::ba + j) = dV_dba(i, j);
 
-      // if (j < 2)
-      F_.coeffRef(Index::V + i, Index::Wg + j) = dV_dWg(i, j);
+      if (j < 2) {
+        // NOTE: Wg is 2-dim, i.e., NO z-component
+        F_.coeffRef(Index::V + i, Index::Wg + j) = dV_dWg(i, j);
+      }
     }
   }