template class sophus::Sim2Base

Overview

Sim2 base type - implements Sim2 class but is storage agnostic. More…

#include <sim2.h>

template <class TDerived>
class Sim2Base {
public:
    // typedefs

    using Scalar = typename Eigen::internal::traits<TDerived>::Scalar ;
    using TranslationType = typename Eigen::internal::traits<TDerived>::TranslationType ;
    using RxSo2Type = typename Eigen::internal::traits<TDerived>::RxSo2Type ;
    using Transformation = Eigen::Matrix<Scalar, kMatrixDim, kMatrixDim> ;
    using Point = Eigen::Vector2<Scalar> ;
    using HomogeneousPoint = Eigen::Vector3<Scalar> ;
    using Line = Eigen::ParametrizedLine<Scalar, 2> ;
    using Hyperplane = Eigen::Hyperplane<Scalar, 2> ;
    using Tangent = Eigen::Vector<Scalar, kDoF> ;
    using Adjoint = Eigen::Matrix<Scalar, kDoF, kDoF> ;
    using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename TOtherDerived::Scalar>::ReturnType ;
    using Sim2Product = Sim2<ReturnScalar<TOtherDerived>> ;
    using PointProduct = Eigen::Vector2<ReturnScalar<TPointDerived>> ;
    using HomogeneousPointProduct = Eigen::Vector3<ReturnScalar<THPointDerived>> ;

    // fields

    static constexpr int kDoF = 4;
    static constexpr int kNumParameters = 4;
    static constexpr int kMatrixDim = 3;
    static constexpr int kPointDim = 2;

    // methods

    SOPHUS_FUNC Adjoint adj() const;

    template <class TNewScalarType>
    SOPHUS_FUNC Sim2<TNewScalarType> cast() const;

    SOPHUS_FUNC Sim2<Scalar> inverse() const;
    SOPHUS_FUNC Tangent log() const;
    SOPHUS_FUNC Transformation matrix() const;
    SOPHUS_FUNC Eigen::Matrix<Scalar, 2, 3> matrix2x3() const;

    template <class TOtherDerived>
    SOPHUS_FUNC Sim2Base<TDerived>& operator=(Sim2Base<TOtherDerived> const& other);

    template <class TOtherDerived>
    SOPHUS_FUNC Sim2Product<TOtherDerived> operator*(Sim2Base<TOtherDerived> const& other) const;

    template <
        typename TPointDerived,
        typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 2>::value>::type
        >
    SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const;

    template <
        typename THPointDerived,
        typename = typename std::enable_if<IsFixedSizeVector<THPointDerived, 3>::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;
    SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const;

    template <
        typename TOtherDerived,
        typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type
        >
    SOPHUS_FUNC Sim2Base<TDerived>& operator*=(Sim2Base<TOtherDerived> const& other);

    SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const;
    SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const;
    SOPHUS_FUNC void setComplex(Eigen::Vector2<Scalar> const& z);
    SOPHUS_FUNC Eigen::internal::traits<TDerived>::RxSo2Type::ComplexType const& complex() const;
    SOPHUS_FUNC Eigen::Matrix2<Scalar> rotationMatrix() const;
    SOPHUS_FUNC RxSo2Type& rxso2();
    SOPHUS_FUNC RxSo2Type const& rxso2() const;
    SOPHUS_FUNC Scalar scale() const;
    SOPHUS_FUNC void setRotationMatrix(Eigen::Matrix2<Scalar>& mat_r);
    SOPHUS_FUNC void setScale(Scalar const& scale);
    SOPHUS_FUNC void setScaledRotationMatrix(Eigen::Matrix2<Scalar> const& mat_scaled_rot);
    SOPHUS_FUNC TranslationType& translation();
    SOPHUS_FUNC TranslationType const& translation() const;
};

// direct descendants

template <class TScalar, int kOptions>
class Map<sophus::Sim2<TScalar> const, kOptions>;

template <class TScalar, int kOptions>
class Map<sophus::Sim2<TScalar>, kOptions>;

Detailed Documentation

Sim2 base type - implements Sim2 class but is storage agnostic.

Sim(2) is the group of rotations and translation and scaling in 2d. It is the semi-direct product of R+xSO(2) and the 2d Euclidean vector space. The class is represented using a composition of RxSo2 for scaling plus rotation and a 2-vector for translation.

  • 3x3 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 4-vector: [upsilon, omega, sigma],

