just want to test in-car

opendbc/car/ford/radar_interface.py

@@ -27,6 +27,63 @@
 
 cmap = plt.cm.get_cmap('tab20', 20)  # 'tab20' colormap with 20 colors
 
+
+class HierarchicalClustering:
+  def __init__(self, threshold):
+    self.threshold = threshold
+
+  def fit_predict(self, data):
+    """
+    Clusters the data based on the closeness threshold.
+
+    Parameters:
+    - data: List of points, where each point is a list of coordinates.
+
+    Returns:
+    - labels: List of cluster labels corresponding to each point.
+    """
+    n = len(data)
+    # Initialize each point to be in its own cluster
+    parent = [i for i in range(n)]  # Union-Find parent pointers
+
+    def find(i):
+      # Find the root of the cluster containing point i
+      while parent[i] != i:
+        parent[i] = parent[parent[i]]  # Path compression
+        i = parent[i]
+      return i
+
+    def union(i, j):
+      # Merge the clusters containing points i and j
+      pi, pj = find(i), find(j)
+      if pi != pj:
+        parent[pj] = pi  # Merge cluster pj into cluster pi
+
+    # Merge clusters where the distance between points is within the threshold
+    for i in range(n):
+      for j in range(i + 1, n):
+        dist = self.euclidean_distance(data[i], data[j])
+        if dist <= self.threshold:
+          union(i, j)
+
+    # Assign labels based on cluster representatives
+    clusters = {}
+    labels = [-1] * n
+    label = 0
+    for i in range(n):
+      root = find(i)
+      if root not in clusters:
+        clusters[root] = label
+        label += 1
+      labels[i] = clusters[root]
+
+    return labels
+
+  def euclidean_distance(self, point1, point2):
+    # Calculates the Euclidean distance between two points
+    return sum((a - b) ** 2 for a, b in zip(point1, point2)) ** 0.5
+
+
 class Cluster:
   def __init__(self, pts: list[structs.RadarData.RadarPoint], cluster_id: int):
     self.pts = pts
@@ -82,6 +139,8 @@ def __init__(self, CP):
     # TODO: write simple cluster function
     self.dbscan = DBSCAN(eps=5, min_samples=1)
 
+    self.clustering = HierarchicalClustering(threshold=5)
+
     self.fig, self.ax = plt.subplots()
 
     self.temp_pts = {}
@@ -227,7 +286,8 @@ def _update_delphi_mrr(self):
 
     temp_points_list = list(self.temp_pts.values())
     keys = [[p.dRel, p.yRel, p.vRel] for p in temp_points_list]
-    labels = self.dbscan.fit_predict(keys)
+    # labels = self.dbscan.fit_predict(keys)
+    labels = self.clustering.fit_predict(keys)
     clusters = [[] for _ in range(max(labels) + 1)]
 
     for i, label in enumerate(labels):
@@ -240,84 +300,84 @@ def _update_delphi_mrr(self):
 
     taken_clusters: set[int] = set()
 
-    if len(temp_points_list) == 60:
-      print('clusters', len(clusters))
-      print('len points', len(temp_points_list))
-      print('sum of clusters', sum([len(c) for c in clusters]))
-      print()
-
-      print('self.clusters')
-      for c in self.clusters:
-        print((c.cluster_id, c.dRel, c.yRel, c.vRel, [p.to_dict() for p in c.pts]))
-      # print('----')
-      # print('clusters', [[p.to_dict() for p in c] for c in clusters])
-      print()
+    # if len(temp_points_list) == 60:
+    #   print('clusters', len(clusters))
+    #   print('len points', len(temp_points_list))
+    #   print('sum of clusters', sum([len(c) for c in clusters]))
+    #   print()
+    #
+    #   print('self.clusters')
+    #   for c in self.clusters:
+    #     print((c.cluster_id, c.dRel, c.yRel, c.vRel, [p.to_dict() for p in c.pts]))
+    #   # print('----')
+    #   # print('clusters', [[p.to_dict() for p in c] for c in clusters])
+    #   print()
 
     new_clusters = []
 
-    print('prev clusters', [(c.dRel, c.yRel, c.vRel) for c in self.clusters])
+    # print('prev clusters', [(c.dRel, c.yRel, c.vRel) for c in self.clusters])
 
     for cluster in clusters:  # TODO: make clusters a list of Cluster objects
       dRel = float(np.mean([p.dRel for p in cluster]))
       yRel = float(np.mean([p.yRel for p in cluster]))
       vRel = float(np.mean([p.vRel for p in cluster]))
