Filter more data

data/filter_data.py

 import pickle
 
 import numpy as np
+from numpy.lib.function_base import diff
+import matplotlib.pyplot as plt
 
 # Constants for Stanley controller for GEM
 WHEEL_BASE = 1.75  # m
@@ -19,7 +21,7 @@ def g(cte, phi):
 def f(x, y, theta, steer):
     new_x = x + FORWARD_VEL*np.cos(theta+steer)*CYCLE_TIME
     new_y = y + FORWARD_VEL*np.sin(theta+steer)*CYCLE_TIME
-    new_theta = theta - FORWARD_VEL*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
+    new_theta = theta + FORWARD_VEL*np.sin(steer)/WHEEL_BASE*CYCLE_TIME
     return new_x, new_y, new_theta
 
 
@@ -27,9 +29,21 @@ def v(x, y, theta) -> float:
     return y**2 + theta**2
 
 
+def in_circle(x, y, theta, d, phi) -> bool:
+    a_mat = np.array([[0.0, -1.0, 0.0],
+                      [0.0, 0.0, -1.0]])
+    b_vec = np.zeros(2)
+    state_vec = np.array([x, y, theta])
+    perc_vec = np.array([d, phi])
+
+    diff_vec = perc_vec - (np.dot(state_vec, a_mat.T) + b_vec)
+    return np.dot(diff_vec, diff_vec) <= 0.03
+
+
 def pred(sample) -> bool:
     x, y, theta, d, phi = sample
-    return v(*f(x, y, theta, *g(d, phi))) <= v(x, y, theta)
+    return v(*f(x, y, theta, *g(d, phi))) <= v(x, y, theta) \
+        and in_circle(x, y, theta, d, phi)
 
 
 in_file_name = "collect_images_2021-11-22-17-59-46.cs598.pickle"
@@ -45,12 +59,24 @@ filtered_truth_samples = []
 for truth, samples in truth_samples_seq:
     cte, phi = truth
     filtered_samples = [s for s in samples if pred(s)]
-    if abs(len(samples) - len(filtered_samples)) < 20:
+    if abs(len(samples) - len(filtered_samples)) < 200 and \
+            abs(cte - 0.967823751) <= 0.001 and abs(phi - -0.211146388) <= 0.001:
         print("#Original:", len(samples), "#Filtered:",  len(filtered_samples))
-        filtered_truth_samples.append((truth, samples))
+        filtered_truth_samples.append((truth, filtered_samples))
     else:
+        # print("Skip partition. #Original:", len(samples), "#Filtered:",  len(filtered_samples))
         continue
 
+center = filtered_truth_samples[0][0]
+print("Representative point:", center)
+x_arr, y_arr = np.array(filtered_truth_samples[0][1])[:, 3:5].T
+ax = plt.gca()
+ax.set_aspect("equal")
+ax.add_patch(plt.Circle(center, np.sqrt(0.03), color='g', alpha=0.3))
+plt.scatter(x_arr, y_arr)
+plt.plot(*center, 'go')
+plt.savefig("temp.png")
+
 with open(out_file_name, "wb") as out_file:
     pkl_data["truth_samples"] = filtered_truth_samples
     pickle.dump(pkl_data, out_file)