$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 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 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 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 } // namespace hector_pose_estimation 00082 00083 #endif // HECTOR_POSE_ESTIMATION_BFL_CONVERSIONS_H