conversions.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_CONVERSIONS_H
#define OPEN_WALKER_CORE_CONVERSIONS_H

#include <Eigen/Dense>

#include <tf/LinearMath/Matrix3x3.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf_conversions/tf_eigen.h>

#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>

#include <ow_core/quaternion_ref.h>

namespace ow_core
{
void matrixTFToEigenQuaterniond(const tf::Matrix3x3& t, Eigen::Quaterniond& e);

template <typename _Derived>
void matrixTFToEigenQuaternion(const tf::Matrix3x3& t, Eigen::QuaternionBase<_Derived>& e)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaterniond ed;
  matrixTFToEigenQuaterniond(t, ed);
  e = ed.cast<Scalar>();
}

template <typename _Derived>
void matrixTFToEigenMatrix(const tf::Matrix3x3& t, Eigen::MatrixBase<_Derived>& e)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  typedef Eigen::Matrix<double, _Derived::ColsAtCompileTime, _Derived::RowsAtCompileTime> Base;
  Base ed;
  tf::matrixTFToEigen(t, ed);
  e = ed.template cast<Scalar>();
}

void quaterniondEigenToMatrixTF(const Eigen::Quaterniond& e, tf::Matrix3x3& t);

template <typename _Derived>
void quaternionEigenToMatrixTF(const Eigen::QuaternionBase<_Derived>& e, tf::Matrix3x3& t)
{
  Eigen::Quaterniond ed;
  ed = e.cast();
  quaterniondEigenToMatrixTF(ed, t);
}

template <typename _Derived>
void matrixEigenToMatrixTF(const Eigen::MatrixBase<_Derived>& e, tf::Matrix3x3& t)
{
  typedef Eigen::Matrix<double, _Derived::ColsAtCompileTime, _Derived::RowsAtCompileTime> Base;
  Base ed;
  ed = e.template cast<double>();
  tf::matrixEigenToTF(ed, t);
}

template <typename _Derived>
void quaternionTFToEigenQuaternion(const tf::Quaternion& t, Eigen::QuaternionBase<_Derived>& e)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaterniond ed;
  tf::quaternionTFToEigen(t, ed);
  e = ed.cast<Scalar>();
}

template <typename _Derived>
void quaternionTFToEigenMatrix(const tf::Quaternion& t, Eigen::MatrixBase<_Derived>& e)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaternion<Scalar> eq;
  quaternionTFToEigenQuaternion(t, eq);
  e = eq.toRotationMatrix();
}

template <typename _Derived>
void quaternionEigenToQuaternionTF(const Eigen::QuaternionBase<_Derived>& e, tf::Quaternion& t)
{
  //    typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaterniond ed;
  ed = e.template cast<double>();
  tf::quaternionEigenToTF(ed, t);
}

template <typename _Derived>
void matrixEigenToQuaternionTF(const Eigen::MatrixBase<_Derived>& e, tf::Quaternion& t)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaternion<Scalar> eq(e);
  quaternionEigenToQuaternionTF(eq, t);
}

void quaternionMsgToEigenQuaterniond(const geometry_msgs::Quaternion& t, Eigen::Quaterniond& e);

template <typename _Derived>
void quaternionMsgToEigenQuaternion(const geometry_msgs::Quaternion& t, Eigen::QuaternionBase<_Derived>& e)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaterniond ed;
  quaternionMsgToEigenQuaterniond(t, ed);
  e = ed.cast<Scalar>();
}

template <typename _Derived>
void quaternionMsgToEigenMatrix(const geometry_msgs::Quaternion& t, Eigen::MatrixBase<_Derived>& e)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaternion<Scalar> eq;
  quaternionMsgToEigenQuaternion(t, eq);
  e = eq.toRotationMatrix();
}

void quaterniondEigenToQuaternionMsg(const Eigen::Quaterniond& e, geometry_msgs::Quaternion& t);

template <typename _Derived>
void quaternionEigenToQuaternionMsg(const Eigen::QuaternionBase<_Derived>& e, geometry_msgs::Quaternion& t)
{
  //    typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaterniond ed;
  ed = e.template cast<double>();
  quaterniondEigenToQuaternionMsg(ed, t);
}

template <typename _Derived>
void matrixEigenToQuaternionMsg(const Eigen::MatrixBase<_Derived>& e, geometry_msgs::Quaternion& t)
{
  typedef typename Eigen::internal::traits<_Derived>::Scalar Scalar;
  Eigen::Quaternion<Scalar> eq(e);
  quaternionEigenToQuaternionMsg(eq, t);
}

void vector3dEigenToPointMsg(const Eigen::Vector3d& e, geometry_msgs::Point& t);

template <typename _Derived>
void vector3EigenToPointMsg(const Eigen::MatrixBase<_Derived>& e, geometry_msgs::Point& t)
{
  typedef Eigen::Matrix<double, _Derived::ColsAtCompileTime, _Derived::RowsAtCompileTime> Base;
  Base ed = e.template cast<double>();
  vector3dEigenToPointMsg(ed, t);
}

void vector3dEigenToVector3Msg(const Eigen::Vector3d& e, geometry_msgs::Vector3& t);

template <typename _Derived>
void vector3EigenToVector3Msg(const Eigen::MatrixBase<_Derived>& e, geometry_msgs::Vector3& t)
{
  typedef Eigen::Matrix<double, _Derived::ColsAtCompileTime, _Derived::RowsAtCompileTime> Base;
  Base ed = e.template cast<double>();
  vector3dEigenToVector3Msg(ed, t);
}

void pointMsgToEigenVector3d(const geometry_msgs::Point& t, Eigen::Vector3d& e);

template <typename _Derived>
void pointMsgToEigenVector3(const geometry_msgs::Point& t, Eigen::MatrixBase<_Derived>& e)
{
  typedef typename Eigen::MatrixBase<_Derived>::Scalar Scalar;
  Eigen::Vector3d ed;
  pointMsgToEigenVector3d(t, ed);
  e = ed.cast<Scalar>();
}

void vector3MsgToEigenVector3d(const geometry_msgs::Vector3& t, Eigen::Vector3d& e);

template <typename _Derived>
void vector3MsgToEigenVector3(const geometry_msgs::Vector3& t, Eigen::MatrixBase<_Derived>& e)
{
  typedef typename Eigen::MatrixBase<_Derived>::Scalar Scalar;
  Eigen::Vector3d ed;
  vector3MsgToEigenVector3d(t, ed);
  e = ed.cast<Scalar>();
}

template <typename _Derived>
void vector3EigenToVector3TF(const Eigen::MatrixBase<_Derived>& e, tf::Vector3& t)
{
  Eigen::Vector3d ed;
  ed = e.template cast<double>();
  tf::vectorEigenToTF(ed, t);
}

template <typename _Derived>
void vector3TFToVector3Eigen(const tf::Vector3& t, Eigen::MatrixBase<_Derived>& e)
{
  typedef typename Eigen::MatrixBase<_Derived>::Scalar Scalar;
  Eigen::Vector3d ed;
  tf::vectorTFToEigen(t, ed);
  e = ed.cast<Scalar>();
}

}  // namespace ow_core

#endif  // OPEN_WALKER_CORE_CONVERSIONS_H