tracker.py

Go to the documentation of this file.

#!/usr/bin/env python
 #
 # A ROS interface for the Cortex Motion Capture System Network Stream
 #
 # Copyright 2011 by Daniel Maier, University of Freiburg
 # based on the robular implementation by Joerg Mueller
 #
 # This program is free software: you can redistribute it and/or modify
 # it under the terms of the GNU General Public License as published by
 # the Free Software Foundation, version 3.
 #
 # This program is distributed in the hope that it will be useful,
 # but WITHOUT ANY WARRANTY; without even the implied warranty of
 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 # GNU General Public License for more details.
 #
 # You should have received a copy of the GNU General Public License
 # along with this program.  If not, see <http://www.gnu.org/licenses/>.



import roslib;
roslib.load_manifest('cortex_stream')
import rospy
import tf
import getopt,sys
import argparse
import numpy as np
import math
import ConfigParser, os
from visualization_msgs.msg import MarkerArray
from nav_msgs.msg import Odometry
from prop import Prop,Marker

class Tracker():
    def __init__(self, publish_odom = False):
        self.props = dict()
        self.marker_sub = rospy.Subscriber("cortex_marker_array", MarkerArray, self.marker_callback)
        self.br = tf.TransformBroadcaster()
        self.publish_odom = publish_odom
        if self.publish_odom:
            self.odom_pub = rospy.Publisher("mocap_odom", Odometry)

    def marker_callback(self, markerArray):
        n = len(markerArray.markers)
        for i in range(0,n): # process all n tracked objects
            m = markerArray.markers[i]
            name = m.ns
            nMarkers = len(m.points)
            #print "Found obj %s with %d markers"%(name,nMarkers)
            p = self.props[name]
            ptG = []
            ptO = []
            for (pt,i) in zip(m.points,range(0,len(m.points))):
                if math.isnan(pt.x) or math.isnan(pt.y):
                    #print "NaN found!"
                    continue
                pos = p.Markers[i].position[:]
                ptO.append( [pos[0],pos[1],pos[2],1] )
                ptG.append( [pt.x, pt.y, pt.z, 1] )
                #pos = np.array([pt.x,pt.y,pt.z]).transpose()
                #ptG[:,i] = pos.transpose()

            if len(ptO ) <3 or len(ptG) <3:
                rospy.logwarn("too few valid marker positions for pose estimation. skipping.")
                continue

            # estimate pose of object
            x = np.matrix(ptO)[:,0:3].transpose()
            y = np.matrix(ptG)[:,0:3].transpose()
            mx = np.mean(x,1)
            my = np.mean(y,1)
            K = (y-my) * (x-mx).transpose()
            U,S,V = np.linalg.svd(K)
            SS = np.eye(3)
            if np.linalg.det(U) * np.linalg.det(V) < 0:
                SS[2,2] = -1.0
            else:
                SS[2,2] = 1.0
            R = U*SS*V
            D = np.eye(3)
            D[0,0] = S[0]
            D[1,1] = S[1]
            D[2,2] = S[2]
            T = my - R*mx
            tx = T[0]
            ty = T[1]
            tz = T[2]
            T = np.eye(4)
            T[0:3,0:3] = R
            T[0,3] = tx
            T[1,3] = ty
            T[2,3] = tz
            # publish pose
            try:
                q = tf.transformations.quaternion_from_matrix(T)
            except Exception as e:
                rospy.logerr( "caught quat exception" )
                rospy.logerr( e )
                rospy.logerr( "transform was: " + str(T) )
            #print "br %s-->%s"%(m.header.frame_id,name)
            self.br.sendTransform((tx,ty,tz),
                                  q,
                             m.header.stamp, name,
                             m.header.frame_id)
            if self.publish_odom:
                odom_msg = Odometry()
                odom_msg.header.stamp = m.header.stamp
                odom_msg.header.frame_id = '/map'
                odom_msg.child_frame_id = name
                odom_msg.pose.pose.position.x = tx
                odom_msg.pose.pose.position.y = ty
                odom_msg.pose.pose.position.z = tz
                odom_msg.pose.pose.orientation.x = q[0]
                odom_msg.pose.pose.orientation.y = q[1]
                odom_msg.pose.pose.orientation.z = q[2]
                odom_msg.pose.pose.orientation.w = q[3]
                self.odom_pub.publish(odom_msg)
            #end processing object i
        pass

    def add_prop_from_file(self,file):
        prop = Prop()
        if prop.parse_prop_file(file):
            self.props[prop.Name] = prop
            rospy.loginfo( prop )
        else:
            rospy.logerr( "error adding prop from %s"%(file) )




if __name__=='__main__':
    rospy.init_node('cortex_tacker')
    myargs = rospy.myargv(sys.argv)
    parser = argparse.ArgumentParser(description='Track Cortex rigid body from prop file(s).')
    parser.add_argument('file', metavar='PROPFILE', type=file, nargs='+',
                        help='prop file(s)')
    myargs = myargs[1:]
    #print myargs
    args = parser.parse_args(myargs)

    tracker = Tracker()
    for i in range(0,len(args.file)):
        print "parsing %s"%(args.file[i])
        tracker.add_prop_from_file(args.file[i])

    rospy.spin()
    sys.exit()