Program Listing for File SE3_base.h

↰ Return to documentation for file (manif/impl/se3/SE3_base.h)

#ifndef _MANIF_MANIF_SE3_BASE_H_
#define _MANIF_MANIF_SE3_BASE_H_

#include "manif/impl/se3/SE3_properties.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/so3/SO3_map.h"

namespace manif {

//
// LieGroup
//

template <typename _Derived>
struct SE3Base : LieGroupBase<_Derived>
{
private:

  using Base = LieGroupBase<_Derived>;
  using Type = SE3Base<_Derived>;

public:

  MANIF_GROUP_TYPEDEF
  MANIF_INHERIT_GROUP_AUTO_API
  MANIF_INHERIT_GROUP_OPERATOR

  using Base::coeffs;

  using Rotation       = typename internal::traits<_Derived>::Rotation;
  using Translation    = typename internal::traits<_Derived>::Translation;
  using Transformation = typename internal::traits<_Derived>::Transformation;
  using Isometry       = Eigen::Transform<Scalar, 3, Eigen::Isometry>;

  using QuaternionDataType = Eigen::Quaternion<Scalar>;

  // LieGroup common API

protected:

  using Base::derived;

  MANIF_DEFAULT_CONSTRUCTOR(SE3Base)

public:

  MANIF_GROUP_ML_ASSIGN_OP(SE3Base)


  LieGroup inverse(OptJacobianRef J_minv_m = {}) const;

  Tangent log(OptJacobianRef J_t_m = {}) const;

  MANIF_DEPRECATED
  Tangent lift(OptJacobianRef J_t_m = {}) const;

  template <typename _DerivedOther>
  LieGroup compose(const LieGroupBase<_DerivedOther>& m,
                   OptJacobianRef J_mc_ma = {},
                   OptJacobianRef J_mc_mb = {}) const;

  template <typename _EigenDerived>
  Eigen::Matrix<Scalar, 3, 1>
  act(const Eigen::MatrixBase<_EigenDerived> &v,
      tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 6>>> J_vout_m = {},
      tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v = {}) const;

  Jacobian adj() const;

  // SE3 specific functions

  Transformation transform() const;

  Isometry isometry() const;

  Rotation rotation() const;

  QuaternionDataType quat() const;

  Translation translation() const;

  Scalar x() const;

  Scalar y() const;

  Scalar z() const;

  //Scalar roll() const;
  //Scalar pitch() const;
  //Scalar yaw() const;

  void normalize();

  void quat(const QuaternionDataType& quaternion);

  template <typename _EigenDerived>
  void quat(const Eigen::MatrixBase<_EigenDerived>& quaternion);

  void quat(const SO3<Scalar>& so3);

  void translation(const Translation& translation);

public:

  Eigen::Map<const SO3<Scalar>> asSO3() const
  {
    return Eigen::Map<const SO3<Scalar>>(coeffs().data()+3);
  }

  Eigen::Map<SO3<Scalar>> asSO3()
  {
    return Eigen::Map<SO3<Scalar>>(coeffs().data()+3);
  }
};

template <typename _Derived>
typename SE3Base<_Derived>::Transformation
SE3Base<_Derived>::transform() const
{
  Transformation T = Transformation::Identity();
  T.template topLeftCorner<3,3>()  = rotation();
  T.template topRightCorner<3,1>() = translation();
  return T;
}

template <typename _Derived>
typename SE3Base<_Derived>::Isometry
SE3Base<_Derived>::isometry() const
{
  return Isometry(transform());
}

template <typename _Derived>
typename SE3Base<_Derived>::Rotation
SE3Base<_Derived>::rotation() const
{
  return asSO3().rotation();
}

template <typename _Derived>
typename SE3Base<_Derived>::QuaternionDataType
SE3Base<_Derived>::quat() const
{
  return asSO3().quat();
}

template <typename _Derived>
typename SE3Base<_Derived>::Translation
SE3Base<_Derived>::translation() const
{
  return coeffs().template head<3>();
}

template <typename _Derived>
void SE3Base<_Derived>::quat(const QuaternionDataType& quaternion)
{
  quat(quaternion.coeffs());
}

template <typename _Derived>
template <typename _EigenDerived>
void SE3Base<_Derived>::quat(const Eigen::MatrixBase<_EigenDerived>& quaternion)
{
  using std::abs;
  assert_vector_dim(quaternion, 4);
  MANIF_ASSERT(abs(quaternion.norm()-Scalar(1)) <
               Constants<Scalar>::eps,
               "The quaternion is not normalized !",
               invalid_argument);

  asSO3().coeffs() = quaternion;
}

template <typename _Derived>
void SE3Base<_Derived>::quat(const SO3<Scalar>& so3)
{
  quat(so3.coeffs());
}

template <typename _Derived>
void SE3Base<_Derived>::translation(const Translation& translation)
{
  coeffs().template head<3>() = translation;
}

template <typename _Derived>
typename SE3Base<_Derived>::LieGroup
SE3Base<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
  if (J_minv_m)
  {
    (*J_minv_m) = -adj();
  }

