Skip to content

Commit

Permalink
Merge 320e187 into 916b438
Browse files Browse the repository at this point in the history
  • Loading branch information
@kyberszittya
kyberszittya committed Nov 18, 2018
2 parents 916b438 + 320e187 commit 4b15665
Show file tree
Hide file tree
Showing 3 changed files with 48 additions and 50 deletions.
16 changes: 8 additions & 8 deletions SLAM/EKFSLAM/ekf_slam.py
View file
Expand Up @@ -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])

Expand Down Expand Up @@ -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


Expand Down Expand Up @@ -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

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

Expand All @@ -193,7 +193,7 @@ def jacobH(q, delta, x, i):

F = np.vstack((F1, F2))

H = G.dot(F)
H = G @ F

return H

Expand Down
64 changes: 32 additions & 32 deletions SLAM/GraphBasedSLAM/graph_based_slam.py
View file
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -127,20 +127,20 @@ 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


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

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

Expand All @@ -178,21 +178,21 @@ 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)

# 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
Expand All @@ -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


Expand All @@ -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])):

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

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

Expand All @@ -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
Expand Down Expand Up @@ -299,17 +299,17 @@ 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])
plt.pause(1.0)


if __name__ == '__main__':
main()
main()
18 changes: 8 additions & 10 deletions SLAM/iterative_closest_point/iterative_closest_point.py
View file
Expand Up @@ -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)

Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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)

Expand Down

0 comments on commit 4b15665

Please sign in to comment.