Detailed Description

Navigation state: Pose (rotation, translation) + velocity Following Barrau20icra, this class belongs to the Lie group SE_2(3).

This group is also called "double direct isometries”.

NOTE: While Barrau20icra follow a R,v,t order, we use a R,t,v order to maintain backwards compatibility.

Inheritance diagram for gtsam::NavState:

Testable
GTSAM_EXPORT friend std::ostream &	operator<< (std::ostream &os, const NavState &state)
	Output stream operator.
void	print (const std::string &s="") const
	print
bool	equals (const NavState &other, double tol=1e-8) const
	equals

Constructors
	NavState ()
	Default constructor.
	NavState (const Base &other)
	NavState (const Rot3 &R, const Point3 &t, const Velocity3 &v)
	Construct from attitude, position, velocity.
	NavState (const Pose3 &pose, const Velocity3 &v)
	Construct from pose and velocity.
	NavState (const Matrix3 &R, const Vector6 &tv)
	Construct from SO(3) and R^6.
	NavState (const Matrix5 &T)
	Construct from Matrix5.
static NavState	Create (const Rot3 &R, const Point3 &t, const Velocity3 &v, OptionalJacobian< 9, 3 > H1={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 3 > H3={})
	Named constructor with derivatives.
static NavState	FromPoseVelocity (const Pose3 &pose, const Vector3 &vel, OptionalJacobian< 9, 6 > H1={}, OptionalJacobian< 9, 3 > H2={})
	Named constructor with derivatives.

Group
const Rot3 &	rotation (OptionalJacobian< 3, 9 > H={}) const
	Syntactic sugar.
NavState	retract (const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
	Manifold retract used by optimization.
Vector9	localCoordinates (const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
	Inverse of the optimization chart selected by GTSAM_NAVSTATE_EXPMAP.
static Eigen::Block< Vector9, 3, 1 >	dR (Vector9 &v)
static Eigen::Block< Vector9, 3, 1 >	dP (Vector9 &v)
static Eigen::Block< Vector9, 3, 1 >	dV (Vector9 &v)
static Eigen::Block< const Vector9, 3, 1 >	dR (const Vector9 &v)
static Eigen::Block< const Vector9, 3, 1 >	dP (const Vector9 &v)
static Eigen::Block< const Vector9, 3, 1 >	dV (const Vector9 &v)

Public Member Functions
Component Access
const Rot3 &	attitude (OptionalJacobian< 3, 9 > H={}) const
Point3	position (OptionalJacobian< 3, 9 > H={}) const
Velocity3	velocity (OptionalJacobian< 3, 9 > H={}) const
const Pose3	pose () const
double	range (const Point3 &point, OptionalJacobian< 1, 9 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
	Calculate range to a 3D landmark.
Unit3	bearing (const Point3 &point, OptionalJacobian< 2, 9 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
	Calculate bearing to a 3D landmark.
Derived quantities
Matrix3	R () const
	Return rotation matrix. Induces computation in quaternion mode.
Quaternion	quaternion () const
	Return quaternion. Induces computation in matrix mode.
Vector3	t () const
	Return position as Vector3.
Vector3	v () const
	Return velocity as Vector3.
Velocity3	bodyVelocity (OptionalJacobian< 3, 9 > H={}) const
Dynamics
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
	Integrate forward in time given angular velocity and acceleration in body frame.
Public Member Functions inherited from gtsam::ExtendedPose3< 2, NavState >
	ExtendedPose3 ()
	Construct a fixed-size identity element.
	ExtendedPose3 (size_t k=0)
	Construct a dynamic-size identity element.
	ExtendedPose3 (const ExtendedPose3 &)=default
	Copy constructor.
ExtendedPose3 &	operator= (const ExtendedPose3 &)=default
	Copy assignment.
	ExtendedPose3 (const Rot3 &R, const Matrix3K &x)
	Construct from rotation and 3xk block.
	ExtendedPose3 (const Rot3 &R, const Vecs &... xs)
	Construct a fixed-size state from rotation and K 3-vectors.
	ExtendedPose3 (const MatrixRep &T)
	Construct from homogeneous matrix representation.
void	print (const std::string &s="") const
	Print this state.
bool	equals (const ExtendedPose3 &other, double tol=1e-9) const
	Equality check with tolerance.
size_t	k () const
size_t	dim () const
const Rot3 &	rotation (ComponentJacobian H={}) const
	Rotation component.
Point3	x (size_t i, ComponentJacobian H={}) const
	i-th R^3 component, returned by value.
const Matrix3K &	xMatrix () const
	Access all x_i blocks.
Matrix3K &	xMatrix ()
	Mutable access to all x_i blocks.
This	inverse () const
	Group inverse.
This	operator* (const This &other) const
	Group composition.
Jacobian	AdjointMap () const
	Adjoint map.
MatrixRep	matrix () const
	Homogeneous matrix representation.
Public Member Functions inherited from gtsam::MatrixLieGroup< Class, D, N >
std::enable_if_t< M !=Eigen::Dynamic, int >	dim () const
	Provided fixed dimension in dim() if needed.
Eigen::Matrix< double, internal::product(N, N), 1 >	vec (OptionalJacobian< internal::product(N, N), D > H={}) const
	Vectorize the matrix representation of a Lie group element.
Jacobian	AdjointMap () const
	A generic implementation of AdjointMap for matrix Lie groups.
TangentVector	Adjoint (const TangentVector &xi, ChartJacobian H_this={}, ChartJacobian H_xi={}) const
	Adjoint action on a tangent vector.
TangentVector	AdjointTranspose (const TangentVector &x, ChartJacobian H_this={}, ChartJacobian H_x={}) const
	Dual Adjoint action on a tangent covector.
Public Member Functions inherited from gtsam::LieGroup< Class, D >
std::enable_if_t< M !=Eigen::Dynamic, int >	dim () const
	Provided fixed dimension in dim() if needed.
const Class &	derived () const
Class	compose (const Class &g) const
Class	between (const Class &g) const
Class	compose (const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
Class	between (const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
Class	inverse (ChartJacobian H) const
Class	expmap (const TangentVector &v) const
	expmap as required by manifold concept Applies exponential map to v and composes with *this
TangentVector	logmap (const Class &g) const
	logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
Class	expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
	expmap with optional derivatives, when the class provides them
TangentVector	logmap (const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
	logmap with optional derivatives, when the class provides them
Class	retract (const TangentVector &v) const
	retract as required by manifold concept: applies v at *this
TangentVector	localCoordinates (const Class &g) const
	localCoordinates as required by manifold concept: finds tangent vector between *this and g
Class	retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
	retract with optional derivatives, when the chart provides them
TangentVector	localCoordinates (const Class &g, ChartJacobian H1, ChartJacobian H2={}) const
	localCoordinates with optional derivatives, when the chart provides them
SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

Static Public Attributes
static constexpr auto	dimension = 9
Static Public Attributes inherited from gtsam::ExtendedPose3< 2, NavState >
static constexpr int	K
static constexpr int	dimension
static constexpr int	matrixDim
Static Public Attributes inherited from gtsam::MatrixLieGroup< Class, D, N >
static constexpr auto	dimension
Static Public Attributes inherited from gtsam::LieGroup< Class, D >
static constexpr auto	dimension

Public Types
using	Base = ExtendedPose3<2, NavState>
using	LieAlgebra = Matrix5
using	Vector25 = Eigen::Matrix<double, 25, 1>
Public Types inherited from gtsam::ExtendedPose3< 2, NavState >
using	This
using	Base
using	TangentVector
using	Jacobian
using	ChartJacobian
using	ComponentJacobian
using	MatrixRep
	Homogeneous matrix representation in the group.
using	LieAlgebra
	Lie algebra matrix type used by Hat/Vee.
using	Matrix3K
Public Types inherited from gtsam::MatrixLieGroup< Class, D, N >
using	Base = LieGroup<Class, D>
using	ChartJacobian = typename Base::ChartJacobian
using	Jacobian = typename Base::Jacobian
using	TangentVector = typename Base::TangentVector
using	Vectorized = Eigen::Matrix<double, internal::product(N, N), 1>
using	VectorizedJacobian
Public Types inherited from gtsam::LieGroup< Class, D >
typedef OptionalJacobian< N, N >	ChartJacobian
typedef Eigen::Matrix< double, N, N >	Jacobian
typedef Eigen::Matrix< double, N, 1 >	TangentVector

Classes
struct	AutonomousFlow

Friends
class	PreintegrationBase

Additional Inherited Members
static size_t	Dimension (size_t k)
	Runtime manifold dimension helper.
static This	Identity ()
	Identity element for fixed-size K.
static This	Identity (size_t k=0)
	Identity element for dynamic-size K.
static This	Expmap (const TangentVector &xi, ChartJacobian Hxi={})
	Exponential map from tangent to group.
static TangentVector	Logmap (const This &pose, ChartJacobian Hpose={})
	Logarithm map from group to tangent.
static Jacobian	adjointMap (const TangentVector &xi)
	Lie algebra adjoint map.
static Jacobian	ExpmapDerivative (const TangentVector &xi)
	Jacobian of Expmap.
static Jacobian	LogmapDerivative (const TangentVector &xi)
	Jacobian of Logmap evaluated from tangent coordinates.
static Jacobian	LogmapDerivative (const This &pose)
	Jacobian of Logmap evaluated at a group element.
static LieAlgebra	Hat (const TangentVector &xi)
	Hat operator from tangent to Lie algebra.
static TangentVector	Vee (const LieAlgebra &X)
	Vee operator from Lie algebra to tangent.
Static Public Member Functions inherited from gtsam::MatrixLieGroup< Class, D, N >
static constexpr int	Dim ()
	Static method to get the dimension (compile-time or dynamic).
static Jacobian	adjointMap (const TangentVector &xi)
	Lie algebra adjoint map ad_xi, with optional specialization in derived classes.
static TangentVector	adjoint (const TangentVector &xi, const TangentVector &y, ChartJacobian Hxi={}, ChartJacobian H_y={})
	Lie algebra action ad_xi(y), with optional Jacobians.
static TangentVector	adjointTranspose (const TangentVector &xi, const TangentVector &y, ChartJacobian Hxi={}, ChartJacobian H_y={})
	Dual Lie algebra action ad_xi^T(y), with optional Jacobians.
Static Public Member Functions inherited from gtsam::LieGroup< Class, D >
static constexpr int	Dim ()
	Static method to get the dimension (compile-time or dynamic).
static Class	Retract (const TangentVector &v)
	Retract at origin: possible in Lie group because it has an identity.
static TangentVector	LocalCoordinates (const Class &g)
	LocalCoordinates at origin: possible in Lie group because it has an identity.
static Class	Retract (const TangentVector &v, ChartJacobian H)
	Retract at origin with optional derivative, when the chart provides it.
static TangentVector	LocalCoordinates (const Class &g, ChartJacobian H)
	LocalCoordinates at origin with optional derivative, when provided.
Protected Types inherited from gtsam::ExtendedPose3< 2, NavState >
using	IsDynamic
using	IsFixed
Static Protected Member Functions inherited from gtsam::ExtendedPose3< 2, NavState >
static This	MakeReturn (const ExtendedPose3 &value)
static const ExtendedPose3 &	AsBase (const This &value)
static size_t	RuntimeK (const TangentVector &xi)
static void	ZeroJacobian (ChartJacobian H, Eigen::Index d)
Protected Attributes inherited from gtsam::ExtendedPose3< 2, NavState >
Rot3	R_
	Rotation component.
Matrix3K	t_
	K translation-like columns in world frame.

Member Function Documentation

◆ bearing()

Unit3 gtsam::NavState::bearing	(	const Point3 &	point,
		OptionalJacobian< 2, 9 >	Hself = {},
		OptionalJacobian< 2, 3 >	Hpoint = {} ) const

Calculate bearing to a 3D landmark.

Parameters

point 3D location of landmark

Returns: bearing (Unit3)

◆ range()

double gtsam::NavState::range	(	const Point3 &	point,
		OptionalJacobian< 1, 9 >	Hself = {},
		OptionalJacobian< 1, 3 >	Hpoint = {} ) const

Calculate range to a 3D landmark.

Parameters

point 3D location of landmark

Returns: range (double)

◆ retract()

NavState gtsam::NavState::retract	(	const Vector9 &	v,
		OptionalJacobian< 9, 9 >	H1 = {},
		OptionalJacobian< 9, 9 >	H2 = {} ) const

Manifold retract used by optimization.

The compile-time option GTSAM_NAVSTATE_EXPMAP selects the full Lie Expmap; otherwise this uses the component-wise chart.

◆ update()

NavState gtsam::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

Integrate forward in time given angular velocity and acceleration in body frame.

Uses second order integration for position, returns derivatives except dt.

The documentation for this class was generated from the following files:

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/NavState.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/NavState.cpp

Detailed Description

Testable

Constructors

Group

Public Member Functions

Static Public Attributes

Public Types

Classes

Friends

Additional Inherited Members

Member Function Documentation

◆ bearing()

◆ range()

◆ retract()

◆ update()