Program Listing for File SE3Tangent_base.h

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

#ifndef _MANIF_MANIF_SE3TANGENT_BASE_H_
#define _MANIF_MANIF_SE3TANGENT_BASE_H_

#include "manif/impl/se3/SE3_properties.h"
#include "manif/impl/tangent_base.h"
#include "manif/impl/so3/SO3Tangent_map.h"

namespace manif {

//
// Tangent
//

template <typename _Derived>
struct SE3TangentBase : TangentBase<_Derived>
{
private:

  using Base = TangentBase<_Derived>;
  using Type = SE3TangentBase<_Derived>;

public:

  MANIF_TANGENT_TYPEDEF
  MANIF_INHERIT_TANGENT_OPERATOR

  using LinBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
  using AngBlock = typename DataType::template FixedSegmentReturnType<3>::Type;
  using ConstLinBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;
  using ConstAngBlock = typename DataType::template ConstFixedSegmentReturnType<3>::Type;

  using Base::data;
  using Base::coeffs;

protected:

  using Base::derived;

  MANIF_DEFAULT_CONSTRUCTOR(SE3TangentBase)

public:

  MANIF_TANGENT_ML_ASSIGN_OP(SE3TangentBase)

  // Tangent common API


  LieAlg hat() const;

  LieGroup exp(OptJacobianRef J_m_t = {}) const;

  MANIF_DEPRECATED
  LieGroup retract(OptJacobianRef J_m_t = {}) const;

  Jacobian rjac() const;

  Jacobian ljac() const;

  Jacobian rjacinv() const;

  Jacobian ljacinv() const;

  Jacobian smallAdj() const;

  // SE3Tangent specific API

  LinBlock lin();
  const ConstLinBlock lin() const;

  AngBlock ang();
  const ConstAngBlock ang() const;

//  Scalar x() const;
//  Scalar y() const;
//  Scalar z() const;

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

public:

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

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

// private:

