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
00033
00034
00035
00036
00037
00038
00039
00040
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 }