geometry.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef sophus_ros_conversions_GEOMETRY_HPP_
#define sophus_ros_conversions_GEOMETRY_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
#include <sophus/se3.hpp>
#include <tf/transform_listener.h>
/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace sophus_ros_conversions {

/*****************************************************************************
** Interfaces
*****************************************************************************/

void poseMsgToSophus(const geometry_msgs::Pose &p, Sophus::SE3f &s);

geometry_msgs::Pose sophusToPoseMsg(const Sophus::SE3f& s);

void vector3MsgToSophus(const geometry_msgs::Vector3 &v, Sophus::SE3f::Point &translation);

void transformMsgToSophus(const geometry_msgs::Transform &transform, Sophus::SE3f &se3);


template<typename T>
void stampedTransformToSophus( const tf::StampedTransform & transform, Sophus::SE3Group<T> & se3 )
{
        Eigen::Quaternion<T> q;
        Eigen::Matrix<T,3,1> t;
        
        q.x() = transform.getRotation().getX();
        q.y() = transform.getRotation().getY();
        q.z() = transform.getRotation().getZ();
        q.w() = transform.getRotation().getW();
        t.x() = transform.getOrigin().getX();
        t.y() = transform.getOrigin().getY();
        t.z() = transform.getOrigin().getZ();
        se3 = Sophus::SE3Group<T>(q,t);
}

Sophus::SE3f transformMsgToSophus(const geometry_msgs::Transform &transform);

geometry_msgs::Transform sophusToTransformMsg(const Sophus::SE3f& se3);


/*****************************************************************************
** Trailers
*****************************************************************************/

} // namespace sophus_ros_conversions

#endif /* sophus_ros_conversions_GEOMETRY_HPP_ */