github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #131 from kyberszittya/master

Browse Source 
Changing np.matrix to np.array, changing .dot to @ (Issue #115)
This commit is contained in:
Atsushi Sakai
2018-11-19 20:27:28 +09:00
committed by GitHub
parent 916b4382de 320e1870e2
commit 7f852fd8f7
3 changed files with 48 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -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

64
SLAM/GraphBasedSLAM/graph_based_slam.py
View File

@@ -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()

18
SLAM/iterative_closest_point/iterative_closest_point.py
View File

@@ -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)