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>
17 #include <ecl/exceptions/standard_exception.hpp>
18 #include <ecl/linear_algebra.hpp>
19 #include <Eigen/Core>
20 #include <Eigen/Geometry>
21 #include <iomanip>
22 #include <iostream>
23 #if defined(ECL_CXX11_FOUND)
24  #include <memory>
25 #endif
26 #include <sophus/se3.hpp>
27 #include <sophus/se2.hpp>
28 #include <sophus/so2.hpp>
29 #include <sophus/so3.hpp>
30 #include <string>
31 
32 #include "formatters.hpp"
33 
34 /*****************************************************************************
35 ** Namespace
36 *****************************************************************************/
37 
38 namespace Sophus {
39 
40 /*****************************************************************************
41  ** C++11 Api Only
42  *****************************************************************************/
43 
44 #if defined(ECL_CXX11_FOUND)
45  typedef std::shared_ptr<SE3f> SE3fPtr;
46 
48  Sophus::SE3fPtr points2DToSophusTransform(float from_x, float from_y, float to_x, float to_y);
49 #endif
50 
51 /*****************************************************************************
52 ** Interfaces
53 *****************************************************************************/
54 
55 template<typename T>
56 std::ostream & operator << ( std::ostream & out, const SE3Group<T> & se3 )
57 {
58 // typename SE3Group<T>::Tangent tanget_vector = SE3Group<T>::log( se3 );
59 // out << tanget_vector.transpose();
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();
63  return out;
64 }
65 
66 template<typename T>
67 std::ostream & operator << ( std::ostream & out, const SE2Group<T> & se2 )
68 {
69  typename SE2Group<T>::Tangent tanget_vector = SE2Group<T>::log( se2 );
70  out << tanget_vector.transpose();
71  return out;
72 }
73 
80 Eigen::Vector3f toPose2D(const Sophus::SE3f& pose);
87 Sophus::SE3f toPose3D(const Eigen::Vector3f& pose);
88 
90 {
91 public:
93  Eigen::Quaternionf operator() ( float theta )
94  {
95  Eigen::Matrix3f R = ( Eigen::AngleAxis<float> (static_cast<float> ( theta ), Eigen::Vector3f::UnitZ ()) ).matrix();
96  Eigen::Quaternionf q(R);
97  q.normalize();
98  return q;
99 // Eigen::Quaternionf q;
100 // // in this case x and y part is zero since we assumbed that rotationa round z axis on the xy-plane
101 // q.vec() << 0, 0, sin(theta*0.5f);
102 // q.w() = cos(theta*0.5f);
103 // q.normalize();
104 // return q;
105  }
106 
107  void convert( float theta, Eigen::Quaternionf & q )
108  {
109  q.vec() << 0, 0, sin(theta*0.5f);
110  q.w() = cos(theta*0.5f);
111  }
112 };
113 
114 
115 } // namespace Sophus
116 
117 /*****************************************************************************
118 ** Converters
119 *****************************************************************************/
120 
121 namespace ecl {
122 
126 template<>
127 class Converter<Sophus::SE3f, Eigen::Vector3f> {
128 public:
129  Sophus::SE3f operator()(const Eigen::Vector3f& pose);
130 };
131 
135 template<>
136 class Converter<Eigen::Vector3f, Sophus::SE3f> {
137 public:
138  Eigen::Vector3f operator()(const Sophus::SE3f& pose);
139 };
140 
141 } // namespace ecl
142 
143 
144 #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)
f
Eigen::Quaternionf operator()(float theta)
Sophus::SE3f toPose3D(const Eigen::Vector3f &pose)
Convert a 2 dimensional pose to a full Sophus pose in 3d.


xbot_node
Author(s): Roc, wangpeng@droid.ac.cn
autogenerated on Sat Oct 10 2020 03:28:13