-
Notifications
You must be signed in to change notification settings - Fork 2
/
convert2euclidean.py
121 lines (94 loc) · 6.38 KB
/
convert2euclidean.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
# -*- coding: utf-8 -*-
"""
Created on Thu Jul 23 22:28:29 2015
@author: Shamir
"""
import pandas
import os
import numpy as np
import time
from natsort import natsorted
from pandas import DataFrame
start = time.clock() # start counting time (optional)
source_left = 'C:\\Users\\Shamir\\Desktop\\Grad\\Participant Study\\Hands_Sorted\\P8\\Left_combined\\' # source folder
source_right = 'C:\\Users\\Shamir\\Desktop\\Grad\\Participant Study\\Hands_Sorted\\P8\\Right_combined\\' # naturally sort the file list
destination_left = 'C:\\Users\\Shamir\\Desktop\\Grad\\Participant Study\\Euclidean\\P8\\Left Sorted\\'
destination_right = 'C:\\Users\\Shamir\\Desktop\\Grad\\Participant Study\\Euclidean\\P8\\Right Sorted\\'
fileformat = '.csv'
def Convert2Euclidean(sourcePath, destinationPath):
"""
This function converts Quaterinons to Euler angles and saves it in a CSV file.
input parameter : sourcePath; it takes the path of the directory where the files are stored for processing.
output parameter: destinationPath; it takes the path of the directory where the new data would be stored in corresponding files. The files are numbered from 1 to n, n being the total number of files.
"""
count_errors = 0
count = 1 # start count for file numbers
output_list = [] # temporary storage list before passing data to an array
filelist = os.listdir(sourcePath) # list all the files in the folder
filelist = natsorted(filelist) # naturally sort the file list; this fixes the problem of having improper file order in the list
for file in range(len(filelist)): # len(filelist)
#print "reading file # ", file
csvfile = pandas.read_csv(sourcePath + filelist[file], header = None) # read csv file
csvfile.values[1:] = csvfile.values[1:].astype(float) # convert all strings to floats; ignore header columns
num_rows = len(csvfile) # number of rows in the file
num_columns = len(csvfile.values[0]) # number of columns in the file
# Creater header row
for i in range(0, num_columns, 4):
output_list.append('alpha')
output_list.append('beta')
output_list.append('gamma')
#print "shape of 1st output_list = ", np.shape(output_list)
output_array = np.asarray(output_list) # initialize the storage array with headers
output_list = [] # empty temporary list
#print "shape of output_array = ", np.shape(output_array)
# loop for conversion to Euclidean angles
for i in range(1, num_rows):
for j in range(0, num_columns, 4): # we need to take 4 columns in each iteration; 1 quaternion = 4 elements
# precalculate indexes of the vector elements of each quaternion for faster memory operation
secondIndex = j + 1
thirdIndex = j + 2
fourthIndex = j + 3
qr = float(csvfile.values[i, j])
qx = float(csvfile.values[i, secondIndex])
qy = float(csvfile.values[i, thirdIndex])
qz = float(csvfile.values[i, fourthIndex])
# Calculate the Euler Angles in degrees (multiplying the radian terms with 180/pi)
#print "reading i, j = ", i, j
try:
Alpha = np.arctan ((2*(qr*qx + qy*qz)) / (1 - 2*(np.square(qx) + np.square(qy)))) * 180/np.pi
except:
print 'file, row, col = ', file, i, j
print qr, qx, qy, qz
## Major bug, possibly due to noise. This test value, given that the condition becomes true, should not be used for actual analysis. Instead, please filter the noise through the modified filter.
test = 2*(qr*qy - qx*qz)
if test < -1.0:
count_errors += 1
#test = -1.0
elif test > 1.0:
count_errors += 1
#test = 1.0
Beta = np.arcsin (test) * 180/np.pi
#print Beta
#if isnan(Beta) == True:
#print "qr = ", qr
#print "qx = ", qx
#print "qy = ", qy
#print "qz = ", qz
#break
Gamma = np.arctan ((2*(qr*qz + qx*qy)) / (1 - 2*(np.square(qy) + np.square(qz)))) * 180/np.pi
# save the calculated values in the temporary storage
output_list.append(Alpha)
output_list.append(Beta)
output_list.append(Gamma)
#except ValueError: # if we encounter NaN values
# pass
#print "shape of 2nd output_list = ", np.shape(output_list)
output_array = np.vstack((output_array, output_list)) # insert each converted row into the storage array
output_list = [] # empty temporary list for next iteration
output_array = DataFrame(output_array) # convert complete array into a Pandas Dataframe
output_array.to_csv(destinationPath + str(count) + fileformat, header = False, index = False) # write the dataframe to a csv file
count += 1 # increment file counter
print 'bad Beta values = ', count_errors
Convert2Euclidean(source_left, destination_left)
Convert2Euclidean(source_right, destination_right)
print time.clock() - start, 'seconds taken to execute the program'