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 #ifndef HECTOR_POSE_ESTIMATION_BFL_CONVERSIONS_H
00030 #define HECTOR_POSE_ESTIMATION_BFL_CONVERSIONS_H
00031
00032 #include <geometry_msgs/PoseWithCovariance.h>
00033 #include <bfl/wrappers/matrix/matrix_wrapper.h>
00034 #include <bfl/wrappers/matrix/vector_wrapper.h>
00035
00036 namespace hector_pose_estimation {
00037
00038 void covarianceMsgToBfl(geometry_msgs::PoseWithCovariance::_covariance_type const &msg, MatrixWrapper::SymmetricMatrix_Wrapper &bfl) {
00039 unsigned int dim = sqrt(msg.size());
00040 bfl.resize(dim,false,false);
00041 for(unsigned int i = 0; i < dim; ++i)
00042 for(unsigned int j = 0; j < dim; ++j)
00043 bfl(i+1,j+1) = msg[i*dim+j];
00044 }
00045
00046 void covarianceBflToMsg(MatrixWrapper::SymmetricMatrix const &bfl, geometry_msgs::PoseWithCovariance::_covariance_type &msg) {
00047 unsigned int dim = sqrt(msg.size());
00048 for(unsigned int i = 0; i < dim; ++i)
00049 for(unsigned int j = 0; j < dim; ++j)
00050 msg[i*6+j] = bfl(i+1,j+1);
00051 }
00052
00053 void pointMsgToBfl(geometry_msgs::Point const &msg, MatrixWrapper::ColumnVector_Wrapper &bfl) {
00054 bfl.resize(3);
00055 bfl(1) = msg.x;
00056 bfl(2) = msg.y;
00057 bfl(3) = msg.z;
00058 }
00059
00060 void pointBflToMsg(MatrixWrapper::ColumnVector_Wrapper const &bfl, geometry_msgs::Point &msg) {
00061 msg.x = bfl(1);
00062 msg.y = bfl(2);
00063 msg.z = bfl(3);
00064 }
00065
00066 void quaternionMsgToBfl(geometry_msgs::Quaternion const &msg, MatrixWrapper::ColumnVector_Wrapper &bfl) {
00067 bfl.resize(4);
00068 bfl(1) = msg.w;
00069 bfl(2) = msg.x;
00070 bfl(3) = msg.y;
00071 bfl(4) = msg.z;
00072 }
00073
00074 void quaternionBflToMsg(MatrixWrapper::ColumnVector_Wrapper const &bfl, geometry_msgs::Quaternion &msg) {
00075 msg.w = bfl(1);
00076 msg.x = bfl(2);
00077 msg.y = bfl(3);
00078 msg.z = bfl(4);
00079 }
00080
00081 }
00082
00083 #endif // HECTOR_POSE_ESTIMATION_BFL_CONVERSIONS_H