1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file NavState.h
14 * @brief Navigation state composing of attitude, position, and velocity
15 * @authors Frank Dellaert, Varun Agrawal, Fan Jiang
16 * @date July 2015
17 **/
18
19#pragma once
20
21#include <gtsam/geometry/Pose3.h>
22#include <gtsam/base/Vector.h>
23#include <gtsam/base/Manifold.h>
24
25namespace gtsam {
26
27/// Velocity is currently typedef'd to Vector3
28using Velocity3 = Vector3;
29
30/**
31 * Navigation state: Pose (rotation, translation) + velocity
32 * Following Barrau20icra, this class belongs to the Lie group SE_2(3).
33 * This group is also called "double direct isometries”.
34 *
35 * NOTE: While Barrau20icra follow a R,v,t order,
36 * we use a R,t,v order to maintain backwards compatibility.
37 */
38class GTSAM_EXPORT NavState : public MatrixLieGroup<NavState, 9, 5> {
39 private:
40
41 // TODO(frank):
42 // - should we rename t_ to p_? if not, we should rename dP do dT
43 Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav
44 Point3 t_; ///< position n_t, in nav frame
45 Velocity3 v_; ///< velocity n_v in nav frame
46
47public:
48 using LieAlgebra = Matrix5;
49 using Vector25 = Eigen::Matrix<double, 25, 1>;
50
51 /// @name Constructors
52 /// @{
53
54 /// Default constructor
55 NavState() :
56 t_(0, 0, 0), v_(Vector3::Zero()) {
57 }
58
59 /// Construct from attitude, position, velocity
60 NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
61 R_(R), t_(t), v_(v) {
62 }
63
64 /// Construct from pose and velocity
65 NavState(const Pose3& pose, const Velocity3& v) :
66 R_(pose.rotation()), t_(pose.translation()), v_(v) {
67 }
68
69 /// Construct from SO(3) and R^6
70 NavState(const Matrix3& R, const Vector6& tv) :
71 R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
72 }
73
74 /// Construct from Matrix5
75 NavState(const Matrix5& T) :
76 R_(T.block<3, 3>(startRow: 0, startCol: 0)), t_(T.block<3, 1>(startRow: 0, startCol: 3)), v_(T.block<3, 1>(startRow: 0, startCol: 4)) {
77 }
78
79 /// Named constructor with derivatives
80 static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
81 OptionalJacobian<9, 3> H1 = {},
82 OptionalJacobian<9, 3> H2 = {},
83 OptionalJacobian<9, 3> H3 = {});
84
85 /// Named constructor with derivatives
86 static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
87 OptionalJacobian<9, 6> H1 = {},
88 OptionalJacobian<9, 3> H2 = {});
89
90 /// @}
91 /// @name Component Access
92 /// @{
93
94 const Rot3& attitude(OptionalJacobian<3, 9> H = {}) const;
95 const Point3& position(OptionalJacobian<3, 9> H = {}) const;
96 const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) const;
97
98 const Pose3 pose() const {
99 return Pose3(attitude(), position());
100 }
101
102 /// @}
103 /// @name Derived quantities
104 /// @{
105
106 /// Return rotation matrix. Induces computation in quaternion mode
107 Matrix3 R() const {
108 return R_.matrix();
109 }
110 /// Return quaternion. Induces computation in matrix mode
111 Quaternion quaternion() const {
112 return R_.toQuaternion();
113 }
114 /// Return position as Vector3
115 Vector3 t() const {
116 return t_;
117 }
118 /// Return velocity as Vector3. Computation-free.
119 const Vector3& v() const {
120 return v_;
121 }
122 // Return velocity in body frame
123 Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const;
124
125 /// Return matrix group representation, in MATLAB notation:
126 /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1]
127 Matrix5 matrix() const;
128
129 /// Vectorize 5x5 matrix into a 25-dim vector.
130 Vector25 vec(OptionalJacobian<25, 9> H = {}) const;
131
132 /// @}
133 /// @name Testable
134 /// @{
135
136 /// Output stream operator
137 GTSAM_EXPORT
138 friend std::ostream &operator<<(std::ostream &os, const NavState& state);
139
140 /// print
141 void print(const std::string& s = "") const;
142
143 /// equals
144 bool equals(const NavState& other, double tol = 1e-8) const;
145
146 /// @}
147 /// @name Group
148 /// @{
149
150 /// identity for group operation
151 static NavState Identity() {
152 return NavState();
153 }
154
155 /// inverse transformation with derivatives
156 NavState inverse() const;
157
158 using LieGroup<NavState, 9>::inverse; // version with derivative
159
160 /// compose syntactic sugar
161 NavState operator*(const NavState& T) const {
162 return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_);
163 }
164
165 /// Syntactic sugar
166 const Rot3& rotation() const { return attitude(); };
167
168 // Tangent space sugar.
169 // TODO(frank): move to private navstate namespace in cpp
170 static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
171 return v.segment<3>(start: 0);
172 }
173 static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
174 return v.segment<3>(start: 3);
175 }
176 static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
177 return v.segment<3>(start: 6);
178 }
179 static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
180 return v.segment<3>(start: 0);
181 }
182 static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
183 return v.segment<3>(start: 3);
184 }
185 static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
186 return v.segment<3>(start: 6);
187 }
188
189 /// retract with optional derivatives
190 NavState retract(const Vector9& v, //
191 OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
192 {}) const;
193
194 /// localCoordinates with optional derivatives
195 Vector9 localCoordinates(const NavState& g, //
196 OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
197 {}) const;
198
199 /// @}
200 /// @name Lie Group
201 /// @{
202
203 /**
204 * Exponential map at identity - create a NavState from canonical coordinates
205 * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$
206 */
207 static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {});
208
209 /**
210 * Log map at identity - return the canonical coordinates \f$
211 * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState
212 */
213 static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {});
214
215 /**
216 * Calculate Adjoint map, transforming a twist in this pose's (i.e, body)
217 * frame to the world spatial frame.
218 */
219 Matrix9 AdjointMap() const;
220
221 /**
222 * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
223 * body-fixed velocity, transforming it to the spatial frame
224 * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
225 * Note that H_xib = AdjointMap()
226 */
227 Vector9 Adjoint(const Vector9& xi_b,
228 OptionalJacobian<9, 9> H_this = {},
229 OptionalJacobian<9, 9> H_xib = {}) const;
230
231 /**
232 * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
233 * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^]
234 */
235 static Matrix9 adjointMap(const Vector9& xi);
236
237 /**
238 * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
239 */
240 static Vector9 adjoint(const Vector9& xi, const Vector9& y,
241 OptionalJacobian<9, 9> Hxi = {},
242 OptionalJacobian<9, 9> H_y = {});
243
244 /// Derivative of Expmap
245 static Matrix9 ExpmapDerivative(const Vector9& xi);
246
247 /// Derivative of Logmap
248 static Matrix9 LogmapDerivative(const Vector9& xi);
249
250 /// Derivative of Logmap, NavState version
251 static Matrix9 LogmapDerivative(const NavState& xi);
252
253 // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
254 struct GTSAM_EXPORT ChartAtOrigin {
255 static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {});
256 static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
257 };
258
259 /// Hat maps from tangent vector to Lie algebra
260 static Matrix5 Hat(const Vector9& xi);
261
262 /// Vee maps from Lie algebra to tangent vector
263 static Vector9 Vee(const Matrix5& X);
264
265 /// @}
266 /// @name Dynamics
267 /// @{
268
269 // φ: autonomous flow where velocity acts on position for
270 // dt (R, p, v) -> p += v·dt.
271 struct AutonomousFlow {
272 double dt;
273
274 // Differential at identity (right-trivialized): Φ = I with ∂p/∂v = dt·I.
275 Jacobian dIdentity() const {
276 Jacobian Phi = I_9x9;
277 Phi.template block<3, 3>(startRow: 3, startCol: 6) = I_3x3 * dt;
278 return Phi;
279 }
280
281 // Apply φ(x) by p += v·dt
282 NavState operator()(const NavState& X) const {
283 return {X.attitude(), X.position() + X.velocity() * dt, X.velocity()};
284 }
285 };
286
287/// Integrate forward in time given angular velocity and acceleration in body frame
288 /// Uses second order integration for position, returns derivatives except dt.
289 NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
290 const double dt, OptionalJacobian<9, 9> F = {},
291 OptionalJacobian<9, 3> G1 = {},
292 OptionalJacobian<9, 3> G2 = {}) const;
293
294 /// Compute tangent space contribution due to Coriolis forces
295 Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
296 OptionalJacobian<9, 9> H = {}) const;
297
298 /// Correct preintegrated tangent vector with our velocity and rotated gravity,
299 /// taking into account Coriolis forces if omegaCoriolis is given.
300 Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
301 const std::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
302 false, OptionalJacobian<9, 9> H1 = {},
303 OptionalJacobian<9, 9> H2 = {}) const;
304
305 /// @}
306
307private:
308 /// @{
309 /// serialization
310#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
311 friend class boost::serialization::access;
312 template<class ARCHIVE>
313 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
314 ar & BOOST_SERIALIZATION_NVP(R_);
315 ar & BOOST_SERIALIZATION_NVP(t_);
316 ar & BOOST_SERIALIZATION_NVP(v_);
317 }
318#endif
319 /// @}
320};
321
322// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
323template <>
324struct traits<NavState> : public internal::MatrixLieGroup<NavState, 5> {};
325
326template <>
327struct traits<const NavState> : public internal::MatrixLieGroup<NavState, 5> {};
328
329} // namespace gtsam
330