From 0310eb4e7b63cbb131f6d5fc624d2e706218fd79 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 25 Jul 2015 10:02:38 +0200
Subject: [PATCH] Fixed compilation errors

---
 gtsam/navigation/PreintegrationBase.cpp | 13 ++++++++-----
 gtsam/navigation/PreintegrationBase.h   | 20 +++-----------------
 2 files changed, 11 insertions(+), 22 deletions(-)

diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 496569599..108063c22 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -126,9 +126,10 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc
   update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
 }
 
-void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
-    const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc,
-    Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
+std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
+    const Vector3& measuredAcc, const Vector3& measuredOmega,
+    boost::optional<const Pose3&> body_P_sensor) const {
+  Vector3 correctedAcc, correctedOmega;
   correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
   correctedOmega = biasHat_.correctGyroscope(measuredOmega);
 
@@ -136,12 +137,14 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
   // (originally in the IMU frame) into the body frame
   if (body_P_sensor) {
     Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-    correctedOmega = body_R_sensor * correctedOmega;  // rotation rate vector in the body frame
+    correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
     Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
     correctedAcc = body_R_sensor * correctedAcc
-        - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+        - body_omega_body__cross * body_omega_body__cross
+            * body_P_sensor->translation().vector();
     // linear acceleration vector in the body frame
   }
+  return std::make_pair(correctedAcc, correctedOmega);
 }
 
 /// Predict state at time j
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 1b2d6f783..3528231bf 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -133,24 +133,10 @@ class PreintegrationBase : public PreintegratedRotation {
                                     const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
                                     double deltaT);
 
-  boost::tuple<Vector3, Vector3>
+  std::pair<Vector3, Vector3>
   correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
-      const Vector3& measuredOmega, boost::optional<const Pose3&> body_P_sensor) {
-    Vector3 correctedAcc, correctedOmega;
-    correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
-    correctedOmega = biasHat_.correctGyroscope(measuredOmega);
-
-    // Then compensate for sensor-body displacement: we express the quantities
-    // (originally in the IMU frame) into the body frame
-    if(body_P_sensor){
-      Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-      correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
-      Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
-      correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
-      // linear acceleration vector in the body frame
-    }
-    return boost::make_tuple(correctedAcc, correctedOmega);
-  }
+      const Vector3& measuredOmega,
+      boost::optional<const Pose3&> body_P_sensor) const;
 
   /// Predict state at time j
   PoseVelocityBias predict(