pal_util.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2012, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * @file pal_util.cpp
00035  * @author Bence Magyar
00036  * @date April 2012
00037  * @version 0.1
00038  * @brief Util functions used in BLORT.
00039  */
00040 
00041 #include <blort/blort/pal_util.h>
00042 #include <opencv2/imgproc/imgproc.hpp>
00043 #include <cmath>
00044 #include <sstream>
00045 #include <iostream>
00046 #include <algorithm>
00047 
00048 namespace pal_blort
00049 {
00050     //FIXME: check 'root' and put ./ instead of / to the beginning of the paths if it's empty
00051     std::string addRoot(const std::string& obj, const std::string& root)
00052     {
00053         std::stringstream ss;
00054         ss << root << "/" << obj;
00055         return ss.str();
00056     }
00057 
00058     geometry_msgs::Pose tgPose2RosPose(const TomGine::tgPose &pose)
00059     {
00060         geometry_msgs::Pose res;
00061         res.position.x = pose.t.x;
00062         res.position.y = pose.t.y;
00063         res.position.z = pose.t.z;
00064         res.orientation.x = pose.q.x;
00065         res.orientation.y = pose.q.y;
00066         res.orientation.z = pose.q.z;
00067         res.orientation.w = pose.q.w;
00068         return res;
00069     }
00070 
00071     TomGine::tgPose rosPose2TgPose(const geometry_msgs::Pose &pose)
00072     {
00073         TomGine::tgPose res;
00074         res.t.x = pose.position.x;
00075         res.t.y = pose.position.y;
00076         res.t.z = pose.position.z;
00077         res.q.x = pose.orientation.x;
00078         res.q.y = pose.orientation.y;
00079         res.q.z = pose.orientation.z;
00080         res.q.w = pose.orientation.w;
00081         return res;
00082     }
00083 
00084     tf::Transform rosPose2TfTransform(const geometry_msgs::Pose &pose)
00085     {
00086         tf::Transform result;
00087         result.setOrigin(tf::Vector3(pose.position.x,
00088                                      pose.position.y,
00089                                      pose.position.z));
00090         result.setRotation(tf::Quaternion(pose.orientation.x,
00091                                           pose.orientation.y,
00092                                           pose.orientation.z,
00093                                           pose.orientation.w
00094                                           ));
00095         return result;
00096     }
00097 
00098     geometry_msgs::Pose tfTransform2RosPose(const tf::Transform &transform)
00099     {
00100         geometry_msgs::Pose result;
00101         const tf::Vector3 position = transform.getOrigin();
00102         const tf::Quaternion orientation = transform.getRotation();
00103         result.position.x = position.x();
00104         result.position.y = position.y();
00105         result.position.z = position.z();
00106         result.orientation.x = orientation.x();
00107         result.orientation.y = orientation.y();
00108         result.orientation.z = orientation.z();
00109         result.orientation.w = orientation.w();
00110 
00111         return result;
00112     }
00113 
00114     geometry_msgs::Pose blortPosesToRosPose(geometry_msgs::Pose reference,
00115                                             geometry_msgs::Pose target)
00116     {
00117         const tf::Transform blort_reference_frame =
00118                 pal_blort::rosPose2TfTransform(reference);
00119         const tf::Transform blort_target_pose =
00120                 pal_blort::rosPose2TfTransform(target);
00121 
00122         return pal_blort::tfTransform2RosPose(blort_reference_frame.inverse()*blort_target_pose);
00123     }
00124 
00125     geometry_msgs::Pose poseAbsDiff(geometry_msgs::Pose pose1, geometry_msgs::Pose pose2)
00126     {
00127         geometry_msgs::Pose result;
00128         result.position.x = abs(pose1.position.x-pose2.position.x);
00129         result.position.y = abs(pose1.position.y-pose2.position.y);
00130         result.position.z = abs(pose1.position.z-pose2.position.z);
00131         result.orientation.x = abs(pose1.orientation.x-pose2.orientation.x);
00132         result.orientation.y = abs(pose1.orientation.y-pose2.orientation.y);
00133         result.orientation.z = abs(pose1.orientation.z-pose2.orientation.z);
00134         result.orientation.w = abs(pose1.orientation.w-pose2.orientation.w);
00135         return result;
00136     }
00137 
00138     bool poseValidate(geometry_msgs::Pose known_pose,
00139                       geometry_msgs::Pose pose_estimate,
00140                       geometry_msgs::Pose max_error)
00141     {
00142         const geometry_msgs::Pose diff = poseAbsDiff(pose_estimate, known_pose);
00143         if((diff.position.x - max_error.position.x < 0) &&
00144            (diff.position.y - max_error.position.y < 0) &&
00145            (diff.position.z - max_error.position.z < 0) &&
00146            (diff.orientation.x - max_error.orientation.x < 0) &&
00147            (diff.orientation.y - max_error.orientation.y < 0) &&
00148            (diff.orientation.z - max_error.orientation.z < 0) &&
00149            (diff.orientation.w - max_error.orientation.w < 0))
00150         {
00151             return true;
00152         }
00153         else
00154         {
00155             return false;
00156         }
00157     }
00158 
00159     cv::Mat quaternionTo3x3cvMat(geometry_msgs::Quaternion quaternion)
00160     {
00161         float x2 = quaternion.x * quaternion.x;
00162         float y2 = quaternion.y * quaternion.y;
00163         float z2 = quaternion.z * quaternion.z;
00164         float xy = quaternion.x * quaternion.y;
00165         float xz = quaternion.x * quaternion.z;
00166         float yz = quaternion.y * quaternion.z;
00167         float wx = quaternion.w * quaternion.x;
00168         float wy = quaternion.w * quaternion.y;
00169         float wz = quaternion.w * quaternion.z;
00170 
00171         cv::Mat result(3,3, CV_32FC1);
00172         result.at<float>(0,0) = 1.0f - 2.0f * (y2 + z2);
00173         result.at<float>(0,1) = 2.0f * (xy - wz);
00174         result.at<float>(0,2) = 2.0f * (xz + wy);
00175 
00176         result.at<float>(1,0) = 2.0f * (xy + wz);
00177         result.at<float>(1,1) = 1.0f - 2.0f * (x2 + z2);
00178         result.at<float>(1,2) = 2.0f * (yz - wx);
00179 
00180         result.at<float>(2,0) = 2.0f * (xz - wy);
00181         result.at<float>(2,1) = 2.0f * (yz + wx);
00182         result.at<float>(2,2) = 1.0f - 2.0f * (x2 + y2);
00183 
00184         return result;
00185     }
00186 }


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:25