51 void poseMsgToEigenImpl(
const geometry_msgs::Pose &m, T &e)
53 e = Eigen::Translation3d(m.position.x,
56 Eigen::Quaterniond(m.orientation.w,
63 void poseEigenToMsgImpl(
const T &e, geometry_msgs::Pose &m)
65 m.position.x = e.translation()[0];
66 m.position.y = e.translation()[1];
67 m.position.z = e.translation()[2];
68 Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
69 m.orientation.x = q.x();
70 m.orientation.y = q.y();
71 m.orientation.z = q.z();
72 m.orientation.w = q.w();
73 if (m.orientation.w < 0) {
74 m.orientation.x *= -1;
75 m.orientation.y *= -1;
76 m.orientation.z *= -1;
77 m.orientation.w *= -1;
82 void transformMsgToEigenImpl(
const geometry_msgs::Transform &m, T &e)
84 e = Eigen::Translation3d(m.translation.x,
87 Eigen::Quaterniond(m.rotation.w,
94 void transformEigenToMsgImpl(
const T &e, geometry_msgs::Transform &m)
96 m.translation.x = e.translation()[0];
97 m.translation.y = e.translation()[1];
98 m.translation.z = e.translation()[2];
99 Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
100 m.rotation.x = q.x();
101 m.rotation.y = q.y();
102 m.rotation.z = q.z();
103 m.rotation.w = q.w();
104 if (m.rotation.w < 0) {
115 poseMsgToEigenImpl(m, e);
120 poseMsgToEigenImpl(m, e);
125 poseEigenToMsgImpl(e, m);
130 poseEigenToMsgImpl(e, m);
135 e = Eigen::Quaterniond(m.w, m.x, m.y, m.z);
148 transformMsgToEigenImpl(m, e);
153 transformMsgToEigenImpl(m, e);
158 transformEigenToMsgImpl(e, m);
163 transformEigenToMsgImpl(e, m);