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 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     // Fill in identity in last row
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 } // namespace tf


tf_conversions
Author(s): Tully Foote
autogenerated on Mon Oct 6 2014 00:18:24