Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "tf_conversions/tf_eigen.h"
00033
00034 namespace tf {
00035
00036 void matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e)
00037 {
00038 for(int i=0; i<3; i++)
00039 for(int j=0; j<3; j++)
00040 e(i,j) = t[i][j];
00041 }
00042
00043 void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t)
00044 {
00045 for(int i=0; i<3; i++)
00046 for(int j=0; j<3; j++)
00047 t[i][j] = e(i,j);
00048 }
00049
00050 void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e)
00051 {
00052 transformTFToEigen(t, e);
00053 }
00054
00055 void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
00056 {
00057 transformEigenToTF(e, t);
00058 }
00059
00060 void quaternionTFToEigen(const tf::Quaternion& t, Eigen::Quaterniond& e)
00061 {
00062 e = Eigen::Quaterniond(t[3],t[0],t[1],t[2]);
00063 }
00064
00065 void quaternionEigenToTF(const Eigen::Quaterniond& e, tf::Quaternion& t)
00066 {
00067 t[0] = e.x();
00068 t[1] = e.y();
00069 t[2] = e.z();
00070 t[3] = e.w();
00071 }
00072
00073 void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
00074 {
00075 for(int i=0; i<3; i++)
00076 {
00077 e.matrix()(i,3) = t.getOrigin()[i];
00078 for(int j=0; j<3; j++)
00079 {
00080 e.matrix()(i,j) = t.getBasis()[i][j];
00081 }
00082 }
00083
00084 for (int col = 0 ; col < 3; col ++)
00085 e.matrix()(3, col) = 0;
00086 e.matrix()(3,3) = 1;
00087 };
00088
00089 void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
00090 {
00091 t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
00092 t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1),e.matrix()(0,2),
00093 e.matrix()(1,0), e.matrix()(1,1),e.matrix()(1,2),
00094 e.matrix()(2,0), e.matrix()(2,1),e.matrix()(2,2)));
00095 }
00096
00097 void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3d& e)
00098 {
00099 e(0) = t[0];
00100 e(1) = t[1];
00101 e(2) = t[2];
00102 }
00103
00104 void vectorEigenToTF(const Eigen::Vector3d& e, tf::Vector3& t)
00105 {
00106 t[0] = e(0);
00107 t[1] = e(1);
00108 t[2] = e(2);
00109 }
00110
00111 }