param_utils/get_param_specializations/eigen.hpp
Go to the documentation of this file.
1 #pragma once
2 
11 #include <string>
12 #include <vector>
13 
14 #include <Eigen/Core>
15 #include <Eigen/Geometry>
16 
17 namespace cras
18 {
19 
20 namespace
21 {
22  template <typename Scalar>
23  void checkSize(const ::std::vector<Scalar>& v, const size_t i, const ::std::string& type)
24  {
25  if (v.size() != i)
26  throw ::std::runtime_error("Cannot construct " + type + " from " + ::std::to_string(v.size()) + " elements " +
27  "(expected " + ::std::to_string(i) + ").");
28  }
29 }
30 
31 DEFINE_CONVERTING_GET_PARAM(::Eigen::Vector3d, ::std::vector<double>, "",
32  [](const ::std::vector<double>& v){ checkSize(v, 3, "vector"); return ::Eigen::Vector3d(v.data()); })
33 
34 DEFINE_CONVERTING_GET_PARAM(::Eigen::Vector3f, ::std::vector<float>, "",
35  [](const ::std::vector<float>& v){ checkSize(v, 3, "vector"); return ::Eigen::Vector3f(v.data()); })
36 
37 DEFINE_CONVERTING_GET_PARAM(::Eigen::Vector4d, ::std::vector<double>, "",
38  [](const ::std::vector<double>& v){ checkSize(v, 4, "vector"); return ::Eigen::Vector4d(v.data()); })
39 
40 DEFINE_CONVERTING_GET_PARAM(::Eigen::Vector4f, ::std::vector<float>, "",
41  [](const ::std::vector<float>& v){ checkSize(v, 4, "vector"); return ::Eigen::Vector4f(v.data()); })
42 
43 DEFINE_CONVERTING_GET_PARAM(::Eigen::Matrix3d, ::std::vector<double>, "",
44  ([](const ::std::vector<double>& v){ checkSize(v, 9, "3x3 matrix");
45  return ::Eigen::Matrix<double, 3, 3, ::Eigen::RowMajor>(v.data()); }))
46 
47 DEFINE_CONVERTING_GET_PARAM(::Eigen::Matrix3f, ::std::vector<float>, "",
48  ([](const ::std::vector<float>& v){ checkSize(v, 9, "3x3 matrix");
49  return ::Eigen::Matrix<float, 3, 3, ::Eigen::RowMajor>(v.data()); }))
50 
51 DEFINE_CONVERTING_GET_PARAM(::Eigen::Matrix4d, ::std::vector<double>, "",
52  ([](const ::std::vector<double>& v){ checkSize(v, 16, "4x4 matrix");
53  return ::Eigen::Matrix<double, 4, 4, ::Eigen::RowMajor>(v.data()); }))
54 
55 DEFINE_CONVERTING_GET_PARAM(::Eigen::Matrix4f, ::std::vector<float>, "",
56  ([](const ::std::vector<float>& v){ checkSize(v, 16, "4x4 matrix");
57  return ::Eigen::Matrix<float, 4, 4, ::Eigen::RowMajor>(v.data()); }))
58 
59 DEFINE_CONVERTING_GET_PARAM(::Eigen::Quaterniond, ::std::vector<double>, "",
60  [](const ::std::vector<double>& v)
61  {
62  switch (v.size())
63  {
64  case 3:
65  return ::Eigen::Quaterniond(
66  ::Eigen::AngleAxisd(v[0], ::Eigen::Vector3d::UnitX()) *
67  ::Eigen::AngleAxisd(v[1], ::Eigen::Vector3d::UnitY()) *
68  ::Eigen::AngleAxisd(v[2], ::Eigen::Vector3d::UnitZ()));
69  case 4:
70  return ::Eigen::Quaterniond(v[3], v[0], v[1], v[2]); // w first!
71  default:
72  throw ::std::runtime_error("Cannot construct quaternion from " + ::std::to_string(v.size()) + " elements.");
73  }
74  })
75 
76 DEFINE_CONVERTING_GET_PARAM(::Eigen::Quaternionf, ::std::vector<float>, "",
77  [](const ::std::vector<float>& v)
78  {
79  switch (v.size())
80  {
81  case 3:
82  return ::Eigen::Quaternionf(
83  ::Eigen::AngleAxisf(v[0], ::Eigen::Vector3f::UnitX()) *
84  ::Eigen::AngleAxisf(v[1], ::Eigen::Vector3f::UnitY()) *
85  ::Eigen::AngleAxisf(v[2], ::Eigen::Vector3f::UnitZ()));
86  case 4:
87  return ::Eigen::Quaternionf(v[3], v[0], v[1], v[2]); // w first!
88  default:
89  throw ::std::runtime_error("Cannot construct quaternion from " + ::std::to_string(v.size()) + " elements.");
90  }
91  })
92 
93 DEFINE_CONVERTING_GET_PARAM(::Eigen::Isometry3d, ::std::vector<double>, "",
94  [](const ::std::vector<double>& v)
95  {
96  auto result = ::Eigen::Isometry3d::Identity();
97  switch (v.size())
98  {
99  case 6:
100  result.translationExt() = ::Eigen::Vector3d(v[0], v[1], v[2]);
101  result.linearExt() = ::Eigen::Quaterniond(
102  ::Eigen::AngleAxisd(v[3], ::Eigen::Vector3d::UnitX()) *
103  ::Eigen::AngleAxisd(v[4], ::Eigen::Vector3d::UnitY()) *
104  ::Eigen::AngleAxisd(v[5], ::Eigen::Vector3d::UnitZ())).toRotationMatrix();
105  return result;
106  case 7:
107  result.translationExt() = ::Eigen::Vector3d(v[0], v[1], v[2]);
108  result.linearExt() = ::Eigen::Quaterniond(v[6], v[3], v[4], v[5]).toRotationMatrix(); // w first!
109  return result;
110  case 16:
111  result = (::Eigen::Matrix<double, 4, 4, ::Eigen::RowMajor>(v.data()));
112  return result;
113  default:
114  throw ::std::runtime_error("Cannot construct Eigen isometry from " + ::std::to_string(v.size()) + " elements.");
115  }
116  })
117 
118 DEFINE_CONVERTING_GET_PARAM(::Eigen::Isometry3f, ::std::vector<float>, "",
119  [](const ::std::vector<float>& v)
120  {
121  auto result = ::Eigen::Isometry3f::Identity();
122  switch (v.size())
123  {
124  case 6:
125  result.translationExt() = ::Eigen::Vector3f(v[0], v[1], v[2]);
126  result.linearExt() = ::Eigen::Quaternionf(
127  ::Eigen::AngleAxisf(v[3], ::Eigen::Vector3f::UnitX()) *
128  ::Eigen::AngleAxisf(v[4], ::Eigen::Vector3f::UnitY()) *
129  ::Eigen::AngleAxisf(v[5], ::Eigen::Vector3f::UnitZ())).toRotationMatrix();
130  return result;
131  case 7:
132  result.translationExt() = ::Eigen::Vector3f(v[0], v[1], v[2]);
133  result.linearExt() = ::Eigen::Quaternionf(v[6], v[3], v[4], v[5]).toRotationMatrix(); // w first!
134  return result;
135  case 16:
136  result = (::Eigen::Matrix<float, 4, 4, ::Eigen::RowMajor>(v.data()));
137  return result;
138  default:
139  throw ::std::runtime_error("Cannot construct Eigen isometry from " + ::std::to_string(v.size()) + " elements.");
140  }
141  })
142 
143 
144 }
Eigen
cras
Definition: any.hpp:15
cras::DEFINE_CONVERTING_GET_PARAM
DEFINE_CONVERTING_GET_PARAM(::Eigen::Vector3d, ::std::vector< double >, "", [](const ::std::vector< double > &v){ checkSize(v, 3, "vector");return ::Eigen::Vector3d(v.data());}) DEFINE_CONVERTING_GET_PARAM(
Definition: param_utils/get_param_specializations/eigen.hpp:31
std
cras::to_string
inline ::std::string to_string(const ::Eigen::Matrix< Scalar, Rows, Cols, Options, MaxRows, MaxCols > &value)
Definition: string_utils/eigen.hpp:21


cras_cpp_common
Author(s): Martin Pecka
autogenerated on Sun Jan 14 2024 03:48:14