tf_eigen.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009-2012, Willow Garage, Inc.
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  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Adam Leeper, Stuart Glaser
31 
33 
34 namespace tf {
35 
36  void matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e)
37  {
38  for(int i=0; i<3; i++)
39  for(int j=0; j<3; j++)
40  e(i,j) = t[i][j];
41  }
42 
43  void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
44  {
45  for(int i=0; i<3; i++)
46  for(int j=0; j<3; j++)
47  t[i][j] = e(i,j);
48  }
49 
50  void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e)
51  {
52  transformTFToEigen(t, e);
53  }
54 
55  void poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e)
56  {
57  transformTFToEigen(t, e);
58  }
59 
60  void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
61  {
62  transformEigenToTF(e, t);
63  }
64 
65  void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t)
66  {
67  transformEigenToTF(e, t);
68  }
69 
70  void quaternionTFToEigen(const tf::Quaternion& t, Eigen::Quaterniond& e)
71  {
72  e = Eigen::Quaterniond(t[3],t[0],t[1],t[2]);
73  }
74 
75  void quaternionEigenToTF(const Eigen::Quaterniond& e, tf::Quaternion& t)
76  {
77  t[0] = e.x();
78  t[1] = e.y();
79  t[2] = e.z();
80  t[3] = e.w();
81  }
82 
83  namespace {
84  template<typename Transform>
85  void transformTFToEigenImpl(const tf::Transform &t, Transform & e)
86  {
87  for(int i=0; i<3; i++)
88  {
89  e.matrix()(i,3) = t.getOrigin()[i];
90  for(int j=0; j<3; j++)
91  {
92  e.matrix()(i,j) = t.getBasis()[i][j];
93  }
94  }
95  // Fill in identity in last row
96  for (int col = 0 ; col < 3; col ++)
97  e.matrix()(3, col) = 0;
98  e.matrix()(3,3) = 1;
99  }
100 
101  template<typename T>
102  void transformEigenToTFImpl(const T &e, tf::Transform &t)
103  {
104  t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
105  t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1), e.matrix()(0,2),
106  e.matrix()(1,0), e.matrix()(1,1), e.matrix()(1,2),
107  e.matrix()(2,0), e.matrix()(2,1), e.matrix()(2,2)));
108  }
109  }
110 
111  void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
112  {
113  transformTFToEigenImpl(t, e);
114  };
115 
116  void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
117  {
118  transformTFToEigenImpl(t, e);
119  };
120 
121  void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
122  {
123  transformEigenToTFImpl(e, t);
124  }
125 
126  void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t)
127  {
128  transformEigenToTFImpl(e, t);
129  }
130 
131  void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3d& e)
132  {
133  e(0) = t[0];
134  e(1) = t[1];
135  e(2) = t[2];
136  }
137 
138  void vectorEigenToTF(const Eigen::Vector3d& e, tf::Vector3& t)
139  {
140  t[0] = e(0);
141  t[1] = e(1);
142  t[2] = e(2);
143  }
144 
145 } // namespace tf
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
Converts an Eigen Affine3d into a tf Transform.
Definition: tf_eigen.cpp:121
void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
Converts an Eigen Affine3d into a tf Transform.
Definition: tf_eigen.cpp:60
void matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e)
Converts a tf Matrix3x3 into an Eigen Quaternion.
Definition: tf_eigen.cpp:36
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
Converts a tf Transform into an Eigen Affine3d.
Definition: tf_eigen.cpp:111
void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
Converts an Eigen Quaternion into a tf Matrix3x3.
Definition: tf_eigen.cpp:43
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
void vectorEigenToTF(const Eigen::Vector3d &e, tf::Vector3 &t)
Converts an Eigen Vector3d into a tf Vector3.
Definition: tf_eigen.cpp:138
void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e)
Converts a tf Pose into an Eigen Affine3d.
Definition: tf_eigen.cpp:50
TFSIMD_FORCE_INLINE void setBasis(const Matrix3x3 &basis)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e)
Converts a tf Quaternion into an Eigen Quaternion.
Definition: tf_eigen.cpp:70
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e)
Converts a tf Vector3 into an Eigen Vector3d.
Definition: tf_eigen.cpp:131
void quaternionEigenToTF(const Eigen::Quaterniond &e, tf::Quaternion &t)
Converts an Eigen Quaternion into a tf Quaternion.
Definition: tf_eigen.cpp:75
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)


tf_conversions
Author(s): Tully Foote
autogenerated on Mon Feb 28 2022 22:26:30