diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index b7da575a..960e6265 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -8,6 +8,7 @@ author: Atsushi Sakai (@Atsushi_twi) import numpy as np import math +import copy import matplotlib.pyplot as plt @@ -25,22 +26,53 @@ M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association. STATE_SIZE = 3 # State size [x,y,yaw] LM_SIZE = 2 # LM srate size [x,y] +MAX_ITR = 20 + show_animation = True -def graph_based_slam(xEst, PEst, u, z): +def graph_based_slam(xEst, PEst, u, z, hxDR, hz): - # Predict - S = STATE_SIZE - xEst[0:S] = motion_model(xEst[0:S], u) + x_opt = copy.deepcopy(hxDR) - # Update - for iz in range(len(z[:, 0])): # for each observation - pass + for itr in range(20): + # pos_edges = [] - xEst[2] = pi_2_pi(xEst[2]) + # # このfor文では、HalfEdgeからグラフの辺を作っていきます。 + # for i in range(len(actual_landmarks.positions)): # ランドマークごとにHalfEdgeからEdgeを作る + # es = list(filter(lambda e: e.landmark_id == i, obs_edges)) # 同じランドマークIDを持つHalfEdgeの抽出 + # ps = list(itertools.combinations(es,2)) # esの要素のペアを全通り作る + # for p in ps: + # pos_edges.append(Edge(p[0],p[1])) # エッジを登録 - return xEst, None + n = len(hz) * 3 + H = np.zeros((n, n)) + b = np.zeros((n, 1)) + + # for e in pos_edges: + # e.addInfo(matH,vecb) + + # H[0:3, 0:3] += np.identity(3) * 10000 # to fix origin + H += np.identity(n) * 10000 # to fix origin + + dx = - np.linalg.inv(H).dot(b) + # print(dx) + + for i in range(len(hz)): + x_opt[0, i] += dx[i * 3, 0] + x_opt[1, i] += dx[i * 3 + 1, 0] + x_opt[2, i] += dx[i * 3 + 2, 0] + + # # HalfEdgeに登録してある推定値も更新 + # for e in obs_edges: + # e.update(robot.guess_poses) + + diff = dx.T.dot(dx) + print("iteration: %d, diff: %f" % (itr + 1, diff)) + if dx[0, 0] < 1.0e-5: + break + + return x_opt, None def calc_input(): @@ -94,94 +126,6 @@ def motion_model(x, u): return x -def calc_n_LM(x): - n = int((len(x) - STATE_SIZE) / LM_SIZE) - return n - - -def jacob_motion(x, u): - - Fx = np.hstack((np.eye(STATE_SIZE), np.zeros( - (STATE_SIZE, LM_SIZE * calc_n_LM(x))))) - - jF = np.matrix([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])], - [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 - - return G, Fx, - - -def calc_LM_Pos(x, z): - - zp = np.zeros((2, 1)) - - zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1]) - zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1]) - - return zp - - -def get_LM_Pos_from_state(x, ind): - - lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :] - - return lm - - -def search_correspond_LM_ID(xAug, PAug, zi): - """ - Landmark association with Mahalanobis distance - """ - - nLM = calc_n_LM(xAug) - - mdist = [] - - for i in range(nLM): - lm = get_LM_Pos_from_state(xAug, i) - y, S, H = calc_innovation(lm, xAug, PAug, zi, i) - mdist.append(y.T * np.linalg.inv(S) * y) - - mdist.append(M_DIST_TH) # new landmark - - minid = mdist.index(min(mdist)) - - return minid - - -def calc_innovation(lm, xEst, PEst, z, LMid): - delta = lm - xEst[0:2] - q = (delta.T * delta)[0, 0] - zangle = math.atan2(delta[1], delta[0]) - xEst[2] - zp = [math.sqrt(q), pi_2_pi(zangle)] - y = (z - zp).T - y[1] = pi_2_pi(y[1]) - H = jacobH(q, delta, xEst, LMid + 1) - S = H * PEst * H.T + Cx[0:2, 0:2] - - return y, S, H - - -def jacobH(q, delta, x, i): - sq = math.sqrt(q) - G = np.matrix([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]], - [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]]) - - G = G / q - nLM = calc_n_LM(x) - F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM)))) - F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))), - np.eye(2), np.zeros((2, 2 * nLM - 2 * i)))) - - F = np.vstack((F1, F2)) - - H = G * F - - return H - - def pi_2_pi(angle): while(angle > math.pi): angle = angle - 2.0 * math.pi @@ -211,9 +155,9 @@ def main(): xDR = np.matrix(np.zeros((STATE_SIZE, 1))) # Dead reckoning # history - hxEst = xEst hxTrue = xTrue hxDR = xTrue + hz = [] while SIM_TIME >= time: time += DT @@ -221,13 +165,12 @@ def main(): xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) - xEst, PEst = graph_based_slam(xEst, PEst, ud, z) + hxDR = np.hstack((hxDR, xDR)) + hz.append(z) - x_state = xEst[0:STATE_SIZE] + x_opt, PEst = graph_based_slam(xEst, PEst, ud, z, hxDR, hz) # store data history - hxEst = np.hstack((hxEst, x_state)) - hxDR = np.hstack((hxDR, xDR)) hxTrue = np.hstack((hxTrue, xTrue)) if show_animation: @@ -240,8 +183,8 @@ def main(): np.array(hxTrue[1, :]).flatten(), "-b") plt.plot(np.array(hxDR[0, :]).flatten(), np.array(hxDR[1, :]).flatten(), "-k") - plt.plot(np.array(hxEst[0, :]).flatten(), - np.array(hxEst[1, :]).flatten(), "-r") + plt.plot(np.array(x_opt[0, :]).flatten(), + np.array(x_opt[1, :]).flatten(), "-r") plt.axis("equal") plt.grid(True) plt.pause(0.001)