tf2_eigen.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) Koji Terada
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  *
14  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24  * POSSIBILITY OF SUCH DAMAGE.
25  */
26 
29 #ifndef TF2_EIGEN_H
30 #define TF2_EIGEN_H
31 
32 #include <tf2/convert.h>
33 #include <Eigen/Geometry>
34 #include <geometry_msgs/QuaternionStamped.h>
35 #include <geometry_msgs/PointStamped.h>
36 #include <geometry_msgs/PoseStamped.h>
37 #include <geometry_msgs/Twist.h>
38 
39 
40 namespace tf2
41 {
42 
47  inline
48  Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform& t) {
49  return Eigen::Isometry3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z)
50  * Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z));
51 }
52 
57 inline
58 Eigen::Isometry3d transformToEigen(const geometry_msgs::TransformStamped& t) {
59  return transformToEigen(t.transform);
60 }
61 
66 inline
67 geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T)
68 {
69  geometry_msgs::TransformStamped t;
70  t.transform.translation.x = T.translation().x();
71  t.transform.translation.y = T.translation().y();
72  t.transform.translation.z = T.translation().z();
73 
74  Eigen::Quaterniond q(T.linear()); // assuming that upper 3x3 matrix is orthonormal
75  t.transform.rotation.x = q.x();
76  t.transform.rotation.y = q.y();
77  t.transform.rotation.z = q.z();
78  t.transform.rotation.w = q.w();
79 
80  return t;
81 }
82 
87 inline
88 geometry_msgs::TransformStamped eigenToTransform(const Eigen::Isometry3d& T)
89 {
90  geometry_msgs::TransformStamped t;
91  t.transform.translation.x = T.translation().x();
92  t.transform.translation.y = T.translation().y();
93  t.transform.translation.z = T.translation().z();
94 
95  Eigen::Quaterniond q(T.rotation());
96  t.transform.rotation.x = q.x();
97  t.transform.rotation.y = q.y();
98  t.transform.rotation.z = q.z();
99  t.transform.rotation.w = q.w();
100 
101  return t;
102 }
103 
113 template <>
114 inline
115 void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geometry_msgs::TransformStamped& transform)
116 {
117  t_out = Eigen::Vector3d(transformToEigen(transform) * t_in);
118 }
119 
125 inline
126 geometry_msgs::Point toMsg(const Eigen::Vector3d& in)
127 {
128  geometry_msgs::Point msg;
129  msg.x = in.x();
130  msg.y = in.y();
131  msg.z = in.z();
132  return msg;
133 }
134 
140 inline
141 void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out)
142 {
143  out.x() = msg.x;
144  out.y() = msg.y;
145  out.z() = msg.z;
146 }
147 
153 inline
154 geometry_msgs::Vector3& toMsg(const Eigen::Vector3d& in, geometry_msgs::Vector3& out)
155 {
156  out.x = in.x();
157  out.y = in.y();
158  out.z = in.z();
159  return out;
160 }
161 
167 inline
168 void fromMsg(const geometry_msgs::Vector3& msg, Eigen::Vector3d& out)
169 {
170  out.x() = msg.x;
171  out.y() = msg.y;
172  out.z() = msg.z;
173 }
174 
181 template <>
182 inline
185  const geometry_msgs::TransformStamped& transform) {
186  t_out = tf2::Stamped<Eigen::Vector3d>(transformToEigen(transform) * t_in,
187  transform.header.stamp,
188  transform.header.frame_id);
189 }
190 
196 inline
197 geometry_msgs::PointStamped toMsg(const tf2::Stamped<Eigen::Vector3d>& in)
198 {
199  geometry_msgs::PointStamped msg;
200  msg.header.stamp = in.stamp_;
201  msg.header.frame_id = in.frame_id_;
202  msg.point = toMsg(static_cast<const Eigen::Vector3d&>(in));
203  return msg;
204 }
205 
211 inline
212 void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<Eigen::Vector3d>& out) {
213  out.stamp_ = msg.header.stamp;
214  out.frame_id_ = msg.header.frame_id;
215  fromMsg(msg.point, static_cast<Eigen::Vector3d&>(out));
216 }
217 
227 template <>
228 inline
229 void doTransform(const Eigen::Affine3d& t_in,
230  Eigen::Affine3d& t_out,
231  const geometry_msgs::TransformStamped& transform) {
232  t_out = Eigen::Affine3d(transformToEigen(transform) * t_in);
233 }
234 
235 template <>
236 inline
237 void doTransform(const Eigen::Isometry3d& t_in,
238  Eigen::Isometry3d& t_out,
239  const geometry_msgs::TransformStamped& transform) {
240  t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in);
241 }
242 
248 inline
249 geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) {
250  geometry_msgs::Quaternion msg;
251  msg.w = in.w();
252  msg.x = in.x();
253  msg.y = in.y();
254  msg.z = in.z();
255  return msg;
256 }
257 
263 inline
264 void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) {
265  out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
266 }
267 
277 template<>
278 inline
279 void doTransform(const Eigen::Quaterniond& t_in,
280  Eigen::Quaterniond& t_out,
281  const geometry_msgs::TransformStamped& transform) {
282  Eigen::Quaterniond t;
283  fromMsg(transform.transform.rotation, t);
284  t_out = t.inverse() * t_in * t;
285 }
286 
292 inline
293 geometry_msgs::QuaternionStamped toMsg(const Stamped<Eigen::Quaterniond>& in) {
294  geometry_msgs::QuaternionStamped msg;
295  msg.header.stamp = in.stamp_;
296  msg.header.frame_id = in.frame_id_;
297  msg.quaternion = toMsg(static_cast<const Eigen::Quaterniond&>(in));
298  return msg;
299 }
300 
306 inline
307 void fromMsg(const geometry_msgs::QuaternionStamped& msg, Stamped<Eigen::Quaterniond>& out) {
308  out.frame_id_ = msg.header.frame_id;
309  out.stamp_ = msg.header.stamp;
310  fromMsg(msg.quaternion, static_cast<Eigen::Quaterniond&>(out));
311 }
312 
319 template <>
320 inline
323  const geometry_msgs::TransformStamped& transform) {
324  t_out.frame_id_ = transform.header.frame_id;
325  t_out.stamp_ = transform.header.stamp;
326  doTransform(static_cast<const Eigen::Quaterniond&>(t_in), static_cast<Eigen::Quaterniond&>(t_out), transform);
327 }
328 
334 inline
335 geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
336  geometry_msgs::Pose msg;
337  msg.position.x = in.translation().x();
338  msg.position.y = in.translation().y();
339  msg.position.z = in.translation().z();
340  Eigen::Quaterniond q(in.linear());
341  msg.orientation.x = q.x();
342  msg.orientation.y = q.y();
343  msg.orientation.z = q.z();
344  msg.orientation.w = q.w();
345  if (msg.orientation.w < 0) {
346  msg.orientation.x *= -1;
347  msg.orientation.y *= -1;
348  msg.orientation.z *= -1;
349  msg.orientation.w *= -1;
350  }
351  return msg;
352 }
353 
359 inline
360 geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) {
361  geometry_msgs::Pose msg;
362  msg.position.x = in.translation().x();
363  msg.position.y = in.translation().y();
364  msg.position.z = in.translation().z();
365  Eigen::Quaterniond q(in.linear());
366  msg.orientation.x = q.x();
367  msg.orientation.y = q.y();
368  msg.orientation.z = q.z();
369  msg.orientation.w = q.w();
370  if (msg.orientation.w < 0) {
371  msg.orientation.x *= -1;
372  msg.orientation.y *= -1;
373  msg.orientation.z *= -1;
374  msg.orientation.w *= -1;
375  }
376  return msg;
377 }
378 
384 inline
385 void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
386  out = Eigen::Affine3d(
387  Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
388  Eigen::Quaterniond(msg.orientation.w,
389  msg.orientation.x,
390  msg.orientation.y,
391  msg.orientation.z));
392 }
393 
399 inline
400 void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) {
401  out = Eigen::Isometry3d(
402  Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
403  Eigen::Quaterniond(msg.orientation.w,
404  msg.orientation.x,
405  msg.orientation.y,
406  msg.orientation.z));
407 }
408 
414 inline
415 geometry_msgs::Twist toMsg(const Eigen::Matrix<double,6,1>& in) {
416  geometry_msgs::Twist msg;
417  msg.linear.x = in[0];
418  msg.linear.y = in[1];
419  msg.linear.z = in[2];
420  msg.angular.x = in[3];
421  msg.angular.y = in[4];
422  msg.angular.z = in[5];
423  return msg;
424 }
425 
431 inline
432 void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix<double,6,1>& out) {
433  out[0] = msg.linear.x;
434  out[1] = msg.linear.y;
435  out[2] = msg.linear.z;
436  out[3] = msg.angular.x;
437  out[4] = msg.angular.y;
438  out[5] = msg.angular.z;
439 }
440 
450 template <>
451 inline
454  const geometry_msgs::TransformStamped& transform) {
455  t_out = tf2::Stamped<Eigen::Affine3d>(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id);
456 }
457 
467 template <>
468 inline
471  const geometry_msgs::TransformStamped& transform) {
472  t_out = tf2::Stamped<Eigen::Isometry3d>(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id);
473 }
474 
480 inline
481 geometry_msgs::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d>& in)
482 {
483  geometry_msgs::PoseStamped msg;
484  msg.header.stamp = in.stamp_;
485  msg.header.frame_id = in.frame_id_;
486  msg.pose = toMsg(static_cast<const Eigen::Affine3d&>(in));
487  return msg;
488 }
489 
490 inline
491 geometry_msgs::PoseStamped toMsg(const tf2::Stamped<Eigen::Isometry3d>& in)
492 {
493  geometry_msgs::PoseStamped msg;
494  msg.header.stamp = in.stamp_;
495  msg.header.frame_id = in.frame_id_;
496  msg.pose = toMsg(static_cast<const Eigen::Isometry3d&>(in));
497  return msg;
498 }
499 
505 inline
506 void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<Eigen::Affine3d>& out)
507 {
508  out.stamp_ = msg.header.stamp;
509  out.frame_id_ = msg.header.frame_id;
510  fromMsg(msg.pose, static_cast<Eigen::Affine3d&>(out));
511 }
512 
513 inline
514 void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<Eigen::Isometry3d>& out)
515 {
516  out.stamp_ = msg.header.stamp;
517  out.frame_id_ = msg.header.frame_id;
518  fromMsg(msg.pose, static_cast<Eigen::Isometry3d&>(out));
519 }
520 
521 } // namespace
522 
523 
524 namespace Eigen {
525 // This is needed to make the usage of the following conversion functions usable in tf2::convert().
526 // According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or
527 // in an associated namespace of one of its arguments. The stamped versions of this conversion
528 // functions work because they have tf2::Stamped as an argument which is the same namespace as
529 // which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is
530 // defined in tf2, so it take the following definitions in Eigen namespace to make them usable in
531 // tf2::convert().
532 
533 inline
534 geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
535  return tf2::toMsg(in);
536 }
537 
538 inline
539 geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) {
540  return tf2::toMsg(in);
541 }
542 
543 inline
544 void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) {
545  tf2::fromMsg(msg, out);
546 }
547 
548 inline
549 geometry_msgs::Point toMsg(const Eigen::Vector3d& in) {
550  return tf2::toMsg(in);
551 }
552 
553 inline
554 void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
555  tf2::fromMsg(msg, out);
556 }
557 
558 inline
559 void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) {
560  tf2::fromMsg(msg, out);
561 }
562 
563 inline
564 geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) {
565  return tf2::toMsg(in);
566 }
567 
568 inline
569 void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) {
570  tf2::fromMsg(msg, out);
571 }
572 
573 inline
574 geometry_msgs::Twist toMsg(const Eigen::Matrix<double,6,1>& in) {
575  return tf2::toMsg(in);
576 }
577 
578 inline
579 void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix<double,6,1>& out) {
580  tf2::fromMsg(msg, out);
581 }
582 
583 } // namespace
584 
585 #endif // TF2_EIGEN_H
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. ...
Definition: tf2_eigen.h:67
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
ros::Time stamp_
void fromMsg(const A &, B &b)
B toMsg(const A &a)
std::string frame_id_
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
Convert a timestamped transform to the equivalent Eigen data type.
Definition: tf2_eigen.h:48


tf2_eigen
Author(s): Koji Terada
autogenerated on Mon Jun 27 2022 02:43:11