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
00049 #include "visp/vpConfig.h"
00050 #include "3dpose.h"
00051 #include <cmath>
00052
00053 #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1)
00054 #include <visp/vpQuaternionVector.h>
00055 #else
00056 #include "../vpQuaternionVector.h"
00057 #include <visp/vpRotationMatrix.h>
00058 #endif
00059 #include <visp/vpTranslationVector.h>
00060
00061
00062 namespace visp_bridge{
00063
00064 #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1)
00065 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Pose& pose){
00066 vpHomogeneousMatrix mat;
00067 vpTranslationVector vec(pose.position.x,pose.position.y,pose.position.z);
00068 vpQuaternionVector q(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
00069 mat.buildFrom(vec,q);
00070
00071 return mat;
00072 }
00073
00074 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Transform& trans){
00075 vpHomogeneousMatrix mat;
00076 vpTranslationVector vec(trans.translation.x,trans.translation.y,trans.translation.z);
00077 vpQuaternionVector q(trans.rotation.x,trans.rotation.y,trans.rotation.z,trans.rotation.w);
00078 mat.buildFrom(vec,q);
00079
00080 return mat;
00081 }
00082
00083 geometry_msgs::Transform toGeometryMsgsTransform(vpHomogeneousMatrix& mat){
00084 geometry_msgs::Transform trans;
00085 vpQuaternionVector q;
00086 mat.extract(q);
00087 trans.rotation.x = q.x();
00088 trans.rotation.y = q.y();
00089 trans.rotation.z = q.z();
00090 trans.rotation.w = q.w();
00091
00092
00093 trans.translation.x = mat[0][3];
00094 trans.translation.y = mat[1][3];
00095 trans.translation.z = mat[2][3];
00096
00097 return trans;
00098 }
00099
00100 geometry_msgs::Pose toGeometryMsgsPose(vpHomogeneousMatrix& mat){
00101 geometry_msgs::Pose pose;
00102
00103 vpThetaUVector tu(mat);
00104 vpColVector u;
00105 double theta;
00106 tu.extract(theta, u);
00107
00108 theta *= 0.5;
00109
00110 double sinTheta_2 = sin(theta);
00111
00112 pose.orientation.x = u[0] * sinTheta_2;
00113 pose.orientation.y = u[1] * sinTheta_2;
00114 pose.orientation.z = u[2] * sinTheta_2;
00115 pose.orientation.w = cos(theta);
00116
00117 pose.position.x = mat[0][3];
00118 pose.position.y = mat[1][3];
00119 pose.position.z = mat[2][3];
00120
00121 return pose;
00122 }
00123
00124
00125 #else
00126 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Transform& trans){
00127 vpHomogeneousMatrix mat;
00128 vpTranslationVector vec(trans.translation.x,trans.translation.y,trans.translation.z);
00129 vpRotationMatrix rmat;
00130
00131 double a = trans.rotation.x;
00132 double b = trans.rotation.y;
00133 double c = trans.rotation.z;
00134 double d = trans.rotation.w;
00135 rmat[0][0] = a*a+b*b-c*c-d*d;
00136 rmat[0][1] = 2*b*c-2*a*d;
00137 rmat[0][2] = 2*a*c+2*b*d;
00138
00139 rmat[1][0] = 2*a*d+2*b*c;
00140 rmat[1][1] = a*a-b*b+c*c-d*d;
00141 rmat[1][2] = 2*c*d-2*a*b;
00142
00143 rmat[2][0] = 2*b*d-2*a*c;
00144 rmat[2][1] = 2*a*b+2*c*d;
00145 rmat[2][2] = a*a-b*b-c*c+d*d;
00146
00147 mat.buildFrom(vec,rmat);
00148
00149 return mat;
00150 }
00151
00152 geometry_msgs::Transform toGeometryMsgsTransform(vpHomogeneousMatrix& mat){
00153 geometry_msgs::Transform trans;
00154 vpRotationMatrix rmat;
00155 mat.extract(rmat);
00156 vpQuaternionVector q(rmat);
00157
00158 trans.rotation.x = q.x();
00159 trans.rotation.y = q.y();
00160 trans.rotation.z = q.z();
00161 trans.rotation.w = q.w();
00162
00163
00164 trans.translation.x = mat[0][3];
00165 trans.translation.y = mat[1][3];
00166 trans.translation.z = mat[2][3];
00167
00168 return trans;
00169 }
00170
00171 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Pose& pose){
00172 vpHomogeneousMatrix mat;
00173 vpTranslationVector vec(pose.position.x,pose.position.y,pose.position.z);
00174 vpRotationMatrix rmat;
00175
00176 double a = pose.orientation.x;
00177 double b = pose.orientation.y;
00178 double c = pose.orientation.z;
00179 double d = pose.orientation.w;
00180 rmat[0][0] = a*a+b*b-c*c-d*d;
00181 rmat[0][1] = 2*b*c-2*a*d;
00182 rmat[0][2] = 2*a*c+2*b*d;
00183
00184 rmat[1][0] = 2*a*d+2*b*c;
00185 rmat[1][1] = a*a-b*b+c*c-d*d;
00186 rmat[1][2] = 2*c*d-2*a*b;
00187
00188 rmat[2][0] = 2*b*d-2*a*c;
00189 rmat[2][1] = 2*a*b+2*c*d;
00190 rmat[2][2] = a*a-b*b-c*c+d*d;
00191
00192 mat.buildFrom(vec,rmat);
00193
00194 return mat;
00195 }
00196 #endif
00197 }