def __init__(self, ID=0, SE3=None, rigidMarkers=None, meanError=0): self.ID = ID if SE3 and len(SE3) == 7: self.SE3 = array(SE3) else: self.SE3 = zeros((7, )) if rigidMarkers: self.rigidMarkers = dict(rigidMarkers) else: self.rigidMarkers = {} self.meanError = meanError self.roll, self.pitch, self.yaw = map( rad2deg, rot2ZYXeuler(quaternion2rot(self.SE3[3:])))
def __init__(self, ID=0, SE3=None, rigidMarkers=None, meanError=0): self.ID = ID if SE3 and len(SE3) == 7: self.SE3 = array(SE3) else: self.SE3 = zeros((7,)) if rigidMarkers: self.rigidMarkers = dict(rigidMarkers) else: self.rigidMarkers = {} self.meanError = meanError self.roll, self.pitch, self.yaw = map(rad2deg, rot2ZYXeuler(quaternion2rot(self.SE3[3:])))
def trackInfo(self): ''' Note: only for ZYX Euler angles ''' # Receive the data into a buffer # self.__socket.recv_into(self.__buffer, self.buffersize) # print ' '.join(map(hex, self.__buffer)) msg = self.__socket.recv(4096) # print len(msg) # print ' '.join(map(hex, bytearray(msg))) # Parse data markerSets, rigidBodies = parseNatNet(msg) # markerSets, rigidBodies = parseNatNet(self.__buffer) # # for rigidBodyID in self.trackedObjects: # rigidBody = rigidBodies[rigidBodyID] # # Check if trackable is seen # if not any(rigidBody.SE3): # return None # # print OptiTrack data -- TODO: recode this with logging library if False: print '--------------------------------------------' print 'Real time info in OptiTrack system (m, deg):' # the same data displayed in the Motive/OptiTrack panel for rigidBodyID in self.trackedObjects: rigidBody = rigidBodies[rigidBodyID] # rigidBody.SE3[3:] = array(list([rigidBody.SE3[6], rigidBody.SE3[3], rigidBody.SE3[5], rigidBody.SE3[4]])) # + list(rigidBody.SE3[3:6])) print rigidBody.SE3[3:] print quaternion2rot(rigidBody.SE3[3:]) euler = rot2ZYXeuler(quaternion2rot(rigidBody.SE3[3:])) # roll, pitch, yaw print euler print rigidBodyID, '->', rigidBody.SE3[:3], map(rad2deg, euler) return rigidBodies