Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('cortex_stream')
00004 import rospy
00005 import math,io,sys
00006 from numpy import *
00007 from prop import Prop,Marker
00008 import argparse
00009 import tf
00010
00011
00012
00013
00014 class Transformer:
00015 def __init__(self):
00016 pass
00017
00018 def compute_transform_from_file(self,file):
00019 prop = Prop()
00020 if not prop.parse_prop_file(file):
00021 rospy.logerr( "error computing transform from prop file %s"%(file) )
00022 return None
00023 else:
00024 rospy.loginfo( prop )
00025 T = self.compute_transform_from_prop(prop)
00026 if T==None:
00027 rospy.logerr('got no transform. Failed..')
00028 else:
00029 rospy.loginfo('got transform:\n%s'%(str(T)))
00030 rospy.loginfo('applying to markers in prop file:')
00031 markers_t = []
00032 I = matrix(tf.transformations.inverse_matrix(T))
00033 for m in prop.Markers:
00034 t = m.position
00035 t.append(1)
00036 v = matrix(t)
00037
00038 t = I*v.transpose()
00039 t=t/t[3]
00040 mt = Marker()
00041 mt.name = m.name
00042 mt.position = t[:3]
00043 print mt
00044
00045
00046 return T
00047
00048
00049
00050 def compute_transform_from_prop(self,axis):
00051
00052
00053
00054
00055 front_markers = filter(lambda marker: marker.name == 'front', axis.Markers)
00056 left_markers = filter(lambda marker: marker.name == 'left', axis.Markers)
00057 right_markers = filter(lambda marker: marker.name == 'right', axis.Markers)
00058 if len(front_markers)!= 1 or len(left_markers)!=1 or len(right_markers)!=1:
00059 rospy.logerr('could not find unique front, left, and right markers')
00060 return None
00061 front = array(front_markers[0].position)
00062 left = array(left_markers[0].position)
00063 right = array(right_markers[0].position)
00064
00065
00066
00067
00068 m1 = (right+left)/2.0
00069 m2 = front
00070 m3 = left
00071 scale = 1.0
00072 e1 = m2-m1
00073 e1 = e1/linalg.norm(e1)
00074 e2 = m3-m1
00075 e2 = e2 - (e1* dot(e1,e2))
00076 if dot(e1,e2) > 1e-6:
00077 print 'error: e1*e2 is not close to zero but %f'%(dot(e1,e2))
00078 e2 = e2/linalg.norm(e2)
00079 e3 = cross(e1,e2)
00080 m = tf.transformations.identity_matrix()
00081 m[:3,0] = e1[:3]
00082 m[:3,1] = e2[:3]
00083 m[:3,2] = e3[:3]
00084 m[:3,3] = m1[:3]
00085 return m
00086
00087
00088
00089
00090
00091
00092 if __name__=='__main__':
00093 parser = argparse.ArgumentParser(description='Compute transform from markers from prop file containing markers "front" "left" and "right" .')
00094 parser.add_argument('file', metavar='PROPFILE', type=file, nargs='+',
00095 help='prop file(s)')
00096 args = parser.parse_args()
00097
00098 transformer = Transformer()
00099 for i in range(0,len(args.file)):
00100 transformer.compute_transform_from_file(args.file[i])
00101
00102 sys.exit()
00103
00104