helpers.hpp
Go to the documentation of this file.
00001 
00004 /*****************************************************************************
00005 ** Ifdefs
00006 *****************************************************************************/
00007 
00008 #ifndef yocs_math_toolkit_SOPHUS_HELPERS_HPP_
00009 #define yocs_math_toolkit_SOPHUS_HELPERS_HPP_
00010 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <ecl/config/macros.hpp>
00016 #include <ecl/converters.hpp>
00017 #include <ecl/exceptions/standard_exception.hpp>
00018 #include <ecl/linear_algebra.hpp>
00019 #include <Eigen/Core>
00020 #include <Eigen/Geometry>
00021 #include <iomanip>
00022 #include <iostream>
00023 #if defined(ECL_CXX11_FOUND)
00024     #include <memory>
00025 #endif
00026 #include <sophus/se3.hpp>
00027 #include <sophus/se2.hpp>
00028 #include <sophus/so2.hpp>
00029 #include <sophus/so3.hpp>
00030 #include <string>
00031 
00032 #include "formatters.hpp"
00033 
00034 /*****************************************************************************
00035 ** Namespace
00036 *****************************************************************************/
00037 
00038 namespace Sophus {
00039 
00040 /*****************************************************************************
00041  ** C++11 Api Only
00042  *****************************************************************************/
00043 
00044 #if defined(ECL_CXX11_FOUND)
00045     typedef std::shared_ptr<SE3f> SE3fPtr;
00046 
00048     Sophus::SE3fPtr points2DToSophusTransform(float from_x, float from_y, float to_x, float to_y);
00049 #endif
00050 
00051 /*****************************************************************************
00052 ** Interfaces
00053 *****************************************************************************/
00054 
00055 template<typename T>
00056 std::ostream & operator << ( std::ostream & out, const SE3Group<T> & se3 )
00057 {
00058 //  typename SE3Group<T>::Tangent tanget_vector = SE3Group<T>::log( se3 );
00059 //  out << tanget_vector.transpose();
00060   const Eigen::Matrix<T,3,1> & t = se3.translation();
00061   const Eigen::Quaternion<T> & q = se3.unit_quaternion();
00062   out << t.transpose() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w();
00063   return out;
00064 }
00065 
00066 template<typename T>
00067 std::ostream & operator << ( std::ostream & out, const SE2Group<T> & se2 )
00068 {
00069   typename SE2Group<T>::Tangent tanget_vector = SE2Group<T>::log( se2 );
00070   out << tanget_vector.transpose();
00071   return out;
00072 }
00073 
00080 Eigen::Vector3f toPose2D(const Sophus::SE3f& pose);
00087 Sophus::SE3f toPose3D(const Eigen::Vector3f& pose);
00088 
00089 class PlanarRotation2Quaternion
00090 {
00091 public:
00092   PlanarRotation2Quaternion(){}
00093   Eigen::Quaternionf operator() ( float theta )
00094   {
00095     Eigen::Matrix3f R = ( Eigen::AngleAxis<float> (static_cast<float> ( theta ), Eigen::Vector3f::UnitZ ()) ).matrix();
00096     Eigen::Quaternionf q(R);
00097     q.normalize();
00098     return q;
00099 //    Eigen::Quaternionf q;
00100 //    // in this case x and y part is zero since we assumbed that rotationa round z axis on the xy-plane
00101 //    q.vec() << 0, 0, sin(theta*0.5f);
00102 //    q.w() = cos(theta*0.5f);
00103 //    q.normalize();
00104 //    return q;
00105   }
00106 
00107   void convert( float theta, Eigen::Quaternionf & q )
00108   {
00109     q.vec() << 0, 0, sin(theta*0.5f);
00110     q.w() = cos(theta*0.5f);
00111   }
00112 };
00113 
00114 
00115 } // namespace Sophus
00116 
00117 /*****************************************************************************
00118 ** Converters
00119 *****************************************************************************/
00120 
00121 namespace ecl {
00122 
00126 template<>
00127 class Converter<Sophus::SE3f, Eigen::Vector3f> {
00128 public:
00129   Sophus::SE3f operator()(const Eigen::Vector3f& pose);
00130 };
00131 
00135 template<>
00136 class Converter<Eigen::Vector3f, Sophus::SE3f> {
00137 public:
00138   Eigen::Vector3f operator()(const Sophus::SE3f& pose);
00139 };
00140 
00141 } // namespace ecl
00142 
00143 
00144 #endif /* yocs_math_toolkit_SOPHUS_HELPERS_HPP_ */


ecl_linear_algebra
Author(s): Daniel Stonier
autogenerated on Mon Jul 3 2017 02:21:35