From 2ae55d74823f36ab1514a2e44f0292214a53f28a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 5 Apr 2025 19:00:25 -0400
Subject: [PATCH] Add PreintegratedRotation to wrapper

---
 gtsam/navigation/PreintegratedRotation.cpp |  2 +-
 gtsam/navigation/PreintegratedRotation.h   | 14 ++++++-------
 gtsam/navigation/navigation.i              | 23 ++++++++++++++++++++++
 3 files changed, 31 insertions(+), 8 deletions(-)

diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp
index 6c8e510e5..00e41fa98 100644
--- a/gtsam/navigation/PreintegratedRotation.cpp
+++ b/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");
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index a887ef321..9473ec51d 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/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_.
diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i
index 99567509a..f66a38b47 100644
--- a/gtsam/navigation/navigation.i
+++ b/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);