expose unit3/rot3 symmetry, abc python bindings for EqF

Author: rohan-bansal

gtsam/navigation/EquivariantFilter.h

@@ -222,6 +222,34 @@ class EquivariantFilter : public ManifoldEKF<M> {
     Base::predict(xi_next, Phi, Q_manifold);
   }
 
+  /**
+   * @brief Propagate the filter state with explicit Jacobian and lift vector.
+   *
+   * This overload is wrapper-friendly: takes a lifted tangent vector
+   * directly, avoiding templated Lift functors. It uses first-order Euler
+   * discretization (K = 1).
+   *
+   * @param Lambda Lifted tangent vector in the group tangent space.
+   * @param A Error dynamics matrix (DimM x DimM).
+   * @param Qc Process noise covariance on the manifold (continuous-time).
+   * @param dt Time step.
+   */
+  void predictWithJacobianEuler(const Vector& Lambda, const MatrixM& A,
+                                const MatrixM& Qc, double dt) {
+    TangentG lambda_g;
+    if constexpr (TangentG::RowsAtCompileTime == Eigen::Dynamic) {
+      lambda_g.resize(DimG);
+    }
+    lambda_g = Lambda;
+
+    g_ = traits<G>::Compose(g_, traits<G>::Expmap(lambda_g * dt));
+    M xi_next = act_on_ref_(g_);
+
+    MatrixM Phi = transitionMatrix<1>(A, dt);
+    CovarianceM Q_manifold = Qc * dt;
+    Base::predict(xi_next, Phi, Q_manifold);
+  }
+
   /**
    * Measurement update: Corrects the state and covariance using a
    * pre-calculated predicted measurement and its Jacobian.

gtsam/navigation/navigation.i

@@ -651,6 +651,51 @@ virtual class InvariantEKF : gtsam::LeftLinearEKF<G> {
   void predict(const gtsam::Vector& u, double dt, gtsam::Matrix Q);
 };
 
+// ---------------------------------------------------------------------------
+// Equivariant Filter (attitude example wrapper)
+#include <gtsam/navigation/EquivariantFilter.h>
+#include <gtsam_unstable/geometry/EquivariantFilterAttitude.h>
+template <M = {gtsam::Unit3}, Symmetry = {gtsam::attitude_example::Symmetry}>
+virtual class EquivariantFilter : gtsam::ManifoldEKF<M> {
+  // Constructors
+  EquivariantFilter(const M& xi_ref, gtsam::Matrix Sigma);
+  EquivariantFilter(const M& xi_ref, gtsam::Matrix Sigma,
+                    const Symmetry::Group& X0);
+
+  // Accessors
+  gtsam::Matrix errorCovariance() const;
+  gtsam::Matrix covariance() const;
+  Symmetry::Group groupEstimate() const;
+
+  // Wrapper-friendly predict
+  void predictWithJacobianEuler(const gtsam::Vector& Lambda, gtsam::Matrix A,
+                                gtsam::Matrix Qc, double dt);
+
+  // Only vector-based measurements are supported in wrapper
+  void updateWithVector(const gtsam::Vector& prediction, const gtsam::Matrix& H,
+                        const gtsam::Vector& z, const gtsam::Matrix& R);
+};
+
+// ---------------------------------------------------------------------------
+// ABC Equivariant Filter wrapper (N=1)
+#include <gtsam_unstable/geometry/ABCEqFWrapper.h>
+namespace abc {
+class AbcEquivariantFilter1 {
+  AbcEquivariantFilter1();
+  AbcEquivariantFilter1(const gtsam::Matrix& Sigma0);
+
+  void predict(const gtsam::Vector& omega, const gtsam::Matrix& inputCovariance,
+               double dt);
+  void update(const gtsam::Unit3& y, const gtsam::Unit3& d,
+              const gtsam::Matrix& R, int cal_idx);
+
+  gtsam::Rot3 attitude() const;
+  gtsam::Vector bias() const;
+  gtsam::Rot3 calibration(size_t i) const;
+  gtsam::Matrix errorCovariance() const;
+};
+}  // namespace abc
+
 // Specialized NavState IMU EKF
 #include <gtsam/navigation/NavStateImuEKF.h>
 class NavStateImuEKF : gtsam::LeftLinearEKF<gtsam::NavState> {

gtsam_unstable/geometry/ABCEqFWrapper.h

@@ -0,0 +1,71 @@
+/**
+ * @file ABCEqFWrapper.h
+ * @brief Wrapper-friendly ABC EqF helper class for Python bindings.
+ *
+ * This class mirrors the logic in examples/AbcEquivariantFilterExample.cpp
+ * but exposes a simple, non-templated API for wrapping.
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/navigation/EquivariantFilter.h>
+#include <gtsam_unstable/geometry/ABC.h>
+
+namespace gtsam {
+namespace abc {
+
+class AbcEquivariantFilter1 {
+ public:
+  static constexpr size_t N = 1;
+  using State1 = State<N>;
+  using Group1 = Group<N>;
+  using Symmetry1 = Symmetry<N>;
+  using EqFilter1 = gtsam::EquivariantFilter<State1, Symmetry1>;
+
+  AbcEquivariantFilter1() : AbcEquivariantFilter1(Matrix::Identity(6 + 3 * N, 6 + 3 * N)) {}
+
+  explicit AbcEquivariantFilter1(const Matrix& Sigma0)
+      : filter_(State1::identity(), Sigma0) {}
+
+  /// Propagate filter using the explicit matrices API from ABC.
+  void predict(const Vector3& omega, const Matrix6& inputCovariance, double dt) {
+    const Matrix Q = inputProcessNoise<N>(inputCovariance);
+    const Vector6 u = toInputVector(omega);
+    const Lift<N> lift_u(u);
+    const typename InputAction<N>::Orbit psi_u(u);
+
+    const Group1 X_hat = filter_.groupEstimate();
+    const Matrix A = stateMatrixA<N>(psi_u, X_hat);
+    const Matrix B = inputMatrixB<N>(X_hat);
+    const Matrix Qc = B * Q * B.transpose();
+
+    filter_.predictWithJacobian<2>(lift_u, A, Qc, dt);
+  }
+
+  /// Update filter with a direction measurement.
+  void update(const Unit3& y, const Unit3& d, const Matrix3& R, int cal_idx) {
+    const Innovation<N> innovation(y, d, cal_idx);
+    const Group1 X_hat = filter_.groupEstimate();
+    const Matrix3 D = outputMatrixD<N>(X_hat, cal_idx);
+    const Matrix3 R_adjusted = D * R * D.transpose();
+    filter_.update<Vector3>(innovation, Z_3x1, R_adjusted);
+  }
+
+  /// Accessors for current estimate.
+  Rot3 attitude() const { return filter_.state().R; }
+  Vector3 bias() const { return filter_.state().b; }
+  Rot3 calibration(size_t i) const { return filter_.state().S[i]; }
+  Matrix errorCovariance() const { return filter_.errorCovariance(); }
+
+ private:
+  EqFilter1 filter_;
+};
+
+}  // namespace abc
+}  // namespace gtsam
+
+

gtsam_unstable/geometry/EquivariantFilterAttitude.h

@@ -0,0 +1,40 @@
+/**
+ * @file EquivariantFilterAttitude.h
+ * @brief Attitude-only EqF helper types for Python bindings.
+ *
+ * Provides the symmetry action used by the Unit3/Rot3 attitude example in
+ * tests/testEquivariantFilter.cpp so it can be instantiated in wrappers.
+ */
+
+#pragma once
+
+#include <gtsam/base/GroupAction.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/navigation/EquivariantFilter.h>
+
+namespace gtsam {
+namespace attitude_example {
+
+using M = Unit3;
+using G = Rot3;
+
+/**
+ * Symmetry: right group action on Unit3.
+ *   φ_η(Q) = Q^T η.
+ */
+struct Symmetry : public GroupAction<Symmetry, G, M> {
+  static constexpr ActionType type = ActionType::Right;
+
+  M operator()(const M& eta, const G& Q,
+               OptionalJacobian<2, 2> H_eta = {},
+               OptionalJacobian<2, 3> H_Q = {}) const {
+    return Q.unrotate(eta, H_Q, H_eta);
+  }
+};
+
+}  // namespace attitude_example
+}  // namespace gtsam
+
+