Attempt Coordinate Transform

- Renamed some variables to indicate that they are a list
- Fixed some typos
- Close file after opening
- Attempt implementation of robot to global frame transformation
- Add conditions to skip some point clouds when creating full point cloud

src/visualization/point_cloud_vis.py

@@ -10,16 +10,19 @@ import struct
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
-class GTPose:
-    def __init__(self, time, x, y, z, r, p, h):
-        self.time = time
-        self.x = x
-        self.y = y
-        self.z = z
-        self.r = r
-        self.p = p
-        self.h = h
+
+class GTPoses:
+    def __init__(self, time_list, x_list, y_list, z_list, r_list, p_list, h_list):
+        self.time_list = time_list
+        self.x_list = x_list
+        self.y_list = y_list
+        self.z_list = z_list
+        self.r_list = r_list
+        self.p_list = p_list
+        self.h_list = h_list
+        self.length = len(time_list)
 
 class PointCloud:
     def __init__(self, time):
@@ -27,26 +30,27 @@ class PointCloud:
         self.x_list = []
         self.y_list = []
         self.z_list = []
+        self.length = 0
     
     def add_point(self, x, y, z):
-        self.time += time
-        self.x_list += x
-        self.y_list += y
-        self.z_list += z
+        self.x_list += [x]
+        self.y_list += [y]
+        self.z_list += [z]
+        self.length += 1
 
 
 def read_gt(file):
-    gt = np.loadtxt(file, delimeter=",")
+    gt = np.loadtxt(file, delimiter=",")
     
-    time = gt[:, 0]
-    x = gt[:, 1]
-    y = gt[:, 2]
-    z = gt[:, 3]
-    r = gt[:, 4]
-    p = gt[:, 5]
-    h = gt[:, 6]
+    time_list = list(gt[:, 0])
+    x_list = gt[:, 1]
+    y_list = gt[:, 2]
+    z_list = gt[:, 3]
+    r_list = gt[:, 4]
+    p_list = gt[:, 5]
+    h_list = gt[:, 6]
 
-    return GTPose(time, x, y, z, r, p, h)
+    return GTPoses(time_list, x_list, y_list, z_list, r_list, p_list, h_list)
 
 
 def convert(x_s, y_s, z_s):
@@ -61,7 +65,7 @@ def convert(x_s, y_s, z_s):
 
 
 def read_vel(file):
-    time = os.path.splittext(os.path.basename(file))[0]
+    time = os.path.splitext(os.path.basename(file))[0]
     pc = PointCloud(time)
     f_bin = open(file, "rb")
     
@@ -81,41 +85,82 @@ def read_vel(file):
         x, y, z = convert(x, y, z)
         pc.add_point(x, y, -z)
     
+    f_bin.close()
     return pc
 
 
 def r_to_g_frame(gt, pc):
-    # TODO: Interpolate gt to find corresponding pose for pc
+    pc_global = PointCloud(pc.time)
+    
+    # Interpolate gt to find corresponding pose for pc
+    t_x = np.interp(x=pc.time, xp=gt.time_list, fp=gt.x_list)
+    t_y = np.interp(x=pc.time, xp=gt.time_list, fp=gt.y_list)
+    t_z = np.interp(x=pc.time, xp=gt.time_list, fp=gt.z_list)
+    R_r = np.interp(x=pc.time, xp=gt.time_list, fp=gt.r_list)
+    R_p = np.interp(x=pc.time, xp=gt.time_list, fp=gt.p_list)
+    R_h = np.interp(x=pc.time, xp=gt.time_list, fp=gt.h_list)
 
-    # TODO: Transform pc from robot frame to global frame
-    pass
+    # Transform pc from robot frame to global frame
+    r = (R.from_euler('xyz', [R_r, R_p, R_h], degrees=False)).as_matrix()
+    p = [t_x, t_y, t_z]
+    n = [r[0,0], r[1,0], r[2,0]]
+    o = [r[0,1], r[1,1], r[2,1]]
+    a = [r[0,2], r[1,2], r[2,2]]
+
+    T = np.matrix([[n[0], o[0], a[0], p[0]],
+                   [n[1], o[1], a[1], p[1]],
+                   [n[2], o[2], a[2], p[2]],
+                   [0, 0, 0, 1]])
+    # T = np.matrix([[n[0], n[1], n[2], -np.dot(p, n)],
+    #                [o[0], o[1], o[2], -np.dot(p, o)],
+    #                [a[0], a[1], a[2], -np.dot(p, a)],
+    #                [0, 0, 0, 1]])
+    
+    for i in range(pc.length):
+        point_local = np.matrix([[pc.x_list[i]],
+                                 [pc.y_list[i]],
+                                 [pc.z_list[i]],
+                                 [1]])
+        point_global = T * point_local
+        pc_global.add_point(point_global[0], point_global[1], point_global[2])
+
+    return pc_global
 
 
 def main(args):
-    if len(sys.argv) != 2:
-        print("Expecting 2 arguments: ground truth filepath and data folder")
+    if len(sys.argv) != 3:
+        print("Expecting 3 arguments: python point_cloud_vis.py [ground truth filepath] [velodyne sync folder]")
         return 1
     
-    ground_truth_file = sys.arv[1]
+    ground_truth_file = sys.argv[1]
     data_path = sys.argv[2]
 
-    x = []
-    y = []
-    z = []
+    x_list = []
+    y_list = []
+    z_list = []
     gt = read_gt(ground_truth_file)
     
     fig = plt.figure()
     ax = fig.add_subplot(111, projection='3d')
 
+    count = -1
+
     for filename in os.listdir(data_path):
-        pc = read_vel(ground_truth_file, data_path + '/' + filename)
+        count += 1
+
+        if count == 50:
+            break
+        elif count % 5 != 0:
+            continue
+
+        pc = read_vel(data_path + '/' + filename)
         pc = r_to_g_frame(gt, pc)
         
-        x += pc.x_list
-        y += pc.y_list
-        z += pc.z_list
+        x_list += pc.x_list
+        y_list += pc.y_list
+        z_list += pc.z_list
 
-    ax.scatter(x, y, z, c=z, s=5, linewidths=0)
+    ax.scatter(x_list, y_list, z_list, c=z_list, s=5, linewidths=0)
     plt.show()
 
     return 0