github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-12 08:28:05 -05:00
Code Issues Packages Projects Releases Wiki Activity

code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2018-03-25 18:09:57 -07:00
parent 6e429f61d1
commit 3e94bc7eee
1 changed files with 41 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
SLAM/GraphBasedSLAM/graph_based_slam.py
View File

@@ -48,7 +48,7 @@ class Edge():
self.id2 = 0
def cal_ob_sigma(d):
def cal_observation_sigma(d):
sigma = np.zeros((3, 3))
sigma[0, 0] = (d * C_SIGMA1)**2
@@ -58,6 +58,14 @@ def cal_ob_sigma(d):
return sigma
def calc_rotational_matrix(angle):
Rt = np.matrix([[math.cos(angle), -math.sin(angle), 0],
[math.sin(angle), math.cos(angle), 0],
[0, 0, 1.0]])
return Rt
def calc_edges(xlist, zlist):
edges = []
@@ -66,32 +74,27 @@ def calc_edges(xlist, zlist):
for (t, td) in zids:
xt, yt, yawt = xlist[0, t], xlist[1, t], xlist[2, t]
xtd, ytd, yawtd = xlist[0, td], xlist[1, td], xlist[2, td]
dt, anglet, phit = zlist[t][0, 0], zlist[t][1, 0], zlist[t][2, 0]
dtd, angletd, phitd = zlist[td][0, 0], zlist[td][1, 0], zlist[td][2, 0]
edge = Edge()
t1 = dt * math.cos(yawt + anglet)
t2 = dtd * math.cos(yawtd + angletd)
t3 = dt * math.sin(yawt + anglet)
t4 = dtd * math.sin(yawtd + angletd)
tangle1 = yawt + anglet
tangle2 = yawt + anglet
t1 = dt * math.cos(tangle1)
t2 = dtd * math.cos(tangle2)
t3 = dt * math.sin(tangle1)
t4 = dtd * math.sin(tangle2)
edge.e[0, 0] = xtd - xt - t1 + t2
edge.e[1, 0] = ytd - yt - t3 + t4
edge.e[2, 0] = yawtd - yawt - phit + phitd
sig_t = cal_ob_sigma(dt)
sig_td = cal_ob_sigma(dtd)
sig_t = cal_observation_sigma(dt)
sig_td = cal_observation_sigma(dtd)
Rt = np.matrix([[math.cos(yawt + anglet), -math.sin(yawt + anglet), 0],
[math.sin(yawt + anglet), math.cos(yawt + anglet), 0],
[0, 0, 1.0]])
Rtd = np.matrix([[math.cos(yawtd + angletd), -math.sin(yawtd + angletd), 0],
[math.sin(yawtd + angletd),
math.cos(yawtd + angletd), 0],
[0, 0, 1.0]])
Rt = calc_rotational_matrix(tangle1)
Rtd = calc_rotational_matrix(tangle2)
edge.omega = np.linalg.inv(Rt * sig_t * Rt.T + Rtd * sig_td * Rtd.T)
edge.d_t, edge.d_td = dt, dtd
@@ -115,12 +118,27 @@ def calc_jacobian(edge):
[0, 1.0, edge.d_td * math.cos(td)],
[0, 0, 1.0]])
# print(A)
# print(B)
return A, B
def fill_H_and_b(H, b, edge):
A, B = calc_jacobian(edge)
id1 = edge.id1 * 3
id2 = edge.id2 * 3
H[id1:id1 + 3, id1:id1 + 3] += A.T * edge.omega * A
H[id1:id1 + 3, id2:id2 + 3] += A.T * edge.omega * B
H[id2:id2 + 3, id1:id1 + 3] += B.T * edge.omega * A
H[id2:id2 + 3, id2:id2 + 3] += B.T * edge.omega * B
b[id1:id1 + 3, 0] += (A.T * edge.omega * edge.e)
b[id2:id2 + 3, 0] += (B.T * edge.omega * edge.e)
return H, b
def graph_based_slam(xEst, PEst, u, z, hxDR, hz):
x_opt = copy.deepcopy(hxDR)
@@ -128,34 +146,20 @@ def graph_based_slam(xEst, PEst, u, z, hxDR, hz):
for itr in range(MAX_ITR):
edges = calc_edges(x_opt, hz)
print("nedges:", len(edges))
# print("n edges:", len(edges))
H = np.matrix(np.zeros((n, n)))
b = np.matrix(np.zeros((n, 1)))
for edge in edges:
A, B = calc_jacobian(edge)
id1 = edge.id1 * 3
id2 = edge.id2 * 3
H[id1:id1 + 3, id1:id1 + 3] += A.T * edge.omega * A
H[id1:id1 + 3, id2:id2 + 3] += A.T * edge.omega * B
H[id2:id2 + 3, id1:id1 + 3] += B.T * edge.omega * A
H[id2:id2 + 3, id2:id2 + 3] += B.T * edge.omega * B
b[id1:id1 + 3, 0] += (A.T * edge.omega * edge.e)
b[id2:id2 + 3, 0] += (B.T * edge.omega * edge.e)
H, b = fill_H_and_b(H, b, edge)
H[0:3, 0:3] += np.identity(3) * 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]
x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]
diff = dx.T.dot(dx)
print("iteration: %d, diff: %f" % (itr + 1, diff))
@@ -185,7 +189,7 @@ def observation(xTrue, xd, u, RFID):
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0]
phi = angle - xTrue[2, 0]
phi = angle + xTrue[2, 0]
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise