14 #include <Eigen/Geometry>
20 template <
class t_Visitor,
typename t_Scalar,
int t_flags>
23 Eigen::Matrix<t_Scalar, 3, 1, t_flags> &left,
24 const geometry_msgs::Vector3 &right,
25 const typename t_Visitor::Parameters & )
27 ARILES2_TRACE_FUNCTION;
33 template <
class t_Visitor,
typename t_Scalar,
int t_flags>
36 Eigen::Matrix<t_Scalar, Eigen::Dynamic, 1, t_flags> &left,
37 const geometry_msgs::Vector3 &right,
38 const typename t_Visitor::Parameters & )
40 ARILES2_TRACE_FUNCTION;
48 template <
class t_Visitor,
typename t_Scalar,
int t_options>
51 Eigen::Quaternion<t_Scalar, t_options> &left,
52 const geometry_msgs::Quaternion &right,
53 const typename t_Visitor::Parameters & )
55 ARILES2_TRACE_FUNCTION;
63 template <
class t_Visitor,
typename t_Scalar,
int t_mode,
int t_options>
66 Eigen::Transform<t_Scalar, 3, t_mode, t_options> &left,
67 const geometry_msgs::Transform &right,
68 const typename t_Visitor::Parameters ¶m)
70 ARILES2_TRACE_FUNCTION;
72 Eigen::Quaternion<t_Scalar> quaternion;
73 Eigen::Matrix<t_Scalar, 3, 1> translation;
79 left.linear() = quaternion.toRotationMatrix();
80 left.translation() = translation;
87 template <
class t_Visitor,
typename t_Scalar,
int t_flags>
90 const Eigen::Matrix<t_Scalar, 3, 1, t_flags> &left,
91 geometry_msgs::Vector3 &right,
92 const typename t_Visitor::Parameters & )
94 ARILES2_TRACE_FUNCTION;
100 template <
class t_Visitor,
typename t_XprType,
bool t_InnerPanel>
103 const Eigen::Block<t_XprType, 3, 1, t_InnerPanel> &left,
104 geometry_msgs::Vector3 &right,
105 const typename t_Visitor::Parameters & )
107 ARILES2_TRACE_FUNCTION;
113 template <
class t_Visitor,
typename t_Scalar,
int t_flags>
116 const Eigen::Matrix<t_Scalar, Eigen::Dynamic, 1, t_flags> &left,
117 geometry_msgs::Vector3 &right,
118 const typename t_Visitor::Parameters & )
120 ARILES2_TRACE_FUNCTION;
121 ARILES2_ASSERT(3 == left.size(),
"Wrong entry size.");
128 template <
class t_Visitor,
typename t_Scalar,
int t_options>
131 const Eigen::Quaternion<t_Scalar, t_options> &left,
132 geometry_msgs::Quaternion &right,
133 const typename t_Visitor::Parameters & )
135 ARILES2_TRACE_FUNCTION;
143 template <
class t_Visitor,
typename t_Scalar,
int t_mode,
int t_options>
146 const Eigen::Transform<t_Scalar, 3, t_mode, t_options> &left,
147 geometry_msgs::Transform &right,
148 const typename t_Visitor::Parameters ¶m)
150 ARILES2_TRACE_FUNCTION;
151 apply_copyto(visitor, Eigen::Quaternion<t_Scalar>(left.linear()), right.rotation, param);
152 apply_copyto(visitor, left.translation(), right.translation, param);