pose_utils.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # Software License Agreement (Modified BSD License)
00005 #
00006 #  Copyright (c) 2012, PAL Robotics, S.L.
00007 #  All rights reserved.
00008 #
00009 #  Redistribution and use in source and binary forms, with or without
00010 #  modification, are permitted provided that the following conditions
00011 #  are met:
00012 #
00013 #   * Redistributions of source code must retain the above copyright
00014 #     notice, this list of conditions and the following disclaimer.
00015 #   * Redistributions in binary form must reproduce the above
00016 #     copyright notice, this list of conditions and the following
00017 #     disclaimer in the documentation and/or other materials provided
00018 #     with the distribution.
00019 #   * Neither the name of PAL Robotics, S.L. nor the names of its
00020 #     contributors may be used to endorse or promote products derived
00021 #     from this software without specific prior written permission.
00022 #
00023 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 #  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 #  POSSIBILITY OF SUCH DAMAGE.
00035 #
00036 
00037 import roslib; roslib.load_manifest('pal_blort')
00038 import re
00039 import subprocess
00040 from multiprocessing import Process
00041 from geometry_msgs.msg import Pose
00042 import sys
00043 
00044 def readPose(filename):
00045     f = open(filename, 'r')
00046     content = f.read()
00047     f.close()
00048     vects = re.findall(r'\[.*\]', content)
00049     pose = Pose()
00050     position = re.findall(r'[-0-9.]+', vects[0])
00051     orientation = re.findall(r'[-0-9.]+', vects[1])
00052     
00053     pose.position.x = float(position[0])
00054     pose.position.y = float(position[1])
00055     pose.position.z = float(position[2])
00056     pose.orientation.x = float(orientation[0])
00057     pose.orientation.y = float(orientation[1])
00058     pose.orientation.z = float(orientation[2])
00059     pose.orientation.w = float(orientation[3])
00060     return pose
00061 
00062 def readPoses(filename):
00063     poses = []
00064     f = open(filename, 'r')
00065     content = f.read()
00066     f.close()
00067     numbers = re.findall(r'[-0-9.]+', content)
00068     for i in range(0,len(numbers), 7):
00069         pose = Pose()
00070         pose.position.x = float(numbers[i])
00071         pose.position.y = float(numbers[i+1])
00072         pose.position.z = float(numbers[i+2])
00073         pose.orientation.x = float(numbers[i+3])
00074         pose.orientation.y = float(numbers[i+4])
00075         pose.orientation.z = float(numbers[i+5])
00076         pose.orientation.w = float(numbers[i+6])
00077         poses.append(pose)
00078     return poses
00079 
00080 def poseDiff(pose1, pose2):
00081     result = Pose()
00082     result.position.x = abs(pose1.position.x - pose2.position.x)
00083     result.position.y = abs(pose1.position.y-pose2.position.y)
00084     result.position.z = abs(pose1.position.z-pose2.position.z)
00085     result.orientation.x = abs(pose1.orientation.x-pose2.orientation.x)
00086     result.orientation.y = abs(pose1.orientation.y-pose2.orientation.y)
00087     result.orientation.z = abs(pose1.orientation.z-pose2.orientation.z)
00088     result.orientation.w = abs(pose1.orientation.w-pose2.orientation.w)
00089     return result
00090 
00091 def poseListMean(poseList):
00092     result = Pose()
00093     count = float(len(poseList))
00094     result.position.x = sum([act_pose.position.x for act_pose in poseList]) / count
00095     result.position.y = sum([act_pose.position.y for act_pose in poseList]) / count
00096     result.position.z = sum([act_pose.position.z for act_pose in poseList]) / count
00097     result.orientation.x = sum([act_pose.orientation.x for act_pose in poseList]) / count
00098     result.orientation.y = sum([act_pose.orientation.y for act_pose in poseList]) / count
00099     result.orientation.z = sum([act_pose.orientation.z for act_pose in poseList]) / count
00100     result.orientation.w = sum([act_pose.orientation.w for act_pose in poseList]) / count
00101     return result
00102 
00103 def poseListAvgDiff(pose, poselist):
00104     diffs = [poseDiff(pose, act_pose) for act_pose in poselist]
00105     return poseListMean(diffs)
00106 
00107 def poseListVariance(poseList):
00108     meanPose = poseListMean(poseList)
00109     return poseDiffList(meanPose, poseList)
00110 
00111 def core():
00112     subprocess.call("roscore", executable="bash", shell=False)
00113 
00114 def disparity():
00115     subprocess.call("roslaunch pal_disparity_segmentation disparity_check.launch", executable="bash", shell=False)
00116 
00117 def blort():
00118     subprocess.call("roslaunch pal_blort blort_singleshot_disparity.launch", executable="bash", shell=False)
00119 
00120 def disparity():
00121     subprocess.call("roslaunch pal_disparity_segmentation disparity_check.launch", executable="bash", shell=False)
00122 
00123 def launch_procs():
00124     processes = [Process(target=core), Process(target=disparity), Process(target=blort)]
00125     for p in processes:
00126         p.start()
00127     return processes
00128 
00129 def poseValidate(pose, pose_estimate, max_error):
00130     diff = poseDiff(pose, pose_estimate)
00131     if (diff.position.x - max_error.position.x < 0) and \
00132         (diff.position.y - max_error.position.y < 0) and \
00133         (diff.position.z - max_error.position.z < 0) and \
00134         (diff.orientation.x - max_error.orientation.x < 0) and \
00135         (diff.orientation.y - max_error.orientation.y < 0) and \
00136         (diff.orientation.z - max_error.orientation.z < 0) and \
00137         (diff.orientation.w - max_error.orientation.w < 0):
00138         return True
00139     else:
00140         return False
00141 
00142 def validatePosesFromFile(pose, max_error, filename):
00143     poses = readPoses(filename)
00144     return [poseValidate(pose, act, max_error) for act in poses]
00145 
00146 def geomQuatTo3x3Mat(quaternion):
00147     x2 = quaternion.x * quaternion.x;
00148     y2 = quaternion.y * quaternion.y;
00149     z2 = quaternion.z * quaternion.z;
00150     xy = quaternion.x * quaternion.y;
00151     xz = quaternion.x * quaternion.z;
00152     yz = quaternion.y * quaternion.z;
00153     wx = quaternion.w * quaternion.x;
00154     wy = quaternion.w * quaternion.y;
00155     wz = quaternion.w * quaternion.z;
00156 
00157     result =  [[1.0-(2.0*(y2 + z2)), 2.0*(xy - wz), 2.0*(xz + wy)], \
00158                [2.0*(xy + wz), 1.0-2.0*(x2 + z2), 2.0*(yz - wx)], \
00159                [2.0*(xz - wy), 2.0*(yz + wx), 1.0-(2.0*(x2 + y2))]]
00160     return result
00161 


blort_ros
Author(s): Bence Magyar
autogenerated on Thu Jan 2 2014 11:39:12