977 lines
29 KiB
C++
977 lines
29 KiB
C++
// This file is part of Sophus.
|
|
//
|
|
// Copyright 2012-2013 Hauke Strasdat
|
|
//
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
// of this software and associated documentation files (the "Software"), to
|
|
// deal in the Software without restriction, including without limitation the
|
|
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
|
|
// sell copies of the Software, and to permit persons to whom the Software is
|
|
// furnished to do so, subject to the following conditions:
|
|
//
|
|
// The above copyright notice and this permission notice shall be included in
|
|
// all copies or substantial portions of the Software.
|
|
//
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
|
// IN THE SOFTWARE.
|
|
|
|
#ifndef SOPHUS_SIM3_HPP
|
|
#define SOPHUS_SIM3_HPP
|
|
|
|
#include "rxso3.hpp"
|
|
|
|
////////////////////////////////////////////////////////////////////////////
|
|
// Forward Declarations / typedefs
|
|
////////////////////////////////////////////////////////////////////////////
|
|
|
|
namespace Sophus {
|
|
template<typename _Scalar, int _Options=0> class Sim3Group;
|
|
typedef Sim3Group<double> Sim3 EIGEN_DEPRECATED;
|
|
typedef Sim3Group<double> Sim3d; /**< double precision Sim3 */
|
|
typedef Sim3Group<float> Sim3f; /**< single precision Sim3 */
|
|
typedef Matrix<double,7,1> Vector7d;
|
|
typedef Matrix<double,7,7> Matrix7d;
|
|
typedef Matrix<float,7,1> Vector7f;
|
|
typedef Matrix<float,7,7> Matrix7f;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////
|
|
// Eigen Traits (For querying derived types in CRTP hierarchy)
|
|
////////////////////////////////////////////////////////////////////////////
|
|
|
|
namespace Eigen {
|
|
namespace internal {
|
|
|
|
template<typename _Scalar, int _Options>
|
|
struct traits<Sophus::Sim3Group<_Scalar,_Options> > {
|
|
typedef _Scalar Scalar;
|
|
typedef Matrix<Scalar,3,1> TranslationType;
|
|
typedef Sophus::RxSO3Group<Scalar> RxSO3Type;
|
|
};
|
|
|
|
template<typename _Scalar, int _Options>
|
|
struct traits<Map<Sophus::Sim3Group<_Scalar>, _Options> >
|
|
: traits<Sophus::Sim3Group<_Scalar, _Options> > {
|
|
typedef _Scalar Scalar;
|
|
typedef Map<Matrix<Scalar,3,1>,_Options> TranslationType;
|
|
typedef Map<Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
|
|
};
|
|
|
|
template<typename _Scalar, int _Options>
|
|
struct traits<Map<const Sophus::Sim3Group<_Scalar>, _Options> >
|
|
: traits<const Sophus::Sim3Group<_Scalar, _Options> > {
|
|
typedef _Scalar Scalar;
|
|
typedef Map<const Matrix<Scalar,3,1>,_Options> TranslationType;
|
|
typedef Map<const Sophus::RxSO3Group<Scalar>,_Options> RxSO3Type;
|
|
};
|
|
|
|
}
|
|
}
|
|
|
|
namespace Sophus {
|
|
using namespace Eigen;
|
|
using namespace std;
|
|
|
|
/**
|
|
* \brief Sim3 base type - implements Sim3 class but is storage agnostic
|
|
*
|
|
* [add more detailed description/tutorial]
|
|
*/
|
|
template<typename Derived>
|
|
class Sim3GroupBase {
|
|
public:
|
|
/** \brief scalar type */
|
|
typedef typename internal::traits<Derived>::Scalar Scalar;
|
|
/** \brief translation reference type */
|
|
typedef typename internal::traits<Derived>::TranslationType &
|
|
TranslationReference;
|
|
/** \brief translation const reference type */
|
|
typedef const typename internal::traits<Derived>::TranslationType &
|
|
ConstTranslationReference;
|
|
/** \brief RxSO3 reference type */
|
|
typedef typename internal::traits<Derived>::RxSO3Type &
|
|
RxSO3Reference;
|
|
/** \brief RxSO3 const reference type */
|
|
typedef const typename internal::traits<Derived>::RxSO3Type &
|
|
ConstRxSO3Reference;
|
|
|
|
|
|
/** \brief degree of freedom of group
|
|
* (three for translation, three for rotation, one for scale) */
|
|
static const int DoF = 7;
|
|
/** \brief number of internal parameters used
|
|
* (quaternion for rotation and scale + translation 3-vector) */
|
|
static const int num_parameters = 7;
|
|
/** \brief group transformations are NxN matrices */
|
|
static const int N = 4;
|
|
/** \brief group transfomation type */
|
|
typedef Matrix<Scalar,N,N> Transformation;
|
|
/** \brief point type */
|
|
typedef Matrix<Scalar,3,1> Point;
|
|
/** \brief tangent vector type */
|
|
typedef Matrix<Scalar,DoF,1> Tangent;
|
|
/** \brief adjoint transformation type */
|
|
typedef Matrix<Scalar,DoF,DoF> Adjoint;
|
|
|
|
/**
|
|
* \brief Adjoint transformation
|
|
*
|
|
* This function return the adjoint transformation \f$ Ad \f$ of the
|
|
* group instance \f$ A \f$ such that for all \f$ x \f$
|
|
* it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
|
|
* with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
|
|
*/
|
|
inline
|
|
const Adjoint Adj() const {
|
|
const Matrix<Scalar,3,3> & R = rxso3().rotationMatrix();
|
|
Adjoint res;
|
|
res.setZero();
|
|
res.block(0,0,3,3) = scale()*R;
|
|
res.block(0,3,3,3) = SO3Group<Scalar>::hat(translation())*R;
|
|
res.block(0,6,3,1) = -translation();
|
|
res.block(3,3,3,3) = R;
|
|
res(6,6) = 1;
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* \returns copy of instance casted to NewScalarType
|
|
*/
|
|
template<typename NewScalarType>
|
|
inline Sim3Group<NewScalarType> cast() const {
|
|
return
|
|
Sim3Group<NewScalarType>(rxso3().template cast<NewScalarType>(),
|
|
translation().template cast<NewScalarType>() );
|
|
}
|
|
|
|
/**
|
|
* \brief In-place group multiplication
|
|
*
|
|
* Same as operator*=() for Sim3.
|
|
*
|
|
* \see operator*()
|
|
*/
|
|
inline
|
|
void fastMultiply(const Sim3Group<Scalar>& other) {
|
|
translation() += (rxso3() * other.translation());
|
|
rxso3() *= other.rxso3();
|
|
}
|
|
|
|
/**
|
|
* \returns Group inverse of instance
|
|
*/
|
|
inline
|
|
const Sim3Group<Scalar> inverse() const {
|
|
const RxSO3Group<Scalar> invR = rxso3().inverse();
|
|
return Sim3Group<Scalar>(invR, invR*(translation()
|
|
*static_cast<Scalar>(-1) ) );
|
|
}
|
|
|
|
/**
|
|
* \brief Logarithmic map
|
|
*
|
|
* \returns tangent space representation
|
|
* (translational part and rotation vector) of instance
|
|
*
|
|
* \see log().
|
|
*/
|
|
inline
|
|
const Tangent log() const {
|
|
return log(*this);
|
|
}
|
|
|
|
/**
|
|
* \returns 4x4 matrix representation of instance
|
|
*/
|
|
inline
|
|
const Transformation matrix() const {
|
|
Transformation homogenious_matrix;
|
|
homogenious_matrix.setIdentity();
|
|
homogenious_matrix.block(0,0,3,3) = rxso3().matrix();
|
|
homogenious_matrix.col(3).head(3) = translation();
|
|
return homogenious_matrix;
|
|
}
|
|
|
|
/**
|
|
* \returns 3x4 matrix representation of instance
|
|
*
|
|
* It returns the three first row of matrix().
|
|
*/
|
|
inline
|
|
const Matrix<Scalar,3,4> matrix3x4() const {
|
|
Matrix<Scalar,3,4> matrix;
|
|
matrix.block(0,0,3,3) = rxso3().matrix();
|
|
matrix.col(3) = translation();
|
|
return matrix;
|
|
}
|
|
|
|
/**
|
|
* \brief Assignment operator
|
|
*/
|
|
template<typename OtherDerived> inline
|
|
Sim3GroupBase<Derived>& operator=
|
|
(const Sim3GroupBase<OtherDerived> & other) {
|
|
rxso3() = other.rxso3();
|
|
translation() = other.translation();
|
|
return *this;
|
|
}
|
|
|
|
/**
|
|
* \brief Group multiplication
|
|
* \see operator*=()
|
|
*/
|
|
inline
|
|
const Sim3Group<Scalar> operator*(const Sim3Group<Scalar>& other) const {
|
|
Sim3Group<Scalar> result(*this);
|
|
result *= other;
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* \brief Group action on \f$ \mathbf{R}^3 \f$
|
|
*
|
|
* \param p point \f$p \in \mathbf{R}^3 \f$
|
|
* \returns point \f$p' \in \mathbf{R}^3 \f$,
|
|
* rotated, scaled and translated version of \f$p\f$
|
|
*
|
|
* This function scales, rotates and translates point \f$ p \f$
|
|
* in \f$ \mathbf{R}^3 \f$ by the Sim3 transformation \f$sR,t\f$
|
|
* (=scaled rotation matrix, translation vector): \f$ p' = sR\cdot p + t \f$.
|
|
*/
|
|
inline
|
|
const Point operator*(const Point & p) const {
|
|
return rxso3()*p + translation();
|
|
}
|
|
|
|
/**
|
|
* \brief In-place group multiplication
|
|
*
|
|
* \see operator*()
|
|
*/
|
|
inline
|
|
void operator*=(const Sim3Group<Scalar>& other) {
|
|
translation() += (rxso3() * other.translation());
|
|
rxso3() *= other.rxso3();
|
|
}
|
|
|
|
/**
|
|
* \brief Mutator of quaternion
|
|
*/
|
|
inline
|
|
typename internal::traits<Derived>::RxSO3Type::QuaternionReference
|
|
quaternion() {
|
|
return rxso3().quaternion();
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of quaternion
|
|
*/
|
|
inline
|
|
typename internal::traits<Derived>::RxSO3Type::ConstQuaternionReference
|
|
quaternion() const {
|
|
return rxso3().quaternion();
|
|
}
|
|
|
|
/**
|
|
* \returns Rotation matrix
|
|
*
|
|
* deprecated: use rotationMatrix() instead.
|
|
*/
|
|
inline
|
|
EIGEN_DEPRECATED const Transformation rotation_matrix() const {
|
|
return rxso3().rotationMatrix();
|
|
}
|
|
|
|
/**
|
|
* \returns Rotation matrix
|
|
*/
|
|
inline
|
|
const Matrix<Scalar,3,3> rotationMatrix() const {
|
|
return rxso3().rotationMatrix();
|
|
}
|
|
|
|
/**
|
|
* \brief Mutator of RxSO3 group
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
RxSO3Reference rxso3() {
|
|
return static_cast<Derived*>(this)->rxso3();
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of RxSO3 group
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstRxSO3Reference rxso3() const {
|
|
return static_cast<const Derived*>(this)->rxso3();
|
|
}
|
|
|
|
/**
|
|
* \returns scale
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
const Scalar scale() const {
|
|
return rxso3().scale();
|
|
}
|
|
|
|
/**
|
|
* \brief Setter of quaternion using rotation matrix, leaves scale untouched
|
|
*
|
|
* \param R a 3x3 rotation matrix
|
|
* \pre the 3x3 matrix should be orthogonal and have a determinant of 1
|
|
*/
|
|
inline
|
|
void setRotationMatrix
|
|
(const Matrix<Scalar,3,3> & R) {
|
|
rxso3().setRotationMatrix(R);
|
|
}
|
|
|
|
/**
|
|
* \brief Scale setter
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
void setScale(const Scalar & scale) {
|
|
rxso3().setScale(scale);
|
|
}
|
|
|
|
/**
|
|
* \brief Setter of quaternion using scaled rotation matrix
|
|
*
|
|
* \param sR a 3x3 scaled rotation matrix
|
|
* \pre the 3x3 matrix should be "scaled orthogonal"
|
|
* and have a positive determinant
|
|
*/
|
|
inline
|
|
void setScaledRotationMatrix
|
|
(const Matrix<Scalar,3,3> & sR) {
|
|
rxso3().setScaledRotationMatrix(sR);
|
|
}
|
|
|
|
/**
|
|
* \brief Mutator of translation vector
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
TranslationReference translation() {
|
|
return static_cast<Derived*>(this)->translation();
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of translation vector
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstTranslationReference translation() const {
|
|
return static_cast<const Derived*>(this)->translation();
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////
|
|
// public static functions
|
|
////////////////////////////////////////////////////////////////////////////
|
|
|
|
/**
|
|
* \param b 7-vector representation of Lie algebra element
|
|
* \returns derivative of Lie bracket
|
|
*
|
|
* This function returns \f$ \frac{\partial}{\partial a} [a, b]_{sim3} \f$
|
|
* with \f$ [a, b]_{sim3} \f$ being the lieBracket() of the Lie algebra sim3.
|
|
*
|
|
* \see lieBracket()
|
|
*/
|
|
inline static
|
|
const Adjoint d_lieBracketab_by_d_a(const Tangent & b) {
|
|
const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
|
|
const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
|
|
Scalar sigma2 = b[6];
|
|
|
|
Adjoint res;
|
|
res.setZero();
|
|
res.template topLeftCorner<3,3>()
|
|
= -SO3::hat(omega2)-sigma2*Matrix3d::Identity();
|
|
res.template block<3,3>(0,3) = -SO3::hat(upsilon2);
|
|
res.template topRightCorner<3,1>() = upsilon2;
|
|
res.template block<3,3>(3,3) = -SO3::hat(omega2);
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* \brief Group exponential
|
|
*
|
|
* \param a tangent space element (7-vector)
|
|
* \returns corresponding element of the group Sim3
|
|
*
|
|
* The first three components of \f$ a \f$ represent the translational
|
|
* part \f$ \upsilon \f$ in the tangent space of Sim3, while the last three
|
|
* components of \f$ a \f$ represents the rotation vector \f$ \omega \f$.
|
|
*
|
|
* To be more specific, this function computes \f$ \exp(\widehat{a}) \f$
|
|
* with \f$ \exp(\cdot) \f$ being the matrix exponential
|
|
* and \f$ \widehat{\cdot} \f$ the hat()-operator of Sim3.
|
|
*
|
|
* \see hat()
|
|
* \see log()
|
|
*/
|
|
inline static
|
|
const Sim3Group<Scalar> exp(const Tangent & a) {
|
|
const Matrix<Scalar,3,1> & upsilon = a.segment(0,3);
|
|
const Matrix<Scalar,3,1> & omega = a.segment(3,3);
|
|
Scalar sigma = a[6];
|
|
Scalar theta;
|
|
RxSO3Group<Scalar> rxso3
|
|
= RxSO3Group<Scalar>::expAndTheta(a.template tail<4>(), &theta);
|
|
const Matrix<Scalar,3,3> & Omega = SO3Group<Scalar>::hat(omega);
|
|
const Matrix<Scalar,3,3> & W = calcW(theta, sigma, rxso3.scale(), Omega);
|
|
return Sim3Group<Scalar>(rxso3, W*upsilon);
|
|
}
|
|
|
|
/**
|
|
* \brief Generators
|
|
*
|
|
* \pre \f$ i \in \{0,1,2,3,4,5,6\} \f$
|
|
* \returns \f$ i \f$th generator \f$ G_i \f$ of Sim3
|
|
*
|
|
* The infinitesimal generators of Sim3 are: \f[
|
|
* G_0 = \left( \begin{array}{cccc}
|
|
* 0& 0& 0& 1\\
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* \end{array} \right),
|
|
* G_1 = \left( \begin{array}{cccc}
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& 0& 1\\
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* \end{array} \right),
|
|
* G_2 = \left( \begin{array}{cccc}
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& 0& 1\\
|
|
* 0& 0& 0& 0\\
|
|
* \end{array} \right).
|
|
* G_3 = \left( \begin{array}{cccc}
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& -1& 0\\
|
|
* 0& 1& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* \end{array} \right),
|
|
* G_4 = \left( \begin{array}{cccc}
|
|
* 0& 0& 1& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* -1& 0& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* \end{array} \right),
|
|
* G_5 = \left( \begin{array}{cccc}
|
|
* 0& -1& 0& 0\\
|
|
* 1& 0& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* \end{array} \right),
|
|
* G_6 = \left( \begin{array}{cccc}
|
|
* 1& 0& 0& 0\\
|
|
* 0& 1& 0& 0\\
|
|
* 0& 0& 1& 0\\
|
|
* 0& 0& 0& 0\\
|
|
* \end{array} \right).
|
|
* \f]
|
|
* \see hat()
|
|
*/
|
|
inline static
|
|
const Transformation generator(int i) {
|
|
if (i<0 || i>6) {
|
|
throw SophusException("i is not in range [0,6].");
|
|
}
|
|
Tangent e;
|
|
e.setZero();
|
|
e[i] = static_cast<Scalar>(1);
|
|
return hat(e);
|
|
}
|
|
|
|
/**
|
|
* \brief hat-operator
|
|
*
|
|
* \param omega 7-vector representation of Lie algebra element
|
|
* \returns 4x4-matrix representatin of Lie algebra element
|
|
*
|
|
* Formally, the hat-operator of Sim3 is defined
|
|
* as \f$ \widehat{\cdot}: \mathbf{R}^7 \rightarrow \mathbf{R}^{4\times 4},
|
|
* \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$
|
|
* with \f$ G_i \f$ being the ith infinitesial generator().
|
|
*
|
|
* \see generator()
|
|
* \see vee()
|
|
*/
|
|
inline static
|
|
const Transformation hat(const Tangent & v) {
|
|
Transformation Omega;
|
|
Omega.template topLeftCorner<3,3>()
|
|
= RxSO3Group<Scalar>::hat(v.template tail<4>());
|
|
Omega.col(3).template head<3>() = v.template head<3>();
|
|
Omega.row(3).setZero();
|
|
return Omega;
|
|
}
|
|
|
|
/**
|
|
* \brief Lie bracket
|
|
*
|
|
* \param a 7-vector representation of Lie algebra element
|
|
* \param b 7-vector representation of Lie algebra element
|
|
* \returns 7-vector representation of Lie algebra element
|
|
*
|
|
* It computes the bracket of Sim3. To be more specific, it
|
|
* computes \f$ [a, b]_{sim3}
|
|
* := [\widehat{a}, \widehat{b}]^\vee \f$
|
|
* with \f$ [A,B] = AB-BA \f$ being the matrix
|
|
* commutator, \f$ \widehat{\cdot} \f$ the
|
|
* hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of Sim3.
|
|
*
|
|
* \see hat()
|
|
* \see vee()
|
|
*/
|
|
inline static
|
|
const Tangent lieBracket(const Tangent & a,
|
|
const Tangent & b) {
|
|
const Matrix<Scalar,3,1> & upsilon1 = a.template head<3>();
|
|
const Matrix<Scalar,3,1> & upsilon2 = b.template head<3>();
|
|
const Matrix<Scalar,3,1> & omega1 = a.template segment<3>(3);
|
|
const Matrix<Scalar,3,1> & omega2 = b.template segment<3>(3);
|
|
Scalar sigma1 = a[6];
|
|
Scalar sigma2 = b[6];
|
|
|
|
Tangent res;
|
|
res.template head<3>() =
|
|
SO3Group<Scalar>::hat(omega1)*upsilon2
|
|
+ SO3Group<Scalar>::hat(upsilon1)*omega2
|
|
+ sigma1*upsilon2 - sigma2*upsilon1;
|
|
res.template segment<3>(3) = omega1.cross(omega2);
|
|
res[6] = static_cast<Scalar>(0);
|
|
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* \brief Logarithmic map
|
|
*
|
|
* \param other element of the group Sim3
|
|
* \returns corresponding tangent space element
|
|
* (translational part \f$ \upsilon \f$
|
|
* and rotation vector \f$ \omega \f$)
|
|
*
|
|
* Computes the logarithmic, the inverse of the group exponential.
|
|
* To be specific, this function computes \f$ \log({\cdot})^\vee \f$
|
|
* with \f$ \vee(\cdot) \f$ being the matrix logarithm
|
|
* and \f$ \vee{\cdot} \f$ the vee()-operator of Sim3.
|
|
*
|
|
* \see exp()
|
|
* \see vee()
|
|
*/
|
|
inline static
|
|
const Tangent log(const Sim3Group<Scalar> & other) {
|
|
Tangent res;
|
|
Scalar theta;
|
|
const Matrix<Scalar,4,1> & omega_sigma
|
|
= RxSO3Group<Scalar>::logAndTheta(other.rxso3(), &theta);
|
|
const Matrix<Scalar,3,1> & omega = omega_sigma.template head<3>();
|
|
Scalar sigma = omega_sigma[3];
|
|
const Matrix<Scalar,3,3> & W
|
|
= calcW(theta, sigma, other.scale(), SO3Group<Scalar>::hat(omega));
|
|
res.segment(0,3) = W.partialPivLu().solve(other.translation());
|
|
res.segment(3,3) = omega;
|
|
res[6] = sigma;
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* \brief vee-operator
|
|
*
|
|
* \param Omega 4x4-matrix representation of Lie algebra element
|
|
* \returns 7-vector representatin of Lie algebra element
|
|
*
|
|
* This is the inverse of the hat()-operator.
|
|
*
|
|
* \see hat()
|
|
*/
|
|
inline static
|
|
const Tangent vee(const Transformation & Omega) {
|
|
Tangent upsilon_omega_sigma;
|
|
upsilon_omega_sigma.template head<3>()
|
|
= Omega.col(3).template head<3>();
|
|
upsilon_omega_sigma.template tail<4>()
|
|
= RxSO3Group<Scalar>::vee(Omega.template topLeftCorner<3,3>());
|
|
return upsilon_omega_sigma;
|
|
}
|
|
|
|
private:
|
|
static
|
|
Matrix<Scalar,3,3> calcW(const Scalar & theta,
|
|
const Scalar & sigma,
|
|
const Scalar & scale,
|
|
const Matrix<Scalar,3,3> & Omega){
|
|
static const Matrix<Scalar,3,3> I
|
|
= Matrix<Scalar,3,3>::Identity();
|
|
static const Scalar one = static_cast<Scalar>(1.);
|
|
static const Scalar half = static_cast<Scalar>(1./2.);
|
|
Matrix<Scalar,3,3> Omega2 = Omega*Omega;
|
|
|
|
Scalar A,B,C;
|
|
if (std::abs(sigma)<SophusConstants<Scalar>::epsilon()) {
|
|
C = one;
|
|
if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
|
|
A = half;
|
|
B = static_cast<Scalar>(1./6.);
|
|
} else {
|
|
Scalar theta_sq = theta*theta;
|
|
A = (one-std::cos(theta))/theta_sq;
|
|
B = (theta-std::sin(theta))/(theta_sq*theta);
|
|
}
|
|
} else {
|
|
C = (scale-one)/sigma;
|
|
if (std::abs(theta)<SophusConstants<Scalar>::epsilon()) {
|
|
Scalar sigma_sq = sigma*sigma;
|
|
A = ((sigma-one)*scale+one)/sigma_sq;
|
|
B = ((half*sigma*sigma-sigma+one)*scale)/(sigma_sq*sigma);
|
|
} else {
|
|
Scalar theta_sq = theta*theta;
|
|
Scalar a = scale*std::sin(theta);
|
|
Scalar b = scale*std::cos(theta);
|
|
Scalar c = theta_sq+sigma*sigma;
|
|
A = (a*sigma+ (one-b)*theta)/(theta*c);
|
|
B = (C-((b-one)*sigma+a*theta)/(c))*one/(theta_sq);
|
|
}
|
|
}
|
|
return A*Omega + B*Omega2 + C*I;
|
|
}
|
|
};
|
|
|
|
/**
|
|
* \brief Sim3 default type - Constructors and default storage for Sim3 Type
|
|
*/
|
|
template<typename _Scalar, int _Options>
|
|
class Sim3Group : public Sim3GroupBase<Sim3Group<_Scalar,_Options> > {
|
|
typedef Sim3GroupBase<Sim3Group<_Scalar,_Options> > Base;
|
|
public:
|
|
/** \brief scalar type */
|
|
typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
|
|
::Scalar Scalar;
|
|
/** \brief RxSO3 reference type */
|
|
typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
|
|
::RxSO3Type & RxSO3Reference;
|
|
/** \brief RxSO3 const reference type */
|
|
typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
|
|
::RxSO3Type & ConstRxSO3Reference;
|
|
/** \brief translation reference type */
|
|
typedef typename internal::traits<Sim3Group<_Scalar,_Options> >
|
|
::TranslationType & TranslationReference;
|
|
/** \brief translation const reference type */
|
|
typedef const typename internal::traits<Sim3Group<_Scalar,_Options> >
|
|
::TranslationType & ConstTranslationReference;
|
|
|
|
/** \brief degree of freedom of group */
|
|
static const int DoF = Base::DoF;
|
|
/** \brief number of internal parameters used */
|
|
static const int num_parameters = Base::num_parameters;
|
|
/** \brief group transformations are NxN matrices */
|
|
static const int N = Base::N;
|
|
/** \brief group transfomation type */
|
|
typedef typename Base::Transformation Transformation;
|
|
/** \brief point type */
|
|
typedef typename Base::Point Point;
|
|
/** \brief tangent vector type */
|
|
typedef typename Base::Tangent Tangent;
|
|
/** \brief adjoint transformation type */
|
|
typedef typename Base::Adjoint Adjoint;
|
|
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
/**
|
|
* \brief Default constructor
|
|
*
|
|
* Initialize Quaternion to identity rotation and translation to zero.
|
|
*/
|
|
inline
|
|
Sim3Group()
|
|
: translation_( Matrix<Scalar,3,1>::Zero() )
|
|
{
|
|
}
|
|
|
|
/**
|
|
* \brief Copy constructor
|
|
*/
|
|
template<typename OtherDerived> inline
|
|
Sim3Group(const Sim3GroupBase<OtherDerived> & other)
|
|
: rxso3_(other.rxso3()), translation_(other.translation()) {
|
|
}
|
|
|
|
/**
|
|
* \brief Constructor from RxSO3 and translation vector
|
|
*/
|
|
template<typename OtherDerived> inline
|
|
Sim3Group(const RxSO3GroupBase<OtherDerived> & rxso3,
|
|
const Point & translation)
|
|
: rxso3_(rxso3), translation_(translation) {
|
|
}
|
|
|
|
/**
|
|
* \brief Constructor from quaternion and translation vector
|
|
*
|
|
* \pre quaternion must not be zero
|
|
*/
|
|
inline
|
|
Sim3Group(const Quaternion<Scalar> & quaternion,
|
|
const Point & translation)
|
|
: rxso3_(quaternion), translation_(translation) {
|
|
}
|
|
|
|
/**
|
|
* \brief Constructor from 4x4 matrix
|
|
*
|
|
* \pre top-left 3x3 sub-matrix need to be "scaled orthogonal"
|
|
* with positive determinant of
|
|
*/
|
|
inline explicit
|
|
Sim3Group(const Eigen::Matrix<Scalar,4,4>& T)
|
|
: rxso3_(T.template topLeftCorner<3,3>()),
|
|
translation_(T.template block<3,1>(0,3)) {
|
|
}
|
|
|
|
/**
|
|
* \returns pointer to internal data
|
|
*
|
|
* This provides unsafe read/write access to internal data. Sim3 is
|
|
* represented by a pair of an RxSO3 element (4 parameters) and translation
|
|
* vector (three parameters).
|
|
*
|
|
* Note: The first three Scalars represent the imaginary parts, while the
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
Scalar* data() {
|
|
// rxso3_ and translation_ are layed out sequentially with no padding
|
|
return rxso3_.data();
|
|
}
|
|
|
|
/**
|
|
* \returns const pointer to internal data
|
|
*
|
|
* Const version of data().
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
const Scalar* data() const {
|
|
// rxso3_ and translation_ are layed out sequentially with no padding
|
|
return rxso3_.data();
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of RxSO3
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
RxSO3Reference rxso3() {
|
|
return rxso3_;
|
|
}
|
|
|
|
/**
|
|
* \brief Mutator of RxSO3
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstRxSO3Reference rxso3() const {
|
|
return rxso3_;
|
|
}
|
|
|
|
/**
|
|
* \brief Mutator of translation vector
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
TranslationReference translation() {
|
|
return translation_;
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of translation vector
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstTranslationReference translation() const {
|
|
return translation_;
|
|
}
|
|
|
|
protected:
|
|
Sophus::RxSO3Group<Scalar> rxso3_;
|
|
Matrix<Scalar,3,1> translation_;
|
|
};
|
|
|
|
|
|
} // end namespace
|
|
|
|
|
|
namespace Eigen {
|
|
/**
|
|
* \brief Specialisation of Eigen::Map for Sim3GroupBase
|
|
*
|
|
* Allows us to wrap Sim3 Objects around POD array
|
|
* (e.g. external c style quaternion)
|
|
*/
|
|
template<typename _Scalar, int _Options>
|
|
class Map<Sophus::Sim3Group<_Scalar>, _Options>
|
|
: public Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> > {
|
|
typedef Sophus::Sim3GroupBase<Map<Sophus::Sim3Group<_Scalar>, _Options> >
|
|
Base;
|
|
|
|
public:
|
|
/** \brief scalar type */
|
|
typedef typename internal::traits<Map>::Scalar Scalar;
|
|
/** \brief translation reference type */
|
|
typedef typename internal::traits<Map>::TranslationType &
|
|
TranslationReference;
|
|
/** \brief translation const reference type */
|
|
typedef const typename internal::traits<Map>::TranslationType &
|
|
ConstTranslationReference;
|
|
/** \brief RxSO3 reference type */
|
|
typedef typename internal::traits<Map>::RxSO3Type &
|
|
RxSO3Reference;
|
|
/** \brief RxSO3 const reference type */
|
|
typedef const typename internal::traits<Map>::RxSO3Type &
|
|
ConstRxSO3Reference;
|
|
|
|
|
|
/** \brief degree of freedom of group */
|
|
static const int DoF = Base::DoF;
|
|
/** \brief number of internal parameters used */
|
|
static const int num_parameters = Base::num_parameters;
|
|
/** \brief group transformations are NxN matrices */
|
|
static const int N = Base::N;
|
|
/** \brief group transfomation type */
|
|
typedef typename Base::Transformation Transformation;
|
|
/** \brief point type */
|
|
typedef typename Base::Point Point;
|
|
/** \brief tangent vector type */
|
|
typedef typename Base::Tangent Tangent;
|
|
/** \brief adjoint transformation type */
|
|
typedef typename Base::Adjoint Adjoint;
|
|
|
|
EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
|
|
using Base::operator*=;
|
|
using Base::operator*;
|
|
|
|
EIGEN_STRONG_INLINE
|
|
Map(Scalar* coeffs)
|
|
: rxso3_(coeffs),
|
|
translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
|
|
}
|
|
|
|
/**
|
|
* \brief Mutator of RxSO3
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
RxSO3Reference rxso3() {
|
|
return rxso3_;
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of RxSO3
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstRxSO3Reference rxso3() const {
|
|
return rxso3_;
|
|
}
|
|
|
|
/**
|
|
* \brief Mutator of translation vector
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
TranslationReference translation() {
|
|
return translation_;
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of translation vector
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstTranslationReference translation() const {
|
|
return translation_;
|
|
}
|
|
|
|
protected:
|
|
Map<Sophus::RxSO3Group<Scalar>,_Options> rxso3_;
|
|
Map<Matrix<Scalar,3,1>,_Options> translation_;
|
|
};
|
|
|
|
/**
|
|
* \brief Specialisation of Eigen::Map for const Sim3GroupBase
|
|
*
|
|
* Allows us to wrap Sim3 Objects around POD array
|
|
* (e.g. external c style quaternion)
|
|
*/
|
|
template<typename _Scalar, int _Options>
|
|
class Map<const Sophus::Sim3Group<_Scalar>, _Options>
|
|
: public Sophus::Sim3GroupBase<
|
|
Map<const Sophus::Sim3Group<_Scalar>, _Options> > {
|
|
typedef Sophus::Sim3GroupBase<
|
|
Map<const Sophus::Sim3Group<_Scalar>, _Options> > Base;
|
|
|
|
public:
|
|
/** \brief scalar type */
|
|
typedef typename internal::traits<Map>::Scalar Scalar;
|
|
/** \brief translation type */
|
|
typedef const typename internal::traits<Map>::TranslationType &
|
|
ConstTranslationReference;
|
|
/** \brief RxSO3 const reference type */
|
|
typedef const typename internal::traits<Map>::RxSO3Type &
|
|
ConstRxSO3Reference;
|
|
|
|
/** \brief degree of freedom of group */
|
|
static const int DoF = Base::DoF;
|
|
/** \brief number of internal parameters used */
|
|
static const int num_parameters = Base::num_parameters;
|
|
/** \brief group transformations are NxN matrices */
|
|
static const int N = Base::N;
|
|
/** \brief group transfomation type */
|
|
typedef typename Base::Transformation Transformation;
|
|
/** \brief point type */
|
|
typedef typename Base::Point Point;
|
|
/** \brief tangent vector type */
|
|
typedef typename Base::Tangent Tangent;
|
|
/** \brief adjoint transformation type */
|
|
typedef typename Base::Adjoint Adjoint;
|
|
|
|
EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
|
|
using Base::operator*=;
|
|
using Base::operator*;
|
|
|
|
EIGEN_STRONG_INLINE
|
|
Map(const Scalar* coeffs)
|
|
: rxso3_(coeffs),
|
|
translation_(coeffs+Sophus::RxSO3Group<Scalar>::num_parameters) {
|
|
}
|
|
|
|
EIGEN_STRONG_INLINE
|
|
Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs)
|
|
: translation_(trans_coeffs), rxso3_(rot_coeffs){
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of RxSO3
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstRxSO3Reference rxso3() const {
|
|
return rxso3_;
|
|
}
|
|
|
|
/**
|
|
* \brief Accessor of translation vector
|
|
*/
|
|
EIGEN_STRONG_INLINE
|
|
ConstTranslationReference translation() const {
|
|
return translation_;
|
|
}
|
|
|
|
protected:
|
|
const Map<const Sophus::RxSO3Group<Scalar>,_Options> rxso3_;
|
|
const Map<const Matrix<Scalar,3,1>,_Options> translation_;
|
|
};
|
|
|
|
}
|
|
|
|
#endif
|