From 3ad58c36f8901662d97d062d1eeea6bc9e221e92 Mon Sep 17 00:00:00 2001
From: Mahmoud Ahmed Ali <mahmoudali2929@gmail.com>
Date: Tue, 14 Feb 2023 17:58:43 +0000
Subject: [PATCH] Add pose file
---
pose.py | 66 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 66 insertions(+)
create mode 100644 pose.py
diff --git a/pose.py b/pose.py
new file mode 100644
index 0000000..ee0fd8b
--- /dev/null
+++ b/pose.py
@@ -0,0 +1,66 @@
+import math
+import numpy as np
+import json
+from scipy.spatial.transform import Rotation
+
+
+def convert2(xyz):
+ (R, P, Y) = (xyz[0], xyz[1], xyz[2])
+ Q = Rotation.from_euler(seq='xyz', angles=[R, P, Y], degrees=False).as_quat()
+ r = Rotation.from_quat(Q)
+ rotation = r.as_matrix()
+
+ return rotation
+
+
+def transform_pose(data_name):
+ transformation = np.matrix([[0.0000000, -1.0000000, 0.0000000],
+ [0.0000000, 0.0000000, -1.0000000],
+ [1.0000000, 0.0000000, 0.0000000]])
+ cont1, cont2, cont3 = 0, 0, 0
+ for p in range(4995):
+ with open(f'{data_name}/Pose/{p}.json', 'r') as f:
+ data = json.load(f)
+
+ for i in range(len(data)):
+ if data[i]['id'] == 4:
+ cont1 += 1
+ rpy = data[i]['pose']['rpy']
+ rot = convert2(rpy)
+ R_exp = transformation @ rot
+ R_exp = np.array(R_exp)
+
+ xyz = data[i]['pose']['xyz']
+ T_exp = transformation @ xyz
+ T_exp = np.array(T_exp)
+ num_arr = np.c_[R_exp, T_exp[0]]
+ np.save(f'{data_name}/Pose_transformed/Banana/{p}.npy', num_arr) # save
+
+ elif data[i]['id'] == 5:
+ cont2 += 1
+ rpy = data[i]['pose']['rpy']
+ rot = convert2(rpy)
+ R_exp = transformation @ rot
+ R_exp = np.array(R_exp)
+
+ xyz = data[i]['pose']['xyz']
+ T_exp = transformation @ xyz
+ T_exp = np.array(T_exp)
+ num_arr = np.c_[R_exp, T_exp[0]]
+ np.save(f'{data_name}/Pose_transformed/Orange/{p}.npy', num_arr) # save
+ elif data[i]['id'] == 6:
+ cont3 += 1
+ rpy = data[i]['pose']['rpy']
+ rot = convert2(rpy)
+ R_exp = transformation @ rot
+ R_exp = np.array(R_exp)
+
+ xyz = data[i]['pose']['xyz']
+ T_exp = transformation @ xyz
+ T_exp = np.array(T_exp)
+ num_arr = np.c_[R_exp, T_exp[0]]
+ np.save(f'{data_name}/Pose_transformed/Pear/{p}.npy', num_arr) # save
+ else:
+ continue
+ print(cont1, cont2, cont3)
+
--
GitLab