OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
okvis::kinematics::Transformation Class Reference

A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of translation r_AB (represented in frame A) and Quaternion q_AB (as an Eigen Quaternion). see also the RSS'13 / IJRR'14 paper or the Thesis. Follows some of the functionality of the SchweizerMesser library by Paul Furgale, but uses Eigen quaternions underneath. More...

#include <Transformation.hpp>

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW Transformation ()
 Default constructor: initialises a unit transformation. More...
 
 Transformation (const Transformation &other)
 Copy constructor: nothing fancy but takes care of not doing bad stuff with internal caching. More...
 
 Transformation (Transformation &&other)
 Move constructor: nothing fancy but takes care of not doing bad stuff with internal caching. More...
 
 Transformation (const Eigen::Vector3d &r_AB, const Eigen::Quaterniond &q_AB)
 Construct from a translation and quaternion. More...
 
 Transformation (const Eigen::Matrix4d &T_AB)
 Construct from a homogeneous transformation matrix. More...
 
 ~Transformation ()
 Trivial destructor. More...
 
template<typename Derived_coeffs >
bool setCoeffs (const Eigen::MatrixBase< Derived_coeffs > &coeffs)
 Parameter setting, all 7. More...
 
template<typename Derived_coeffs >
bool setParameters (const Eigen::MatrixBase< Derived_coeffs > &parameters)
 Parameter setting, all 7. More...
 
Eigen::Matrix4d T () const
 The underlying homogeneous transformation matrix. More...
 
const Eigen::Matrix3d & C () const
 Returns the rotation matrix (cached). More...
 
const Eigen::Map
< Eigen::Vector3d > & 
r () const
 Returns the translation vector r_AB (represented in frame A). More...
 
const Eigen::Map
< Eigen::Quaterniond > & 
q () const
 Returns the Quaternion q_AB (as an Eigen Quaternion). More...
 
Eigen::Matrix< double, 3, 4 > T3x4 () const
 Get the upper 3x4 part of the homogeneous transformation matrix T_AB. More...
 
const Eigen::Matrix< double, 7, 1 > & coeffs () const
 The coefficients (parameters) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention). More...
 
const Eigen::Matrix< double, 7, 1 > & parameters () const
 The parameters (coefficients) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention). More...
 
const double * parameterPtr () const
 Get the parameters — support for ceres. More...
 
void setRandom ()
 Set this to a random transformation. More...
 
void setRandom (double translationMaxMeters, double rotationMaxRadians)
 Set this to a random transformation with bounded rotation and translation. More...
 
void set (const Eigen::Matrix4d &T_AB)
 Set from a homogeneous transformation matrix. More...
 
void set (const Eigen::Vector3d &r_AB, const Eigen::Quaternion< double > &q_AB)
 Set from a translation and quaternion. More...
 
void setIdentity ()
 Set this transformation to identity. More...
 
Transformation inverse () const
 Returns a copy of the transformation inverted. More...
 
Transformation operator* (const Transformation &rhs) const
 Multiplication with another transformation object. More...
 
Eigen::Vector3d operator* (const Eigen::Vector3d &rhs) const
 Transform a direction as v_A = C_AB*v_B (with rhs = hp_B).. More...
 
Eigen::Vector4d operator* (const Eigen::Vector4d &rhs) const
 Transform a homogenous point as hp_B = T_AB*hp_B (with rhs = hp_B). More...
 
Transformationoperator= (const Transformation &rhs)
 Assignment – copy. Takes care of proper caching. More...
 
template<typename Derived_delta >
bool oplus (const Eigen::MatrixBase< Derived_delta > &delta)
 Apply a small update with delta being 6x1. More...
 
template<typename Derived_delta , typename Derived_jacobian >
bool oplus (const Eigen::MatrixBase< Derived_delta > &delta, const Eigen::MatrixBase< Derived_jacobian > &jacobian)
 Apply a small update with delta being 6x1 – the Jacobian is a 7 by 6 matrix. More...
 
template<typename Derived_jacobian >
bool oplusJacobian (const Eigen::MatrixBase< Derived_jacobian > &jacobian) const
 Get the Jacobian of the oplus operation (a 7 by 6 matrix). More...
 
template<typename Derived_jacobian >
bool liftJacobian (const Eigen::MatrixBase< Derived_jacobian > &jacobian) const
 Gets the jacobian dx/dChi, i.e. lift the minimal Jacobian to a full one (as needed by ceres). More...
 

