8 #ifndef yocs_math_toolkit_SOPHUS_HELPERS_HPP_ 9 #define yocs_math_toolkit_SOPHUS_HELPERS_HPP_ 20 #include <Eigen/Geometry> 23 #if defined(ECL_CXX11_FOUND) 26 #include <sophus/se3.hpp> 27 #include <sophus/se2.hpp> 28 #include <sophus/so2.hpp> 29 #include <sophus/so3.hpp> 44 #if defined(ECL_CXX11_FOUND) 45 typedef std::shared_ptr<SE3f> SE3fPtr;
48 Sophus::SE3fPtr points2DToSophusTransform(
float from_x,
float from_y,
float to_x,
float to_y);
56 std::ostream & operator << ( std::ostream & out, const SE3Group<T> & se3 )
60 const Eigen::Matrix<T,3,1> & t = se3.translation();
61 const Eigen::Quaternion<T> & q = se3.unit_quaternion();
62 out << t.transpose() <<
" " << q.x() <<
" " << q.y() <<
" " << q.z() <<
" " << q.w();
67 std::ostream & operator << ( std::ostream & out, const SE2Group<T> & se2 )
69 typename SE2Group<T>::Tangent tanget_vector = SE2Group<T>::log( se2 );
70 out << tanget_vector.transpose();
80 Eigen::Vector3f
toPose2D(
const Sophus::SE3f& pose);
87 Sophus::SE3f
toPose3D(
const Eigen::Vector3f& pose);
95 Eigen::Matrix3f R = ( Eigen::AngleAxis<float> (
static_cast<float> ( theta ), Eigen::Vector3f::UnitZ ()) ).matrix();
96 Eigen::Quaternionf q(R);
107 void convert(
float theta, Eigen::Quaternionf & q )
109 q.vec() << 0, 0, sin(theta*0.5
f);
110 q.w() = cos(theta*0.5f);
129 Sophus::SE3f
operator()(
const Eigen::Vector3f& pose);
138 Eigen::Vector3f
operator()(
const Sophus::SE3f& pose);
Eigen::Vector3f toPose2D(const Sophus::SE3f &pose)
Convert a full Sophus pose into a 2 dimensional pose.
void convert(float theta, Eigen::Quaternionf &q)
Mathematical tools for linear algebra.
Eigen::Quaternionf operator()(float theta)
Sophus::SE3f toPose3D(const Eigen::Vector3f &pose)
Convert a 2 dimensional pose to a full Sophus pose in 3d.
PlanarRotation2Quaternion()
void f(int i) ecl_debug_throw_decl(StandardException)