Program Listing for File SO2_base.h

↰ Return to documentation for file (manif/impl/so2/SO2_base.h)

#ifndef _MANIF_MANIF_SO2_BASE_H_
#define _MANIF_MANIF_SO2_BASE_H_

#include "manif/impl/so2/SO2_properties.h"
#include "manif/impl/lie_group_base.h"

namespace manif {

//
// LieGroup
//

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

  using Base = LieGroupBase<_Derived>;
  using Type = SO2Base<_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 Transformation = typename internal::traits<_Derived>::Transformation;

  // LieGroup common API

protected:

  using Base::derived;

  MANIF_DEFAULT_CONSTRUCTOR(SO2Base)

public:

  MANIF_GROUP_ML_ASSIGN_OP(SO2Base)


  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, 2, 1>
  act(const Eigen::MatrixBase<_EigenDerived> &v,
      tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 1>>> J_vout_m = {},
      tl::optional<Eigen::Ref<Eigen::Matrix<Scalar, 2, 2>>> J_vout_v = {}) const;

  Jacobian adj() const;

  // SO2 specific functions

  Transformation transform() const;

  Rotation rotation() const;

  Scalar real() const;

  Scalar imag() const;

  Scalar angle() const;

  void normalize();

// protected:


//  Scalar& real();
//  Scalar& imag();
};

template <typename _Derived>
typename SO2Base<_Derived>::Transformation
SO2Base<_Derived>::transform() const
{
  Transformation T(Transformation::Identity());
  T.template topLeftCorner<2, 2>() = rotation();
  return T;
}

template <typename _Derived>
typename SO2Base<_Derived>::Rotation
SO2Base<_Derived>::rotation() const
{
  using std::sin;
  using std::cos;
  const Scalar theta = angle();
  return (Rotation() << cos(theta), -sin(theta),
                        sin(theta),  cos(theta)).finished();
}

template <typename _Derived>
typename SO2Base<_Derived>::LieGroup
SO2Base<_Derived>::inverse(OptJacobianRef J_minv_m) const
{
  if (J_minv_m)
    J_minv_m->setConstant(Scalar(-1));

  return LieGroup(real(), -imag());
}

template <typename _Derived>
typename SO2Base<_Derived>::Tangent
SO2Base<_Derived>::log(OptJacobianRef J_t_m) const
{
  if (J_t_m)
    J_t_m->setConstant(Scalar(1));

  return Tangent(angle());
}

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

template <typename _Derived>
template <typename _DerivedOther>
typename SO2Base<_Derived>::LieGroup
SO2Base<_Derived>::compose(
    const LieGroupBase<_DerivedOther>& m,
    OptJacobianRef J_mc_ma,
    OptJacobianRef J_mc_mb) const
{
  using std::abs;

  static_assert(
    std::is_base_of<SO2Base<_DerivedOther>, _DerivedOther>::value,
    "Argument does not inherit from SE2Base !");

  if (J_mc_ma)
    J_mc_ma->setConstant(Scalar(1));

  if (J_mc_mb)
    J_mc_mb->setConstant(Scalar(1));

  const auto& m_so2 = static_cast<const SO2Base<_DerivedOther>&>(m);

  Scalar ret_real = real() * m_so2.real() - imag() * m_so2.imag();
  Scalar ret_imag = real() * m_so2.imag() + imag() * m_so2.real();

  const Scalar ret_sqnorm = ret_real*ret_real+ret_imag*ret_imag;

  if (abs(ret_sqnorm-Scalar(1)) > Constants<Scalar>::eps)
  {
    const Scalar scale = approxSqrtInv(ret_sqnorm);
    ret_real *= scale;
    ret_imag *= scale;
  }

  return LieGroup(ret_real, ret_imag);
}

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

  if (J_vout_m)
  {
    J_vout_m->noalias() = R * skew(Scalar(1)) * v;
  }

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

  return R * v;
}

template <typename _Derived>
typename SO2Base<_Derived>::Jacobian
SO2Base<_Derived>::adj() const
{
  static const Jacobian adj = Jacobian::Constant(Scalar(1));
  return adj;
}

// SO2 specific function

template <typename _Derived>
/*const*/ typename SO2Base<_Derived>::Scalar/*&*/
SO2Base<_Derived>::real() const
{
  return coeffs().x();
}

template <typename _Derived>
/*const*/ typename SO2Base<_Derived>::Scalar/*&*/
SO2Base<_Derived>::imag() const
{
  return coeffs().y();
}

template <typename _Derived>
typename SO2Base<_Derived>::Scalar
SO2Base<_Derived>::angle() const
{
  using std::atan2;
  return atan2(imag(), real());
}

//template <typename _Derived>
//typename SO2Base<_Derived>::Scalar&
//SO2Base<_Derived>::real()
//{
//  return coeffs.x();
//}

//template <typename _Derived>
//typename SO2Base<_Derived>::Scalar&
//SO2Base<_Derived>::imag()
//{
//  return coeffs.y();
//}

template <typename _Derived>
void SO2Base<_Derived>::normalize()
{
  coeffs().normalize();
}

namespace internal {

template <typename Derived>
struct RandomEvaluatorImpl<SO2Base<Derived>>
{
  template <typename T>
  static void run(T& m)
  {
    using Tangent = typename LieGroupBase<Derived>::Tangent;
    m = Tangent::Random().exp();
  }
};

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

template <typename Derived, typename NewScalar>
struct CastEvaluatorImpl<SO2Base<Derived>, NewScalar> {
  template <typename T>
  static auto run(const T& o) -> typename Derived::template LieGroupTemplate<NewScalar> {
    return typename Derived::template LieGroupTemplate<NewScalar>(NewScalar(o.angle()));
  }
};

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

#endif /* _MANIF_MANIF_SO2_BASE_H_ */