diff --git a/Mapping/kmeans_clustering/kmeans_clustering.py b/Mapping/kmeans_clustering/kmeans_clustering.py index 7900e304..53458130 100644 --- a/Mapping/kmeans_clustering/kmeans_clustering.py +++ b/Mapping/kmeans_clustering/kmeans_clustering.py @@ -6,97 +6,87 @@ author: Atsushi Sakai (@Atsushi_twi) """ -import numpy as np import math import matplotlib.pyplot as plt import random +# k means parameters +MAX_LOOP = 10 +DCOST_TH = 0.1 show_animation = True +def kmeans_clustering(rx, ry, nc): + clusters = Clusters(rx, ry, nc) + clusters.calc_centroid() + + pre_cost = float("inf") + for loop in range(MAX_LOOP): + print("loop:", loop) + cost = clusters.update_clusters() + clusters.calc_centroid() + + d_cost = abs(cost - pre_cost) + if d_cost < DCOST_TH: + break + pre_cost = cost + + return clusters + + class Clusters: - def __init__(self, x, y, nlabel): + def __init__(self, x, y, n_label): self.x = x self.y = y - self.ndata = len(self.x) - self.nlabel = nlabel - self.labels = [random.randint(0, nlabel - 1) - for _ in range(self.ndata)] - self.cx = [0.0 for _ in range(nlabel)] - self.cy = [0.0 for _ in range(nlabel)] + self.n_data = len(self.x) + self.n_label = n_label + self.labels = [random.randint(0, n_label - 1) + for _ in range(self.n_data)] + self.center_x = [0.0 for _ in range(n_label)] + self.center_y = [0.0 for _ in range(n_label)] + + def plot_cluster(self): + for label in set(self.labels): + x, y = self._get_labeled_x_y(label) + plt.plot(x, y, ".") + + def calc_centroid(self): + for label in set(self.labels): + x, y = self._get_labeled_x_y(label) + n_data = len(x) + self.center_x[label] = sum(x) / n_data + self.center_y[label] = sum(y) / n_data + + def update_clusters(self): + cost = 0.0 + + for ip in range(self.n_data): + px = self.x[ip] + py = self.y[ip] + + dx = [icx - px for icx in self.center_x] + dy = [icy - py for icy in self.center_y] + + dist_list = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] + min_dist = min(dist_list) + min_id = dist_list.index(min_dist) + self.labels[ip] = min_id + cost += min_dist + + return cost + + def _get_labeled_x_y(self, target_label): + x = [self.x[i] for i, label in enumerate(self.labels) if label == target_label] + y = [self.y[i] for i, label in enumerate(self.labels) if label == target_label] + return x, y -def kmeans_clustering(rx, ry, nc): - - clusters = Clusters(rx, ry, nc) - clusters = calc_centroid(clusters) - - MAX_LOOP = 10 - DCOST_TH = 0.1 - pcost = 100.0 - for loop in range(MAX_LOOP): - # print("Loop:", loop) - clusters, cost = update_clusters(clusters) - clusters = calc_centroid(clusters) - - dcost = abs(cost - pcost) - if dcost < DCOST_TH: - break - pcost = cost - - return clusters - - -def calc_centroid(clusters): - - for ic in range(clusters.nlabel): - x, y = calc_labeled_points(ic, clusters) - ndata = len(x) - clusters.cx[ic] = sum(x) / ndata - clusters.cy[ic] = sum(y) / ndata - - return clusters - - -def update_clusters(clusters): - cost = 0.0 - - for ip in range(clusters.ndata): - px = clusters.x[ip] - py = clusters.y[ip] - - dx = [icx - px for icx in clusters.cx] - dy = [icy - py for icy in clusters.cy] - - dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)] - mind = min(dlist) - min_id = dlist.index(mind) - clusters.labels[ip] = min_id - cost += mind - - return clusters, cost - - -def calc_labeled_points(ic, clusters): - - inds = np.array([i for i in range(clusters.ndata) - if clusters.labels[i] == ic]) - tx = np.array(clusters.x) - ty = np.array(clusters.y) - - x = tx[inds] - y = ty[inds] - - return x, y - - -def calc_raw_data(cx, cy, npoints, rand_d): - +def calc_raw_data(cx, cy, n_points, rand_d): rx, ry = [], [] for (icx, icy) in zip(cx, cy): - for _ in range(npoints): + for _ in range(n_points): rx.append(icx + rand_d * (random.random() - 0.5)) ry.append(icy + rand_d * (random.random() - 0.5)) @@ -104,48 +94,28 @@ def calc_raw_data(cx, cy, npoints, rand_d): def update_positions(cx, cy): - + # object moving parameters DX1 = 0.4 DY1 = 0.5 - - cx[0] += DX1 - cy[0] += DY1 - DX2 = -0.3 DY2 = -0.5 + cx[0] += DX1 + cy[0] += DY1 cx[1] += DX2 cy[1] += DY2 return cx, cy -def calc_association(cx, cy, clusters): - - inds = [] - - for ic, _ in enumerate(cx): - tcx = cx[ic] - tcy = cy[ic] - - dx = [icx - tcx for icx in clusters.cx] - dy = [icy - tcy for icy in clusters.cy] - - dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)] - min_id = dlist.index(min(dlist)) - inds.append(min_id) - - return inds - - def main(): print(__file__ + " start!!") cx = [0.0, 8.0] cy = [0.0, 8.0] - npoints = 10 + n_points = 10 rand_d = 3.0 - ncluster = 2 + n_cluster = 2 sim_time = 15.0 dt = 1.0 time = 0.0 @@ -154,22 +124,19 @@ def main(): print("Time:", time) time += dt - # simulate objects + # objects moving simulation cx, cy = update_positions(cx, cy) - rx, ry = calc_raw_data(cx, cy, npoints, rand_d) + raw_x, raw_y = calc_raw_data(cx, cy, n_points, rand_d) - clusters = kmeans_clustering(rx, ry, ncluster) + clusters = kmeans_clustering(raw_x, raw_y, n_cluster) # for animation if show_animation: # pragma: no cover plt.cla() plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) - inds = calc_association(cx, cy, clusters) - for ic in inds: - x, y = calc_labeled_points(ic, clusters) - plt.plot(x, y, "x") - plt.plot(cx, cy, "o") + clusters.plot_cluster() + plt.plot(cx, cy, "or") plt.xlim(-2.0, 10.0) plt.ylim(-2.0, 10.0) plt.pause(dt) diff --git a/SLAM/EKFSLAM/ekf_slam.ipynb b/SLAM/EKFSLAM/ekf_slam.ipynb index f8009395..ccf5e5bb 100644 --- a/SLAM/EKFSLAM/ekf_slam.ipynb +++ b/SLAM/EKFSLAM/ekf_slam.ipynb @@ -309,7 +309,7 @@ " G, Fx = jacob_motion(xEst[0:S], u)\n", " # Fx is an an identity matrix of size (STATE_SIZE)\n", " # sigma = G*sigma*G.T + Noise\n", - " PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx\n", + " PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx\n", " return xEst, PEst, G, Fx" ] }, @@ -584,7 +584,7 @@ " [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],\n", " [0.0, 0.0, 0.0]])\n", "\n", - " G = np.eye(STATE_SIZE) + Fx.T * jF * Fx\n", + " G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx\n", " if calc_n_LM(x) > 0:\n", " print(Fx.shape)\n", " return G, Fx,\n", diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 33a9b1dd..b6698be2 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -31,7 +31,7 @@ def ekf_slam(xEst, PEst, u, z): S = STATE_SIZE xEst[0:S] = motion_model(xEst[0:S], u) G, Fx = jacob_motion(xEst[0:S], u) - PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx + PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx initP = np.eye(2) # Update @@ -119,7 +119,7 @@ def jacob_motion(x, u): [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])], [0.0, 0.0, 0.0]]) - G = np.eye(STATE_SIZE) + Fx.T * jF * Fx + G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx return G, Fx,