  template <typename _EigenDerived>
  static void fillQ(Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>> Q,
                    const Eigen::MatrixBase<_EigenDerived>& c);
};

template <typename _Derived>
typename SE3TangentBase<_Derived>::LieGroup
SE3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
{
  using std::sqrt;
  using std::cos;
  using std::sin;

  if (J_m_t)
  {
    *J_m_t = rjac();
  }

  return LieGroup(asSO3().ljac()*lin(), asSO3().exp().quat());
}

template <typename _Derived>
typename SE3TangentBase<_Derived>::LieGroup
SE3TangentBase<_Derived>::retract(OptJacobianRef J_m_t) const
{
  return exp(J_m_t);
}

template <typename _Derived>
typename SE3TangentBase<_Derived>::LieAlg
SE3TangentBase<_Derived>::hat() const
{
  return (LieAlg() <<
    Scalar(0)           , Scalar(-coeffs()(5)), Scalar( coeffs()(4)), Scalar(coeffs()(0)),
    Scalar( coeffs()(5)), Scalar(0)           , Scalar(-coeffs()(3)), Scalar(coeffs()(1)),
    Scalar(-coeffs()(4)), Scalar( coeffs()(3)), Scalar(0)           , Scalar(coeffs()(2)),
    Scalar(0)           , Scalar(0)           , Scalar(0)           , Scalar(0)
          ).finished();
}

template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::rjac() const
{
  Jacobian Jr;
  Jr.template bottomLeftCorner<3,3>().setZero();
  Jr.template topLeftCorner<3,3>() = asSO3().rjac();
  Jr.template bottomRightCorner<3,3>() = Jr.template topLeftCorner<3,3>();
  fillQ( Jr.template topRightCorner<3,3>(), -coeffs() );

  return Jr;
}

template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::ljac() const
{
  Jacobian Jl;
  Jl.template bottomLeftCorner<3,3>().setZero();
  Jl.template topLeftCorner<3,3>() = asSO3().ljac();
  Jl.template bottomRightCorner<3,3>() = Jl.template topLeftCorner<3,3>();
  fillQ( Jl.template topRightCorner<3,3>(), coeffs() );

  return Jl;
}

template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::rjacinv() const
{
  Jacobian Jr_inv;
  fillQ( Jr_inv.template bottomLeftCorner<3,3>(), -coeffs() ); // serves as temporary Q
  Jr_inv.template topLeftCorner<3,3>() = asSO3().rjacinv();
  Jr_inv.template bottomRightCorner<3,3>() = Jr_inv.template topLeftCorner<3,3>();
  Jr_inv.template topRightCorner<3,3>().noalias() =
      -Jr_inv.template topLeftCorner<3,3>()    *
       Jr_inv.template bottomLeftCorner<3,3>() *
       Jr_inv.template topLeftCorner<3,3>();
  Jr_inv.template bottomLeftCorner<3,3>().setZero();

  return Jr_inv;
}

template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::ljacinv() const
{
  Jacobian Jl_inv;
  fillQ( Jl_inv.template bottomLeftCorner<3,3>(), coeffs() ); // serves as temporary Q
  Jl_inv.template topLeftCorner<3,3>() = asSO3().ljacinv();
  Jl_inv.template bottomRightCorner<3,3>() = Jl_inv.template topLeftCorner<3,3>();
  Jl_inv.template topRightCorner<3,3>().noalias() =
      -Jl_inv.template topLeftCorner<3,3>()    *
       Jl_inv.template bottomLeftCorner<3,3>() *
       Jl_inv.template topLeftCorner<3,3>();
  Jl_inv.template bottomLeftCorner<3,3>().setZero();

  return Jl_inv;
}

template <typename _Derived>
template <typename _EigenDerived>
void SE3TangentBase<_Derived>::fillQ(
  Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>> Q,
  const Eigen::MatrixBase<_EigenDerived>& c)
{
    using std::cos;
    using std::sin;
    using std::sqrt;

    const Scalar theta_sq = c.template tail<3>().squaredNorm();

    Scalar A(0.5), B, C, D;

    // Small angle approximation
    if (theta_sq <= Constants<Scalar>::eps)
    {
      B =  Scalar(1./6.)  + Scalar(1./120.)  * theta_sq;
      C = -Scalar(1./24.) + Scalar(1./720.)  * theta_sq;
      D = -Scalar(1./60.);
    }
    else
    {
      const Scalar theta     = sqrt(theta_sq);
      const Scalar sin_theta = sin(theta);
      const Scalar cos_theta = cos(theta);

      B = (theta - sin_theta) / (theta_sq*theta);
      C = (Scalar(1) - theta_sq/Scalar(2) - cos_theta) / (theta_sq*theta_sq);
      D = (C - Scalar(3)*(theta-sin_theta-theta_sq*theta/Scalar(6)) / (theta_sq*theta_sq*theta));

      // http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf
  //    C = (theta_sq+Scalar(2)*cos_theta-Scalar(2)) / (Scalar(2)*theta_sq*theta_sq);
  //    D = (Scalar(2)*theta - Scalar(3)*sin_theta + theta*cos_theta) / (Scalar(2)*theta_sq*theta_sq*theta);
    }

    const Eigen::Matrix<Scalar, 3, 3> V   = skew(c.template head<3>());
    const Eigen::Matrix<Scalar, 3, 3> W   = skew(c.template tail<3>());
    const Eigen::Matrix<Scalar, 3, 3> VW  = V * W;
    const Eigen::Matrix<Scalar, 3, 3> WV  = VW.transpose();       // Note on this change wrt. Barfoot: it happens that V*W = (W*V).transpose() !!!
    const Eigen::Matrix<Scalar, 3, 3> WVW = WV * W;
    const Eigen::Matrix<Scalar, 3, 3> VWW = VW * W;
    Q.noalias() =
        + A * V
        + B * (WV + VW + WVW)
        - C * (VWW - VWW.transpose() - Scalar(3) * WVW)           // Note on this change wrt. Barfoot: it happens that V*W*W = -(W*W*V).transpose() !!!
        - D * WVW * W;                                            // Note on this change wrt. Barfoot: it happens that W*V*W*W = W*W*V*W !!!
}


template <typename _Derived>
typename SE3TangentBase<_Derived>::Jacobian
SE3TangentBase<_Derived>::smallAdj() const
{

  Jacobian smallAdj;
  smallAdj.template topRightCorner<3,3>() = skew(lin());
  smallAdj.template topLeftCorner<3,3>() = skew(ang());
  smallAdj.template bottomRightCorner<3,3>() = smallAdj.template topLeftCorner<3,3>();
  smallAdj.template bottomLeftCorner<3,3>().setZero();

  return smallAdj;
}

// SE3Tangent specific API

template <typename _Derived>
typename SE3TangentBase<_Derived>::LinBlock
SE3TangentBase<_Derived>::lin()
{
  return coeffs().template head<3>();
}

template <typename _Derived>
const typename SE3TangentBase<_Derived>::ConstLinBlock
SE3TangentBase<_Derived>::lin() const
{
  return coeffs().template head<3>();
}

template <typename _Derived>
typename SE3TangentBase<_Derived>::AngBlock
SE3TangentBase<_Derived>::ang()
{
  return coeffs().template tail<3>();
}

template <typename _Derived>
const typename SE3TangentBase<_Derived>::ConstAngBlock
SE3TangentBase<_Derived>::ang() const
{
  return coeffs().template tail<3>();
}

//template <typename _Derived>
//typename SE3TangentBase<_Derived>::Scalar
//SE3TangentBase<_Derived>::x() const
//{
//  return data()->x();
//}

//template <typename _Derived>
//typename SE3TangentBase<_Derived>::Scalar
//SE3TangentBase<_Derived>::y() const
//{
//  return data()->y();
//}

//template <typename _Derived>
//typename SE3TangentBase<_Derived>::Scalar
//SE3TangentBase<_Derived>::z() const
//{
//  return data()->z();
//}

namespace internal {

template <typename Derived>
struct GeneratorEvaluator<SE3TangentBase<Derived>>
{
  static typename SE3TangentBase<Derived>::LieAlg
  run(const unsigned int i)
  {
    using LieAlg = typename SE3TangentBase<Derived>::LieAlg;
    using Scalar = typename SE3TangentBase<Derived>::Scalar;

    switch (i)
    {
      case 0:
      {
        static const LieAlg E0(
                (LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(1),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
        return E0;
      }
      case 1:
      {
        static const LieAlg E1(
                (LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(1),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
        return E1;
      }
      case 2:
      {
        static const LieAlg E2(
                (LieAlg() << Scalar(0), Scalar(0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(1),
                             Scalar(0), Scalar(0), Scalar(0), Scalar(0) ).finished());
        return E2;
      }
      case 3:
      {
        static const LieAlg E3(
                (LieAlg() << Scalar(0), Scalar(0), Scalar( 0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar(-1), Scalar(0),
                             Scalar(0), Scalar(1), Scalar( 0), Scalar(0),
                             Scalar(0), Scalar(0), Scalar( 0), Scalar(0) ).finished());
        return E3;
      }
      case 4:
      {
        static const LieAlg E4(
                (LieAlg() << Scalar( 0), Scalar(0), Scalar(1), Scalar(0),
                             Scalar( 0), Scalar(0), Scalar(0), Scalar(0),
                             Scalar(-1), Scalar(0), Scalar(0), Scalar(0),
                             Scalar( 0), Scalar(0), Scalar(0), Scalar(0) ).finished());
        return E4;
      }
      case 5:
      {
        static const LieAlg E5(
                (LieAlg() << Scalar(0), Scalar(-1), Scalar(0), Scalar(0),
                             Scalar(1), Scalar( 0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar( 0), Scalar(0), Scalar(0),
                             Scalar(0), Scalar( 0), Scalar(0), Scalar(0) ).finished());
        return E5;
      }
      default:
        MANIF_THROW("Index i must be in [0,5]!", invalid_argument);
        break;
    }

    return LieAlg{};
  }
};

template <typename Derived>
struct RandomEvaluatorImpl<SE3TangentBase<Derived>>
{
  static void run(SE3TangentBase<Derived>& m)
  {
    m.coeffs().template head<3>().setRandom();
    // In ball of radius PI
    m.coeffs().template tail<3>() = randPointInBall(MANIF_PI).template cast<typename Derived::Scalar>();
  }
};

template <typename Derived>
struct VeeEvaluatorImpl<SE3TangentBase<Derived>> {
  template <typename TL, typename TR>
  static void run(TL& t, const TR& v) {
    t.coeffs() << v(0, 3), v(1, 3), v(2, 3), v(2, 1), v(0, 2), v(1, 0);
  }
};

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

#endif /* _MANIF_MANIF_SE3_BASE_H_ */