where upsilon is the translational velocity 3-vector and omega the rotational velocity 3-vector, and sigma = log(s).

  • Internal 4-vector representation: [t0, t1, zr, zi],

with t0, t1 are the translational components, and zr the real part and zi the imaginary part of a non-zero complex number. Here the scale s is equal to the norm of the complex number s = |z|.

See RxSo2 for more details of the scaling + rotation representation in 2d.

Sim(2) is neither compact, nor a commutative group.

See RxSo2 for more details of the scaling + rotation representation in 2d.

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 SIM2 operations.

Fields

static constexpr int kDoF = 4

Degrees of freedom of manifold, number of dimensions in tangent space (two for translation, one for rotation and one for scaling).

static constexpr int kNumParameters = 4

Number of internal parameters used (2-tuple for complex number, two for translation).

static constexpr int kMatrixDim = 3

Group transformations are 3x3 matrices.

static constexpr int kPointDim = 2

Points are 2-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 Sim2<TNewScalarType> cast() const

Returns copy of instance casted to NewScalarType.

SOPHUS_FUNC Sim2<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(2).

SOPHUS_FUNC Transformation matrix() const

Returns 3x3 matrix representation of the instance.

It has the following form:

| s*R t | | o 1 |

where R is a 2x2 rotation matrix, s a scale factor, t a translation 2-vector and o a 2-column vector of zeros.

SOPHUS_FUNC Eigen::Matrix<Scalar, 2, 3> matrix2x3() const

Returns the significant first two rows of the matrix above.

template <class TOtherDerived>
SOPHUS_FUNC Sim2Base<TDerived>& operator=(Sim2Base<TOtherDerived> const& other)

Assignment-like operator from OtherDerived.

template <class TOtherDerived>
SOPHUS_FUNC Sim2Product<TOtherDerived> operator*(Sim2Base<TOtherDerived> const& other) const

Group multiplication, which is rotation plus scaling concatenation.

Note: That scaling is calculated with saturation. See RxSo2 for details.

template <
    typename TPointDerived,
    typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 2>::value>::type
    >
SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const

Group action on 2-points.

This function rotates, scales and translates a two dimensional point p by the Sim(2) 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, 3>::value>::type
    >
SOPHUS_FUNC HomogeneousPointProduct<THPointDerived> operator*(Eigen::MatrixBase<THPointDerived> const& p) const

Group action on homogeneous 2-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(2) element:

Origin o is rotated, scaled and translated Direction d is rotated

SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const

Group action on hyper-planes.

This function rotates a hyper-plane n.x + d = 0 by the Sim2 element:

Normal vector n is rotated Offset d is scaled and adjusted for translation

Note that in 2d-case hyper-planes are just another parametrization of lines

SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const

Returns internal parameters of Sim(2).

It returns (c[0], c[1], t[0], t[1]), with c being the complex number, t the translation 3-vector.

template <
    typename TOtherDerived,
    typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type
    >
SOPHUS_FUNC Sim2Base<TDerived>& operator*=(Sim2Base<TOtherDerived> const& other)

In-place group multiplication. This method is only valid if the return type of the multiplication is compatible with this So2 ‘s Scalar type.

SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const

Returns derivative of this * Sim2::exp(x) wrt. 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 void setComplex(Eigen::Vector2<Scalar> const& z)

Setter of non-zero complex number.

Precondition: z must not be close to zero.

SOPHUS_FUNC Eigen::internal::traits<TDerived>::RxSo2Type::ComplexType const& complex() const

Accessor of complex number.

SOPHUS_FUNC Eigen::Matrix2<Scalar> rotationMatrix() const

Returns Rotation matrix.

SOPHUS_FUNC RxSo2Type& rxso2()

Mutator of So2 group.

SOPHUS_FUNC RxSo2Type const& rxso2() const

Accessor of So2 group.

SOPHUS_FUNC Scalar scale() const

Returns scale.

SOPHUS_FUNC void setRotationMatrix(Eigen::Matrix2<Scalar>& mat_r)

Setter of complex number 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::Matrix2<Scalar> const& mat_scaled_rot)

Setter of complexnumber using scaled rotation matrix sR.

Precondition: The 2x2 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.