Program Listing for File SGal3_base.h

↰ Return to documentation for file (manif/impl/sgal3/SGal3_base.h)

#ifndef _MANIF_MANIF_SGAL3_BASE_H_
#define _MANIF_MANIF_SGAL3_BASE_H_

#include "manif/impl/sgal3/SGal3_properties.h"
#include "manif/impl/lie_group_base.h"
#include "manif/impl/so3/SO3_map.h"
#include "manif/impl/se3/SE3_map.h"

namespace manif {

//
// LieGroup
//

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

  using Base = LieGroupBase<_Derived>;
  using Type = SGal3Base<_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 LinearVelocity = typename internal::traits<_Derived>::LinearVelocity;
  using Time           = Scalar;
  using Transformation = Eigen::Matrix<Scalar, 5, 5>;
  using Isometry       = Eigen::Matrix<Scalar, 5, 5>;
  using QuaternionDataType = Eigen::Quaternion<Scalar>;

  // LieGroup common API

protected:

  using Base::derived;

  MANIF_DEFAULT_CONSTRUCTOR(SGal3Base)

public:

  MANIF_GROUP_ML_ASSIGN_OP(SGal3Base)


  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> &p,
    tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_pout_m = {},
    tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_pout_p = {}
  ) const;

  Jacobian adj() const;

  // SGal3 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;

  LinearVelocity linearVelocity() const;

  Scalar vx() const;

  Scalar vy() const;

  Scalar vz() const;

  Scalar t() const;

  void normalize();

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 SGal3Base<_Derived>::Transformation
SGal3Base<_Derived>::transform() const {
  Eigen::Matrix<Scalar, 5, 5> T;
  T.template topLeftCorner<3, 3>() = rotation();
  T.template block<3, 1>(0, 3) = linearVelocity();
  T.template topRightCorner<3, 1>() = translation();
  T.template bottomLeftCorner<2, 3>().setZero();
  T.template bottomRightCorner<2, 2>().setIdentity();
  T(3, 4) = t();
  return T;
}

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

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

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

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

template <typename _Derived>
typename SGal3Base<_Derived>::LinearVelocity
SGal3Base<_Derived>::linearVelocity() const {
  return coeffs().template segment<3>(7);
}

template <typename _Derived>
typename SGal3Base<_Derived>::LieGroup
SGal3Base<_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()-t()*linearVelocity())),
     so3inv,
    -so3inv.act(linearVelocity()),
    -t()
  );
}

template <typename _Derived>
typename SGal3Base<_Derived>::Tangent
SGal3Base<_Derived>::log(OptJacobianRef J_t_m) const {
  const SO3Tangent<Scalar> so3tan = asSO3().log();

  Eigen::Matrix<Scalar, 3, 3> E;
  Tangent::fillE(E, Eigen::Map<const SO3Tangent<Scalar>>(so3tan.data()));

  const LinearVelocity nu = so3tan.ljacinv() * linearVelocity();

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

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

  return tan;
}

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

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

  const auto& m_sgal3 = static_cast<const SGal3Base<_DerivedOther>&>(m);

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

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

  return LieGroup(
    rotation() * m_sgal3.translation() + m_sgal3.t() * linearVelocity() + translation(),
    asSO3() * m_sgal3.asSO3(),
    rotation() * m_sgal3.linearVelocity() + linearVelocity(),
    t() + m_sgal3.t()
  );
}

template <typename _Derived>
template <typename _EigenDerived>
Eigen::Matrix<typename SGal3Base<_Derived>::Scalar, 3, 1>
SGal3Base<_Derived>::act(
  const Eigen::MatrixBase<_EigenDerived> &p,
  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 10>>> J_pout_m,
  tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>>> J_pout_p
) const {
  assert_vector_dim(p, 3);

  const Rotation R(rotation());

  if (J_pout_m) {
    J_pout_m->template topLeftCorner<3, 3>() = R;
    J_pout_m->template block<3, 3>(0, 3).setZero();
    J_pout_m->template block<3, 3>(0, 6).noalias() = -R * skew(p);
    J_pout_m->template topRightCorner<3, 1>() = linearVelocity();
  }

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

  return translation() + R * p;
}


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

  Jacobian Adj;
  Adj.template topLeftCorner<3, 3>() = rotation();
  Adj.template block<3, 3>(0, 3).noalias() = -t() * Adj.template topLeftCorner<3, 3>();
  Adj.template block<3, 3>(0, 6).noalias() =
    skew(translation() - t() * linearVelocity()) * Adj.template topLeftCorner<3, 3>();
  Adj.template topRightCorner<3, 1>() = linearVelocity();

  Adj.template block<3, 3>(3, 3) = Adj.template topLeftCorner<3, 3>();
  Adj.template block<3, 3>(3, 6).noalias() =
    skew(linearVelocity()) * Adj.template topLeftCorner<3, 3>();

  Adj.template block<3,  3>(6, 6) = Adj.template topLeftCorner<3, 3>();

  Adj.template bottomLeftCorner<7, 3>().setZero();
  Adj.template block<4, 3>(6, 3).setZero();
  Adj.template block<1, 3>(9, 6).setZero();
  Adj.template block<6, 1>(3, 9).setZero();

  Adj(9, 9) = Scalar(1);

  return Adj;
}

// SGal3 specific function

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

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

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

template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::vx() const {
  return coeffs()(7);
}

template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::vy() const {
  return coeffs()(8);
}

template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::vz() const {
  return coeffs()(9);
}

template <typename _Derived>
typename SGal3Base<_Derived>::Scalar
SGal3Base<_Derived>::t() const {
  return coeffs()(10);
}

template <typename _Derived>
void SGal3Base<_Derived>::normalize() {
  coeffs().template segment<4>(3).normalize();
}

namespace internal {

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

    typename LieGroup::DataType data = LieGroup::DataType::Random();
    data.template segment<4>(3) = randQuat<Scalar>().coeffs();

    m = LieGroup(data);
  }
};

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

template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SGal3Base<Derived>, NewScalar> {
  template <typename T>
  static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
    return typename Derived::template LieGroupTemplate<NewScalar>(
      o.translation().template cast<NewScalar>(),
      o.quat().template cast<NewScalar>().normalized(),
      o.linearVelocity().template cast<NewScalar>(),
      NewScalar(o.t())
    );
  }
};

} // namespace internal
} // namespace manif

#endif // _MANIF_MANIF_SGAL3_BASE_H_