Add PreintegratedRotation to wrapper

gtsam/navigation/PreintegratedRotation.cpp

@@ -27,7 +27,7 @@ namespace gtsam {
 
 void PreintegratedRotationParams::print(const string& s) const {
   cout << (s.empty() ? s : s + "\n") << endl;
-  cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
+  cout << "gyroscopeCovariance:\n" << gyroscopeCovariance << endl;
   if (omegaCoriolis)
     cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
   if (body_P_sensor) body_P_sensor->print("body_P_sensor");

gtsam/navigation/PreintegratedRotation.h

@@ -126,12 +126,12 @@ class GTSAM_EXPORT PreintegratedRotation {
   Rot3 deltaRij_;             ///< Preintegrated relative orientation (in frame i)
   Matrix3 delRdelBiasOmega_;  ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
 
-  /// Default constructor for serialization
-  PreintegratedRotation() {}
-
- public:
+  public:
   /// @name Constructors
   /// @{
+    
+  /// Default constructor for serialization
+  PreintegratedRotation() {}
 
   /// Default constructor, resets integration to zero
   explicit PreintegratedRotation(const std::shared_ptr<Params>& p) : p_(p) {
@@ -149,9 +149,6 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @name Basic utilities
   /// @{
 
-  /// Re-initialize PreintegratedMeasurements
-  void resetIntegration();
-
   /// check parameters equality: checks whether shared pointer points to same Params object.
   bool matchesParamsWith(const PreintegratedRotation& other) const {
     return p_ == other.p_;
@@ -175,6 +172,9 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @name Main functionality
   /// @{
 
+  /// Re-initialize PreintegratedMeasurements
+  void resetIntegration();
+
   /**
    * @brief Calculate an incremental rotation given the gyro measurement and a
    * time interval, and update both deltaTij_ and deltaRij_.

gtsam/navigation/navigation.i

@@ -97,6 +97,29 @@ virtual class PreintegratedRotationParams {
   void serialize() const;
 };
 
+class PreintegratedRotation {
+  // Constructors
+  PreintegratedRotation(const gtsam::PreintegratedRotationParams* params);
+
+  // Standard Interface
+  void resetIntegration();
+  void integrateGyroMeasurement(const gtsam::Vector&  measuredOmega, const gtsam::Vector& biasHat, double deltaT);
+  gtsam::Rot3 biascorrectedDeltaRij(const gtsam::Vector& biasOmegaIncr) const;
+  gtsam::Vector integrateCoriolis(const gtsam::Rot3& rot_i) const;
+
+  // Access instance variables
+  double deltaTij() const;
+  gtsam::Rot3 deltaRij() const;
+  gtsam::Matrix delRdelBiasOmega() const;
+
+  // Testable
+  void print(string s = "") const;
+  bool equals(const gtsam::PreintegratedRotation& expected, double tol) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 #include <gtsam/navigation/PreintegrationParams.h>
 virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   PreintegrationParams(gtsam::Vector n_gravity);