Allwillcome/XsensDot2Mtw4OpenSense

The previous rotation matrix is not in order, the correct code is as follows

Opened this issue · 0 comments

def transformed_Xsens_dot_data_RealTime(filename):
regex = r"(.)_.(.*).*.csv"
mysearch = re.search(regex,filename)
IMU_ID = mysearch.group(1)
prefix_IMU = mysearch.group(2)
new_filename = "XSDot" + "_" + prefix_IMU + IMU_ID + ".txt"
path_new_filename = os.path.join(path_new_file, new_filename)

   #Read File 读取文件
   df = pd.read_csv(filename,skiprows=10)
   #print(df.head(10))                  
   df_euler = df[["Euler_X","Euler_Y","Euler_Z"]]                  #Select the column in which euler Angle is located 选择欧拉角所在的列

   #Make a quaternion conversion 进行四元数的转换
   r = R.from_euler("xyz",df_euler,degrees=True)           #Initialize a quaternion 初始化四元数
   matrix = r.as_matrix()                                  #Converts a quaternion to a rotation matrix 将四元数转换为旋转矩阵
   (matrix_rows,b,c) = matrix.shape                        #The rotation matrix is (n,3,3) and there is no way to convert to Datafram 旋转矩阵是(n,3,3)没有办法转换为 Datafram
   matrix_reshpae = matrix.reshape(matrix_rows,9)          #Convert to n rows and 9 columns of data 转换为 n 行 9 列的数据
   
   #Write the converted data to Excel 将转换好的数据写入到 Excel 当中
   header_list = ["Mat[1][1]","Mat[1][2]","Mat[1][3]",
                 "Mat[2][1]","Mat[2][2]","Mat[2][3]",
                 "Mat[3][1]","Mat[3][2]","Mat[3][3]"]     #According to the results of Xsens rotation matrix, the corresponding columns are assigned table headers 对照 Xsens 旋转矩阵结果,为对应列赋予表头
   df_matrix = pd.DataFrame(matrix_reshpae,columns=header_list)
   
   #Align the Xsens MVN built with Dot data 将 Dot 数据构建的 Xsens MVN 一致
   PacketCounter = df['PacketCounter'] 
   SampleTimeFine = df["SampleTimeFine"]
   Acc = df[["FreeAcc_X","FreeAcc_Y","FreeAcc_Z"]] 
   #Acc = df[["Acc_X","Acc_Y","Acc_Z"]]                     #the difference of offline and realtime
   Acc.columns = ["Acc_X","Acc_Y","Acc_Z"]                 #Name the free acceleration acceleration, keeping the same format as the Xsens MVN data将自由加速度命名为加速度,保持和 Xsens MVN 数据格式相同
   Year = [] 
   Month = []
   Day = []
   Second = []
   UTC_Nano = []
   UTC_Year = []
   UTC_Month = []
   UTC_Day = []
   UTC_Hour = []
   UTC_Minute = []
   UTC_Second = []
   UTC_Valid = []
   df_othercolumns = pd.DataFrame({'Year':Year, 'Month':Month, 'Day':Day, 'Second':Second,
          'UTC_Nano':UTC_Nano, 'UTC_Year':UTC_Year, 'UTC_Month':UTC_Month, 'UTC_Day':UTC_Day, 'UTC_Hour':UTC_Hour,
          'UTC_Minute':UTC_Minute, 'UTC_Second':UTC_Second, 'UTC_Valid':UTC_Valid})

   #The structure of the splice transformation 拼接转换的结构
   frame = [PacketCounter,SampleTimeFine,df_othercolumns,Acc,df_matrix]
   transformed_df = pd.concat(frame,axis=1)                              #Splice multiple Dataframs horizontally axis=1 横向拼接多个 DataFram
   MTw_columns = ['PacketCounter', 'SampleTimeFine', 'Year', 'Month', 'Day', 'Second',
   'UTC_Nano', 'UTC_Year', 'UTC_Month', 'UTC_Day', 'UTC_Hour',
   'UTC_Minute', 'UTC_Second', 'UTC_Valid', 'Acc_X', 'Acc_Y', 'Acc_Z',
   'Mat[1][1]', 'Mat[2][1]', 'Mat[3][1]', 'Mat[1][2]', 'Mat[2][2]',
   'Mat[3][2]', 'Mat[1][3]', 'Mat[2][3]', 'Mat[3][3]']
   transformed_df = transformed_df[MTw_columns]
   transformed_df.to_csv(path_new_filename,
                        index=False,sep="\t",float_format='%.6f') 

   with open(path_new_filename,"r+") as f:
       content = f.read()
       f.seek(0,0)
       f.write("// Start Time: Unknown\n// Update Rate: 100.0Hz\n//Filter Profile: human (46.1)\n// Option Flags: AHS Disabled ICC Disabled\n// Firmware Version: 4.0.2\n"+content)
   print(new_filename,",Done😎")                                      #报告转换完成的文件

new_file = "IMUData"
path_new_file = os.path.join(path_dir,new_file)
os.chdir(path_dir)
path_original_Xsens_dot = os.chdir(".\Original_XsensDot_Data_Offline")
print(os.getcwd())
for file in os.listdir():
#Filter out all.csv files to avoid folder processing; otherwise, the system will report errors
#筛选出所有的 .csv 文件,避免对文件夹进行处理,否则系统会报错
if ".csv" in file:
transformed_Xsens_dot_data_Offline(file)
print("🎃All Xsens Dot original files have been converted")