Static Public Member Functions

static Transformation Identity ()
 Get an identity transformation. More...
 

Protected Member Functions

void updateC ()
 Update the caching of the rotation matrix. More...
 

Protected Attributes

Eigen::Matrix< double, 7, 1 > parameters_
 Concatenated parameters [r;q]. More...
 
Eigen::Map< Eigen::Vector3d > r_
 Translation {A}r{B}. More...
 
Eigen::Map< Eigen::Quaterniond > q_
 Quaternion q_{AB}. More...
 
Eigen::Matrix3d C_
 The cached DCM C_{AB}. More...
 

Detailed Description

A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of translation r_AB (represented in frame A) and Quaternion q_AB (as an Eigen Quaternion). see also the RSS'13 / IJRR'14 paper or the Thesis. Follows some of the functionality of the SchweizerMesser library by Paul Furgale, but uses Eigen quaternions underneath.

Warning
This means the convention is different to SchweizerMesser and the RSS'13 / IJRR'14 paper / the Thesis

Constructor & Destructor Documentation

okvis::kinematics::Transformation::Transformation ( )
inline

Default constructor: initialises a unit transformation.

okvis::kinematics::Transformation::Transformation ( const Transformation other)
inline

Copy constructor: nothing fancy but takes care of not doing bad stuff with internal caching.

okvis::kinematics::Transformation::Transformation ( Transformation &&  other)
inline

Move constructor: nothing fancy but takes care of not doing bad stuff with internal caching.

okvis::kinematics::Transformation::Transformation ( const Eigen::Vector3d &  r_AB,
const Eigen::Quaterniond &  q_AB 
)
inline

Construct from a translation and quaternion.

Parameters
[in]r_ABThe translation r_AB (represented in frame A).
[in]q_ABThe Quaternion q_AB (as an Eigen Quaternion).
okvis::kinematics::Transformation::Transformation ( const Eigen::Matrix4d &  T_AB)
inlineexplicit

Construct from a homogeneous transformation matrix.

Parameters
[in]T_ABThe homogeneous transformation matrix.
okvis::kinematics::Transformation::~Transformation ( )
inline

Trivial destructor.

Member Function Documentation

const Eigen::Matrix3d & okvis::kinematics::Transformation::C ( ) const
inline

Returns the rotation matrix (cached).

const Eigen::Matrix<double, 7, 1>& okvis::kinematics::Transformation::coeffs ( ) const
inline

The coefficients (parameters) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).

Transformation okvis::kinematics::Transformation::Identity ( )
inlinestatic

Get an identity transformation.

Transformation okvis::kinematics::Transformation::inverse ( ) const
inline

Returns a copy of the transformation inverted.

template<typename Derived_jacobian >
bool okvis::kinematics::Transformation::liftJacobian ( const Eigen::MatrixBase< Derived_jacobian > &  jacobian) const
inline

Gets the jacobian dx/dChi, i.e. lift the minimal Jacobian to a full one (as needed by ceres).

Returns
True on success.
Transformation okvis::kinematics::Transformation::operator* ( const Transformation rhs) const
inline

Multiplication with another transformation object.

Parameters
[in]rhsThe right-hand side transformation for this to be multiplied with.
Eigen::Vector3d okvis::kinematics::Transformation::operator* ( const Eigen::Vector3d &  rhs) const
inline

Transform a direction as v_A = C_AB*v_B (with rhs = hp_B)..

Warning
This only applies the rotation!
Parameters
[in]rhsThe right-hand side direction for this to be multiplied with.
Eigen::Vector4d okvis::kinematics::Transformation::operator* ( const Eigen::Vector4d &  rhs) const
inline

Transform a homogenous point as hp_B = T_AB*hp_B (with rhs = hp_B).

Parameters
[in]rhsThe right-hand side direction for this to be multiplied with.
Transformation & okvis::kinematics::Transformation::operator= ( const Transformation rhs)
inline

Assignment – copy. Takes care of proper caching.

Parameters
[in]rhsThe rhs for this to be assigned to.
template<typename Derived_delta >
bool okvis::kinematics::Transformation::oplus ( const Eigen::MatrixBase< Derived_delta > &  delta)
inline

Apply a small update with delta being 6x1.

