homogeneous_transformation.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_HOMOGENEOUS_TRANSFORMATION_H
#define OPEN_WALKER_CORE_HOMOGENEOUS_TRANSFORMATION_H

#include <ow_core/rotation3_ref.h>
#include <ow_core/linear_position_ref.h>

namespace ow_core
{
template <typename _Scalar>
class HomogeneousTransformation : 
  public Eigen::Transform<_Scalar, 3, Eigen::Affine>
{
public:
  typedef _Scalar Scalar;
  typedef Eigen::Transform<_Scalar, 3, Eigen::Affine> Base;
  typedef typename Eigen::Transform<_Scalar, 3, Eigen::Affine>::MatrixType Matrix;

  static const HomogeneousTransformation<Scalar>& Default()
  {
    static const HomogeneousTransformation v = Matrix::Identity().matrix();
    return v;
  }

public:
  explicit HomogeneousTransformation() :
    Base(Default())
  {
  }

  HomogeneousTransformation(const Base& other) :
    Base(other)
  {
  }

  template <typename OtherDerived>
  HomogeneousTransformation(const Eigen::EigenBase<OtherDerived>& other) :
    Base(other.derived())
  {
  }

  explicit HomogeneousTransformation(const typename Base::TranslationType& other) :
    Base(other)
  {
  }

  template <typename OtherDerived>
  explicit HomogeneousTransformation(const Eigen::RotationBase<OtherDerived, 3>& other) :
    Base(other)
  {
  }

  explicit HomogeneousTransformation(const tf::Transform& other)
  {
    operator=(other);
  }

  explicit HomogeneousTransformation(const geometry_msgs::Pose& other)
  {
    operator=(other);
  }

  using Base::operator=;

  void operator=(const tf::Transform& other)
  {
    position() = other.getOrigin();
    orientation() = other.getRotation();
  }

  void operator=(const geometry_msgs::Pose& other)
  {
    position() = other.position;
    orientation() = other.orientation;
  }

  operator tf::Transform() const
  {
    return tf::Transform(orientation().toQuaternionTF(), position());
  }

  operator geometry_msgs::Pose() const
  {
    geometry_msgs::Pose X;
    X.position = position();
    X.orientation = orientation();
    return X;
  }

  tf::Transform toTransformTf()
  {
    return static_cast<tf::Transform>(*this);
  }

  geometry_msgs::Pose toPoseMsg()
  {
    return static_cast<geometry_msgs::Pose>(*this);
  }

  LinearPositionRef<Matrix> position()
  {
    return LinearPositionRef<Matrix>(this->matrix(),0,3);
  }

  LinearPositionRef<const Matrix> position() const
  {
    return LinearPositionRef<const Matrix>(this->matrix(),0,3);
  }

  Rotation3Ref<Matrix> orientation()
  {
    return Rotation3Ref<Matrix>(this->matrix());
  }

  Rotation3Ref<const Matrix> orientation() const
  {
    return Rotation3Ref<const Matrix>(this->matrix());
  }

  std::string toString() const
  {
    std::ostringstream out;
    out << this->matrix();
    return out.str();
  }
};

}  // namespace ow_core

#endif  // OPEN_WALKER_CORE_HOMOGENEOUS_TRANSFORMATION_H