Merge pull request AtsushiSakai#131 from kyberszittya/master

Changing np.matrix to np.array, changing .dot to @ (Issue AtsushiSakai#115)

Author: AtsushiSakai

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
 

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

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)