gtsam  4.1.0
gtsam
NavState.h
Go to the documentation of this file.
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 
19 #pragma once
20 
21 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/base/Vector.h>
23 #include <gtsam/base/Manifold.h>
24 
25 namespace gtsam {
26 
28 typedef Vector3 Velocity3;
29 
34 class GTSAM_EXPORT NavState {
35 private:
36 
37  // TODO(frank):
38  // - should we rename t_ to p_? if not, we should rename dP do dT
39  Rot3 R_;
40  Point3 t_;
41  Velocity3 v_;
42 
43 public:
44 
45  enum {
46  dimension = 9
47  };
48 
49  typedef std::pair<Point3, Velocity3> PositionAndVelocity;
50 
53 
56  t_(0, 0, 0), v_(Vector3::Zero()) {
57  }
59  NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
60  R_(R), t_(t), v_(v) {
61  }
63  NavState(const Pose3& pose, const Velocity3& v) :
64  R_(pose.rotation()), t_(pose.translation()), v_(v) {
65  }
67  NavState(const Matrix3& R, const Vector6& tv) :
68  R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
69  }
71  static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
75  static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
77 
81 
82  const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
83  const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
84  const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
85 
86  const Pose3 pose() const {
87  return Pose3(attitude(), position());
88  }
89 
93 
95  Matrix3 R() const {
96  return R_.matrix();
97  }
99  Quaternion quaternion() const {
100  return R_.toQuaternion();
101  }
103  Vector3 t() const {
104  return t_;
105  }
107  const Vector3& v() const {
108  return v_;
109  }
110  // Return velocity in body frame
111  Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
112 
116  Matrix7 matrix() const;
117 
121 
123  GTSAM_EXPORT
124  friend std::ostream &operator<<(std::ostream &os, const NavState& state);
125 
127  void print(const std::string& s = "") const;
128 
130  bool equals(const NavState& other, double tol = 1e-8) const;
131 
135 
136  // Tangent space sugar.
137  // TODO(frank): move to private navstate namespace in cpp
138  static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
139  return v.segment<3>(0);
140  }
141  static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
142  return v.segment<3>(3);
143  }
144  static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
145  return v.segment<3>(6);
146  }
147  static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
148  return v.segment<3>(0);
149  }
150  static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
151  return v.segment<3>(3);
152  }
153  static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
154  return v.segment<3>(6);
155  }
156 
158  NavState retract(const Vector9& v, //
159  OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
160  boost::none) const;
161 
163  Vector9 localCoordinates(const NavState& g, //
164  OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
165  boost::none) const;
166 
170 
173  NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
174  const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
175  OptionalJacobian<9, 3> G2) const;
176 
178  Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
179  OptionalJacobian<9, 9> H = boost::none) const;
180 
183  Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
184  const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
185  false, OptionalJacobian<9, 9> H1 = boost::none,
186  OptionalJacobian<9, 9> H2 = boost::none) const;
187 
189 
190 private:
193  friend class boost::serialization::access;
194  template<class ARCHIVE>
195  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
196  ar & BOOST_SERIALIZATION_NVP(R_);
197  ar & BOOST_SERIALIZATION_NVP(t_);
198  ar & BOOST_SERIALIZATION_NVP(v_);
199  }
201 };
202 
203 // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
204 template<>
205 struct traits<NavState> : internal::Manifold<NavState> {
206 };
207 
208 } // namespace gtsam
gtsam::NavState::t
Vector3 t() const
Return position as Vector3.
Definition: NavState.h:103
gtsam::NavState::quaternion
Quaternion quaternion() const
Return quaternion. Induces computation in matrix mode.
Definition: NavState.h:99
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::NavState::NavState
NavState(const Pose3 &pose, const Velocity3 &v)
Construct from pose and velocity.
Definition: NavState.h:63
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition: Rot3M.cpp:219
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition: NavState.h:107
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::NavState::NavState
NavState()
Default constructor.
Definition: NavState.h:55
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition: NavState.h:95
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::NavState::NavState
NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v)
Construct from attitude, position, velocity.
Definition: NavState.h:59
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::Rot3
Definition: Rot3.h:59
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Compute the quaternion representation of this rotation.
Definition: Rot3M.cpp:233
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
Manifold.h
Base class and basic functions for Manifold types.
gtsam::NavState::NavState
NavState(const Matrix3 &R, const Vector6 &tv)
Construct from SO(3) and R^6.
Definition: NavState.h:67