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 
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_;
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 = {}) const;
83  const Point3& position(OptionalJacobian<3, 9> H = {}) const;
84  const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) const;
85 
86  const Pose3 pose() const {
87  return Pose3(attitude(), position());
88  }
89 
93 
95  Matrix3 R() const {
96  return R_.matrix();
97  }
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
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 = {}, OptionalJacobian<9, 9> H2 =
160  {}) const;
161 
163  Vector9 localCoordinates(const NavState& g, //
164  OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
165  {}) 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 = {}) const;
180 
183  Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
184  const std::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
185  false, OptionalJacobian<9, 9> H1 = {},
186  OptionalJacobian<9, 9> H2 = {}) const;
187 
189 
190 private:
193 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
194  friend class boost::serialization::access;
195  template<class ARCHIVE>
196  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
197  ar & BOOST_SERIALIZATION_NVP(R_);
198  ar & BOOST_SERIALIZATION_NVP(t_);
199  ar & BOOST_SERIALIZATION_NVP(v_);
200  }
201 #endif
202 };
204 
205 // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
206 template<>
207 struct traits<NavState> : internal::Manifold<NavState> {
208 };
209 
210 } // namespace gtsam
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NavState::dR
static Eigen::Block< Vector9, 3, 1 > dR(Vector9 &v)
Definition: NavState.h:138
relicense.update
def update(text)
Definition: relicense.py:46
tail
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type tail(NType n)
Definition: BlockMethods.h:1257
gtsam::NavState::dV
static Eigen::Block< const Vector9, 3, 1 > dV(const Vector9 &v)
Definition: NavState.h:153
gtsam::translation
Point3_ translation(const Pose3_ &pose)
Definition: slam/expressions.h:93
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
Vector.h
typedef and functions to augment Eigen's VectorXd
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::NavState::t_
Point3 t_
position n_t, in nav frame
Definition: NavState.h:40
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
gtsam::NavState::NavState
NavState()
Default constructor.
Definition: NavState.h:55
gtsam::NavState::dP
static Eigen::Block< Vector9, 3, 1 > dP(Vector9 &v)
Definition: NavState.h:141
gtsam::position
Point3_ position(const NavState_ &X)
Definition: navigation/expressions.h:37
gtsam::NavState::PositionAndVelocity
std::pair< Point3, Velocity3 > PositionAndVelocity
Definition: NavState.h:49
gtsam::NavState::NavState
NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v)
Construct from attitude, position, velocity.
Definition: NavState.h:59
dt
const double dt
Definition: testVelocityConstraint.cpp:15
vel
static const Velocity3 vel(0.4, 0.5, 0.6)
gtsam::NavState
Definition: NavState.h:34
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition: NavState.h:107
os
ofstream os("timeSchurFactors.csv")
gtsam::NavState::NavState
NavState(const Matrix3 &R, const Vector6 &tv)
Construct from SO(3) and R^6.
Definition: NavState.h:67
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
gtsam::attitude
Rot3_ attitude(const NavState_ &X)
Definition: navigation/expressions.h:34
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NavState::R_
Rot3 R_
Rotation nRb, rotates points/velocities in body to points/velocities in nav.
Definition: NavState.h:39
gtsam::NavState::dP
static Eigen::Block< const Vector9, 3, 1 > dP(const Vector9 &v)
Definition: NavState.h:150
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Definition: Rot3M.cpp:232
gtsam::NavState::v_
Velocity3 v_
velocity n_v in nav frame
Definition: NavState.h:41
gtsam::NavState::t
Vector3 t() const
Return position as Vector3.
Definition: NavState.h:103
bodyVelocity
Vector3 bodyVelocity(const Pose3 &w_t_b, const Vector3 &vec_w, OptionalJacobian< 3, 6 > Hpose={}, OptionalJacobian< 3, 3 > Hvel={})
Definition: testFactorTesting.cpp:29
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition: NavState.h:95
Manifold.h
Base class and basic functions for Manifold types.
head
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
Definition: BlockMethods.h:1208
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::NavState::dV
static Eigen::Block< Vector9, 3, 1 > dV(Vector9 &v)
Definition: NavState.h:144
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
gtsam
traits
Definition: SFMdata.h:40
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:89
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
coriolis
std::function< Vector9(const NavState &, const bool &)> coriolis
Definition: testNavState.cpp:174
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam.examples.ImuFactorISAM2Example.n_gravity
def n_gravity
Definition: ImuFactorISAM2Example.py:31
gtsam::NavState::pose
const Pose3 pose() const
Definition: NavState.h:86
align_3::t
Point2 t(10, 10)
gtsam::NavState::quaternion
Quaternion quaternion() const
Return quaternion. Induces computation in matrix mode.
Definition: NavState.h:99
gtsam::NavState::dR
static Eigen::Block< const Vector9, 3, 1 > dR(const Vector9 &v)
Definition: NavState.h:147
gtsam::NavState::NavState
NavState(const Pose3 &pose, const Velocity3 &v)
Construct from pose and velocity.
Definition: NavState.h:63
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
Pose3.h
3D Pose
make_changelog.state
state
Definition: make_changelog.py:29
gtsam::velocity
Velocity3_ velocity(const NavState_ &X)
Definition: navigation/expressions.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:49