Template Class CartesianBase

Class Documentation

template<class Derived, int M>
class CartesianBase

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

Euclidean vector space as Lie group.

Lie groups can be seen as a generalization over the Euclidean vector space R^M. Here a N-dimensional vector p is represented as a | I p | | o 1 |

On the other hand, Cartesian(M) can be seen as a special case of SE(M) with identity rotation, and hence represents pure translation.

The purpose of this class is two-fold:

  • for educational purpose, to highlight how Lie groups generalize over Euclidean vector spaces.

  • to be used in templated/generic algorithms (such as Sophus::Spline) which are implemented against the Lie group interface.

Obviously, Cartesian(M) can just be represented as a M-tuple.

Cartesian is not compact, but a commutative group. For vector additions it holds a+b = b+a.

See Cartesian class for more details below.

Public Types

using Scalar = typename Eigen::internal::traits<Derived>::Scalar
using ParamsType = typename Eigen::internal::traits<Derived>::ParamsType
using Transformation = Sophus::Matrix<Scalar, N, N>
using Point = Sophus::Vector<Scalar, M>
using HomogeneousPoint = Sophus::Vector<Scalar, N>
using Line = ParametrizedLine<Scalar, M>
using Hyperplane = Eigen::Hyperplane<Scalar, M>
using Tangent = Sophus::Vector<Scalar, DoF>
using Adjoint = Matrix<Scalar, DoF, DoF>
template<typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename OtherDerived::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 Cartesian operations.

template<typename OtherDerived>
using CartesianSum = Cartesian<ReturnScalar<OtherDerived>, M>
template<typename PointDerived>
using PointProduct = Sophus::Vector<ReturnScalar<PointDerived>, M>
template<typename HPointDerived>
using HomogeneousPointProduct = Sophus::Vector<ReturnScalar<HPointDerived>, N>

Public Functions

inline SOPHUS_FUNC Adjoint Adj () const

Adjoint transformation

Always identity of commutative groups.

template<class NewScalarType> inline SOPHUS_FUNC Cartesian< NewScalarType, M > cast () const

Returns copy of instance casted to NewScalarType.

inline SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > Dx_this_mul_exp_x_at_0 () const

Returns derivative of this * exp(x) wrt x at x=0.

inline SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > Dx_log_this_inv_by_x_at_this () const

Returns derivative of log(this^{-1} * x) by x at x=this.

inline SOPHUS_FUNC Cartesian< Scalar, M > inverse () const

Returns group inverse.

The additive inverse.

inline SOPHUS_FUNC Tangent log () const

Logarithmic map

For Euclidean vector space, just the identity. Or to be more precise it just extracts the significant M-vector from the NxN matrix.

inline SOPHUS_FUNC Transformation matrix () const

Returns 4x4 matrix representation of the instance.

It has the following form:

| I p | | o 1 |

template<class OtherDerived> inline SOPHUS_FUNC CartesianBase< Derived, M > & operator= (CartesianBase< OtherDerived, M > const &other)

Group multiplication, are vector additions.

template<typename OtherDerived> inline SOPHUS_FUNC CartesianSum< OtherDerived > operator* (CartesianBase< OtherDerived, M > const &other) const

Group multiplication, are vector additions.

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

Group action on points, again just vector addition.

template<typename HPointDerived, typename = typename std::enable_if<                IsFixedSizeVector<HPointDerived, N>::value>::type> inline SOPHUS_FUNC HomogeneousPointProduct< HPointDerived > operator* (Eigen::MatrixBase< HPointDerived > const &p) const

Group action on homogeneous points. See above for more details.

inline SOPHUS_FUNC Line operator* (Line const &l) const

Group action on lines.

inline SOPHUS_FUNC Hyperplane operator* (Hyperplane const &p) const

Group action on planes.

template<typename OtherDerived, typename = typename std::enable_if<                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type> inline SOPHUS_FUNC CartesianBase< Derived, M > & operator*= (CartesianBase< OtherDerived, M > const &other)

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

inline SOPHUS_FUNC ParamsType & params ()

Mutator of params vector.

inline SOPHUS_FUNC ParamsType const  & params () const

Accessor of params vector

Public Static Attributes

static constexpr int DoF = M

Degrees of freedom of manifold, equals to number of Cartesian coordinates.

static constexpr int num_parameters = M

Number of internal parameters used, also M.

static constexpr int N = M + 1

Group transformations are (M+1)x(M+1) matrices.

static constexpr int Dim = M