From 3bb34679be6637cda7a81feeb42ea0e9813713cb Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Wed, 27 Jan 2016 13:16:52 -0800
Subject: [PATCH] Split into two units

---
 gtsam/navigation/PreintegrationBase.h       |  1 -
 python/handwritten/exportgtsam.cpp          |  2 +
 python/handwritten/navigation/ImuFactor.cpp | 71 +++++++++++++++++++++
 python/handwritten/navigation/Scenario.cpp  | 36 -----------
 4 files changed, 73 insertions(+), 37 deletions(-)
 create mode 100644 python/handwritten/navigation/ImuFactor.cpp

diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index 6e67f8bcf..3055fe9ed 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -30,7 +30,6 @@
 
 namespace gtsam {
 
-#define ALLOW_DEPRECATED_IN_GTSAM4
 #ifdef ALLOW_DEPRECATED_IN_GTSAM4
 /// @deprecated
 struct PoseVelocityBias {
diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp
index 3f74cc56a..7e8c22a82 100644
--- a/python/handwritten/exportgtsam.cpp
+++ b/python/handwritten/exportgtsam.cpp
@@ -53,6 +53,7 @@ void exportBetweenFactors();
 void exportGenericProjectionFactor();
 
 // navigation
+void exportImuFactor();
 void exportScenario();
 
 
@@ -99,5 +100,6 @@ BOOST_PYTHON_MODULE(_libgtsam_python){
   exportBetweenFactors();
   exportGenericProjectionFactor();
 
+  exportImuFactor();
   exportScenario();
 }
diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp
new file mode 100644
index 000000000..17a497158
--- /dev/null
+++ b/python/handwritten/navigation/ImuFactor.cpp
@@ -0,0 +1,71 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @brief wraps ConstantTwistScenario class to python
+ * @author Frank Dellaert
+ **/
+
+#include <boost/python.hpp>
+
+#define NO_IMPORT_ARRAY
+#include <numpy_eigen/NumpyEigenConverter.hpp>
+
+#include "gtsam/navigation/ImuFactor.h"
+
+using namespace boost::python;
+using namespace gtsam;
+
+void exportImuFactor() {
+  class_<NavState>("NavState", init<>())
+      // TODO(frank): overload with jacobians
+      //      .def("attitude", &NavState::attitude)
+      //      .def("position", &NavState::position)
+      //      .def("velocity", &NavState::velocity)
+      .def(repr(self))
+      .def("pose", &NavState::pose);
+
+  class_<imuBias::ConstantBias>("ConstantBias", init<>())
+      .def(init<const Vector3&, const Vector3&>())
+      .def(repr(self));
+
+  class_<PreintegrationParams, boost::shared_ptr<PreintegrationParams> >(
+      "PreintegrationParams", init<const Vector3&>())
+      .def_readwrite("gyroscopeCovariance",
+                     &PreintegrationParams::gyroscopeCovariance)
+      .def_readwrite("omegaCoriolis", &PreintegrationParams::omegaCoriolis)
+      .def_readwrite("body_P_sensor", &PreintegrationParams::body_P_sensor)
+      .def_readwrite("accelerometerCovariance",
+                     &PreintegrationParams::accelerometerCovariance)
+      .def_readwrite("integrationCovariance",
+                     &PreintegrationParams::integrationCovariance)
+      .def_readwrite("use2ndOrderCoriolis",
+                     &PreintegrationParams::use2ndOrderCoriolis)
+      .def_readwrite("n_gravity", &PreintegrationParams::n_gravity)
+
+      .def("MakeSharedD", &PreintegrationParams::MakeSharedD)
+      .staticmethod("MakeSharedD")
+      .def("MakeSharedU", &PreintegrationParams::MakeSharedU)
+      .staticmethod("MakeSharedU");
+
+  class_<PreintegratedImuMeasurements>(
+      "PreintegratedImuMeasurements",
+      init<const boost::shared_ptr<PreintegrationParams>&,
+           const imuBias::ConstantBias&>())
+      .def(repr(self))
+      .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration)
+      .def("integrateMeasurement",
+           &PreintegratedImuMeasurements::integrateMeasurement)
+      .def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov);
+
+  // NOTE(frank): Abstract classes need boost::noncopyable
+  class_<ImuFactor, boost::noncopyable>("ImuFactor", no_init);
+}
diff --git a/python/handwritten/navigation/Scenario.cpp b/python/handwritten/navigation/Scenario.cpp
index 936d4ef18..a49a02cc0 100644
--- a/python/handwritten/navigation/Scenario.cpp
+++ b/python/handwritten/navigation/Scenario.cpp
@@ -48,42 +48,6 @@ void exportScenario() {
   def("ScenarioPointer", &ScenarioPointer,
       return_value_policy<reference_existing_object>());
 
-  class_<PreintegrationParams, boost::shared_ptr<PreintegrationParams> >(
-      "PreintegrationParams", init<const Vector3&>())
-      .def_readwrite("gyroscopeCovariance",
-                     &PreintegrationParams::gyroscopeCovariance)
-      .def_readwrite("omegaCoriolis", &PreintegrationParams::omegaCoriolis)
-      .def_readwrite("body_P_sensor", &PreintegrationParams::body_P_sensor)
-      .def_readwrite("accelerometerCovariance",
-                     &PreintegrationParams::accelerometerCovariance)
-      .def_readwrite("integrationCovariance",
-                     &PreintegrationParams::integrationCovariance)
-      .def_readwrite("use2ndOrderCoriolis",
-                     &PreintegrationParams::use2ndOrderCoriolis)
-      .def_readwrite("n_gravity", &PreintegrationParams::n_gravity)
-
-      .def("MakeSharedD", &PreintegrationParams::MakeSharedD)
-      .staticmethod("MakeSharedD")
-      .def("MakeSharedU", &PreintegrationParams::MakeSharedU)
-      .staticmethod("MakeSharedU");
-
-  class_<PreintegratedImuMeasurements>(
-      "PreintegratedImuMeasurements",
-      init<const boost::shared_ptr<PreintegrationParams>&,
-           const imuBias::ConstantBias&>()).def(repr(self));
-
-  class_<NavState>("NavState", init<>())
-      // TODO(frank): overload with jacobians
-      //      .def("attitude", &NavState::attitude)
-      //      .def("position", &NavState::position)
-      //      .def("velocity", &NavState::velocity)
-      .def(repr(self))
-      .def("pose", &NavState::pose);
-
-  class_<imuBias::ConstantBias>("ConstantBias", init<>())
-      .def(init<const Vector3&, const Vector3&>())
-      .def(repr(self));
-
   class_<ScenarioRunner>(
       "ScenarioRunner",
       init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&,