Merge pull request #153 from tardifjp/fix/PreintegratedMeasurementsPython

Fix python wrappers for BetweenFactorVector and PreintegratedCombined…

gtsam.h

@@ -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,

gtsam/navigation/CombinedImuFactor.h

@@ -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:
-
+  typedef PreintegrationCombinedParams Params;
-  /// 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
-  };
 
  protected:
   /* Covariance matrix of the preintegrated measurements