mocap_prop_offset.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('cortex_stream')
import rospy
import math,io,sys
from numpy import *
from prop import Prop,Marker
import argparse
import tf

# fern:Pose6D p; p.transform(vector v) macht p.rot*v + p.trans


class Transformer:
    def __init__(self):
        pass

    def compute_transform_from_file(self,file):
       prop = Prop() 
       if not prop.parse_prop_file(file):
            rospy.logerr( "error computing transform from prop file %s"%(file) )
            return None
       else:
            rospy.loginfo( prop )
            T = self.compute_transform_from_prop(prop)
            if T==None:
                rospy.logerr('got no transform. Failed..')
            else:
                rospy.loginfo('got transform:\n%s'%(str(T)))
                rospy.loginfo('applying to markers in prop file:')
                markers_t = []
                I = matrix(tf.transformations.inverse_matrix(T))
                for m in prop.Markers:
                    t = m.position
                    t.append(1)
                    v = matrix(t)
                    #print I
                    t =  I*v.transpose()
                    t=t/t[3]
                    mt = Marker()
                    mt.name = m.name
                    mt.position = t[:3]
                    print mt
                    #print '--> %s'%str( vt.transpose() )
                    #print mt
            return T
                
                    

    def compute_transform_from_prop(self,axis):
        # (axis.body.size() != 1)
        #return -1;
        # scale = 0.001 # Motion Analysis works with millimeters, we work in meters
        #front_marker = find(lambda marker: marker.name == 'front', axis.Markers)
        front_markers = filter(lambda marker: marker.name == 'front', axis.Markers)
        left_markers = filter(lambda marker: marker.name == 'left', axis.Markers)
        right_markers = filter(lambda marker: marker.name == 'right', axis.Markers)
        if len(front_markers)!= 1 or len(left_markers)!=1 or len(right_markers)!=1:
            rospy.logerr('could not find unique front, left, and right markers')
            return None
        front = array(front_markers[0].position)
        left = array(left_markers[0].position)
        right = array(right_markers[0].position)
        #print 'found front,left,right as:'
        #print front
        #print left
        #print right
        m1 = (right+left)/2.0
        m2 = front
        m3 = left
        scale = 1.0
        e1 = m2-m1
        e1 = e1/linalg.norm(e1)
        e2 = m3-m1
        e2 = e2 - (e1* dot(e1,e2))
        if dot(e1,e2) > 1e-6:
            print 'error: e1*e2 is not close to zero but %f'%(dot(e1,e2))
        e2 = e2/linalg.norm(e2)
        e3 = cross(e1,e2)
        m = tf.transformations.identity_matrix()
        m[:3,0] = e1[:3]
        m[:3,1] = e2[:3]
        m[:3,2] = e3[:3]
        m[:3,3] = m1[:3]
        return m
        #tf.transformations.quaternion
        #pose = numpy.matrix()




if __name__=='__main__':
    parser = argparse.ArgumentParser(description='Compute transform from markers from prop file containing markers "front" "left" and "right" .')
    parser.add_argument('file', metavar='PROPFILE', type=file, nargs='+',
                        help='prop file(s)')
    args = parser.parse_args()
    
    transformer = Transformer()
    for i in range(0,len(args.file)):
        transformer.compute_transform_from_file(args.file[i])
    
    sys.exit()