.. _program_listing_file__tmp_ws_src_ecl_core_ecl_linear_algebra_include_ecl_linear_algebra_sophus_helpers.hpp:

Program Listing for File helpers.hpp
====================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ecl_core_ecl_linear_algebra_include_ecl_linear_algebra_sophus_helpers.hpp>` (``/tmp/ws/src/ecl_core/ecl_linear_algebra/include/ecl/linear_algebra/sophus/helpers.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   /*****************************************************************************
   ** Ifdefs
   *****************************************************************************/
   
   #ifndef yocs_math_toolkit_SOPHUS_HELPERS_HPP_
   #define yocs_math_toolkit_SOPHUS_HELPERS_HPP_
   
   /*****************************************************************************
   ** Includes
   *****************************************************************************/
   
   #include <ecl/config/macros.hpp>
   #include <ecl/converters.hpp>
   #include <ecl/exceptions/standard_exception.hpp>
   #include <Eigen/Core>
   #include <Eigen/Geometry>
   #include <iomanip>
   #include <iostream>
   #if defined(ECL_CXX11_FOUND)
       #include <memory>
   #endif
   #include <sophus/se3.hpp>
   #include <sophus/se2.hpp>
   #include <sophus/so2.hpp>
   #include <sophus/so3.hpp>
   #include <string>
   
   #include "formatters.hpp"
   
   /*****************************************************************************
   ** Namespace
   *****************************************************************************/
   
   namespace Sophus {
   
   /*****************************************************************************
    ** C++11 Api Only
    *****************************************************************************/
   
   #if defined(ECL_CXX11_FOUND)
       typedef std::shared_ptr<SE3f> SE3fPtr;
   
       Sophus::SE3fPtr points2DToSophusTransform(float from_x, float from_y, float to_x, float to_y);
   #endif
   
   /*****************************************************************************
   ** Interfaces
   *****************************************************************************/
   
   template<typename T>
   std::ostream & operator << ( std::ostream & out, const SE3<T> & se3 )
   {
   //  typename SE3<T>::Tangent tanget_vector = SE3<T>::log( se3 );
   //  out << tanget_vector.transpose();
     const Eigen::Matrix<T,3,1> & t = se3.translation();
     const Eigen::Quaternion<T> & q = se3.unit_quaternion();
     out << t.transpose() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w();
     return out;
   }
   
   template<typename T>
   std::ostream & operator << ( std::ostream & out, const SE2<T> & se2 )
   {
     typename SE2<T>::Tangent tanget_vector = SE2<T>::log( se2 );
     out << tanget_vector.transpose();
     return out;
   }
   
   Eigen::Vector3f toPose2D(const Sophus::SE3f& pose);
   Sophus::SE3f toPose3D(const Eigen::Vector3f& pose);
   
   class PlanarRotation2Quaternion
   {
   public:
     PlanarRotation2Quaternion(){}
     Eigen::Quaternionf operator() ( float theta )
     {
       Eigen::Matrix3f R = ( Eigen::AngleAxis<float> (static_cast<float> ( theta ), Eigen::Vector3f::UnitZ ()) ).matrix();
       Eigen::Quaternionf q(R);
       q.normalize();
       return q;
   //    Eigen::Quaternionf q;
   //    // in this case x and y part is zero since we assumbed that rotationa round z axis on the xy-plane
   //    q.vec() << 0, 0, sin(theta*0.5f);
   //    q.w() = cos(theta*0.5f);
   //    q.normalize();
   //    return q;
     }
   
     void convert( float theta, Eigen::Quaternionf & q )
     {
       q.vec() << 0, 0, sin(theta*0.5f);
       q.w() = cos(theta*0.5f);
     }
   };
   
   
   } // namespace Sophus
   
   /*****************************************************************************
   ** Converters
   *****************************************************************************/
   
   namespace ecl {
   
   template<>
   class Converter<Sophus::SE3f, Eigen::Vector3f> {
   public:
     Sophus::SE3f operator()(const Eigen::Vector3f& pose);
   };
   
   template<>
   class Converter<Eigen::Vector3f, Sophus::SE3f> {
   public:
     Eigen::Vector3f operator()(const Sophus::SE3f& pose);
   };
   
   } // namespace ecl
   
   
   #endif /* yocs_math_toolkit_SOPHUS_HELPERS_HPP_ */