From 8a45320ae26bc29982d41c94e6fcb438cc95febd Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 28 Jan 2016 09:54:47 -0800 Subject: [PATCH] Fixed MATLAB wrapper --- gtsam.h | 209 ++++++++++++++------------ gtsam/navigation/PreintegrationBase.h | 6 + 2 files changed, 121 insertions(+), 94 deletions(-) diff --git a/gtsam.h b/gtsam.h index 7aada0dc5..ce2e225f7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2447,7 +2447,7 @@ namespace imuBias { #include class ConstantBias { - // Standard Constructor + // Constructors ConstantBias(); ConstantBias(Vector biasAcc, Vector biasGyro); @@ -2479,99 +2479,120 @@ class ConstantBias { }///\namespace imuBias -//#include -//class PoseVelocityBias{ -// PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); -// }; -//class PreintegratedImuMeasurements { -// // Standard Constructor -// PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); -// PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); -// // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); -// -// // Testable -// void print(string s) const; -// bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); -// -// double deltaTij() const; -// gtsam::Rot3 deltaRij() const; -// Vector deltaPij() const; -// Vector deltaVij() const; -// Vector biasHatVector() const; -// Matrix delPdelBiasAcc() const; -// Matrix delPdelBiasOmega() const; -// Matrix delVdelBiasAcc() const; -// Matrix delVdelBiasOmega() const; -// Matrix delRdelBiasOmega() const; -// Matrix preintMeasCov() const; -// -// // Standard Interface -// void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); -// gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, -// Vector gravity, Vector omegaCoriolis) const; -//}; -// -//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::PreintegratedImuMeasurements& 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::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, -// const gtsam::Pose3& body_P_sensor); -// // Standard Interface -// gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; -//}; -// -//#include -//class PreintegratedCombinedMeasurements { -// // Standard Constructor -// PreintegratedCombinedMeasurements( -// const gtsam::imuBias::ConstantBias& bias, -// Matrix measuredAccCovariance, -// Matrix measuredOmegaCovariance, -// Matrix integrationErrorCovariance, -// Matrix biasAccCovariance, -// Matrix biasOmegaCovariance, -// Matrix biasAccOmegaInit); -// PreintegratedCombinedMeasurements( -// const gtsam::imuBias::ConstantBias& bias, -// Matrix measuredAccCovariance, -// Matrix measuredOmegaCovariance, -// Matrix integrationErrorCovariance, -// Matrix biasAccCovariance, -// Matrix biasOmegaCovariance, -// Matrix biasAccOmegaInit, -// bool use2ndOrderIntegration); -// // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); -// -// // Testable -// void print(string s) const; -// bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); -// -// double deltaTij() const; -// gtsam::Rot3 deltaRij() const; -// Vector deltaPij() const; -// Vector deltaVij() const; -// Vector biasHatVector() const; -// Matrix delPdelBiasAcc() const; -// Matrix delPdelBiasOmega() const; -// Matrix delVdelBiasAcc() const; -// Matrix delVdelBiasOmega() const; -// Matrix delRdelBiasOmega() const; -// Matrix preintMeasCov() const; -// -// // Standard Interface -// void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); -// gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, -// Vector gravity, Vector omegaCoriolis) const; -//}; -// -//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::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); -// // Standard Interface -// gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; -//}; -// +#include +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::NavState& expected, double tol) const; + + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; +}; + +#include +class PreintegrationParams { + PreintegrationParams(Vector n_gravity); + // TODO(frank): add setters/getters or make this MATLAB wrapper feature +}; + +#include +virtual class PreintegrationBase { + // Constructors + PreintegrationBase(const gtsam::PreintegrationParams* params); + PreintegrationBase(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationBase& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + + // Standard Interface + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; + +#include +virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + Matrix preintMeasCov() const; +}; + +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::PreintegratedImuMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias); +}; + +#include +virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { + // Standard Constructor + PreintegratedCombinedMeasurements(const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, Matrix biasAccCovariance, + Matrix biasOmegaCovariance, Matrix biasAccOmegaInit); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, + double tol); + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + Matrix preintMeasCov() const; +}; + +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::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j); +}; + #include class PreintegratedAhrsMeasurements { // Standard Constructor diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 55866bc62..97e12c7ad 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -280,6 +280,12 @@ public: /// @} + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{