From 13cca7be005b26c3c74dcafb92ff47d33a1189e6 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 19 Jul 2015 21:23:16 -0700
Subject: [PATCH] MakeParams

---
 gtsam/navigation/ImuFactor.cpp | 18 +++++++++++++++---
 gtsam/navigation/ImuFactor.h   |  9 ++++++++-
 2 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 8f7e28385..b34cf1f19 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -31,6 +31,20 @@ using namespace std;
 //------------------------------------------------------------------------------
 // Inner class PreintegratedMeasurements
 //------------------------------------------------------------------------------
+boost::shared_ptr<PreintegratedImuMeasurements::Params> PreintegratedImuMeasurements::MakeParams(
+    const Matrix3& measuredAccCovariance,
+    const Matrix3& measuredOmegaCovariance,
+    const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration,
+    bool use2ndOrderCoriolis) {
+  boost::shared_ptr<Params> p = boost::make_shared<Params>();
+  p->accelerometerCovariance = measuredAccCovariance;
+  p->gyroscopeCovariance = measuredOmegaCovariance;
+  p->integrationCovariance = integrationErrorCovariance;
+  p->use2ndOrderIntegration = use2ndOrderIntegration;
+  p->use2ndOrderCoriolis = use2ndOrderCoriolis;
+  return p;
+}
+//------------------------------------------------------------------------------
 void PreintegratedImuMeasurements::print(const string& s) const {
   PreintegrationBase::print(s);
 }
@@ -116,9 +130,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
 PreintegratedImuMeasurements::PreintegratedImuMeasurements(
     const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
     const Matrix3& measuredOmegaCovariance,
-    const Matrix3& integrationErrorCovariance,
-    const bool use2ndOrderIntegration)
-     {
+    const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
   biasHat_ = biasHat;
   boost::shared_ptr<Params> p = boost::make_shared<Params>();
   p->gyroscopeCovariance = measuredOmegaCovariance;
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 9cd1fcada..1ef7e5679 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -50,6 +50,13 @@ protected:
 
 public:
 
+  /// Construct parameters
+  static boost::shared_ptr<Params> MakeParams(
+      const Matrix3& measuredAccCovariance,
+      const Matrix3& measuredOmegaCovariance,
+      const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration =
+          false, bool use2ndOrderCoriolis = false);
+
  /**
    *  Constructor, initializes the class with no measurements
    *  @param bias Current estimate of acceleration and rotation rate biases
@@ -91,7 +98,7 @@ public:
       const Matrix3& measuredAccCovariance,
       const Matrix3& measuredOmegaCovariance,
       const Matrix3& integrationErrorCovariance,
-      const bool use2ndOrderIntegration = false);
+      bool use2ndOrderIntegration = false);
 
 private: