helpers.hpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Ifdefs
6 *****************************************************************************/
7 
8 #ifndef yocs_math_toolkit_SOPHUS_HELPERS_HPP_
9 #define yocs_math_toolkit_SOPHUS_HELPERS_HPP_
10 
11 /*****************************************************************************
12 ** Includes
13 *****************************************************************************/
14 
15 #include <ecl/config/macros.hpp>
16 #include <ecl/converters.hpp>
18 #include <Eigen/Core>
19 #include <Eigen/Geometry>
20 #include <iomanip>
21 #include <iostream>
22 #if defined(ECL_CXX11_FOUND)
23  #include <memory>
24 #endif
25 #include <sophus/se3.hpp>
26 #include <sophus/se2.hpp>
27 #include <sophus/so2.hpp>
28 #include <sophus/so3.hpp>
29 #include <string>
30 
31 #include "formatters.hpp"
32 
33 /*****************************************************************************
34 ** Namespace
35 *****************************************************************************/
36 
37 namespace Sophus {
38 
39 /*****************************************************************************
40  ** C++11 Api Only
41  *****************************************************************************/
42 
43 #if defined(ECL_CXX11_FOUND)
44  typedef std::shared_ptr<SE3f> SE3fPtr;
45 
47  Sophus::SE3fPtr points2DToSophusTransform(float from_x, float from_y, float to_x, float to_y);
48 #endif
49 
50 /*****************************************************************************
51 ** Interfaces
52 *****************************************************************************/
53 
54 template<typename T>
55 std::ostream & operator << ( std::ostream & out, const SE3<T> & se3 )
56 {
57 // typename SE3<T>::Tangent tanget_vector = SE3<T>::log( se3 );
58 // out << tanget_vector.transpose();
59  const Eigen::Matrix<T,3,1> & t = se3.translation();
60  const Eigen::Quaternion<T> & q = se3.unit_quaternion();
61  out << t.transpose() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w();
62  return out;
63 }
64 
65 template<typename T>
66 std::ostream & operator << ( std::ostream & out, const SE2<T> & se2 )
67 {
68  typename SE2<T>::Tangent tanget_vector = SE2<T>::log( se2 );
69  out << tanget_vector.transpose();
70  return out;
71 }
72 
79 Eigen::Vector3f toPose2D(const Sophus::SE3f& pose);
86 Sophus::SE3f toPose3D(const Eigen::Vector3f& pose);
87 
89 {
90 public:
92  Eigen::Quaternionf operator() ( float theta )
93  {
94  Eigen::Matrix3f R = ( Eigen::AngleAxis<float> (static_cast<float> ( theta ), Eigen::Vector3f::UnitZ ()) ).matrix();
95  Eigen::Quaternionf q(R);
96  q.normalize();
97  return q;
98 // Eigen::Quaternionf q;
99 // // in this case x and y part is zero since we assumbed that rotationa round z axis on the xy-plane
100 // q.vec() << 0, 0, sin(theta*0.5f);
101 // q.w() = cos(theta*0.5f);
102 // q.normalize();
103 // return q;
104  }
105 
106  void convert( float theta, Eigen::Quaternionf & q )
107  {
108  q.vec() << 0, 0, sin(theta*0.5f);
109  q.w() = cos(theta*0.5f);
110  }
111 };
112 
113 
114 } // namespace Sophus
115 
116 /*****************************************************************************
117 ** Converters
118 *****************************************************************************/
119 
120 namespace ecl {
121 
125 template<>
126 class Converter<Sophus::SE3f, Eigen::Vector3f> {
127 public:
128  Sophus::SE3f operator()(const Eigen::Vector3f& pose);
129 };
130 
134 template<>
135 class Converter<Eigen::Vector3f, Sophus::SE3f> {
136 public:
137  Eigen::Vector3f operator()(const Sophus::SE3f& pose);
138 };
139 
140 } // namespace ecl
141 
142 
143 #endif /* yocs_math_toolkit_SOPHUS_HELPERS_HPP_ */
Eigen::Vector3f toPose2D(const Sophus::SE3f &pose)
Convert a full Sophus pose into a 2 dimensional pose.
void convert(float theta, Eigen::Quaternionf &q)
Definition: helpers.hpp:106
Eigen::Quaternionf operator()(float theta)
Definition: helpers.hpp:92
Sophus::SE3f toPose3D(const Eigen::Vector3f &pose)
Convert a 2 dimensional pose to a full Sophus pose in 3d.
void f(int i)


ecl_linear_algebra
Author(s): Daniel Stonier
autogenerated on Mon Feb 28 2022 22:18:38