template class sophus::Se3¶
Overview¶
Se3 using default storage; derived from Se3Base. More…
#include <se3.h> template <class TScalar, int kOptions> class Se3 { public: // typedefs using Scalar = TScalar ; using Transformation = typename Base::Transformation ; using Point = typename Base::Point ; using HomogeneousPoint = typename Base::HomogeneousPoint ; using Tangent = typename Base::Tangent ; using Adjoint = typename Base::Adjoint ; using SO3Member = So3<Scalar, kOptions> ; using TranslationMember = Eigen::Matrix<Scalar, 3, 1, kOptions> ; // fields static constexpr int kDoF = Base::kDoF; static constexpr int kNumParameters = Base::kNumParameters; // construction Se3(); Se3(Se3 const& other); template <class TOtherDerived> Se3(Se3Base<TOtherDerived> const& other); template <class TOtherDerived, class TD> Se3( So3Base<TOtherDerived> const& so3, Eigen::MatrixBase<TD> const& translation ); Se3(Eigen::Matrix3<Scalar> const& rotation_matrix, Point const& translation); Se3(Eigen::Quaternion<Scalar> const& quaternion, Point const& translation); Se3(Eigen::Matrix4<Scalar> const& mat_t); // methods SOPHUS_FUNC Se3& operator=(Se3 const& other); SOPHUS_FUNC Scalar* data(); SOPHUS_FUNC Scalar const* data() const; SOPHUS_FUNC SO3Member& so3(); SOPHUS_FUNC SO3Member const& so3() const; SOPHUS_FUNC TranslationMember& translation(); SOPHUS_FUNC TranslationMember const& translation() const; template <class TOtherDerived> SOPHUS_FUNC Se3Base<TDerived>& operator=(Se3Base<TOtherDerived> const& other); static SOPHUS_FUNC Eigen::Matrix3<Scalar> jacobianUpperRightBlock( Eigen::Vector3<Scalar> const& vec_upsilon, Eigen::Vector3<Scalar> const& vec_omega ); static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobian(Tangent const& vec_upsilon_omega); static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobianInverse(Tangent const& vec_upsilon_omega); static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& upsilon_omega); static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0(); static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i); static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point); static SOPHUS_FUNC Se3<Scalar> exp(Tangent const& vec_a); template <class TS = Scalar> static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, Se3> fitToSe3(Eigen::Matrix4<Scalar> const& t); static SOPHUS_FUNC Transformation generator(int i); static SOPHUS_FUNC Transformation hat(Tangent const& a); static SOPHUS_FUNC Tangent lieBracket(Tangent const& a, Tangent const& b); static SOPHUS_FUNC Se3 rotX(Scalar const& x); static SOPHUS_FUNC Se3 rotY(Scalar const& y); static SOPHUS_FUNC Se3 rotZ(Scalar const& z); template <class TUniformRandomBitGenerator> static Se3 sampleUniform(TUniformRandomBitGenerator& generator); template <class TX, class TY, class TZ> static SOPHUS_FUNC Se3 trans( TX const& x, TY const& y, TZ const& z ); static SOPHUS_FUNC Se3 trans(Eigen::Vector3<Scalar> const& xyz); static SOPHUS_FUNC Se3 transX(Scalar const& x); static SOPHUS_FUNC Se3 transY(Scalar const& y); static SOPHUS_FUNC Se3 transZ(Scalar const& z); static SOPHUS_FUNC Tangent vee(Transformation const& omega); protected: // fields SO3Member so3_; TranslationMember translation_; };
Detailed Documentation¶
Se3 using default storage; derived from Se3Base.
Construction¶
Se3()
Default constructor initializes rigid body motion to the identity.
Se3(Se3 const& other)
Copy constructor.
template <class TOtherDerived> Se3(Se3Base<TOtherDerived> const& other)
Copy-like constructor from OtherDerived.
template <class TOtherDerived, class TD> Se3( So3Base<TOtherDerived> const& so3, Eigen::MatrixBase<TD> const& translation )
Constructor from So3 and translation vector.
Se3(Eigen::Matrix3<Scalar> const& rotation_matrix, Point const& translation)
Constructor from rotation matrix and translation vector.
Precondition: Rotation matrix needs to be orthogonal with determinant of 1.
Se3(Eigen::Quaternion<Scalar> const& quaternion, Point const& translation)
Constructor from quaternion and translation vector.
Precondition: quaternion
must not be close to zero.
Se3(Eigen::Matrix4<Scalar> const& mat_t)
Constructor from 4x4 matrix.
Precondition: Rotation matrix needs to be orthogonal with determinant of 1. The last row must be (0, 0, 0, 1)
.
Methods¶
SOPHUS_FUNC Se3& operator=(Se3 const& other)
Define copy-assignment operator explicitly. The definition of implicit copy assignment operator is deprecated in presence of a user-declared copy constructor (-Wdeprecated-copy in clang >= 13).
SOPHUS_FUNC Scalar* data()
This provides unsafe read/write access to internal data. SO(3) is represented by an Eigen::Quaternion (four parameters). When using direct write access, the user needs to take care of that the quaternion stays normalized.
SOPHUS_FUNC Scalar const* data() const
Const version of data() above.
SOPHUS_FUNC SO3Member& so3()
Mutator of So3.
SOPHUS_FUNC SO3Member const& so3() const
Accessor of So3.
SOPHUS_FUNC TranslationMember& translation()
Mutator of translation vector.
SOPHUS_FUNC TranslationMember const& translation() const
Accessor of translation vector.
template <class TOtherDerived> SOPHUS_FUNC Se3Base<TDerived>& operator=(Se3Base<TOtherDerived> const& other)
Assignment-like operator from OtherDerived.
static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& upsilon_omega)
Returns derivative of exp(x) wrt. x.
static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0()
Returns derivative of exp(x) wrt. x_i at x=0.
static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i)
Returns derivative of expmat(x)
wrt. x_i at x=0
, with expmat(.)
being the matrix exponential.
static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point)
Returns derivative of exp(x) * p wrt. x_i at x=0.
static SOPHUS_FUNC Se3<Scalar> exp(Tangent const& vec_a)
Group exponential.
This functions takes in an element of tangent space (= twist a
) and returns the corresponding element of the group SE(3).
The first three components of a
represent the translational part upsilon
in the tangent space of SE(3), while the last three components of a
represents the rotation vector omega
. To be more specific, this function computes expmat(hat(a))
with expmat(.)
being the matrix exponential and hat(.)
the hat-operator of SE(3), see below.
template <class TS = Scalar> static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, Se3> fitToSe3(Eigen::Matrix4<Scalar> const& t)
Returns closest Se3 given arbitrary 4x4 matrix.
static SOPHUS_FUNC Transformation generator(int i)
Returns the ith infinitesimal generators of SE(3).
The infinitesimal generators of SE(3) are:
| 0 0 0 1 | G_0 = | 0 0 0 0 | | 0 0 0 0 | | 0 0 0 0 | | 0 0 0 0 | G_1 = | 0 0 0 1 | | 0 0 0 0 | | 0 0 0 0 | | 0 0 0 0 | G_2 = | 0 0 0 0 | | 0 0 0 1 | | 0 0 0 0 | | 0 0 0 0 | G_3 = | 0 0 -1 0 | | 0 1 0 0 | | 0 0 0 0 | | 0 0 1 0 | G_4 = | 0 0 0 0 | | -1 0 0 0 | | 0 0 0 0 | | 0 -1 0 0 | G_5 = | 1 0 0 0 | | 0 0 0 0 | | 0 0 0 0 |
Precondition: i
must be in [0, 5].
static SOPHUS_FUNC Transformation hat(Tangent const& a)
hat-operator
It takes in the 6-vector representation (= twist) and returns the corresponding matrix representation of Lie algebra element.
Formally, the hat() -operator of SE(3) is defined as
hat(.): R^6 -> R^{4x4}, hat(a) = sum_i a_i * G_i
(for i=0,…,5)
with G_i
being the ith infinitesimal generator of SE(3).
The corresponding inverse is the vee() -operator, see below.
static SOPHUS_FUNC Tangent lieBracket(Tangent const& a, Tangent const& b)
Lie bracket.
It computes the Lie bracket of SE(3). To be more specific, it computes
[omega_1, omega_2]_se3 := vee([hat(omega_1), hat(omega_2)])
with [A,B] := AB-BA
being the matrix commutator, hat(.)
the hat() -operator and vee(.)
the vee() -operator of SE(3).
static SOPHUS_FUNC Se3 rotX(Scalar const& x)
Construct x-axis rotation.
static SOPHUS_FUNC Se3 rotY(Scalar const& y)
Construct y-axis rotation.
static SOPHUS_FUNC Se3 rotZ(Scalar const& z)
Construct z-axis rotation.
template <class TUniformRandomBitGenerator> static Se3 sampleUniform(TUniformRandomBitGenerator& generator)
Draw uniform sample from SE(3) manifold.
Translations are drawn component-wise from the range [-1, 1].
template <class TX, class TY, class TZ> static SOPHUS_FUNC Se3 trans( TX const& x, TY const& y, TZ const& z )
Construct a translation only Se3 instance.
static SOPHUS_FUNC Se3 transX(Scalar const& x)
Construct x-axis translation.
static SOPHUS_FUNC Se3 transY(Scalar const& y)
Construct y-axis translation.
static SOPHUS_FUNC Se3 transZ(Scalar const& z)
Construct z-axis translation.
static SOPHUS_FUNC Tangent vee(Transformation const& omega)
vee-operator
It takes 4x4-matrix representation Omega
and maps it to the corresponding 6-vector representation of Lie algebra.
This is the inverse of the hat() -operator, see above.
Precondition: Omega
must have the following structure:
| 0 -f e a |
| f 0 -d b |
| -e d 0 c
| 0 0 0 0 | .