3dpose.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: file.cpp 3496 2011-11-22 15:14:32Z fnovotny $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2011 by INRIA. All rights reserved.
00007  * 
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional 
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  * 
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  * 
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  * Contact visp@irisa.fr if any conditions of this licensing are
00034  * not clear to you.
00035  *
00036  * Description:
00037  * conversions between ROS and ViSP structures representing a 3D pose
00038  *
00039  * Authors:
00040  * Filip Novotny
00041  * 
00042  *
00043  *****************************************************************************/
00044 
00050 #include <cmath>
00051 
00052 #include <visp/vpConfig.h>
00053 
00054 #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1)
00055 #  include <visp/vpQuaternionVector.h>
00056 #else
00057 #  include <visp/vpRotationMatrix.h>
00058 #  include "visp_bridge/vpQuaternionVector.h"
00059 #endif
00060 #include <visp/vpTranslationVector.h>
00061 
00062 #include "visp_bridge/3dpose.h"
00063 
00064 namespace visp_bridge{
00065 
00066 #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1)
00067   vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Pose& pose){
00068     vpHomogeneousMatrix mat;
00069     vpTranslationVector vec(pose.position.x,pose.position.y,pose.position.z);
00070     vpQuaternionVector q(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
00071     mat.buildFrom(vec,q);
00072 
00073     return mat;
00074   }
00075 
00076   vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Transform& trans){
00077      vpHomogeneousMatrix mat;
00078      vpTranslationVector vec(trans.translation.x,trans.translation.y,trans.translation.z);
00079      vpQuaternionVector q(trans.rotation.x,trans.rotation.y,trans.rotation.z,trans.rotation.w);
00080      mat.buildFrom(vec,q);
00081 
00082      return mat;
00083    }
00084 
00085   geometry_msgs::Transform toGeometryMsgsTransform(vpHomogeneousMatrix& mat){
00086     geometry_msgs::Transform trans;
00087     vpQuaternionVector q;
00088     mat.extract(q);
00089     trans.rotation.x = q.x();
00090     trans.rotation.y = q.y();
00091     trans.rotation.z = q.z();
00092     trans.rotation.w = q.w();
00093 
00094 
00095     trans.translation.x = mat[0][3];
00096     trans.translation.y = mat[1][3];
00097     trans.translation.z = mat[2][3];
00098 
00099     return trans;
00100   }
00101 
00102   geometry_msgs::Pose toGeometryMsgsPose(vpHomogeneousMatrix& mat){
00103     geometry_msgs::Pose pose;
00104 
00105         vpThetaUVector tu(mat);
00106         vpColVector u;
00107         double theta;
00108         tu.extract(theta, u);
00109 
00110         theta *= 0.5;
00111 
00112         double sinTheta_2 = sin(theta);
00113 
00114         pose.orientation.x = u[0] * sinTheta_2;
00115         pose.orientation.y = u[1] * sinTheta_2;
00116         pose.orientation.z = u[2] * sinTheta_2;
00117         pose.orientation.w = cos(theta);
00118 
00119         pose.position.x = mat[0][3];
00120         pose.position.y = mat[1][3];
00121         pose.position.z = mat[2][3];
00122 
00123         return pose;
00124   }
00125 
00126 
00127 #else
00128   vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Transform& trans){
00129       vpHomogeneousMatrix mat;
00130       vpTranslationVector vec(trans.translation.x,trans.translation.y,trans.translation.z);
00131       vpRotationMatrix rmat;
00132 
00133       double a = trans.rotation.x;
00134       double b = trans.rotation.y;
00135       double c = trans.rotation.z;
00136       double d = trans.rotation.w;
00137       rmat[0][0] = a*a+b*b-c*c-d*d;
00138       rmat[0][1] = 2*b*c-2*a*d;
00139       rmat[0][2] = 2*a*c+2*b*d;
00140 
00141       rmat[1][0] = 2*a*d+2*b*c;
00142       rmat[1][1] = a*a-b*b+c*c-d*d;
00143       rmat[1][2] = 2*c*d-2*a*b;
00144 
00145       rmat[2][0] = 2*b*d-2*a*c;
00146       rmat[2][1] = 2*a*b+2*c*d;
00147       rmat[2][2] = a*a-b*b-c*c+d*d;
00148 
00149       mat.buildFrom(vec,rmat);
00150 
00151       return mat;
00152     }
00153 
00154     geometry_msgs::Transform toGeometryMsgsTransform(vpHomogeneousMatrix& mat){
00155       geometry_msgs::Transform trans;
00156       vpRotationMatrix rmat;
00157       mat.extract(rmat);
00158       vpQuaternionVector q(rmat);
00159 
00160       trans.rotation.x = q.x();
00161       trans.rotation.y = q.y();
00162       trans.rotation.z = q.z();
00163       trans.rotation.w = q.w();
00164 
00165 
00166       trans.translation.x = mat[0][3];
00167       trans.translation.y = mat[1][3];
00168       trans.translation.z = mat[2][3];
00169 
00170       return trans;
00171     }
00172 
00173     vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Pose& pose){
00174       vpHomogeneousMatrix mat;
00175       vpTranslationVector vec(pose.position.x,pose.position.y,pose.position.z);
00176       vpRotationMatrix rmat;
00177 
00178       double a = pose.orientation.x;
00179       double b = pose.orientation.y;
00180       double c = pose.orientation.z;
00181       double d = pose.orientation.w;
00182       rmat[0][0] = a*a+b*b-c*c-d*d;
00183       rmat[0][1] = 2*b*c-2*a*d;
00184       rmat[0][2] = 2*a*c+2*b*d;
00185 
00186       rmat[1][0] = 2*a*d+2*b*c;
00187       rmat[1][1] = a*a-b*b+c*c-d*d;
00188       rmat[1][2] = 2*c*d-2*a*b;
00189 
00190       rmat[2][0] = 2*b*d-2*a*c;
00191       rmat[2][1] = 2*a*b+2*c*d;
00192       rmat[2][2] = a*a-b*b-c*c+d*d;
00193 
00194       mat.buildFrom(vec,rmat);
00195 
00196       return mat;
00197     }
00198 #endif
00199 }


visp_bridge
Author(s): Filip Novotny
autogenerated on Fri Aug 28 2015 13:36:28