Merge pull request #2122 from mkielo3/gal

Adding Galilean3 Lie group

gtsam/geometry/Event.cpp

@@ -17,7 +17,7 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam/geometry/Event.h>
 #include <iostream>
 
 namespace gtsam {

gtsam/geometry/Event.h

@@ -20,7 +20,7 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
-#include <gtsam_unstable/dllexport.h>
+#include <gtsam/dllexport.h>
 
 #include <cmath>
 #include <iosfwd>
@@ -34,7 +34,7 @@ namespace gtsam {
  * SLAM, where we have "time of arrival" measurements at a set of sensors. The
  * TOA functor below provides a measurement function for those applications.
  */
-class GTSAM_UNSTABLE_EXPORT Event {
+class GTSAM_EXPORT Event {
   double time_;      ///< Time event was generated
   Point3 location_;  ///< Location at time event was generated
 
@@ -84,7 +84,7 @@ template <>
 struct traits<Event> : internal::Manifold<Event> {};
 
 /// Time of arrival to given sensor
-class TimeOfArrival {
+class GTSAM_EXPORT TimeOfArrival {
   const double speed_;  ///< signal speed
 
  public:

gtsam/geometry/Gal3.cpp

@@ -0,0 +1,493 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    Gal3.cpp
+ * @brief   Implementation of 3D Galilean Group SGal(3) state
+ * @authors Matt Kielo, Scott Baker, Frank Dellaert
+ * @date    April 30, 2025
+ *
+ * This implementation is based on the paper:
+ * Kelly, J. (2023). "All About the Galilean Group SGal(3)"
+ * arXiv:2312.07555
+ *
+ * All section, equation, and page references in comments throughout this file
+ * refer to the aforementioned paper.
+ */
+
+
+#include <gtsam/geometry/Gal3.h>
+#include <gtsam/geometry/SO3.h>
+#include <gtsam/geometry/Event.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/geometry/concepts.h>
+
+#include <iostream>
+#include <cmath>
+#include <functional>
+
+namespace gtsam {
+
+//------------------------------------------------------------------------------
+// Constants and Helper function for Expmap/Logmap
+//------------------------------------------------------------------------------
+namespace { // Anonymous namespace for internal linkage
+  constexpr double kSmallAngleThreshold = 1e-10;
+
+  // The type of the Lie algebra (matrix representation)
+  using LieAlgebra = Matrix5;
+
+  // Helper functions for accessing tangent vector components
+  Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
+  Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
+  Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
+  Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
+  // Const versions
+  Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
+  Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
+  Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
+  Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
+
+} // end anonymous namespace
+
+//------------------------------------------------------------------------------
+// Static Constructor/Create functions
+//------------------------------------------------------------------------------
+Gal3 Gal3::Create(const Rot3& R, const Point3& r, const Velocity3& v, double t,
+                    OptionalJacobian<10, 3> H1, OptionalJacobian<10, 3> H2,
+                    OptionalJacobian<10, 3> H3, OptionalJacobian<10, 1> H4) {
+      if (H1) {
+        H1->setZero();
+        H1->block<3, 3>(6, 0) = Matrix3::Identity();
+      }
+      if (H2) {
+        H2->setZero();
+        H2->block<3, 3>(0, 0) = R.transpose();
+      }
+      if (H3) {
+        H3->setZero();
+        H3->block<3, 3>(3, 0) = R.transpose();
+      }
+      if (H4) {
+        H4->setZero();
+        Vector3 drho_dt = -R.transpose() * v;
+        H4->block<3, 1>(0, 0) = drho_dt;
+        (*H4)(9, 0) = 1.0;
+      }
+      return Gal3(R, r, v, t);
+}
+
+//------------------------------------------------------------------------------
+Gal3 Gal3::FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t,
+                                OptionalJacobian<10, 6> H1, OptionalJacobian<10, 3> H2,
+                                OptionalJacobian<10, 1> H3) {
+    const Rot3& R = pose.rotation();
+    const Point3& r = pose.translation();
+    if (H1) {
+        H1->setZero();
+        H1->block<3, 3>(6, 0) = Matrix3::Identity();
+        H1->block<3, 3>(0, 3) = Matrix3::Identity();
+    }
+    if (H2) {
+        H2->setZero();
+        H2->block<3, 3>(3, 0) = R.transpose();
+    }
+    if (H3) {
+        H3->setZero();
+        Vector3 drho_dt = -R.transpose() * v;
+        H3->block<3, 1>(0, 0) = drho_dt;
+        (*H3)(9, 0) = 1.0;
+    }
+    return Gal3(R, r, v, t);
+}
+
+//------------------------------------------------------------------------------
+// Constructors
+//------------------------------------------------------------------------------
+Gal3::Gal3(const Matrix5& M) {
+    // Constructor from 5x5 matrix representation (Equation 9, Page 5)
+    if (std::abs(M(3, 3) - 1.0) > 1e-9 || std::abs(M(4, 4) - 1.0) > 1e-9 ||
+        M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) {
+        throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones.");
+    }
+    R_ = Rot3(M.block<3, 3>(0, 0));
+    v_ = M.block<3, 1>(0, 3);
+    r_ = Point3(M.block<3, 1>(0, 4));
+    t_ = M(3, 4);
+}
+
+//------------------------------------------------------------------------------
+// Component Access
+//------------------------------------------------------------------------------
+const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const {
+    if (H) {
+        H->setZero();
+        H->block<3, 3>(0, 6) = Matrix3::Identity();
+    }
+    return R_;
+}
+
+//------------------------------------------------------------------------------
+const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const {
+     if (H) {
+        H->setZero();
+        H->block<3,3>(0, 0) = R_.matrix();
+        H->block<3,1>(0, 9) = v_;
+    }
+    return r_;
+}
+
+//------------------------------------------------------------------------------
+const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const {
+     if (H) {
+        H->setZero();
+        H->block<3, 3>(0, 3) = R_.matrix();
+     }
+    return v_;
+}
+
+//------------------------------------------------------------------------------
+const double& Gal3::time(OptionalJacobian<1, 10> H) const {
+    if (H) {
+        H->setZero();
+        (*H)(0, 9) = 1.0;
+    }
+    return t_;
+}
+
+//------------------------------------------------------------------------------
+// Matrix Representation
+//------------------------------------------------------------------------------
+Matrix5 Gal3::matrix() const {
+    // Returns 5x5 matrix representation as in Equation 9, Page 5
+    Matrix5 M = Matrix5::Identity();
+    M.block<3, 3>(0, 0) = R_.matrix();
+    M.block<3, 1>(0, 3) = v_;
+    M.block<3, 1>(0, 4) = Vector3(r_);
+    M(3, 4) = t_;
+    M.block<1,3>(3,0).setZero();
+    M.block<1,4>(4,0).setZero();
+    return M;
+}
+
+//------------------------------------------------------------------------------
+// Stream operator
+//------------------------------------------------------------------------------
+std::ostream& operator<<(std::ostream& os, const Gal3& state) {
+    os << "R: " << state.R_ << "\n";
+    os << "r: " << state.r_.transpose() << "\n";
+    os << "v: " << state.v_.transpose() << "\n";
+    os << "t: " << state.t_;
+    return os;
+}
+
+//------------------------------------------------------------------------------
+// Testable Requirements
+//------------------------------------------------------------------------------
+void Gal3::print(const std::string& s) const {
+    std::cout << (s.empty() ? "" : s + " ");
+    std::cout << *this << std::endl;
+}
+
+//------------------------------------------------------------------------------
+bool Gal3::equals(const Gal3& other, double tol) const {
+    return R_.equals(other.R_, tol) &&
+           traits<Point3>::Equals(r_, other.r_, tol) &&
+           traits<Velocity3>::Equals(v_, other.v_, tol) &&
+           std::abs(t_ - other.t_) < tol;
+}
+
+//------------------------------------------------------------------------------
+// Group Operations
+//------------------------------------------------------------------------------
+Gal3 Gal3::inverse() const {
+    // Implements inverse formula from Equation 10, Page 5
+    const Rot3 Rinv = R_.inverse();
+    const Velocity3 v_inv = -(Rinv.rotate(v_));
+    const Point3 r_inv = -(Rinv.rotate(Vector3(r_) - t_ * v_));
+    const double t_inv = -t_;
+    return Gal3(Rinv, r_inv, v_inv, t_inv);
+}
+
+//------------------------------------------------------------------------------
+Gal3 Gal3::operator*(const Gal3& other) const {
+    // Implements group composition through matrix multiplication
+    const Gal3& g1 = *this;
+    const Gal3& g2 = other;
+
+    const Rot3 R_comp = g1.R_.compose(g2.R_);
+    const Vector3 r1_vec(g1.r_);
+    const Vector3 r2_vec(g2.r_);
+    const Vector3 r_comp_vec = g1.R_.rotate(r2_vec) + g2.t_ * g1.v_ + r1_vec;
+    const Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_;
+    const double t_comp = g1.t_ + g2.t_;
+
+    return Gal3(R_comp, Point3(r_comp_vec), v_comp, t_comp);
+}
+
+//------------------------------------------------------------------------------
+// Lie Group Static Functions
+//------------------------------------------------------------------------------
+gtsam::Gal3 gtsam::Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) {
+    // Implements exponential map from Equations 16-19, Pages 7-8
+    const Vector3 rho_tan = rho(xi);
+    const Vector3 nu_tan = nu(xi);
+    const Vector3 theta_tan = theta(xi);
+    const double t_tan_val = t_tan(xi)(0);
+
+    const gtsam::so3::DexpFunctor dexp_functor(theta_tan);
+    const Rot3 R = Rot3::Expmap(theta_tan);
+    const Matrix3 Jl_theta = dexp_functor.leftJacobian();
+
+    Matrix3 E;
+    if (dexp_functor.nearZero) {
+         // Small angle approximation for E matrix (from Equation 19, Page 8)
+         E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor.W + (1.0 / 24.0) * dexp_functor.WW;
+    } else {
+         // Closed form for E matrix (from Equation 19, Page 8)
+         const double B_E = (1.0 - 2.0 * dexp_functor.B) / (2.0 * dexp_functor.theta2);
+         E = 0.5 * Matrix3::Identity() + dexp_functor.C * dexp_functor.W + B_E * dexp_functor.WW;
+    }
+
+    const Point3 r_final = Point3(Jl_theta * rho_tan + E * (t_tan_val * nu_tan));
+    const Velocity3 v_final = Jl_theta * nu_tan;
+    const double t_final = t_tan_val;
+
+    Gal3 result(R, r_final, v_final, t_final);
+
+    if (Hxi) {
+        *Hxi = Gal3::ExpmapDerivative(xi);
+    }
+
+    return result;
+}
+
+//------------------------------------------------------------------------------
+Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) {
+    // Implements logarithmic map from Equations 20-23, Page 8
+    const Vector3 theta_vec = Rot3::Logmap(g.R_);
+    const gtsam::so3::DexpFunctor dexp_functor_log(theta_vec);
+    const Matrix3 Jl_theta_inv = dexp_functor_log.leftJacobianInverse();
+
+    Matrix3 E;
+    if (dexp_functor_log.nearZero) {
+         // Small angle approximation for E matrix
+         E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor_log.W + (1.0 / 24.0) * dexp_functor_log.WW;
+    } else {
+         // Closed form for E matrix (from Equation 19, Page 8)
+         const double B_E = (1.0 - 2.0 * dexp_functor_log.B) / (2.0 * dexp_functor_log.theta2);
+         E = 0.5 * Matrix3::Identity() + dexp_functor_log.C * dexp_functor_log.W + B_E * dexp_functor_log.WW;
+    }
+
+    const Vector3 r_vec = Vector3(g.r_);
+    const Velocity3& v_vec = g.v_;
+    const double& t_val = g.t_;
+
+    // Implementation of Equation 23, Page 8
+    const Vector3 nu_tan = Jl_theta_inv * v_vec;
+    const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan));
+    const double t_tan_val = t_val;
+
+    Vector10 xi;
+    rho(xi) = rho_tan;
+    nu(xi) = nu_tan;
+    theta(xi) = theta_vec;
+    t_tan(xi)(0) = t_tan_val;
+
+    if (Hg) {
+        *Hg = Gal3::LogmapDerivative(g);
+    }
+
+    return xi;
+}
+
+//------------------------------------------------------------------------------
+Matrix10 Gal3::AdjointMap() const {
+    // Implements adjoint map as in Equation 26, Page 9
+    const Matrix3 Rmat = R_.matrix();
+    const Vector3 v_vec = v_;
+    const Vector3 r_minus_tv = Vector3(r_) - t_ * v_;
+
+    Matrix10 Ad = Matrix10::Zero();
+
+    Ad.block<3,3>(0,0) = Rmat;
+    Ad.block<3,3>(0,3) = -t_ * Rmat;
+    Ad.block<3,3>(0,6) = skewSymmetric(r_minus_tv) * Rmat;
+    Ad.block<3,1>(0,9) = v_vec;
+
+    Ad.block<3,3>(3,3) = Rmat;
+    Ad.block<3,3>(3,6) = skewSymmetric(v_vec) * Rmat;
+
+    Ad.block<3,3>(6,6) = Rmat;
+
+    Ad(9,9) = 1.0;
+
+    return Ad;
+}
+
+//------------------------------------------------------------------------------
+Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const {
+    Matrix10 Ad = AdjointMap();
+    Vector10 y = Ad * xi;
+
+    if (H_xi) {
+        *H_xi = Ad;
+    }
+
+    if (H_g) {
+        // NOTE: Using numerical derivative for the Jacobian with respect to
+        // the group element instead of deriving the analytical expression.
+        // Future work to use analytical instead.
+        std::function<Vector10(const Gal3&, const Vector10&)> adjoint_action_wrt_g =
+          [&](const Gal3& g_in, const Vector10& xi_in) {
+              return g_in.Adjoint(xi_in);
+          };
+        *H_g = numericalDerivative21(adjoint_action_wrt_g, *this, xi, 1e-7);
+    }
+    return y;
+}
+
+//------------------------------------------------------------------------------
+Matrix10 Gal3::adjointMap(const Vector10& xi) {
+    // Implements adjoint representation as in Equation 28, Page 10
+    const Matrix3 Theta_hat = skewSymmetric(theta(xi));
+    const Matrix3 Nu_hat = skewSymmetric(nu(xi));
+    const Matrix3 Rho_hat = skewSymmetric(rho(xi));
+    const double t_val = t_tan(xi)(0);
+    const Vector3 nu_vec = nu(xi);
+
+    Matrix10 ad = Matrix10::Zero();
+
+    ad.block<3,3>(0,0) = Theta_hat;
+    ad.block<3,3>(0,3) = -t_val * Matrix3::Identity();
+    ad.block<3,3>(0,6) = Rho_hat;
+    ad.block<3,1>(0,9) = nu_vec;
+
+    ad.block<3,3>(3,3) = Theta_hat;
+    ad.block<3,3>(3,6) = Nu_hat;
+
+    ad.block<3,3>(6,6) = Theta_hat;
+
+    return ad;
+}
+
+//------------------------------------------------------------------------------
+Vector10 Gal3::adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi, OptionalJacobian<10, 10> Hy) {
+    Matrix10 ad_xi = adjointMap(xi);
+    if (Hy) *Hy = ad_xi;
+    if (Hxi) {
+         *Hxi = -adjointMap(y);
+    }
+    return ad_xi * y;
+}
+
+//------------------------------------------------------------------------------
+Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) {
+    // Related to the left Jacobian in Equations 31-36, Pages 10-11
+    // NOTE: Using numerical approximation instead of implementing the analytical
+    // expression for the Jacobian. Future work to replace this
+    // with analytical derivative.
+    if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity();
+    std::function<Gal3(const Vector10&)> fn =
+        [](const Vector10& v) { return Gal3::Expmap(v); };
+    return numericalDerivative11<Gal3, Vector10>(fn, xi, 1e-5);
+}
+
+//------------------------------------------------------------------------------
+Matrix10 Gal3::LogmapDerivative(const Gal3& g) {
+    // Related to the inverse of left Jacobian in Equations 31-36, Pages 10-11
+    // NOTE: Using numerical approximation instead of implementing the analytical
+    // expression for the inverse Jacobian. Future work to replace this
+    // with analytical derivative.
+    Vector10 xi = Gal3::Logmap(g);
+    if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity();
+    std::function<Vector10(const Gal3&)> fn =
+        [](const Gal3& g_in) { return Gal3::Logmap(g_in); };
+    return numericalDerivative11<Vector10, Gal3>(fn, g, 1e-5);
+}
+
+//------------------------------------------------------------------------------
+// Lie Algebra (Hat/Vee maps)
+//------------------------------------------------------------------------------
+Matrix5 Gal3::Hat(const Vector10& xi) {
+    // Implements hat operator as in Equation 13, Page 6
+    const Vector3 rho_tan = rho(xi);
+    const Vector3 nu_tan = nu(xi);
+    const Vector3 theta_tan = theta(xi);
+    const double t_tan_val = t_tan(xi)(0);
+
+    Matrix5 X = Matrix5::Zero();
+    X.block<3, 3>(0, 0) = skewSymmetric(theta_tan);
+    X.block<3, 1>(0, 3) = nu_tan;
+    X.block<3, 1>(0, 4) = rho_tan;
+    X(3, 4) = t_tan_val;
+    return X;
+}
+
+//------------------------------------------------------------------------------
+Vector10 Gal3::Vee(const Matrix5& X) {
+    // Implements vee operator (inverse of hat operator in Equation 13, Page 6)
+    if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9 || std::abs(X(3,3)) > 1e-9) {
+     throw std::invalid_argument("Matrix is not in sgal(3)");
+    }
+
+    Vector10 xi;
+    rho(xi) = X.block<3, 1>(0, 4);
+    nu(xi) = X.block<3, 1>(0, 3);
+    const Matrix3& S = X.block<3, 3>(0, 0);
+    theta(xi) << S(2, 1), S(0, 2), S(1, 0);
+    t_tan(xi)(0) = X(3, 4);
+    return xi;
+}
+
+//------------------------------------------------------------------------------
+// ChartAtOrigin
+//------------------------------------------------------------------------------
+Gal3 Gal3::ChartAtOrigin::Retract(const Vector10& xi, ChartJacobian Hxi) {
+  return Gal3::Expmap(xi, Hxi);
+}
+
+//------------------------------------------------------------------------------
+Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) {
+  return Gal3::Logmap(g, Hg);
+}
+
+//------------------------------------------------------------------------------
+Event Gal3::act(const Event& e, OptionalJacobian<4, 10> Hself,
+                OptionalJacobian<4, 4> He) const {
+  // Implements group action on events (spacetime points) as described in Section 4.1, Page 3-4
+  const double& t_in = e.time();
+  const Point3& p_in = e.location();
+
+  const double t_out = t_in + t_;
+  const Point3 p_out = R_.rotate(p_in) + v_ * t_in + r_;
+
+  if (He) {
+    He->setZero();
+    (*He)(0, 0) = 1.0;
+    He->block<3, 1>(1, 0) = v_;
+    He->block<3, 3>(1, 1) = R_.matrix();
+  }
+
+  if (Hself) {
+    Hself->setZero();
+    const Matrix3 Rmat = R_.matrix();
+
+    (*Hself)(0, 9) = 1.0;
+    Hself->block<3, 3>(1, 0) = Rmat;
+    Hself->block<3, 3>(1, 3) = Rmat * t_in;
+    Hself->block<3, 3>(1, 6) = -Rmat * skewSymmetric(p_in);
+    Hself->block<3, 1>(1, 9) = v_;
+  }
+
+  return Event(t_out, p_out);
+}
+
+} // namespace gtsam

