From b126d98609e50bd6b565fcfd154471de7a3430b5 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Mon, 8 Dec 2014 20:20:03 -0500
Subject: [PATCH] included suggestions from Frank

---
 gtsam/navigation/ImuFactor.cpp           |  2 +-
 gtsam/navigation/PreintegratedRotation.h |  6 +-----
 gtsam/navigation/PreintegrationBase.h    | 11 +++++++----
 3 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 862d081f2..1aa261d34 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -101,7 +101,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
   }
   if(G_test){
     // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
-    // This in only for testing
+    // This in only for testing & documentation
     G_test->resize(9,9);
     //           intNoise         accNoise      omegaNoise
     (*G_test) << I_3x3 * deltaT,   Z_3x3,        Z_3x3,                                 // pos
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index b5dea4a6c..d949ee21c 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -76,12 +76,8 @@ public:
   /// Update preintegrated measurements
   void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
       boost::optional<Matrix3&> H = boost::none){
-    deltaRij_ = deltaRij_ * incrR;
+    deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
     deltaTij_ += deltaT;
-    if(H){
-      H->resize(3,3);
-      *H = incrR.transpose();
-    }
   }
 
   /**
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 200219fb3..11db97642 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -126,16 +126,19 @@ public:
       deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
     }
     deltaVij_ += temp;
-    Matrix3 F_angles_angles;
-    const Matrix3 R_i = deltaRij();
-    updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
-    Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
+
     if(F){
+      Matrix3 F_angles_angles;
+      const Matrix3 R_i = deltaRij();
+      updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
+      Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
       F->resize(9,9);
       //    pos          vel              angle
       *F << I_3x3,       I_3x3 * deltaT,  Z_3x3,          // pos
             Z_3x3,       I_3x3,           F_vel_angles,   // vel
             Z_3x3,       Z_3x3,           F_angles_angles;// angle
+    }else{
+      updateIntegratedRotationAndDeltaT(incrR,deltaT);
     }
   }