Loading...
Searching...
No Matches
NavState.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
typedef Vector3 Velocity3;
class GTSAM_EXPORT NavState {
private:
// TODO(frank):
// - should we rename t_ to p_? if not, we should rename dP do dT
Rot3 R_;
Point3 t_;
Velocity3 v_;
public:
enum {
dimension = 9
};
typedef std::pair<Point3, Velocity3> PositionAndVelocity;
NavState() :
t_(0, 0, 0), v_(Vector3::Zero()) {
}
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
R_(R), t_(t), v_(v) {
}
NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(v) {
}
NavState(const Matrix3& R, const Vector6& tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 3> H3);
static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
const Pose3 pose() const {
return Pose3(attitude(), position());
}
Matrix3 R() const {
return R_.matrix();
}
Quaternion quaternion() const {
return R_.toQuaternion();
}
Vector3 t() const {
return t_;
}
const Vector3& v() const {
return v_;
}
// Return velocity in body frame
Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
Matrix7 matrix() const;
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const NavState& state);
void print(const std::string& s = "") const;
bool equals(const NavState& other, double tol = 1e-8) const;
// Tangent space sugar.
// TODO(frank): move to private navstate namespace in cpp
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
return v.segment<3>(0);
}
static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
return v.segment<3>(3);
}
static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
return v.segment<3>(6);
}
static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
return v.segment<3>(0);
}
static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
return v.segment<3>(3);
}
static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
return v.segment<3>(6);
}
NavState retract(const Vector9& v, //
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
Vector9 localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const;
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
OptionalJacobian<9, 9> H = boost::none) const;
Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
private:
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(t_);
ar & BOOST_SERIALIZATION_NVP(v_);
}
};
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<>
struct traits<NavState> : internal::Manifold<NavState> {
};
} // namespace gtsam
Vector.h
typedef and functions to augment Eigen's VectorXd
Manifold.h
Base class and basic functions for Manifold types.
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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:36
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition NavState.h:28
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::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
gtsam::NavState::NavState
NavState()
Default constructor.
Definition NavState.h:55
gtsam::NavState::NavState
NavState(const Matrix3 &R, const Vector6 &tv)
Construct from SO(3) and R^6.
Definition NavState.h:67
gtsam::NavState::NavState
NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v)
Construct from attitude, position, velocity.
Definition NavState.h:59
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition NavState.h:95
gtsam::NavState::NavState
NavState(const Pose3 &pose, const Velocity3 &v)
Construct from pose and velocity.
Definition NavState.h:63
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition NavState.h:107
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