Template Parameters
Derived_deltaDeducible matrix type.
Parameters
[in]deltaThe 6x1 minimal update.
Returns
True on success.
template<typename Derived_delta , typename Derived_jacobian >
bool okvis::kinematics::Transformation::oplus ( const Eigen::MatrixBase< Derived_delta > &  delta,
const Eigen::MatrixBase< Derived_jacobian > &  jacobian 
)
inline

Apply a small update with delta being 6x1 – the Jacobian is a 7 by 6 matrix.

Parameters
[in]deltaThe 6x1 minimal update.
[out]jacobianThe output Jacobian.
Returns
True on success.
template<typename Derived_jacobian >
bool okvis::kinematics::Transformation::oplusJacobian ( const Eigen::MatrixBase< Derived_jacobian > &  jacobian) const
inline

Get the Jacobian of the oplus operation (a 7 by 6 matrix).

Parameters
[out]jacobianThe output Jacobian.
Returns
True on success.
const double* okvis::kinematics::Transformation::parameterPtr ( ) const
inline

Get the parameters — support for ceres.

Warning
USE WITH CARE!
const Eigen::Matrix<double, 7, 1>& okvis::kinematics::Transformation::parameters ( ) const
inline

The parameters (coefficients) as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).

const Eigen::Map< Eigen::Quaterniond > & okvis::kinematics::Transformation::q ( ) const
inline

Returns the Quaternion q_AB (as an Eigen Quaternion).

const Eigen::Map< Eigen::Vector3d > & okvis::kinematics::Transformation::r ( ) const
inline

Returns the translation vector r_AB (represented in frame A).

void okvis::kinematics::Transformation::set ( const Eigen::Matrix4d &  T_AB)
inline

Set from a homogeneous transformation matrix.

Parameters
[in]T_ABThe homogeneous transformation matrix.
void okvis::kinematics::Transformation::set ( const Eigen::Vector3d &  r_AB,
const Eigen::Quaternion< double > &  q_AB 
)
inline

Set from a translation and quaternion.

Parameters
[in]r_ABThe translation r_AB (represented in frame A).
[in]q_ABThe Quaternion q_AB (as an Eigen Quaternion).
template<typename Derived_coeffs >
bool okvis::kinematics::Transformation::setCoeffs ( const Eigen::MatrixBase< Derived_coeffs > &  coeffs)
inline

Parameter setting, all 7.

Template Parameters
Derived_coeffsDeducible matrix type.
Parameters
[in]coeffsThe parameters as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
void okvis::kinematics::Transformation::setIdentity ( )
inline

Set this transformation to identity.

template<typename Derived_coeffs >
bool okvis::kinematics::Transformation::setParameters ( const Eigen::MatrixBase< Derived_coeffs > &  parameters)
inline

Parameter setting, all 7.

Template Parameters
Derived_coeffsDeducible matrix type.
Parameters
[in]parametersThe parameters as [r_AB,q_AB], q_AB as [x,y,z,w] (Eigen internal convention).
void okvis::kinematics::Transformation::setRandom ( )
inline

Set this to a random transformation.

void okvis::kinematics::Transformation::setRandom ( double  translationMaxMeters,
double  rotationMaxRadians 
)
inline

Set this to a random transformation with bounded rotation and translation.

Parameters
[in]translationMaxMetersMaximum translation [m].
[in]rotationMaxRadiansMaximum rotation [rad].
Eigen::Matrix4d okvis::kinematics::Transformation::T ( ) const
inline

The underlying homogeneous transformation matrix.

Eigen::Matrix< double, 3, 4 > okvis::kinematics::Transformation::T3x4 ( ) const
inline

Get the upper 3x4 part of the homogeneous transformation matrix T_AB.

void okvis::kinematics::Transformation::updateC ( )
inlineprotected

Update the caching of the rotation matrix.

Member Data Documentation

Eigen::Matrix3d okvis::kinematics::Transformation::C_
protected

The cached DCM C_{AB}.

Eigen::Matrix<double, 7, 1> okvis::kinematics::Transformation::parameters_
protected

Concatenated parameters [r;q].

Eigen::Map<Eigen::Quaterniond> okvis::kinematics::Transformation::q_
protected

Quaternion q_{AB}.

Eigen::Map<Eigen::Vector3d> okvis::kinematics::Transformation::r_
protected

Translation {A}r{B}.


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