Merge pull request borglab#2214 from borglab/feature/docsGal3

Add Gal3 and SL4 docs

Author: dellaert

.github/workflows/vcpkg.yml

@@ -16,15 +16,15 @@ jobs:
       fail-fast: false
       matrix:
         include:      
-          - os: windows-latest
-            triplet: x64-windows-release
-            build_type: Release
-            test_target: RUN_TESTS
-            binary_cache: C:\Users\runneradmin\AppData\Local\vcpkg\archives
-            python: python
-            # check_constraint_program fail on windows. should fix it for windows.
-            # Remove this excluded test when you fix it for windows.
-            ctest_extra_flags: -E check_constraint_program
+          #          - os: windows-latest
+#            triplet: x64-windows-release
+#            build_type: Release
+#            test_target: RUN_TESTS
+#            binary_cache: C:\Users\runneradmin\AppData\Local\vcpkg\archives
+#            python: python
+#            # check_constraint_program fail on windows. should fix it for windows.
+#            # Remove this excluded test when you fix it for windows.
+#            ctest_extra_flags: -E check_constraint_program
           - os: ubuntu-latest
             triplet: x64-linux-release
             build_type: Release

gtsam/geometry/Gal3.h

@@ -16,13 +16,12 @@
 
 #pragma once
 
-#include <gtsam/geometry/Pose3.h> // Includes Rot3, Point3
+#include <gtsam/base/MatrixLieGroup.h>
 #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 <gtsam/geometry/Pose3.h>
 
-#include <cmath> // For std::sqrt, std::cos, std::sin
-#include <functional> // For std::function used in numerical derivatives
+#include <cmath>       // For std::sqrt, std::cos, std::sin
+#include <functional>  // For std::function used in numerical derivatives
 
 namespace gtsam {
 
@@ -38,40 +37,41 @@ using Velocity3 = Vector3;
  */
 class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
  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
+  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:
-  using LieAlgebra = Matrix5;
-  using Vector25 = Eigen::Matrix<double, 25, 1>;
-
   /// @name Constructors
   /// @{
 
   /// Default constructor: Identity element
-  Gal3() : R_(Rot3::Identity()), r_(Point3::Zero()), v_(Velocity3::Zero()), t_(0.0) {}
+  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) {}
+  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 = {});
+  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 = {});
+  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
@@ -89,9 +89,13 @@ class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
   /// 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); }
+  // Accessors when viewed as a "pose" manifold
+  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
@@ -101,27 +105,21 @@ class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
   Matrix3 R() const { return R_.matrix(); }
 
   /// Return position as Vector3
-  Vector3 r() const { return Vector3(r_); } // Conversion from Point3
+  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;
-
-  /// Vectorize 5x5 matrix into a 25-dim vector.
-  Vector25 vec(OptionalJacobian<25, 10> H = {}) const;
-
   /// @}
   /// @name Testable
   /// @{
 
   /// Output stream operator
   GTSAM_EXPORT
-  friend std::ostream &operator<<(std::ostream &os, const Gal3& state);
+  friend std::ostream& operator<<(std::ostream& os, const Gal3& state);
 
   /// Print with optional string prefix
   void print(const std::string& s = "") const;
@@ -160,26 +158,29 @@ class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
             OptionalJacobian<4, 4> He = {}) const;
 
   /// @}
-  /// @name Lie Group Static Functions
+  /// @name Lie Group
   /// @{
 
   /// Exponential map at identity: tangent vector xi -> manifold element g
-  static Gal3 Expmap(const TangentVector& xi, OptionalJacobian<10, 10> Hxi = {});
+  static Gal3 Expmap(const TangentVector& xi,
+                     OptionalJacobian<10, 10> Hxi = {});
 
   /// Logarithmic map at identity: manifold element g -> tangent vector xi
   static TangentVector Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});
 
   /// Calculate Adjoint map Ad_g
   Jacobian AdjointMap() const;
 
-  /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at identity
-  TangentVector Adjoint(const TangentVector& xi_base, OptionalJacobian<10, 10> H_g = {},
-                  OptionalJacobian<10, 10> H_xi = {}) const;
+  /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at
+  /// identity
+  TangentVector Adjoint(const TangentVector& xi_base,
+                        OptionalJacobian<10, 10> H_g = {},
+                        OptionalJacobian<10, 10> H_xi = {}) const;
 
   /// The adjoint action `ad(xi, y)` = `adjointMap(xi) * y`
   static TangentVector adjoint(const TangentVector& xi, const TangentVector& y,
-                         OptionalJacobian<10, 10> Hxi = {},
-                         OptionalJacobian<10, 10> Hy = {});
+                               OptionalJacobian<10, 10> Hxi = {},
+                               OptionalJacobian<10, 10> Hy = {});
 
   /// Compute the adjoint map `ad(xi)` associated with tangent vector xi
   static Jacobian adjointMap(const TangentVector& xi);
@@ -196,6 +197,19 @@ class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
     static TangentVector Local(const Gal3& g, ChartJacobian Hg = {});
   };
 
+  /// @}
+  /// @name Matrix Lie Group
+  /// @{
+
+  using LieAlgebra = Matrix5;
+  using Vector25 = Eigen::Matrix<double, 25, 1>;
+
+  /// Return 5x5 homogeneous matrix representation
+  Matrix5 matrix() const;
+
+  /// Vectorize 5x5 matrix into a 25-dim vector.
+  Vector25 vec(OptionalJacobian<25, 10> H = {}) const;
+
   /// Hat operator: maps tangent vector xi to Lie algebra matrix
   static LieAlgebra Hat(const TangentVector& xi);
 
@@ -211,15 +225,15 @@ class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
   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_);
+    ar& BOOST_SERIALIZATION_NVP(R_);
+    ar& BOOST_SERIALIZATION_NVP(r_);
+    ar& BOOST_SERIALIZATION_NVP(v_);
+    ar& BOOST_SERIALIZATION_NVP(t_);
   }
 #endif
   /// @}
 
-}; // class Gal3
+};  // class Gal3
 
 /// Traits specialization for Gal3
 template <>
@@ -228,4 +242,4 @@ struct traits<Gal3> : public internal::MatrixLieGroup<Gal3, 5> {};
 template <>
 struct traits<const Gal3> : public internal::MatrixLieGroup<Gal3, 5> {};
 
-} // namespace gtsam
+}  // namespace gtsam