gtsam/geometry/Gal3.h

@@ -0,0 +1,235 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+
+* All Rights Reserved
+* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+* See LICENSE for the license information
+* -------------------------------------------------------------------------- */
+
+/**
+ * @file    Gal3.h
+ * @brief   3D Galilean Group SGal(3) state (attitude, position, velocity, time)
+ * @authors Matt Kielo, Scott Baker, Frank Dellaert
+ * @date    April 30, 2025
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Pose3.h> // Includes Rot3, Point3
+#include <gtsam/geometry/Event.h>
+#include <gtsam/base/Lie.h>       // For LieGroup base class and traits
+#include <gtsam/base/Manifold.h>  // For Manifold traits
+
+#include <cmath> // For std::sqrt, std::cos, std::sin
+#include <functional> // For std::function used in numerical derivatives
+
+namespace gtsam {
+
+// Forward declaration
+class Gal3;
+
+// Use Vector3 for velocity for consistency with NavState
+using Velocity3 = Vector3;
+// Define Vector10 for tangent space
+using Vector10 = Eigen::Matrix<double, 10, 1>;
+// Define Matrix5 for Lie Algebra matrix representation
+using Matrix5 = Eigen::Matrix<double, 5, 5>;
+// Define Matrix10 for Jacobians
+using Matrix10 = Eigen::Matrix<double, 10, 10>;
+
+/**
+ * Represents an element of the 3D Galilean group SGal(3).
+ * Internal state: rotation, translation, velocity, time.
+ */
+class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
+ private:
+  Rot3 R_;      ///< Rotation world R body
+  Point3 r_;    ///< Position in world frame, n_r_b
+  Velocity3 v_; ///< Velocity in world frame, n_v_b
+  double t_;    ///< Time component
+
+ public:
+
+  /// The dimension of the tangent space
+  inline static constexpr size_t dimension = 10;
+
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor: Identity element
+  Gal3() : R_(Rot3::Identity()), r_(Point3::Zero()), v_(Velocity3::Zero()), t_(0.0) {}
+
+  /// Construct from attitude, position, velocity, time
+  Gal3(const Rot3& R, const Point3& r, const Velocity3& v, double t) :
+    R_(R), r_(r), v_(v), t_(t) {}
+
+  /// Construct from a 5x5 matrix representation
+  explicit Gal3(const Matrix5& M);
+
+  /// Named constructor from components with derivatives
+  static Gal3 Create(const Rot3& R, const Point3& r, const Velocity3& v, double t,
+                    OptionalJacobian<10, 3> H1 = {},
+                    OptionalJacobian<10, 3> H2 = {},
+                    OptionalJacobian<10, 3> H3 = {},
+                    OptionalJacobian<10, 1> H4 = {});
+
+  /// Named constructor from Pose3, velocity, and time with derivatives
+  static Gal3 FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t,
+                                OptionalJacobian<10, 6> H1 = {},
+                                OptionalJacobian<10, 3> H2 = {},
+                                OptionalJacobian<10, 1> H3 = {});
+
+  /// @}
+  /// @name Component Access
+  /// @{
+
+  /// Access rotation component (Rot3)
+  const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const;
+
+  /// Access translation component (Point3)
+  const Point3& translation(OptionalJacobian<3, 10> H = {}) const;
+
+  /// Access velocity component (Vector3)
+  const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const;
+
+  /// Access time component (double)
+  const double& time(OptionalJacobian<1, 10> H = {}) const;
+
+  // Convenience accessors matching NavState names
+  const Rot3& attitude(OptionalJacobian<3, 10> H = {}) const { return rotation(H); }
+  const Point3& position(OptionalJacobian<3, 10> H = {}) const { return translation(H); }
+
+  /// @}
+  /// @name Derived quantities
+  /// @{
+
+  /// Return rotation matrix (Matrix3)
+  Matrix3 R() const { return R_.matrix(); }
+
+  /// Return position as Vector3
+  Vector3 r() const { return Vector3(r_); } // Conversion from Point3
+
+  /// Return velocity as Vector3
+  const Vector3& v() const { return v_; }
+
+  /// Return time scalar
+  const double& t() const { return t_; }
+
+  /// Return 5x5 homogeneous matrix representation
+  Matrix5 matrix() const;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Output stream operator
+  GTSAM_EXPORT
+  friend std::ostream &operator<<(std::ostream &os, const Gal3& state);
+
+  /// Print with optional string prefix
+  void print(const std::string& s = "") const;
+
+  /// Check equality within tolerance
+  bool equals(const Gal3& other, double tol = 1e-9) const;
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// Return the identity element
+  static Gal3 Identity() { return Gal3(); }
+
+  /// Return the inverse of this element
+  Gal3 inverse() const;
+
+  // Bring LieGroup::inverse() into scope (version with derivative)
+  using LieGroup<Gal3, 10>::inverse;
+
+  /// Group composition operator
+  Gal3 operator*(const Gal3& other) const;
+
+  /// @}
+  /// @name Group Action
+  /// @{
+
+  /**
+   * Apply Galilean transform to a spacetime Event
+   * @param e Input event (location, time)
+   * @param Hself Optional Jacobian wrt this Gal3 element's tangent space
+   * @param He Optional Jacobian wrt the input event's tangent space
+   * @return Transformed event
+   */
+  Event act(const Event& e, OptionalJacobian<4, 10> Hself = {},
+            OptionalJacobian<4, 4> He = {}) const;
+
+  /// @}
+  /// @name Lie Group Static Functions
+  /// @{
+
+  /// Exponential map at identity: tangent vector xi -> manifold element g
+  static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {});
+
+  /// Logarithmic map at identity: manifold element g -> tangent vector xi
+  static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});
+
+  /// Calculate Adjoint map Ad_g
+  Matrix10 AdjointMap() const;
+
+  /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at identity
+  Vector10 Adjoint(const Vector10& xi_base, OptionalJacobian<10, 10> H_g = {},
+                  OptionalJacobian<10, 10> H_xi = {}) const;
+
+  /// The adjoint action `ad(xi, y)` = `adjointMap(xi) * y`
+  static Vector10 adjoint(const Vector10& xi, const Vector10& y,
+                         OptionalJacobian<10, 10> Hxi = {},
+                         OptionalJacobian<10, 10> Hy = {});
+
+  /// Compute the adjoint map `ad(xi)` associated with tangent vector xi
+  static Matrix10 adjointMap(const Vector10& xi);
+
+  /// Derivative of Expmap(xi) w.r.t. xi evaluated at xi
+  static Matrix10 ExpmapDerivative(const Vector10& xi);
+
+  /// Derivative of Logmap(g) w.r.t. g
+  static Matrix10 LogmapDerivative(const Gal3& g);
+
+  /// Chart at origin, uses Expmap/Logmap for Retract/Local
+  struct ChartAtOrigin {
+    static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {});
+    static Vector10 Local(const Gal3& g, ChartJacobian Hg = {});
+  };
+
+  /// Hat operator: maps tangent vector xi to Lie algebra matrix
+  static Matrix5 Hat(const Vector10& xi);
+
+  /// Vee operator: maps Lie algebra matrix to tangent vector xi
+  static Vector10 Vee(const Matrix5& X);
+
+  /// @}
+
+ private:
+  /// @name Serialization
+  /// @{
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(R_);
+    ar & BOOST_SERIALIZATION_NVP(r_);
+    ar & BOOST_SERIALIZATION_NVP(v_);
+    ar & BOOST_SERIALIZATION_NVP(t_);
+  }
+#endif
+  /// @}
+
+}; // class Gal3
+
+/// Traits specialization for Gal3
+template <>
+struct traits<Gal3> : public internal::LieGroup<Gal3> {};
+
+template <>
+struct traits<const Gal3> : public internal::LieGroup<Gal3> {};
+
+} // namespace gtsam

