From 3c59168406177e6675627c5006a1b061bbf7115e Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 19 Jul 2015 00:54:25 -0700
Subject: [PATCH] Inlined Logmap and derivatives, removed from
 PreintegratedRotation

---
 gtsam/navigation/AHRSFactor.cpp          |  6 +++++-
 gtsam/navigation/PreintegratedRotation.h | 17 -----------------
 gtsam/navigation/PreintegrationBase.cpp  | 12 ++++++------
 3 files changed, 11 insertions(+), 24 deletions(-)

diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp
index 3e5654427..814b020b1 100644
--- a/gtsam/navigation/AHRSFactor.cpp
+++ b/gtsam/navigation/AHRSFactor.cpp
@@ -83,7 +83,11 @@ void PreintegratedAhrsMeasurements::integrateMeasurement(
 Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias,
     OptionalJacobian<3,3> H) const {
   const Vector3 biasOmegaIncr = bias - biasHat_;
-  return biascorrectedThetaRij(biasOmegaIncr, H);
+  const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr, H);
+  Matrix3 D_omega_biascorrected;
+  const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0);
+  if (H) (*H) = D_omega_biascorrected * (*H);
+  return omega;
 }
 //------------------------------------------------------------------------------
 Vector PreintegratedAhrsMeasurements::DeltaAngles(
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 724d6661f..0475e52e2 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -135,23 +135,6 @@ class PreintegratedRotation {
     return deltaRij_biascorrected;
   }
 
-  /// Get so<3> version of bias corrected rotation, with optional Jacobian
-  // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
-  Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
-      OptionalJacobian<3, 3> H = boost::none) const {
-    // First, we correct deltaRij using the biasOmegaIncr, rotated
-    Matrix3 D_biascorrected_biasOmegaIncr;
-    const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr,
-        H ? &D_biascorrected_biasOmegaIncr : 0);
-
-    // This was done via an expmap, now we go *back* to so<3>
-    Matrix3 D_omega_biascorrected;
-    const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0);
-
-    if (H) (*H) = D_omega_biascorrected * D_biascorrected_biasOmegaIncr;
-    return omega;
-  }
-
   /// Integrate coriolis correction in body frame rot_i
   Vector3 integrateCoriolis(const Rot3& rot_i) const {
     if (!p_->omegaCoriolis) return Vector3::Zero();
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index bef45e044..0f2d96bac 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -193,7 +193,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
   // Evaluate residual error, according to [3]
   /* ---------------------------------------------------------------------------------------------------- */
   const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
-  const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasIncr.gyroscope());
+  Matrix3 D_biascorrected_biasIncr;
+  const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(
+      biasIncr.gyroscope(), H5 ? &D_biascorrected_biasIncr : 0);
   const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr);
   const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr);
   PoseVelocityBias predictedState_j =
@@ -212,10 +214,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
   // Get Get so<3> version of bias corrected rotation
   // If H5 is asked for, we will need the Jacobian, which we store in H5
   // H5 will then be corrected below to take into account the Coriolis effect
-  // TODO(frank): biascorrectedThetaRij calculates deltaRij_biascorrected, which we already have
-  Matrix3 D_cThetaRij_biasOmegaIncr;
-  const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasIncr.gyroscope(),
-                                                           H5 ? &D_cThetaRij_biasOmegaIncr : 0);
+  Matrix3 D_omega_biascorrected;
+  const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, H5 ? &D_omega_biascorrected : 0);
 
   // Coriolis term, NOTE inconsistent with AHRS, where coriolisHat is *after* integration
   // TODO(frank): move derivatives to predict and do coriolis branching there
@@ -272,7 +272,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const
   }
   if (H5) {
     // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
-    const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
+    const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_omega_biascorrected * D_biascorrected_biasIncr;
     (*H5) <<
     Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega),   // dfR/dBias
     -delPdelBiasAcc(), -delPdelBiasOmega(),                         // dfP/dBias