-      print()
-      print('working on cluster', (dRel, yRel, vRel))
+      # print()
+      # print('working on cluster', (dRel, yRel, vRel))
 
       closest_previous_cluster = None
       closest_euclidean_dist = None
-      print('searching! ...')
+      # print('searching! ...')
       for idx, c in enumerate(self.clusters):
         # TODO: need to re-enable this
         # TODO: some clusters might not match optimally with this, but maybe rare enough?
         if c.cluster_id in taken_clusters:
           continue
 
         # if this new cluster is close to any previous ones, use its previous cluster id with the new points and mark the old cluster as used
-        print('comparing with prev cluster', (c.dRel, c.yRel, c.vRel))
+        # print('comparing with prev cluster', (c.dRel, c.yRel, c.vRel))
         euclidean_dist = np.sqrt((c.dRel - dRel) ** 2 + (c.yRel - yRel) ** 2 + (c.vRel - vRel) ** 2)
-        print('got', euclidean_dist)
+        # print('got', euclidean_dist)
         # print(abs(c.dRel - dRel), abs(c.yRel - yRel), euclidean_dist)
         # if abs(c.dRel - dRel) < 5 and abs(c.yRel - yRel) < 5:# and abs(c.vRel - vRel) < 5:
         if euclidean_dist < 5:# and abs(c.vRel - vRel) < 5:
           if closest_previous_cluster is None or euclidean_dist < closest_euclidean_dist:
-            print('new low!')
+            # print('new low!')
             closest_previous_cluster = c
             closest_euclidean_dist = euclidean_dist
           # new_clusters.append(Cluster(cluster, c.cluster_id))
           # taken_clusters.add(idx)
           # break
 
-      print()
+      # print()
       if closest_previous_cluster is not None:
-        print('settled on', closest_euclidean_dist, (closest_previous_cluster.dRel, closest_previous_cluster.yRel, closest_previous_cluster.vRel))
+        # print('settled on', closest_euclidean_dist, (closest_previous_cluster.dRel, closest_previous_cluster.yRel, closest_previous_cluster.vRel))
         # TODO: anything better than deepcopy?
         new_clusters.append(Cluster(copy.deepcopy(cluster), closest_previous_cluster.cluster_id))
         taken_clusters.add(closest_previous_cluster.cluster_id)
         # print('new!', self.cluster_id)
       else:
-        print('making cluster', self.cluster_id, (dRel, yRel, vRel))
+        # print('making cluster', self.cluster_id, (dRel, yRel, vRel))
         new_clusters.append(Cluster(copy.deepcopy(cluster), self.cluster_id))
-        print(new_clusters[-1].dRel, new_clusters[-1].yRel, new_clusters[-1].vRel)
+        # print(new_clusters[-1].dRel, new_clusters[-1].yRel, new_clusters[-1].vRel)
         self.cluster_id += 1
 
-        if len(temp_points_list) == 60:
-          print('new cluster', (new_clusters[-1].cluster_id, new_clusters[-1].dRel, new_clusters[-1].yRel, new_clusters[-1].vRel, [p.to_dict() for p in new_clusters[-1].pts]))
+        # if len(temp_points_list) == 60:
+        #   print('new cluster', (new_clusters[-1].cluster_id, new_clusters[-1].dRel, new_clusters[-1].yRel, new_clusters[-1].vRel, [p.to_dict() for p in new_clusters[-1].pts]))
 
     self.clusters = new_clusters
 
-    print('post clusters', [(c.dRel, c.yRel, c.vRel) for c in self.clusters])
-
-    if len(temp_points_list) == 60:
-    #   print('new self.clusters')
-    #   for c in self.clusters:
-    #     print((c.cluster_id, c.dRel, c.yRel, c.vRel, [p.to_dict() for p in c.pts]))
-      print()
+    # print('post clusters', [(c.dRel, c.yRel, c.vRel) for c in self.clusters])
 
-    # print('clusters', clusters)
-    # print('new_clusters', new_clusters)
-    # print('track_id', self.track_id)
-    # print('cluster_id', self.cluster_id)
+    # if len(temp_points_list) == 60:
+    # #   print('new self.clusters')
+    # #   for c in self.clusters:
+    # #     print((c.cluster_id, c.dRel, c.yRel, c.vRel, [p.to_dict() for p in c.pts]))
+    #   print()
+    #
+    # # print('clusters', clusters)
+    # # print('new_clusters', new_clusters)
+    # # print('track_id', self.track_id)
+    # # print('cluster_id', self.cluster_id)
 
     if PLOT := False:
       self.ax.clear()