diff --git a/gtsam.h b/gtsam.h index 468590ae7..cdf9d70d7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2160,6 +2160,71 @@ virtual class ConstantBias : gtsam::Value { }///\namespace imuBias +#include +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 +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 //************************************************************************* diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h similarity index 100% rename from gtsam_unstable/slam/CombinedImuFactor.h rename to gtsam/navigation/CombinedImuFactor.h diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam/navigation/ImuFactor.h similarity index 100% rename from gtsam_unstable/slam/ImuFactor.h rename to gtsam/navigation/ImuFactor.h diff --git a/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp similarity index 98% rename from gtsam_unstable/slam/tests/testCombinedImuFactor.cpp rename to gtsam/navigation/tests/testCombinedImuFactor.cpp index 3f6d6c4db..e46b73672 100644 --- a/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -15,11 +15,10 @@ * @author Luca Carlone, Stephen Williams, Richard Roberts */ -#include -#include -#include #include #include +#include +#include #include #include #include diff --git a/gtsam_unstable/slam/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp similarity index 99% rename from gtsam_unstable/slam/tests/testImuFactor.cpp rename to gtsam/navigation/tests/testImuFactor.cpp index 82f9e3a88..e312df60c 100644 --- a/gtsam_unstable/slam/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -15,8 +15,7 @@ * @author Luca Carlone, Stephen Williams, Richard Roberts */ -#include -#include +#include #include #include #include diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 2b204095b..2fc4aeb07 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/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 -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 -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 class Mechanization_bRn2 { diff --git a/gtsam/navigation/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h similarity index 100% rename from gtsam/navigation/EquivInertialNavFactor_GlobalVel.h rename to gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h diff --git a/gtsam/navigation/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h similarity index 100% rename from gtsam/navigation/EquivInertialNavFactor_GlobalVel_NoBias.h rename to gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h diff --git a/gtsam/navigation/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h similarity index 100% rename from gtsam/navigation/InertialNavFactor_GlobalVelocity.h rename to gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h diff --git a/gtsam/navigation/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp similarity index 97% rename from gtsam/navigation/tests/testEquivInertialNavFactor_GlobalVel.cpp rename to gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 752f1887a..68c9f01bf 100644 --- a/gtsam/navigation/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -15,7 +15,7 @@ * @author Vadim Indelman, Stephen Williams */ -#include +#include #include #include #include diff --git a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp similarity index 99% rename from gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp rename to gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 60988980c..951e6931c 100644 --- a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/navigation/tests/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp similarity index 98% rename from gtsam/navigation/tests/timeInertialNavFactor_GlobalVelocity.cpp rename to gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp index 32f129570..f036eadeb 100644 --- a/gtsam/navigation/tests/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include