PTransform.h

Go to the documentation of this file.

/*
 * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include "EigenTypedef.h"
#include "fwd.h"
 
namespace sva
{
 
template<typename T>
Eigen::Matrix3<T> RotX(T theta);
 
template<typename T>
Eigen::Matrix3<T> RotY(T theta);
 
template<typename T>
Eigen::Matrix3<T> RotZ(T theta);
 
template<typename T>
Eigen::Vector3<T> rotationError(const Eigen::Matrix3<T> & E_a_b, const Eigen::Matrix3<T> & E_a_c);
 
template<typename T>
Eigen::Vector3<T> rotationVelocity(const Eigen::Matrix3<T> & E_a_b);
 
template<typename T>
MotionVec<T> transformError(const PTransform<T> & X_a_b, const PTransform<T> & X_a_c);
 
template<typename T>
MotionVec<T> transformVelocity(const PTransform<T> & X_a_b);
 
template<typename T>
class PTransform
{
  typedef Eigen::Vector3<T> vector3_t;
  typedef Eigen::Matrix3<T> matrix3_t;
  typedef Eigen::Matrix6<T> matrix6_t;
  typedef Eigen::Quaternion<T> quaternion_t;
 
public:
  static PTransform<T> Identity()
  {
    return PTransform<T>(matrix3_t::Identity(), vector3_t::Zero());
  }
 
public:
  // Constructors
  PTransform() : E_(), r_() {}
 
  template<typename T2>
  PTransform(const PTransform<T2> & pt) : E_(pt.rotation().template cast<T>()), r_(pt.translation().template cast<T>())
  {
  }
 
  PTransform(const matrix3_t & rot, const vector3_t & trans) : E_(rot), r_(trans) {}
 
  PTransform(const quaternion_t & rot, const vector3_t & trans) : E_(rot.matrix()), r_(trans) {}
 
  PTransform(const quaternion_t & rot) : E_(rot.matrix()), r_(vector3_t::Zero()) {}
 
  PTransform(const matrix3_t & rot) : E_(rot), r_(vector3_t::Zero()) {}
 
  PTransform(const vector3_t & trans) : E_(matrix3_t::Identity()), r_(trans) {}
 
  // Accessor
  const matrix3_t & rotation() const
  {
    return E_;
  }
 
  matrix3_t & rotation()
  {
    return E_;
  }
 
  const vector3_t & translation() const
  {
    return r_;
  }
 
  vector3_t & translation()
  {
    return r_;
  }
 
  matrix6_t matrix() const
  {
    matrix6_t m;
    m << E_, matrix3_t::Zero(), -E_ * vector3ToCrossMatrix(r_), E_;
    return m;
  }
 
  matrix6_t dualMatrix() const
  {
    matrix6_t m;
    m << E_, -E_ * vector3ToCrossMatrix(r_), matrix3_t::Zero(), E_;
    return m;
  }
 
  template<typename T2>
  PTransform<T2> cast() const
  {
    return PTransform<T2>(E_.template cast<T2>(), r_.template cast<T2>());
  }
 
  // Operators
  PTransform<T> operator*(const PTransform<T> & pt) const
  {
    return PTransform<T>(E_ * pt.E_, pt.r_ + pt.E_.transpose() * r_);
  }
 
  MotionVec<T> operator*(const MotionVec<T> & mv) const;
 
  Eigen::Vector3<T> angularMul(const MotionVec<T> & mv) const;
  Eigen::Vector3<T> linearMul(const MotionVec<T> & mv) const;
  template<typename Derived>
  void mul(const Eigen::MatrixBase<Derived> & mv, Eigen::MatrixBase<Derived> & result) const;
 
  MotionVec<T> invMul(const MotionVec<T> & mv) const;
  Eigen::Vector3<T> angularInvMul(const MotionVec<T> & mv) const;
  Eigen::Vector3<T> linearInvMul(const MotionVec<T> & mv) const;
  template<typename Derived>
  void invMul(const Eigen::MatrixBase<Derived> & mv, Eigen::MatrixBase<Derived> & result) const;
 
  ForceVec<T> dualMul(const ForceVec<T> & fv) const;
  Eigen::Vector3<T> coupleDualMul(const ForceVec<T> & fv) const;
  Eigen::Vector3<T> forceDualMul(const ForceVec<T> & fv) const;
  template<typename Derived>
  void dualMul(const Eigen::MatrixBase<Derived> & fv, Eigen::MatrixBase<Derived> & result) const;
 
  ForceVec<T> transMul(const ForceVec<T> & fv) const;
  Eigen::Vector3<T> coupleTransMul(const ForceVec<T> & fv) const;
  Eigen::Vector3<T> forceTransMul(const ForceVec<T> & fv) const;
  template<typename Derived>
  void transMul(const Eigen::MatrixBase<Derived> & fv, Eigen::MatrixBase<Derived> & result) const;
 
  RBInertia<T> dualMul(const RBInertia<T> & rbI) const;
  RBInertia<T> transMul(const RBInertia<T> & rbI) const;
 
  ABInertia<T> dualMul(const ABInertia<T> & rbI) const;
  ABInertia<T> transMul(const ABInertia<T> & rbI) const;
 
  PTransform<T> inv() const
  {
    return PTransform<T>(E_.transpose(), -E_ * r_);
  }
 
  bool operator==(const PTransform<T> & pt) const
  {
    return E_ == pt.E_ && r_ == pt.r_;
  }
 
  bool operator!=(const PTransform<T> & pt) const
  {
    return E_ != pt.E_ || r_ != pt.r_;
  }
 
private:
  matrix3_t E_;
  vector3_t r_;
};
 
template<typename T>
inline Eigen::Matrix3<T> RotX(T theta)
{
  T s = std::sin(theta), c = std::cos(theta);
  return (Eigen::Matrix3<T>() << 1., 0., 0., 0., c, s, 0., -s, c).finished();
}
 
template<typename T>
inline Eigen::Matrix3<T> RotY(T theta)
{
  T s = std::sin(theta), c = std::cos(theta);
  return (Eigen::Matrix3<T>() << c, 0., -s, 0., 1., 0., s, 0., c).finished();
}
 
template<typename T>
inline Eigen::Matrix3<T> RotZ(T theta)
{
  T s = std::sin(theta), c = std::cos(theta);
  return (Eigen::Matrix3<T>() << c, s, 0., -s, c, 0., 0., 0., 1.).finished();
}
 
template<typename T>
inline Eigen::Vector3<T> rotationError(const Eigen::Matrix3<T> & E_a_b, const Eigen::Matrix3<T> & E_a_c)
{
  Eigen::Matrix3<T> E_b_c = E_a_c * E_a_b.transpose();
  return Eigen::Vector3<T>(E_a_b.transpose() * rotationVelocity(E_b_c));
}
 
template<typename T>
inline Eigen::Vector3<T> rotationVelocity(const Eigen::Matrix3<T> & E_a_b)
{
  constexpr T eps = std::numeric_limits<T>::epsilon();
  constexpr T sqeps = details::sqrt(eps);
  constexpr T sqsqeps = details::sqrt(sqeps);
  constexpr T pi = 3.1415926535897932;
 
  T trace = E_a_b(0, 0) + E_a_b(1, 1) + E_a_b(2, 2);
  T acosV = (trace - 1.) * 0.5;
  T theta = std::acos(std::min(std::max(acosV, -1.), 1.));
 
  Eigen::Vector3<T> w(-E_a_b(2, 1) + E_a_b(1, 2), -E_a_b(0, 2) + E_a_b(2, 0), -E_a_b(1, 0) + E_a_b(0, 1));
 
  if(1 + trace < sqsqeps) // in case of angle close to pi
  {
    // adapted from https://vision.in.tum.de/_media/members/demmeln/nurlanov2021so3log.pdf, sec2.2
    Eigen::Vector3<T> s = (2 * E_a_b.diagonal() + Eigen::Vector3<T>::Constant(1 - trace)) / (3 - trace);
    Eigen::Vector3<T> tn2 = theta * s.cwiseSqrt();
    // If theta is really close to pi the sign of n is derived from (13)
    // We choose the first non-zero coefficient to be positive arbitrarly
    if(theta > pi - 1e-7)
    {
      if(tn2(0) > 0)
      {
        if(E_a_b(0, 1) + E_a_b(1, 0) < 0)
        {
          tn2(1) = -tn2(1);
        }
        if(E_a_b(0, 2) + E_a_b(2, 0) < 0)
        {
          tn2(2) = -tn2(2);
        }
      }
      else if(tn2(1) > 0) // only if tn2(0) == 0
      {
        if(E_a_b(1, 2) + E_a_b(2, 1) < 0)
        {
          tn2(2) = -tn2(2);
        }
      }
      return tn2;
    }
    // The sign is derived from (9)
    return (w.array() >= 0).select(tn2, -tn2);
  }
  else
  {
    w *= sinc_inv(theta) * 0.5;
  }
 
  return w;
}
 
template<typename T>
inline MotionVec<T> transformError(const PTransform<T> & X_a_b, const PTransform<T> & X_a_c)
{
  PTransform<T> X_b_c = X_a_c * X_a_b.inv();
  return PTransform<T>(Eigen::Matrix3<T>(X_a_b.rotation().transpose())) * transformVelocity(X_b_c);
}
 
template<typename T>
inline MotionVec<T> transformVelocity(const PTransform<T> & X_a_b)
{
  return MotionVec<T>(rotationVelocity(X_a_b.rotation()), X_a_b.translation());
}
 
// interpolate between transformations, t must be between 0 and 1
template<typename T>
PTransform<T> interpolate(const PTransform<T> & from, const PTransform<T> & to, double t)
{
  Eigen::Quaternion<T> qfrom(from.rotation());
  Eigen::Quaternion<T> qto(to.rotation());
  PTransform<T> result(qfrom.slerp(t, qto), (from.translation() * (1. - t) + to.translation() * t));
  return result;
}
 
template<typename T>
inline std::ostream & operator<<(std::ostream & out, const PTransform<T> & pt)
{
  out << pt.matrix();
  return out;
}
 
} // namespace sva