gtsam/geometry/geometry.i

@@ -213,7 +213,7 @@ namespace so3 {
     ExpmapFunctor(const gtsam::Vector3& axis, double angle);
     gtsam::Matrix3 expmap() const;
   };
-  
+
   virtual class DexpFunctor : gtsam::so3::ExpmapFunctor {
     const gtsam::Vector3 omega;
     const double C;  // (1 - A) / theta^2
@@ -228,7 +228,7 @@ namespace so3 {
     gtsam::Vector3 applyRightJacobianInverse(const gtsam::Vector3& v) const;
     gtsam::Vector3 applyLeftJacobian(const gtsam::Vector3& v) const;
     gtsam::Vector3 applyLeftJacobianInverse(const gtsam::Vector3& v) const;
-  };  
+  };
 }
 
 class SO3 {
@@ -427,7 +427,7 @@ class Rot3 {
                       Eigen::Ref<Eigen::MatrixXd> HR,
                       Eigen::Ref<Eigen::MatrixXd> Hp) const;
   gtsam::Unit3 unrotate(const gtsam::Unit3& p) const;
-  
+
   // Standard Interface
   gtsam::Matrix matrix() const;
   gtsam::Matrix transpose() const;
@@ -522,7 +522,7 @@ class Pose2 {
   // enabling serialization functionality
   void serialize() const;
 };
-  
+
 #include <gtsam/geometry/Pose3.h>
 class Pose3 {
   // Standard Constructors
@@ -573,12 +573,12 @@ class Pose3 {
   static gtsam::Matrix6 LogmapDerivative(const gtsam::Pose3& xi);
   static gtsam::Pose3 Expmap(gtsam::Vector xi, Eigen::Ref<Eigen::MatrixXd> Hxi);
   static gtsam::Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose);
-  
+
   gtsam::Pose3 expmap(gtsam::Vector v);
   gtsam::Pose3 expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
   gtsam::Vector logmap(const gtsam::Pose3& p);
   gtsam::Vector logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
-  
+
   gtsam::Matrix AdjointMap() const;
   gtsam::Vector Adjoint(gtsam::Vector xi_b) const;
   gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref<Eigen::MatrixXd> H_this,
@@ -718,7 +718,7 @@ class OrientedPlane3 {
 
   static size_t Dim();
   size_t dim() const;
-  
+
   gtsam::OrientedPlane3 retract(const gtsam::Vector3& v) const;
   gtsam::OrientedPlane3 retract(const gtsam::Vector3& v,
                         Eigen::Ref<Eigen::MatrixXd> H) const;
@@ -1491,7 +1491,7 @@ gtsam::TriangulationResult triangulateSafe(
     const gtsam::Point2Vector& measurements,
     const gtsam::TriangulationParameters& params);
 
-// Cal3Unified versions                                
+// Cal3Unified versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
                                 gtsam::Cal3Unified* sharedCal,
                                 const gtsam::Point2Vector& measurements,
@@ -1515,6 +1515,17 @@ gtsam::TriangulationResult triangulateSafe(
     const gtsam::TriangulationParameters& params);
 
 
+#include <gtsam/geometry/Event.h>
+class Event {
+  Event();
+  Event(double t, const gtsam::Point3& p);
+  Event(double t, double x, double y, double z);
+  double time() const;
+  gtsam::Point3 location() const;
+  double height() const;
+  void print(string s) const;
+};
+
 
 #include <gtsam/geometry/BearingRange.h>
 template <POSE, POINT, BEARING, RANGE>

gtsam/geometry/tests/testEvent.cpp

@@ -19,7 +19,7 @@
 
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/nonlinear/Expression.h>
-#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam/geometry/Event.h>
 
 #include <CppUnitLite/TestHarness.h>
 

gtsam_unstable/examples/TimeOfArrivalExample.cpp

@@ -21,7 +21,7 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/expressions.h>
-#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam/geometry/Event.h>
 #include <gtsam_unstable/slam/TOAFactor.h>
 
 #include <vector>

gtsam_unstable/gtsam_unstable.i

@@ -162,7 +162,7 @@ class BearingS2 {
   void serializable() const; // enabling serialization functionality
 };
 
-  
+
 #include <gtsam_unstable/geometry/SimWall2D.h>
 class SimWall2D {
   SimWall2D();
@@ -379,17 +379,6 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
 
 typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
 
-#include <gtsam_unstable/geometry/Event.h>
-class Event {
-  Event();
-  Event(double t, const gtsam::Point3& p);
-  Event(double t, double x, double y, double z);
-  double time() const;
-  gtsam::Point3 location() const;
-  double height() const;
-  void print(string s) const;
-};
-
 class TimeOfArrival {
   TimeOfArrival();
   TimeOfArrival(double speed);

gtsam_unstable/nonlinear/FixedLagSmoother.h

@@ -18,4 +18,4 @@
 #endif
 
 
-#include <gtsam/nonlinear/FixedLagSmoother.h>
+#include <gtsam/nonlinear/FixedLagSmoother.h>

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h

@@ -18,4 +18,4 @@
 #endif
 
 
-#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
+#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>

gtsam_unstable/slam/TOAFactor.h

@@ -20,7 +20,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/ExpressionFactor.h>
-#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam/geometry/Event.h>
 
 namespace gtsam {
 

gtsam_unstable/slam/tests/testTOAFactor.cpp

@@ -21,7 +21,7 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/expressions.h>
-#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam/geometry/Event.h>
 #include <gtsam_unstable/slam/TOAFactor.h>
 
 #include <CppUnitLite/TestHarness.h>

python/gtsam_unstable/examples/TimeOfArrivalExample.py

@@ -12,8 +12,9 @@ Author: Frank Dellaert
 # pylint: disable=invalid-name, no-name-in-module
 
 from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   NonlinearFactorGraph, Point3, Values, noiseModel)
+                   NonlinearFactorGraph, Point3, Values, noiseModel, Event, 
-from gtsam_unstable import Event, TimeOfArrival, TOAFactor
+                   TimeOfArrival)
+from gtsam_unstable import TOAFactor
 
 # units
 MS = 1e-3