  const SO3<Scalar> so3inv = asSO3().inverse();

  return LieGroup(-so3inv.act(translation()),
                   so3inv);
}

template <typename _Derived>
typename SE3Base<_Derived>::Tangent
SE3Base<_Derived>::log(OptJacobianRef J_t_m) const
{
  using std::abs;
  using std::sqrt;

  const SO3Tangent<Scalar> so3tan = asSO3().log();

  Tangent tan((typename Tangent::DataType() <<
               so3tan.ljacinv()*translation(),
               so3tan.coeffs()).finished());

  if (J_t_m)
  {
    // Jr^-1
    (*J_t_m) = tan.rjacinv();
  }

  return tan;
}

template <typename _Derived>
typename SE3Base<_Derived>::Tangent
SE3Base<_Derived>::lift(OptJacobianRef J_t_m) const
{
  return log(J_t_m);
}

template <typename _Derived>
template <typename _DerivedOther>
typename SE3Base<_Derived>::LieGroup
SE3Base<_Derived>::compose(
    const LieGroupBase<_DerivedOther>& m,
    OptJacobianRef J_mc_ma,
    OptJacobianRef J_mc_mb) const
{
  static_assert(
    std::is_base_of<SE3Base<_DerivedOther>, _DerivedOther>::value,
    "Argument does not inherit from SE3Base !");

  const auto& m_se3 = static_cast<const SE3Base<_DerivedOther>&>(m);

  if (J_mc_ma)
  {
    (*J_mc_ma) = m.inverse().adj();
  }

  if (J_mc_mb)
  {
    J_mc_mb->setIdentity();
  }

  return LieGroup(rotation()*m_se3.translation() + translation(),
                  asSO3().compose(m_se3.asSO3()).quat());
}

template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SE3Base<_Derived>::Scalar, 3, 1>
SE3Base<_Derived>::act(const Eigen::MatrixBase<_EigenDerived> &v,
                       tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 6>>> J_vout_m,
                       tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_vout_v) const
{
  assert_vector_dim(v, 3);
  const Rotation R(rotation());

  if (J_vout_m)
  {
    J_vout_m->template topLeftCorner<3,3>()  =  R;
    J_vout_m->template topRightCorner<3,3>() = -R * skew(v);
  }

  if (J_vout_v)
  {
    (*J_vout_v) = R;
  }

  return translation() + R * v;
}


template <typename _Derived>
typename SE3Base<_Derived>::Jacobian
SE3Base<_Derived>::adj() const
{

  Jacobian Adj;
  Adj.template topLeftCorner<3,3>() = rotation();
  Adj.template bottomRightCorner<3,3>() =
      Adj.template topLeftCorner<3,3>();
  Adj.template topRightCorner<3,3>().noalias() =
    skew(translation()) * Adj.template topLeftCorner<3,3>();
  Adj.template bottomLeftCorner<3,3>().setZero();

  return Adj;
}

// SE3 specific function

template <typename _Derived>
typename SE3Base<_Derived>::Scalar
SE3Base<_Derived>::x() const
{
  return coeffs().x();
}

template <typename _Derived>
typename SE3Base<_Derived>::Scalar
SE3Base<_Derived>::y() const
{
  return coeffs().y();
}

template <typename _Derived>
typename SE3Base<_Derived>::Scalar
SE3Base<_Derived>::z() const
{
  return coeffs().z();
}

template <typename _Derived>
void SE3Base<_Derived>::normalize()
{
  coeffs().template tail<4>().normalize();
}

namespace internal {

template <typename Derived>
struct RandomEvaluatorImpl<SE3Base<Derived>>
{
  template <typename T>
  static void run(T& m)
  {
    using Scalar      = typename SE3Base<Derived>::Scalar;
    using Translation = typename SE3Base<Derived>::Translation;
    using LieGroup    = typename SE3Base<Derived>::LieGroup;

    m = LieGroup(Translation::Random(), randQuat<Scalar>());
  }
};

template <typename Derived>
struct AssignmentEvaluatorImpl<SE3Base<Derived>>
{
  template <typename T>
  static void run_impl(const T& data)
  {
    using std::abs;
    MANIF_ASSERT(
      abs(data.template tail<4>().norm()-typename SE3Base<Derived>::Scalar(1)) <
      Constants<typename SE3Base<Derived>::Scalar>::eps,
      "SE3 assigned data not normalized !",
      manif::invalid_argument
    );
    MANIF_UNUSED_VARIABLE(data);
  }
};

template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SE3Base<Derived>, NewScalar> {
  template <typename T>
  static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
    const typename SE3Base<Derived>::QuaternionDataType q = o.quat();
    const typename SE3Base<Derived>::Translation t = o.translation();

    return typename Derived::template LieGroupTemplate<NewScalar>(
      t.template cast<NewScalar>(), q.template cast<NewScalar>().normalized()
    );
  }
};

} /* namespace internal */
} /* namespace manif */

#endif /* _MANIF_MANIF_SE3_BASE_H_ */