tf2_eigen  master
Eigen binding for tf2.
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__TF2_EIGEN_H_
30 #define TF2_EIGEN__TF2_EIGEN_H_
31 
32 #include <tf2/convert.h>
33 #include <tf2_ros/buffer_interface.h>
34 #include <Eigen/Geometry>
35 #include <geometry_msgs/msg/point_stamped.hpp>
36 #include <geometry_msgs/msg/pose_stamped.hpp>
37 #include <geometry_msgs/msg/quaternion_stamped.hpp>
38 #include <geometry_msgs/msg/twist.hpp>
39 #include <geometry_msgs/msg/point.hpp>
40 #include <geometry_msgs/msg/quaternion.hpp>
41 #include <tf2_ros/buffer.h>
42 
43 
44 namespace tf2
45 {
46 
51 inline
52 Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform & t)
53 {
54  return Eigen::Isometry3d(
55  Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z) *
56  Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z));
57 }
58 
63 inline
64 Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::TransformStamped & t)
65 {
66  return transformToEigen(t.transform);
67 }
68 
73 inline
74 geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Affine3d & T)
75 {
76  geometry_msgs::msg::TransformStamped t;
77  t.transform.translation.x = T.translation().x();
78  t.transform.translation.y = T.translation().y();
79  t.transform.translation.z = T.translation().z();
80 
81  Eigen::Quaterniond q(T.linear()); // assuming that upper 3x3 matrix is orthonormal
82  t.transform.rotation.x = q.x();
83  t.transform.rotation.y = q.y();
84  t.transform.rotation.z = q.z();
85  t.transform.rotation.w = q.w();
86 
87  return t;
88 }
89 
94 inline
95 geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d & T)
96 {
97  geometry_msgs::msg::TransformStamped t;
98  t.transform.translation.x = T.translation().x();
99  t.transform.translation.y = T.translation().y();
100  t.transform.translation.z = T.translation().z();
101 
102  Eigen::Quaterniond q(T.rotation());
103  t.transform.rotation.x = q.x();
104  t.transform.rotation.y = q.y();
105  t.transform.rotation.z = q.z();
106  t.transform.rotation.w = q.w();
107 
108  return t;
109 }
110 
120 template<>
121 inline
122 void doTransform(
123  const Eigen::Vector3d & t_in,
124  Eigen::Vector3d & t_out,
125  const geometry_msgs::msg::TransformStamped & transform)
126 {
127  t_out = Eigen::Vector3d(transformToEigen(transform) * t_in);
128 }
129 
135 inline
136 geometry_msgs::msg::Point toMsg(const Eigen::Vector3d & in)
137 {
138  geometry_msgs::msg::Point msg;
139  msg.x = in.x();
140  msg.y = in.y();
141  msg.z = in.z();
142  return msg;
143 }
144 
150 inline
151 void fromMsg(const geometry_msgs::msg::Point & msg, Eigen::Vector3d & out)
152 {
153  out.x() = msg.x;
154  out.y() = msg.y;
155  out.z() = msg.z;
156 }
157 
163 inline
164 geometry_msgs::msg::Vector3 & toMsg(const Eigen::Vector3d & in, geometry_msgs::msg::Vector3 & out)
165 {
166  out.x = in.x();
167  out.y = in.y();
168  out.z = in.z();
169  return out;
170 }
171 
177 inline
178 void fromMsg(const geometry_msgs::msg::Vector3 & msg, Eigen::Vector3d & out)
179 {
180  out.x() = msg.x;
181  out.y() = msg.y;
182  out.z() = msg.z;
183 }
184 
191 template<>
192 inline
193 void doTransform(
194  const tf2::Stamped<Eigen::Vector3d> & t_in,
195  tf2::Stamped<Eigen::Vector3d> & t_out,
196  const geometry_msgs::msg::TransformStamped & transform)
197 {
198  t_out = tf2::Stamped<Eigen::Vector3d>(
199  transformToEigen(transform) * t_in,
200  tf2_ros::fromMsg(transform.header.stamp),
201  transform.header.frame_id);
202 }
203 
209 inline
210 geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<Eigen::Vector3d> & in)
211 {
212  geometry_msgs::msg::PointStamped msg;
213  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
214  msg.header.frame_id = in.frame_id_;
215  msg.point = toMsg(static_cast<const Eigen::Vector3d &>(in));
216  return msg;
217 }
218 
224 inline
225 void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped<Eigen::Vector3d> & out)
226 {
227  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
228  out.frame_id_ = msg.header.frame_id;
229  fromMsg(msg.point, static_cast<Eigen::Vector3d &>(out));
230 }
231 
241 template<>
242 inline
243 void doTransform(
244  const Eigen::Affine3d & t_in,
245  Eigen::Affine3d & t_out,
246  const geometry_msgs::msg::TransformStamped & transform)
247 {
248  t_out = Eigen::Affine3d(transformToEigen(transform) * t_in);
249 }
250 
251 template<>
252 inline
253 void doTransform(
254  const Eigen::Isometry3d & t_in,
255  Eigen::Isometry3d & t_out,
256  const geometry_msgs::msg::TransformStamped & transform)
257 {
258  t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in);
259 }
260 
266 inline
267 geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond & in)
268 {
269  geometry_msgs::msg::Quaternion msg;
270  msg.w = in.w();
271  msg.x = in.x();
272  msg.y = in.y();
273  msg.z = in.z();
274  return msg;
275 }
276 
282 inline
283 void fromMsg(const geometry_msgs::msg::Quaternion & msg, Eigen::Quaterniond & out)
284 {
285  out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
286 }
287 
297 template<>
298 inline
299 void doTransform(
300  const Eigen::Quaterniond & t_in,
301  Eigen::Quaterniond & t_out,
302  const geometry_msgs::msg::TransformStamped & transform)
303 {
304  Eigen::Quaterniond t;
305  fromMsg(transform.transform.rotation, t);
306  t_out = t * t_in;
307 }
308 
314 inline
315 geometry_msgs::msg::QuaternionStamped toMsg(const Stamped<Eigen::Quaterniond> & in)
316 {
317  geometry_msgs::msg::QuaternionStamped msg;
318  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
319  msg.header.frame_id = in.frame_id_;
320  msg.quaternion = toMsg(static_cast<const Eigen::Quaterniond &>(in));
321  return msg;
322 }
323 
329 inline
330 void fromMsg(const geometry_msgs::msg::QuaternionStamped & msg, Stamped<Eigen::Quaterniond> & out)
331 {
332  out.frame_id_ = msg.header.frame_id;
333  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
334  fromMsg(msg.quaternion, static_cast<Eigen::Quaterniond &>(out));
335 }
336 
343 template<>
344 inline
345 void doTransform(
346  const tf2::Stamped<Eigen::Quaterniond> & t_in,
347  tf2::Stamped<Eigen::Quaterniond> & t_out,
348  const geometry_msgs::msg::TransformStamped & transform)
349 {
350  t_out.frame_id_ = transform.header.frame_id;
351  t_out.stamp_ = tf2_ros::fromMsg(transform.header.stamp);
352  doTransform(
353  static_cast<const Eigen::Quaterniond &>(t_in),
354  static_cast<Eigen::Quaterniond &>(t_out), transform);
355 }
356 
362 inline
363 geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d & in)
364 {
365  geometry_msgs::msg::Pose msg;
366  msg.position.x = in.translation().x();
367  msg.position.y = in.translation().y();
368  msg.position.z = in.translation().z();
369  Eigen::Quaterniond q(in.linear());
370  msg.orientation.x = q.x();
371  msg.orientation.y = q.y();
372  msg.orientation.z = q.z();
373  msg.orientation.w = q.w();
374  return msg;
375 }
376 
382 inline
383 geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d & in)
384 {
385  geometry_msgs::msg::Pose msg;
386  msg.position.x = in.translation().x();
387  msg.position.y = in.translation().y();
388  msg.position.z = in.translation().z();
389  Eigen::Quaterniond q(in.linear());
390  msg.orientation.x = q.x();
391  msg.orientation.y = q.y();
392  msg.orientation.z = q.z();
393  msg.orientation.w = q.w();
394  return msg;
395 }
396 
404 inline
405 geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in)
406 {
407  geometry_msgs::msg::Vector3 msg;
408  msg.x = in(0);
409  msg.y = in(1);
410  msg.z = in(2);
411  return msg;
412 }
413 
419 inline
420 void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Affine3d & out)
421 {
422  out = Eigen::Affine3d(
423  Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
424  Eigen::Quaterniond(
425  msg.orientation.w,
426  msg.orientation.x,
427  msg.orientation.y,
428  msg.orientation.z));
429 }
430 
436 inline
437 void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Isometry3d & out)
438 {
439  out = Eigen::Isometry3d(
440  Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
441  Eigen::Quaterniond(
442  msg.orientation.w,
443  msg.orientation.x,
444  msg.orientation.y,
445  msg.orientation.z));
446 }
447 
453 inline
454 geometry_msgs::msg::Twist toMsg(const Eigen::Matrix<double, 6, 1> & in)
455 {
456  geometry_msgs::msg::Twist msg;
457  msg.linear.x = in[0];
458  msg.linear.y = in[1];
459  msg.linear.z = in[2];
460  msg.angular.x = in[3];
461  msg.angular.y = in[4];
462  msg.angular.z = in[5];
463  return msg;
464 }
465 
471 inline
472 void fromMsg(const geometry_msgs::msg::Twist & msg, Eigen::Matrix<double, 6, 1> & out)
473 {
474  out[0] = msg.linear.x;
475  out[1] = msg.linear.y;
476  out[2] = msg.linear.z;
477  out[3] = msg.angular.x;
478  out[4] = msg.angular.y;
479  out[5] = msg.angular.z;
480 }
481 
491 template<>
492 inline
493 void doTransform(
494  const tf2::Stamped<Eigen::Affine3d> & t_in,
495  tf2::Stamped<Eigen::Affine3d> & t_out,
496  const geometry_msgs::msg::TransformStamped & transform)
497 {
498  t_out = tf2::Stamped<Eigen::Affine3d>(
499  transformToEigen(transform) * t_in, tf2_ros::fromMsg(
500  transform.header.stamp), transform.header.frame_id);
501 }
502 
512 template<>
513 inline
514 void doTransform(
515  const tf2::Stamped<Eigen::Isometry3d> & t_in,
516  tf2::Stamped<Eigen::Isometry3d> & t_out,
517  const geometry_msgs::msg::TransformStamped & transform)
518 {
519  t_out = tf2::Stamped<Eigen::Isometry3d>(
520  transformToEigen(transform) * t_in, tf2_ros::fromMsg(
521  transform.header.stamp), transform.header.frame_id);
522 }
523 
529 inline
530 geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d> & in)
531 {
532  geometry_msgs::msg::PoseStamped msg;
533  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
534  msg.header.frame_id = in.frame_id_;
535  msg.pose = toMsg(static_cast<const Eigen::Affine3d &>(in));
536  return msg;
537 }
538 
539 inline
540 geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<Eigen::Isometry3d> & in)
541 {
542  geometry_msgs::msg::PoseStamped msg;
543  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
544  msg.header.frame_id = in.frame_id_;
545  msg.pose = toMsg(static_cast<const Eigen::Isometry3d &>(in));
546  return msg;
547 }
548 
554 inline
555 void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<Eigen::Affine3d> & out)
556 {
557  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
558  out.frame_id_ = msg.header.frame_id;
559  fromMsg(msg.pose, static_cast<Eigen::Affine3d &>(out));
560 }
561 
562 inline
563 void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<Eigen::Isometry3d> & out)
564 {
565  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
566  out.frame_id_ = msg.header.frame_id;
567  fromMsg(msg.pose, static_cast<Eigen::Isometry3d &>(out));
568 }
569 
570 } // namespace tf2
571 
572 
573 namespace Eigen
574 {
575 // This is needed to make the usage of the following conversion functions usable in tf2::convert().
576 // According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or
577 // in an associated namespace of one of its arguments. The stamped versions of this conversion
578 // functions work because they have tf2::Stamped as an argument which is the same namespace as
579 // which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is
580 // defined in tf2, so it take the following definitions in Eigen namespace to make them usable in
581 // tf2::convert().
582 
583 inline
584 geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d & in)
585 {
586  return tf2::toMsg(in);
587 }
588 
589 inline
590 geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d & in)
591 {
592  return tf2::toMsg(in);
593 }
594 
595 inline
596 void fromMsg(const geometry_msgs::msg::Point & msg, Eigen::Vector3d & out)
597 {
598  tf2::fromMsg(msg, out);
599 }
600 
601 inline
602 geometry_msgs::msg::Point toMsg(const Eigen::Vector3d & in)
603 {
604  return tf2::toMsg(in);
605 }
606 
607 inline
608 void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Affine3d & out)
609 {
610  tf2::fromMsg(msg, out);
611 }
612 
613 inline
614 void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Isometry3d & out)
615 {
616  tf2::fromMsg(msg, out);
617 }
618 
619 inline
620 geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond & in)
621 {
622  return tf2::toMsg(in);
623 }
624 
625 inline
626 geometry_msgs::msg::Twist toMsg(const Eigen::Matrix<double, 6, 1> & in)
627 {
628  return tf2::toMsg(in);
629 }
630 
631 inline
632 void fromMsg(const geometry_msgs::msg::Twist & msg, Eigen::Matrix<double, 6, 1> & out)
633 {
634  tf2::fromMsg(msg, out);
635 }
636 
637 } // namespace Eigen
638 
639 #endif // TF2_EIGEN__TF2_EIGEN_H_
Eigen
Definition: tf2_eigen.h:573
tf2::toMsg2
geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d &in)
Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3. This function is a specialization of...
Definition: tf2_eigen.h:405
tf2::Stamped::stamp_
TimePoint stamp_
tf2::fromMsg
void fromMsg(const A &, B &b)
buffer.h
tf2::Stamped
Eigen::fromMsg
void fromMsg(const geometry_msgs::msg::Point &msg, Eigen::Vector3d &out)
Definition: tf2_eigen.h:596
tf2::Stamped::frame_id_
std::string frame_id_
Eigen::toMsg
geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d &in)
Definition: tf2_eigen.h:584
tf2::eigenToTransform
geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type.
Definition: tf2_eigen.h:74
tf2_ros::fromMsg
tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time &time_msg)
tf2::transformToEigen
Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform &t)
Convert a timestamped transform to the equivalent Eigen data type.
Definition: tf2_eigen.h:52
tf2
tf2::toMsg
B toMsg(const A &a)
convert.h
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::msg::TransformStamped &transform)
tf2_ros::toMsg
builtin_interfaces::msg::Time toMsg(const tf2::TimePoint &t)
buffer_interface.h
Generated by   doxygen 1.8.17