1/* ----------------------------------------------------------------------------
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 * Atlanta, Georgia 30332-0415
4
5* All Rights Reserved
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7* See LICENSE for the license information
8* -------------------------------------------------------------------------- */
9
10/**
11 * @file Gal3.h
12 * @brief 3D Galilean Group SGal(3) state (attitude, position, velocity, time)
13 * @authors Matt Kielo, Scott Baker, Frank Dellaert
14 * @date April 30, 2025
15 */
16
17#pragma once
18
19#include <gtsam/base/MatrixLieGroup.h>
20#include <gtsam/geometry/Event.h>
21#include <gtsam/geometry/Pose3.h>
22
23#include <cmath> // For std::sqrt, std::cos, std::sin
24#include <functional> // For std::function used in numerical derivatives
25
26namespace gtsam {
27
28// Forward declaration
29class Gal3;
30
31// Use Vector3 for velocity for consistency with NavState
32using Velocity3 = Vector3;
33
34/**
35 * Represents an element of the 3D Galilean group SGal(3).
36 * Internal state: rotation, translation, velocity, time.
37 */
38class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
39 private:
40 Rot3 R_; ///< Rotation world R body
41 Point3 r_; ///< Position in world frame, n_r_b
42 Velocity3 v_; ///< Velocity in world frame, n_v_b
43 double t_; ///< Time component
44
45 public:
46 /// @name Constructors
47 /// @{
48
49 /// Default constructor: Identity element
50 Gal3()
51 : R_(Rot3::Identity()),
52 r_(Point3::Zero()),
53 v_(Velocity3::Zero()),
54 t_(0.0) {}
55
56 /// Construct from attitude, position, velocity, time
57 Gal3(const Rot3& R, const Point3& r, const Velocity3& v, double t)
58 : R_(R), r_(r), v_(v), t_(t) {}
59
60 /// Construct from a 5x5 matrix representation
61 explicit Gal3(const Matrix5& M);
62
63 /// Named constructor from components with derivatives
64 static Gal3 Create(const Rot3& R, const Point3& r, const Velocity3& v,
65 double t, OptionalJacobian<10, 3> H1 = {},
66 OptionalJacobian<10, 3> H2 = {},
67 OptionalJacobian<10, 3> H3 = {},
68 OptionalJacobian<10, 1> H4 = {});
69
70 /// Named constructor from Pose3, velocity, and time with derivatives
71 static Gal3 FromPoseVelocityTime(const Pose3& pose, const Velocity3& v,
72 double t, OptionalJacobian<10, 6> H1 = {},
73 OptionalJacobian<10, 3> H2 = {},
74 OptionalJacobian<10, 1> H3 = {});
75
76 /// @}
77 /// @name Component Access
78 /// @{
79
80 /// Access rotation component (Rot3)
81 const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const;
82
83 /// Access translation component (Point3)
84 const Point3& translation(OptionalJacobian<3, 10> H = {}) const;
85
86 /// Access velocity component (Vector3)
87 const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const;
88
89 /// Access time component (double)
90 const double& time(OptionalJacobian<1, 10> H = {}) const;
91
92 // Accessors when viewed as a "pose" manifold
93 const Rot3& attitude(OptionalJacobian<3, 10> H = {}) const {
94 return rotation(H);
95 }
96 const Point3& position(OptionalJacobian<3, 10> H = {}) const {
97 return translation(H);
98 }
99
100 /// @}
101 /// @name Derived quantities
102 /// @{
103
104 /// Return rotation matrix (Matrix3)
105 Matrix3 R() const { return R_.matrix(); }
106
107 /// Return position as Vector3
108 Vector3 r() const { return Vector3(r_); } // Conversion from Point3
109
110 /// Return velocity as Vector3
111 const Vector3& v() const { return v_; }
112
113 /// Return time scalar
114 const double& t() const { return t_; }
115
116 /// @}
117 /// @name Testable
118 /// @{
119
120 /// Output stream operator
121 GTSAM_EXPORT
122 friend std::ostream& operator<<(std::ostream& os, const Gal3& state);
123
124 /// Print with optional string prefix
125 void print(const std::string& s = "") const;
126
127 /// Check equality within tolerance
128 bool equals(const Gal3& other, double tol = 1e-9) const;
129
130 /// @}
131 /// @name Group
132 /// @{
133
134 /// Return the identity element
135 static Gal3 Identity() { return Gal3(); }
136
137 /// Return the inverse of this element
138 Gal3 inverse() const;
139
140 // Bring LieGroup::inverse() into scope (version with derivative)
141 using LieGroup<Gal3, 10>::inverse;
142
143 /// Group composition operator
144 Gal3 operator*(const Gal3& other) const;
145
146 /// @}
147 /// @name Group Action
148 /// @{
149
150 /**
151 * Apply Galilean transform to a spacetime Event
152 * @param e Input event (location, time)
153 * @param Hself Optional Jacobian wrt this Gal3 element's tangent space
154 * @param He Optional Jacobian wrt the input event's tangent space
155 * @return Transformed event
156 */
157 Event act(const Event& e, OptionalJacobian<4, 10> Hself = {},
158 OptionalJacobian<4, 4> He = {}) const;
159
160 /// @}
161 /// @name Lie Group
162 /// @{
163
164 /// Exponential map at identity: tangent vector xi -> manifold element g
165 static Gal3 Expmap(const TangentVector& xi,
166 OptionalJacobian<10, 10> Hxi = {});
167
168 /// Logarithmic map at identity: manifold element g -> tangent vector xi
169 static TangentVector Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});
170
171 /// Calculate Adjoint map Ad_g
172 Jacobian AdjointMap() const;
173
174 /// Apply this element's AdjointMap Ad_g to a tangent vector xi_base at
175 /// identity
176 TangentVector Adjoint(const TangentVector& xi_base,
177 OptionalJacobian<10, 10> H_g = {},
178 OptionalJacobian<10, 10> H_xi = {}) const;
179
180 /// The adjoint action `ad(xi, y)` = `adjointMap(xi) * y`
181 static TangentVector adjoint(const TangentVector& xi, const TangentVector& y,
182 OptionalJacobian<10, 10> Hxi = {},
183 OptionalJacobian<10, 10> Hy = {});
184
185 /// Compute the adjoint map `ad(xi)` associated with tangent vector xi
186 static Jacobian adjointMap(const TangentVector& xi);
187
188 /// Derivative of Expmap(xi) w.r.t. xi evaluated at xi
189 static Jacobian ExpmapDerivative(const TangentVector& xi);
190
191 /// Derivative of Logmap(g) w.r.t. g
192 static Jacobian LogmapDerivative(const Gal3& g);
193
194 /// Chart at origin, uses Expmap/Logmap for Retract/Local
195 struct ChartAtOrigin {
196 static Gal3 Retract(const TangentVector& xi, ChartJacobian Hxi = {});
197 static TangentVector Local(const Gal3& g, ChartJacobian Hg = {});
198 };
199
200 /// @}
201 /// @name Matrix Lie Group
202 /// @{
203
204 using LieAlgebra = Matrix5;
205 using Vector25 = Eigen::Matrix<double, 25, 1>;
206
207 /// Return 5x5 homogeneous matrix representation
208 Matrix5 matrix() const;
209
210 /// Vectorize 5x5 matrix into a 25-dim vector.
211 Vector25 vec(OptionalJacobian<25, 10> H = {}) const;
212
213 /// Hat operator: maps tangent vector xi to Lie algebra matrix
214 static LieAlgebra Hat(const TangentVector& xi);
215
216 /// Vee operator: maps Lie algebra matrix to tangent vector xi
217 static TangentVector Vee(const LieAlgebra& X);
218
219 /// @}
220
221 private:
222 /// @name Serialization
223 /// @{
224#if GTSAM_ENABLE_BOOST_SERIALIZATION
225 friend class boost::serialization::access;
226 template <class ARCHIVE>
227 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
228 ar& BOOST_SERIALIZATION_NVP(R_);
229 ar& BOOST_SERIALIZATION_NVP(r_);
230 ar& BOOST_SERIALIZATION_NVP(v_);
231 ar& BOOST_SERIALIZATION_NVP(t_);
232 }
233#endif
234 /// @}
235
236}; // class Gal3
237
238/// Traits specialization for Gal3
239template <>
240struct traits<Gal3> : public internal::MatrixLieGroup<Gal3, 5> {};
241
242template <>
243struct traits<const Gal3> : public internal::MatrixLieGroup<Gal3, 5> {};
244
245} // namespace gtsam
246