affine_position.py

# -*- coding: utf-8 -*-
"""
Created on Wed Dec 16 20:48:01 2015
@author: ryousuke
"""

import numpy as np
import math

filename = 'C:\\Users\\ryousuke\\Desktop\\lab_data\\12mon_trackfollow\\20151207_follow_ghost\\03-04\\'
#C:\\Users\\ryousuke\\Desktop\\lab_data\\12mon_trackfollow\\20151207_follow_ghost\\03-04\\
f = open(filename + 'affine_position.txt', 'w')

for text in open(filename + 'lpred_track_angle.txt').readlines():

    column = np.array(text.split())
    
    number = int(column[0])    
    pread_x = int(column[1])    
    pread_y = float(column[2])  
    rx = float(column[3])
    ry = float(column[4])
    ax = float(column[5])
    ay = float(column[6])
    
    phi = math.atan2(ay,ax)
    pi = math.pi
    #angle = phi * (180/pi)
    angle = phi
    #aaaaa = phi * (180/pi)
    lpread_x = pread_x + 256
    lpread_y = pread_y + 220
    
    ry = (-1)*ry
    lpread_y = (-1)*lpread_y
    
    lpread_x_affine = lpread_x*math.cos(angle) + lpread_y*math.sin(angle)
    lpread_y_affine = -lpread_x*math.sin(angle) + lpread_y*math.cos(angle)

    rx_affine = rx*math.cos(angle) + ry*math.sin(angle)
    ry_affine = -rx*math.sin(angle) + ry*math.cos(angle)

    mystr = "{0} {1} {2} {3} {4}\n".format(number, lpread_x_affine, lpread_y_affine, rx_affine, ry_affine)
    #mystr = "{0} {1} {2} {3} {4} {5} {6}\n".format(number, lpread_x_affine, lpread_y_affine, rx_affine, ry_affine, lpread_x_affine - rx_affine, lpread_y_affine - ry_affine)
    print mystr
    f.write(mystr)
     
     

f.close()