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

Graph SLAM -> np.array finished

Browse Source
This commit is contained in:
Hajdu Csaba
2018-11-18 14:35:27 +01:00
parent e1af88be23
commit 320e1870e2
2 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

8
SLAM/iterative_closest_point/iterative_closest_point.py
View File

@@ -111,11 +111,11 @@ 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))
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 @ pshift.T
u, s, vh = np.linalg.svd(W)