12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #153 from tardifjp/fix/PreintegratedMeasurementsPython 

Fix python wrappers for BetweenFactorVector and PreintegratedCombined…
release/4.3a0
Frank Dellaert 2019-10-18 17:42:55 -04:00 committed by GitHub
parent 6ef7b48d68 ca02a03c08
commit 021f22ca42
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 78 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam.h
View File

@ -2405,7 +2405,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
template<T = {Vector,gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
@ -2812,7 +2812,33 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
};
#include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
PreintegrationCombinedParams(Vector n_gravity);
static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81
static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
void setBiasAccCovariance(Matrix cov);
void setBiasOmegaCovariance(Matrix cov);
void setBiasAccOmegaInt(Matrix cov);
Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInt() const;
};
class PreintegratedCombinedMeasurements {
// Constructors
PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,

92
gtsam/navigation/CombinedImuFactor.h
View File

@ -54,6 +54,56 @@ typedef ManifoldPreintegration PreintegrationType;
* Robotics: Science and Systems (RSS), 2015.
*/
/// Parameters for pre-integration using PreintegratedCombinedMeasurements:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
/// See two named constructors below for good values of n_gravity in body frame
PreintegrationCombinedParams(const Vector3& n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInt(I_6x6) {
}
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
}
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
}
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
private:
/// Default constructor makes unitialized params struct
PreintegrationCombinedParams() {}
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* PreintegratedCombinedMeasurements integrates the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
@ -67,47 +117,7 @@ typedef ManifoldPreintegration PreintegrationType;
class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params : PreintegrationParams {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
/// See two named constructors below for good values of n_gravity in body frame
Params(const Vector3& n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInt(I_6x6) {
}
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
return boost::shared_ptr<Params>(new Params(Vector3(0, 0, g)));
}
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
return boost::shared_ptr<Params>(new Params(Vector3(0, 0, -g)));
}
private:
/// Default constructor makes unitialized params struct
Params() {}
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
typedef PreintegrationCombinedParams Params;
protected:
/* Covariance matrix of the preintegrated measurements