Moved ImuFactor and CombinedImuFactor into main gtsam, and moved InertialNavFactor family into gtsam_unstable.

gtsam.h

@@ -2160,6 +2160,71 @@ virtual class ConstantBias : gtsam::Value {
 
 }///\namespace imuBias
 
+#include <gtsam/navigation/ImuFactor.h>
+class ImuFactorPreintegratedMeasurements {
+  // Standard Constructor
+  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
+  ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
+
+  // Standard Interface
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+};
+
+virtual class ImuFactor : gtsam::NonlinearFactor {
+  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
+      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
+  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
+      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
+      const gtsam::Pose3& body_P_sensor);
+
+  // Standard Interface
+  gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+      const gtsam::imuBias::ConstantBias& bias,
+      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
+      Vector gravity, Vector omegaCoriolis) const;
+};
+
+#include <gtsam/navigation/CombinedImuFactor.h>
+class CombinedImuFactorPreintegratedMeasurements {
+  // Standard Constructor
+  CombinedImuFactorPreintegratedMeasurements(
+      const gtsam::imuBias::ConstantBias& bias,
+      Matrix measuredAccCovariance,
+      Matrix measuredOmegaCovariance,
+      Matrix integrationErrorCovariance,
+      Matrix biasAccCovariance,
+      Matrix biasOmegaCovariance,
+      Matrix biasAccOmegaInit);
+  CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
+
+  // Standard Interface
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+};
+
+virtual class CombinedImuFactor : gtsam::NonlinearFactor {
+  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
+      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
+      const gtsam::noiseModel::Base* model);
+
+  // Standard Interface
+  gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
+      const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
+      const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
+      Vector gravity, Vector omegaCoriolis) const;
+};
+
 //*************************************************************************
 // Utilities
 //*************************************************************************

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -15,11 +15,10 @@
  * @author  Luca Carlone, Stephen Williams, Richard Roberts
  */
 
-#include <gtsam_unstable/slam/ImuFactor.h>
-#include <gtsam_unstable/slam/CombinedImuFactor.h>
-#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/LieVector.h>

gtsam/navigation/tests/testImuFactor.cpp

@@ -15,8 +15,7 @@
  * @author  Luca Carlone, Stephen Williams, Richard Roberts
  */
 
-#include <gtsam_unstable/slam/ImuFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/navigation/ImuBias.h>

gtsam_unstable/gtsam_unstable.h

@@ -629,72 +629,6 @@ virtual class InvDepthFactorVariant3b : gtsam::NonlinearFactor {
   InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model);
 };
 
-#include <gtsam_unstable/slam/ImuFactor.h>
-class ImuFactorPreintegratedMeasurements {
-  // Standard Constructor
-  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
-  ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
-
-  // Standard Interface
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
-};
-
-virtual class ImuFactor : gtsam::NonlinearFactor {
-  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
-  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
-      const gtsam::Pose3& body_P_sensor);
-
-  // Standard Interface
-  gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
-      const gtsam::imuBias::ConstantBias& bias,
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
-      Vector gravity, Vector omegaCoriolis) const;
-};
-
-
-#include <gtsam_unstable/slam/CombinedImuFactor.h>
-class CombinedImuFactorPreintegratedMeasurements {
-  // Standard Constructor
-  CombinedImuFactorPreintegratedMeasurements(
-      const gtsam::imuBias::ConstantBias& bias,
-      Matrix measuredAccCovariance,
-      Matrix measuredOmegaCovariance,
-      Matrix integrationErrorCovariance,
-      Matrix biasAccCovariance,
-      Matrix biasOmegaCovariance,
-      Matrix biasAccOmegaInit);
-  CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
-
-  // Standard Interface
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
-};
-
-virtual class CombinedImuFactor : gtsam::NonlinearFactor {
-  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
-      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
-      const gtsam::noiseModel::Base* model);
-
-  // Standard Interface
-  gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
-      const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
-      const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
-      Vector gravity, Vector omegaCoriolis) const;
-};
-
 
 #include <gtsam_unstable/slam/Mechanization_bRn2.h>
 class Mechanization_bRn2 {

gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp

@@ -15,7 +15,7 @@
  * @author  Vadim Indelman, Stephen Williams
  */
 
-#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
+#include <gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp

@@ -18,7 +18,7 @@
 #include <iostream>
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/navigation/ImuBias.h>
-#include <gtsam/navigation/InertialNavFactor_GlobalVelocity.h>
+#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>

gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp

@@ -17,7 +17,7 @@
 
 #include <iostream>
 #include <gtsam/navigation/ImuBias.h>
-#include <gtsam/navigation/InertialNavFactor_GlobalVelocity.h>
+#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/Key.h>