clustering using yaw data

main.py

@@ -4,13 +4,14 @@ from collections import namedtuple
 import numpy as np
 import matplotlib.pyplot as plt
 from itertools import groupby
+from math import pi, sin, cos
 
 import csv_parser
 import recog
 import metric
 
 if __name__ == '__main__':
-    data = csv_parser.parse_data_from_csv('test0714.csv')
+    data = csv_parser.parse_data_from_csv('pitchtest0730.csv')
     groups = groupby(data, key = lambda x: x.hwid)
     entries = []
     for k, grp in groups:
@@ -21,9 +22,13 @@ if __name__ == '__main__':
     for ent in entries:
         print(ent)
     x = []
+    yaws = []
     for e in entries:
         x.append([e.lon, e.lat])
+        yaws.append(e.yaw / 180 * pi)
+    print(yaws)
     x = np.array(x)
+    yaws = np.array(yaws)
     db = DBSCAN(eps = 10/6400000, min_samples = 10,
             metric = metric.spherical_distance).fit(x)
     labels = db.labels_
@@ -60,8 +65,8 @@ if __name__ == '__main__':
         plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col),
                          markeredgecolor='k', markersize=6)
 
+        xy = x[class_member_mask]
         if k != -1:
-            xy = x[class_member_mask]
             print("cluster %d:" % k)
             center = np.array([np.average(xy[:, 0]), np.average(xy[:, 1])])
             print("center = %f, %f" % (center[0], center[1]))
@@ -72,4 +77,34 @@ if __name__ == '__main__':
                 rad = max(rad, metric.spherical_distance(center_p, p))
             rad *= 6400 * 1000
             print("radius = %f m" % rad)
+
+        yaw = yaws[class_member_mask]
+        db_yaw = DBSCAN(eps = 0.05, min_samples=100,
+                metric = metric.ang_distance).fit(yaw.reshape(-1, 1))
+        lbs = set(db_yaw.labels_)
+        arrow_colors = [plt.cm.Spectral(each)
+                          for each in np.linspace(0, 1, len(lbs))]
+        for l in lbs:
+            if l != -1:
+                print("  sub-cluster %d: avg yaw = %f" %
+                        (l, 180 / pi * np.average(yaw[(db_yaw.labels_ == l)])))
+
+        kwargs = {'width': 1e-5}
+        for i in range(len(xy)):
+            col = arrow_colors[db_yaw.labels_[i]]
+            if db_yaw.labels_[i] == -1:
+                continue
+                col = [0, 0, 0, 1]
+            '''
+            plt.arrow(xy[i, 0], xy[i, 1],
+                    5e-5 * cos(pi/2 - yaw[i]),
+                    5e-5 * sin(pi/2 - yaw[i]),
+                    **dict(width=1e-7, color=col))
+            '''
+            plt.annotate("", xytext=(xy[i, 0], xy[i, 1]), 
+                    xy=(xy[i, 0] + 2e-5 * sin(pi/2 - yaw[i]),
+                        xy[i, 1] + 2e-5 * cos(pi/2 - yaw[i])),
+                    arrowprops=dict(arrowstyle="->",
+                        color=col))
+
     plt.show()