From 39a7a5b62794a02b2590734ee27398c931bb096f Mon Sep 17 00:00:00 2001
From: jenniferoum <joum3@gatech.edu>
Date: Wed, 26 Mar 2025 21:41:07 -0700
Subject: [PATCH] Update wedge and vee to Rot3 Hat and Vee for EqF

---
 examples/ABC_EQF/EqF.cpp       | 10 +++++-----
 examples/ABC_EQF/G.cpp         |  6 +++---
 examples/ABC_EQF/Input.cpp     |  3 ++-
 examples/ABC_EQF/utilities.cpp | 20 +-------------------
 examples/ABC_EQF/utilities.h   |  2 --
 5 files changed, 11 insertions(+), 30 deletions(-)

diff --git a/examples/ABC_EQF/EqF.cpp b/examples/ABC_EQF/EqF.cpp
index 418d9ac08..32414bb74 100644
--- a/examples/ABC_EQF/EqF.cpp
+++ b/examples/ABC_EQF/EqF.cpp
@@ -38,12 +38,12 @@ State stateAction(const G& X, const State& xi) {
     }
 
     return State(xi.R * X.A,
-                X.A.inverse().matrix() * (xi.b - vee(X.a)),
+                X.A.inverse().matrix() * (xi.b - Rot3::Vee(X.a)),
                 new_S);
 }
 
 Input velocityAction(const G& X, const Input& u) {
-    return Input(X.A.inverse().matrix() * (u.w - vee(X.a)), u.Sigma);
+    return Input(X.A.inverse().matrix() * (u.w - Rot3::Vee(X.a)), u.Sigma);
 }
 
 Vector3 outputAction(const G& X, const Direction& y, int idx) {
@@ -182,7 +182,7 @@ void EqF::update(const Measurement& y) {
 
   Matrix Ct = __measurementMatrixC(y.d, y.cal_idx);
   Vector3 action_result = outputAction(__X_hat.inv(), y.y, y.cal_idx);
-  Vector3 delta_vec = wedge(y.d.d.unitVector()) * action_result;  // Ensure this is the right operation
+  Vector3 delta_vec = Rot3::Hat(y.d.d.unitVector()) * action_result;  // Ensure this is the right operation
   Matrix Dt = __outputMatrixDt(y.cal_idx);
   Matrix S = Ct * __Sigma * Ct.transpose() + Dt * y.Sigma * Dt.transpose();
   Matrix K = __Sigma * Ct.transpose() * S.inverse();
@@ -255,10 +255,10 @@ Matrix EqF::__measurementMatrixC(const Direction& d, int idx) const {
   if (idx >= 0) {
     // Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector()); // WRONG
     // Set the correct 3x3 block in Cc
-    Cc.block<3, 3>(0, 3 * idx) = wedge(d.d.unitVector());
+    Cc.block<3, 3>(0, 3 * idx) = Rot3::Hat(d.d.unitVector());
   }
 
-  Matrix3 wedge_d = wedge(d.d.unitVector());
+  Matrix3 wedge_d = Rot3::Hat(d.d.unitVector());
 
   // This Matrix concatenation was different from the Python version
   // Create the equivalent of:
diff --git a/examples/ABC_EQF/G.cpp b/examples/ABC_EQF/G.cpp
index 6cbdd1e3e..1c9684e90 100644
--- a/examples/ABC_EQF/G.cpp
+++ b/examples/ABC_EQF/G.cpp
@@ -19,7 +19,7 @@ G G::operator*(const G& other) const {
     }
     
     return G(A * other.A,
-            a + wedge(A.matrix() * vee(other.a)),
+            a + Rot3::Hat(A.matrix() * Rot3::Vee(other.a)),
             new_B);
 }
 
@@ -32,7 +32,7 @@ G G::inv() const {
     }
     
     return G(A.inverse(), 
-            -wedge(A_inv * vee(a)),
+            -Rot3::Hat(A_inv * Rot3::Vee(a)),
             B_inv);
 }
 
@@ -50,7 +50,7 @@ G G::exp(const Vector& x) {
     Rot3 A = Rot3::Expmap(x.head<3>());
     
     Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3);
-    Matrix3 a = wedge(a_vee);
+    Matrix3 a = Rot3::Hat(a_vee);
     
     std::vector<Rot3> B;
     for (int i = 0; i < n; i++) {
diff --git a/examples/ABC_EQF/Input.cpp b/examples/ABC_EQF/Input.cpp
index 4e55a4ed0..488102cdb 100644
--- a/examples/ABC_EQF/Input.cpp
+++ b/examples/ABC_EQF/Input.cpp
@@ -5,6 +5,7 @@
 #include "utilities.h"
 #include <Eigen/Dense>
 #include <stdexcept>
+#include "gtsam/geometry/Rot3.h"
 
 Input::Input(const Vector3& w, const Matrix& Sigma)
     : w(w), Sigma(Sigma) {
@@ -20,7 +21,7 @@ Input::Input(const Vector3& w, const Matrix& Sigma)
 }
 
 Matrix3 Input::W() const {
-    return wedge(w);
+    return Rot3::Hat(w);
 }
 
 Input Input::random() {
diff --git a/examples/ABC_EQF/utilities.cpp b/examples/ABC_EQF/utilities.cpp
index e35481ad3..5166cb0d6 100644
--- a/examples/ABC_EQF/utilities.cpp
+++ b/examples/ABC_EQF/utilities.cpp
@@ -5,26 +5,8 @@
 #include <cmath>
 
 // Global configuration
-const std::string COORDINATE = "EXPONENTIAL";
+const std::string COORDINATE = "EXPONENTIAL";  // Alternative: "NORMAL"
 
-Matrix3 wedge(const Vector3& vec) {
-    Matrix3 mat;
-    mat << 0.0, -vec(2), vec(1),
-           vec(2), 0.0, -vec(0),
-           -vec(1), vec(0), 0.0;
-    return mat;
-}
-
-Vector3 vee(const Matrix3& mat) {
-    Vector3 vec;
-    vec << mat(2, 1), mat(0, 2), mat(1, 0);
-    return vec;
-}
-
-// Matrix3 Rot3LeftJacobian(const Vector3& arr) {
-//   return Rot3::ExpmapDerivative(-arr);
-//
-// }
 
 bool checkNorm(const Vector3& x, double tol) {
     return abs(x.norm() - 1) < tol || std::isnan(x.norm());
diff --git a/examples/ABC_EQF/utilities.h b/examples/ABC_EQF/utilities.h
index b3837ad22..50bbf88aa 100644
--- a/examples/ABC_EQF/utilities.h
+++ b/examples/ABC_EQF/utilities.h
@@ -9,7 +9,6 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
-#include <gtsam/geometry/Rot3.h>
 #include <Eigen/Dense>
 #include <functional>
 
@@ -22,7 +21,6 @@ extern const std::string COORDINATE;  // "EXPONENTIAL" or "NORMAL"
  * Utility functions
  */
 Matrix3 wedge(const Vector3& vec);
-Vector3 vee(const Matrix3& mat);
 bool checkNorm(const Vector3& x, double tol = 1e-3);
 Matrix blockDiag(const Matrix& A, const Matrix& B);
 Matrix repBlock(const Matrix& A, int n);