template class sophus::Sim3Base¶
Overview¶
Sim3 base type - implements Sim3 class but is storage agnostic. More…
#include <sim3.h> template <class TDerived> class Sim3Base { public: // typedefs using Scalar = typename Eigen::internal::traits<TDerived>::Scalar ; using TranslationType = typename Eigen::internal::traits<TDerived>::TranslationType ; using RxSo3Type = typename Eigen::internal::traits<TDerived>::RxSo3Type ; using QuaternionType = typename RxSo3Type::QuaternionType ; using Transformation = Eigen::Matrix<Scalar, kMatrixDim, kMatrixDim> ; using Point = Eigen::Vector3<Scalar> ; using HomogeneousPoint = Eigen::Vector4<Scalar> ; using Line = Eigen::ParametrizedLine<Scalar, 3> ; using Hyperplane = Eigen::Hyperplane<Scalar, 3> ; using Tangent = Eigen::Vector<Scalar, kDoF> ; using Adjoint = Eigen::Matrix<Scalar, kDoF, kDoF> ; using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename TOtherDerived::Scalar>::ReturnType ; using Sim3Product = Sim3<ReturnScalar<TOtherDerived>> ; using PointProduct = Eigen::Vector3<ReturnScalar<TPointDerived>> ; using HomogeneousPointProduct = Eigen::Vector4<ReturnScalar<THPointDerived>> ; // fields static constexpr int kDoF = 7; static constexpr int kNumParameters = 7; static constexpr int kMatrixDim = 4; static constexpr int kPointDim = 3; // methods SOPHUS_FUNC Adjoint adj() const; template <class TNewScalarType> SOPHUS_FUNC Sim3<TNewScalarType> cast() const; SOPHUS_FUNC Sim3<Scalar> inverse() const; SOPHUS_FUNC Tangent log() const; SOPHUS_FUNC Transformation matrix() const; SOPHUS_FUNC Eigen::Matrix<Scalar, 3, 4> matrix3x4() const; template <class TOtherDerived> SOPHUS_FUNC Sim3Base<TDerived>& operator=(Sim3Base<TOtherDerived> const& other); template <class TOtherDerived> SOPHUS_FUNC Sim3Product<TOtherDerived> operator*(Sim3Base<TOtherDerived> const& other) const; template < typename TPointDerived, typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 3>::value>::type > SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const; template < typename THPointDerived, typename = typename std::enable_if<IsFixedSizeVector<THPointDerived, 4>::value>::type > SOPHUS_FUNC HomogeneousPointProduct<THPointDerived> operator*(Eigen::MatrixBase<THPointDerived> const& p) const; SOPHUS_FUNC Line operator*(Line const& l) const; SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const; template < typename TOtherDerived, typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type > SOPHUS_FUNC Sim3Base<TDerived>& operator*=(Sim3Base<TOtherDerived> const& other); SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const; SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const; SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const; SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat); SOPHUS_FUNC QuaternionType const& quaternion() const; SOPHUS_FUNC Eigen::Matrix3<Scalar> rotationMatrix() const; SOPHUS_FUNC RxSo3Type& rxso3(); SOPHUS_FUNC RxSo3Type const& rxso3() const; SOPHUS_FUNC Scalar scale() const; SOPHUS_FUNC void setRotationMatrix(Eigen::Matrix3<Scalar>& r); SOPHUS_FUNC void setScale(Scalar const& scale); SOPHUS_FUNC void setScaledRotationMatrix(Eigen::Matrix3<Scalar> const& s_r); SOPHUS_FUNC TranslationType& translation(); SOPHUS_FUNC TranslationType const& translation() const; }; // direct descendants template <class TScalar, int kOptions> class Map<sophus::Sim3<TScalar> const, kOptions>; template <class TScalar, int kOptions> class Map<sophus::Sim3<TScalar>, kOptions>;
Detailed Documentation¶
Sim3 base type - implements Sim3 class but is storage agnostic.
Sim(3) is the group of rotations and translation and scaling in 3d. It is the semi-direct product of R+xSO(3) and the 3d Euclidean vector space. The class is represented using a composition of RxSo3 for scaling plus rotation and a 3-vector for translation.
Sim(3) is neither compact, nor a commutative group.
4x4 Eigen::Matrix representation:
| s*R t | | o 1 |
where R
is a 3x3 rotation matrix, s
a positive scale factor, t
a translation 3-vector and o
a 3-column vector of zeros.
Tangent 7-vector: [upsilon, omega, sigma],
where upsilon
is the translational velocity 3-vector and omega
the rotational velocity 3-vector, and sigma = log(s)
.
Internal 7-representation: [t0, t1, t2, qi0, qi1, qi2, qr],
with t0, t1, t2
are the translational components, and qi0, qi1, q2
the imaginary vector part and qr1
the real part of a non-zero quaternion. Here the scale s
is equal to the squared norm of the quaternion s = |q|^2
.
See RxSo3 for more details of the scaling + rotation representation in 3d.
Typedefs¶
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename TOtherDerived::Scalar>::ReturnType
For binary operations the return type is determined with the ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map types, as well as other compatible scalar types such as Ceres::Jet and double scalars with Sim3 operations.
Fields¶
static constexpr int kDoF = 7
Degrees of freedom of manifold, number of dimensions in tangent space (three for translation, three for rotation and one for scaling).
static constexpr int kNumParameters = 7
Number of internal parameters used (4-tuple for quaternion, three for translation).
static constexpr int kMatrixDim = 4
Group transformations are 4x4 matrices.
static constexpr int kPointDim = 3
Points are 3-dimensional.
Methods¶
SOPHUS_FUNC Adjoint adj() const
Adjoint transformation.
This function return the adjoint transformation Ad
of the group element A
such that for all x
it holds that hat(Ad_A * x) = A * hat(x) A^{-1}
. See hat-operator below.
template <class TNewScalarType> SOPHUS_FUNC Sim3<TNewScalarType> cast() const
Returns copy of instance casted to NewScalarType.
SOPHUS_FUNC Sim3<Scalar> inverse() const
Returns group inverse.
SOPHUS_FUNC Tangent log() const
Logarithmic map.
Computes the logarithm, the inverse of the group exponential which maps element of the group (rigid body transformations) to elements of the tangent space (twist).
To be specific, this function computes vee(logmat(.))
with logmat(.)
being the matrix logarithm and vee(.)
the vee-operator of Sim(3).
SOPHUS_FUNC Transformation matrix() const
Returns 4x4 matrix representation of the instance.
It has the following form:
| s*R t |
| o 1 |
where R
is a 3x3 rotation matrix, s
a scale factor, t
a translation 3-vector and o
a 3-column vector of zeros.
SOPHUS_FUNC Eigen::Matrix<Scalar, 3, 4> matrix3x4() const
Returns the significant first three rows of the matrix above.
template <class TOtherDerived> SOPHUS_FUNC Sim3Base<TDerived>& operator=(Sim3Base<TOtherDerived> const& other)
Assignment-like operator from OtherDerived.
template <class TOtherDerived> SOPHUS_FUNC Sim3Product<TOtherDerived> operator*(Sim3Base<TOtherDerived> const& other) const
Group multiplication, which is rotation plus scaling concatenation.
Note: That scaling is calculated with saturation. See RxSo3 for details.
template < typename TPointDerived, typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 3>::value>::type > SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const
Group action on 3-points.
This function rotates, scales and translates a three dimensional point p
by the Sim(3) element (bar_sR_foo, t_bar)
(= similarity transformation):
p_bar = bar_sR_foo * p_foo + t_bar
.
template < typename THPointDerived, typename = typename std::enable_if<IsFixedSizeVector<THPointDerived, 4>::value>::type > SOPHUS_FUNC HomogeneousPointProduct<THPointDerived> operator*(Eigen::MatrixBase<THPointDerived> const& p) const
Group action on homogeneous 3-points. See above for more details.
SOPHUS_FUNC Line operator*(Line const& l) const
Group action on lines.
This function rotates, scales and translates a parametrized line l(t) = o + t * d
by the Sim(3) element:
Origin o
is rotated, scaled and translated Direction d
is rotated
SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const
Group action on planes.
This function rotates and translates a plane n.x + d = 0
by the Sim(3) element:
Normal vector n
is rotated Offset d
is adjusted for scale and translation
template < typename TOtherDerived, typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type > SOPHUS_FUNC Sim3Base<TDerived>& operator*=(Sim3Base<TOtherDerived> const& other)
In-place group multiplication. This method is only valid if the return type of the multiplication is compatible with this So3 ‘s Scalar type.
SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const
Returns derivative of this * Sim3::exp(x) w.r.t. x at x = 0.
SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const
Returns derivative of log(this^{-1} * x) by x at x=this.
SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const
Returns internal parameters of Sim(3).
It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]), with q being the quaternion, t the translation 3-vector.
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat)
Setter of non-zero quaternion.
Precondition: quat
must not be close to zero.
SOPHUS_FUNC QuaternionType const& quaternion() const
Accessor of quaternion.
SOPHUS_FUNC Eigen::Matrix3<Scalar> rotationMatrix() const
Returns Rotation matrix.
SOPHUS_FUNC RxSo3Type& rxso3()
Mutator of So3 group.
SOPHUS_FUNC RxSo3Type const& rxso3() const
Accessor of So3 group.
SOPHUS_FUNC Scalar scale() const
Returns scale.
SOPHUS_FUNC void setRotationMatrix(Eigen::Matrix3<Scalar>& r)
Setter of quaternion using rotation matrix R
, leaves scale as is.
SOPHUS_FUNC void setScale(Scalar const& scale)
Sets scale and leaves rotation as is.
Note: This function as a significant computational cost, since it has to call the square root twice.
SOPHUS_FUNC void setScaledRotationMatrix(Eigen::Matrix3<Scalar> const& s_r)
Setter of quaternion using scaled rotation matrix sR
.
Precondition: The 3x3 matrix must be “scaled orthogonal” and have a positive determinant.
SOPHUS_FUNC TranslationType& translation()
Mutator of translation vector.
SOPHUS_FUNC TranslationType const& translation() const
Accessor of translation vector.