tf_eigen.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009-2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: Adam Leeper, Stuart Glaser
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 poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e)
00056   {
00057     transformTFToEigen(t, e);
00058   }
00059 
00060   void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
00061   {
00062     transformEigenToTF(e, t);
00063   }
00064 
00065   void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t)
00066   {
00067     transformEigenToTF(e, t);
00068   }
00069 
00070   void quaternionTFToEigen(const tf::Quaternion& t, Eigen::Quaterniond& e)
00071   {
00072     e = Eigen::Quaterniond(t[3],t[0],t[1],t[2]);
00073   }
00074   
00075   void quaternionEigenToTF(const Eigen::Quaterniond& e, tf::Quaternion& t)
00076   {
00077     t[0] = e.x();
00078     t[1] = e.y();
00079     t[2] = e.z();
00080     t[3] = e.w();
00081   }
00082 
00083   namespace {
00084     template<typename Transform>
00085     void transformTFToEigenImpl(const tf::Transform &t, Transform & e)
00086     {
00087       for(int i=0; i<3; i++)
00088       {
00089         e.matrix()(i,3) = t.getOrigin()[i];
00090         for(int j=0; j<3; j++)
00091         {
00092           e.matrix()(i,j) = t.getBasis()[i][j];
00093         }
00094       }
00095       // Fill in identity in last row
00096       for (int col = 0 ; col < 3; col ++)
00097         e.matrix()(3, col) = 0;
00098       e.matrix()(3,3) = 1;
00099     }
00100 
00101     template<typename T>
00102     void transformEigenToTFImpl(const T &e, tf::Transform &t)
00103     {
00104       t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
00105       t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1), e.matrix()(0,2),
00106                                e.matrix()(1,0), e.matrix()(1,1), e.matrix()(1,2),
00107                                e.matrix()(2,0), e.matrix()(2,1), e.matrix()(2,2)));
00108     }
00109   }
00110 
00111   void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
00112   {
00113     transformTFToEigenImpl(t, e);
00114   };
00115 
00116   void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
00117   {
00118     transformTFToEigenImpl(t, e);
00119   };
00120 
00121   void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
00122   {
00123     transformEigenToTFImpl(e, t);
00124   }
00125 
00126   void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t)
00127   {
00128     transformEigenToTFImpl(e, t);
00129   }
00130 
00131   void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3d& e)
00132   {
00133     e(0) = t[0];
00134     e(1) = t[1];
00135     e(2) = t[2];
00136   }
00137 
00138   void vectorEigenToTF(const Eigen::Vector3d& e, tf::Vector3& t)
00139   {
00140     t[0] = e(0);
00141     t[1] = e(1);
00142     t[2] = e(2);
00143   }
00144 
00145 } // namespace tf


tf_conversions
Author(s): Tully Foote
autogenerated on Fri Aug 11 2017 02:22:10