diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index d69a2f8392..8f1d99760f 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -50,9 +50,9 @@ def ekf_slam(xEst, PEst, u, z): lm = get_LM_Pos_from_state(xEst, minid) y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid) - K = PEst.dot(H.T).dot(np.linalg.inv(S)) - xEst = xEst + K.dot(y) - PEst = (np.eye(len(xEst)) - K.dot(H)).dot(PEst) + K = (PEst @ H.T) @ np.linalg.inv(S) + xEst = xEst + (K @ y) + PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst xEst[2] = pi_2_pi(xEst[2]) @@ -104,7 +104,7 @@ def motion_model(x, u): [DT * math.sin(x[2, 0]), 0], [0.0, DT]]) - x = F.dot(x) + B .dot(u) + x = (F @ x) + (B @ u) return x @@ -157,7 +157,7 @@ def search_correspond_LM_ID(xAug, PAug, zi): 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.dot(np.linalg.inv(S)).dot(y)) + mdist.append(y.T @ np.linalg.inv(S) @ y) mdist.append(M_DIST_TH) # new landmark @@ -168,14 +168,14 @@ def search_correspond_LM_ID(xAug, PAug, zi): def calc_innovation(lm, xEst, PEst, z, LMid): delta = lm - xEst[0:2] - q = (delta.T.dot(delta))[0, 0] + q = (delta.T @ delta)[0, 0] #zangle = math.atan2(delta[1], delta[0]) - xEst[2] zangle = math.atan2(delta[1,0], delta[0,0]) - xEst[2] zp = np.array([[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.dot(PEst).dot(H.T) + Cx[0:2, 0:2] + S = H @ PEst @ H.T + Cx[0:2, 0:2] return y, S, H @@ -193,7 +193,7 @@ def jacobH(q, delta, x, i): F = np.vstack((F1, F2)) - H = G.dot(F) + H = G @ F return H diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index a8776c6e33..9fe027c805 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -64,7 +64,7 @@ def cal_observation_sigma(d): def calc_rotational_matrix(angle): - Rt = np.matrix([[math.cos(angle), -math.sin(angle), 0], + Rt = np.array([[math.cos(angle), -math.sin(angle), 0], [math.sin(angle), math.cos(angle), 0], [0, 0, 1.0]]) return Rt @@ -92,7 +92,7 @@ def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1, sig1 = cal_observation_sigma(d1) sig2 = cal_observation_sigma(d2) - edge.omega = np.linalg.inv(Rt1 * sig1 * Rt1.T + Rt2 * sig2 * Rt2.T) + edge.omega = np.linalg.inv(Rt1 @ sig1 @ Rt1.T + Rt2 @ sig2 @ Rt2.T) edge.d1, edge.d2 = d1, d2 edge.yaw1, edge.yaw2 = yaw1, yaw2 @@ -127,7 +127,7 @@ def calc_edges(xlist, zlist): angle1, phi1, d2, angle2, phi2, t1, t2) edges.append(edge) - cost += (edge.e.T * edge.omega * edge.e)[0, 0] + cost += (edge.e.T @ (edge.omega) @ edge.e)[0, 0] print("cost:", cost, ",nedge:", len(edges)) return edges @@ -135,12 +135,12 @@ def calc_edges(xlist, zlist): def calc_jacobian(edge): t1 = edge.yaw1 + edge.angle1 - A = np.matrix([[-1.0, 0, edge.d1 * math.sin(t1)], + A = np.array([[-1.0, 0, edge.d1 * math.sin(t1)], [0, -1.0, -edge.d1 * math.cos(t1)], [0, 0, -1.0]]) t2 = edge.yaw2 + edge.angle2 - B = np.matrix([[1.0, 0, -edge.d2 * math.sin(t2)], + B = np.array([[1.0, 0, -edge.d2 * math.sin(t2)], [0, 1.0, edge.d2 * math.cos(t2)], [0, 0, 1.0]]) @@ -154,13 +154,13 @@ def fill_H_and_b(H, b, edge): id1 = edge.id1 * STATE_SIZE id2 = edge.id2 * STATE_SIZE - H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T * edge.omega * A - H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T * edge.omega * B - H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T * edge.omega * A - H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T * edge.omega * B + H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ A + H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ B + H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ A + H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B - b[id1:id1 + STATE_SIZE, 0] += (A.T * edge.omega * edge.e) - b[id2:id2 + STATE_SIZE, 0] += (B.T * edge.omega * edge.e) + b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e) + b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e) return H, b @@ -178,8 +178,8 @@ def graph_based_slam(x_init, hz): for itr in range(MAX_ITR): edges = calc_edges(x_opt, zlist) - H = np.matrix(np.zeros((n, n))) - b = np.matrix(np.zeros((n, 1))) + H = np.zeros((n, n)) + b = np.zeros((n, 1)) for edge in edges: H, b = fill_H_and_b(H, b, edge) @@ -187,12 +187,12 @@ def graph_based_slam(x_init, hz): # to fix origin H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE) - dx = - np.linalg.inv(H).dot(b) + dx = - np.linalg.inv(H) @ b for i in range(nt): - x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0] + x_opt[0:3, i] += dx[i * 3:i * 3 + 3,0] - diff = dx.T.dot(dx) + diff = dx.T @ dx print("iteration: %d, diff: %f" % (itr + 1, diff)) if diff < 1.0e-5: break @@ -203,7 +203,7 @@ def graph_based_slam(x_init, hz): def calc_input(): v = 1.0 # [m/s] yawrate = 0.1 # [rad/s] - u = np.matrix([v, yawrate]).T + u = np.array([[v, yawrate]]).T return u @@ -212,7 +212,7 @@ def observation(xTrue, xd, u, RFID): xTrue = motion_model(xTrue, u) # add noise to gps x-y - z = np.matrix(np.zeros((0, 4))) + z = np.zeros((0, 4)) for i in range(len(RFID[:, 0])): @@ -224,13 +224,13 @@ def observation(xTrue, xd, u, RFID): if d <= MAX_RANGE: dn = d + np.random.randn() * Qsim[0, 0] # add noise anglen = angle + np.random.randn() * Qsim[1, 1] # add noise - zi = np.matrix([dn, anglen, phi, i]) + zi = np.array([dn, anglen, phi, i]) z = np.vstack((z, zi)) # add noise to input ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] - ud = np.matrix([ud1, ud2]).T + ud = np.array([[ud1, ud2]]).T xd = motion_model(xd, ud) @@ -239,15 +239,15 @@ def observation(xTrue, xd, u, RFID): def motion_model(x, u): - F = np.matrix([[1.0, 0, 0], + F = np.array([[1.0, 0, 0], [0, 1.0, 0], [0, 0, 1.0]]) - B = np.matrix([[DT * math.cos(x[2, 0]), 0], + B = np.array([[DT * math.cos(x[2, 0]), 0], [DT * math.sin(x[2, 0]), 0], [0.0, DT]]) - x = F * x + B * u + x = F @ x + B @ u return x @@ -270,8 +270,8 @@ def main(): ]) # State Vector [x y yaw v]' - xTrue = np.matrix(np.zeros((STATE_SIZE, 1))) - xDR = np.matrix(np.zeros((STATE_SIZE, 1))) # Dead reckoning + xTrue = np.zeros((STATE_SIZE, 1)) + xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning # history hxTrue = xTrue @@ -299,12 +299,12 @@ def main(): plt.plot(RFID[:, 0], RFID[:, 1], "*k") - plt.plot(np.array(hxTrue[0, :]).flatten(), - np.array(hxTrue[1, :]).flatten(), "-b") - plt.plot(np.array(hxDR[0, :]).flatten(), - np.array(hxDR[1, :]).flatten(), "-k") - plt.plot(np.array(x_opt[0, :]).flatten(), - np.array(x_opt[1, :]).flatten(), "-r") + plt.plot(hxTrue[0, :].flatten(), + hxTrue[1, :].flatten(), "-b") + plt.plot(hxDR[0, :].flatten(), + hxDR[1, :].flatten(), "-k") + plt.plot(x_opt[0, :].flatten(), + x_opt[1, :].flatten(), "-r") plt.axis("equal") plt.grid(True) plt.title("Time" + str(time)[0:5]) @@ -312,4 +312,4 @@ def main(): if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index daaeab2c6e..18bfb72365 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -45,7 +45,7 @@ def ICP_matching(ppoints, cpoints): Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) # update current points - cpoints = (Rt.dot(cpoints)) + Tt[:,np.newaxis] + cpoints = (Rt @ cpoints) + Tt[:,np.newaxis] H = update_homogenerous_matrix(H, Rt, Tt) @@ -111,18 +111,17 @@ def nearest_neighbor_assosiation(ppoints, cpoints): def SVD_motion_estimation(ppoints, cpoints): - pm = np.asarray(np.mean(ppoints, axis=1)) - cm = np.asarray(np.mean(cpoints, axis=1)) - print(cm) + pm = np.mean(ppoints, axis=1) + cm = np.mean(cpoints, axis=1) - pshift = np.array(ppoints - pm[:,np.newaxis]) - cshift = np.array(cpoints - cm[:,np.newaxis]) + pshift = ppoints - pm[:,np.newaxis] + cshift = cpoints - cm[:,np.newaxis] - W = cshift.dot(pshift.T) + W = cshift @ pshift.T u, s, vh = np.linalg.svd(W) - R = (u.dot(vh)).T - t = pm - R.dot(cm) + R = (u @ vh).T + t = pm - (R @ cm) return R, t @@ -150,7 +149,6 @@ def main(): cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1] for (x, y) in zip(px, py)] cpoints = np.vstack((cx, cy)) - print(cpoints) R, T = ICP_matching(ppoints, cpoints)