Merge branch 'master' of https://github.com/AtsushiSakai/PythonRobotics

Author: AtsushiSakai

SLAM/EKFSLAM/ekf_slam.py

@@ -1,9 +1,6 @@
 """
-
 Extended Kalman Filter SLAM example
-
 author: Atsushi Sakai (@Atsushi_twi)
-
 """
 
 import numpy as np
@@ -50,13 +47,12 @@ def ekf_slam(xEst, PEst, u, z):
                               np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
             xEst = xAug
             PEst = PAug
-
         lm = get_LM_Pos_from_state(xEst, minid)
         y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
 
-        K = PEst * H.T * np.linalg.inv(S)
-        xEst = xEst + K * y
-        PEst = (np.eye(len(xEst)) - K * H) * PEst
+        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)
 
     xEst[2] = pi_2_pi(xEst[2])
 
@@ -66,7 +62,7 @@ def ekf_slam(xEst, PEst, u, z):
 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
 
 
@@ -75,7 +71,7 @@ def observation(xTrue, xd, u, RFID):
     xTrue = motion_model(xTrue, u)
 
     # add noise to gps x-y
-    z = np.matrix(np.zeros((0, 3)))
+    z = np.zeros((0, 3))
 
     for i in range(len(RFID[:, 0])):
 
@@ -86,31 +82,29 @@ 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, i])
+            zi = np.array([dn, anglen, 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([[
+        u[0, 0] + np.random.randn() * Rsim[0, 0],
+        u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T
 
     xd = motion_model(xd, ud)
-
     return xTrue, z, xd, ud
 
 
 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.dot(x) + B .dot(u)
     return x
 
 
@@ -124,7 +118,7 @@ def jacob_motion(x, u):
     Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
         (STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
 
-    jF = np.matrix([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
+    jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
                     [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
                     [0.0, 0.0, 0.0]])
 
@@ -134,11 +128,12 @@ def jacob_motion(x, u):
 
 
 def calc_LM_Pos(x, z):
-
     zp = np.zeros((2, 1))
 
-    zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
-    zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
+    zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
+    zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
+    #zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
+    #zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
 
     return zp
 
@@ -162,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 * np.linalg.inv(S) * y)
+        mdist.append(y.T.dot(np.linalg.inv(S)).dot(y))
 
     mdist.append(M_DIST_TH)  # new landmark
 
@@ -173,20 +168,21 @@ def search_correspond_LM_ID(xAug, PAug, zi):
 
 def calc_innovation(lm, xEst, PEst, z, LMid):
     delta = lm - xEst[0:2]
-    q = (delta.T * delta)[0, 0]
-    zangle = math.atan2(delta[1], delta[0]) - xEst[2]
-    zp = [math.sqrt(q), pi_2_pi(zangle)]
+    q = (delta.T.dot(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 * PEst * H.T + Cx[0:2, 0:2]
+    S = H.dot(PEst).dot(H.T) + Cx[0:2, 0:2]
 
     return y, S, H
 
 
 def jacobH(q, delta, x, i):
     sq = math.sqrt(q)
-    G = np.matrix([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
+    G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
                    [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
 
     G = G / q
@@ -197,7 +193,7 @@ def jacobH(q, delta, x, i):
 
     F = np.vstack((F1, F2))
 
-    H = G * F
+    H = G.dot(F)
 
     return H
 
@@ -218,11 +214,11 @@ def main():
                      [-5.0, 20.0]])
 
     # State Vector [x y yaw v]'
-    xEst = np.matrix(np.zeros((STATE_SIZE, 1)))
-    xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
+    xEst = np.zeros((STATE_SIZE, 1))
+    xTrue = np.zeros((STATE_SIZE, 1))
     PEst = np.eye(STATE_SIZE)
 
-    xDR = np.matrix(np.zeros((STATE_SIZE, 1)))  # Dead reckoning
+    xDR = np.zeros((STATE_SIZE, 1))  # Dead reckoning
 
     # history
     hxEst = xEst
@@ -239,6 +235,7 @@ def main():
 
         x_state = xEst[0:STATE_SIZE]
 
+
         # store data history
         hxEst = np.hstack((hxEst, x_state))
         hxDR = np.hstack((hxDR, xDR))
@@ -255,16 +252,17 @@ def main():
                 plt.plot(xEst[STATE_SIZE + i * 2],
                          xEst[STATE_SIZE + i * 2 + 1], "xg")
 
-            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(hxEst[0, :]).flatten(),
-                     np.array(hxEst[1, :]).flatten(), "-r")
+            plt.plot(hxTrue[0, :],
+                     hxTrue[1, :], "-b")
+            plt.plot(hxDR[0, :],
+                     hxDR[1, :], "-k")
+            plt.plot(hxEst[0, :],
+                     hxEst[1, :], "-r")
             plt.axis("equal")
             plt.grid(True)
             plt.pause(0.001)
 
 
+
 if __name__ == '__main__':
-    main()
+    main()

SLAM/iterative_closest_point/iterative_closest_point.py

@@ -1,9 +1,6 @@
 """
-
 Iterative Closest Point (ICP) SLAM example
-
 author: Atsushi Sakai (@Atsushi_twi)
-
 """
 
 import math
@@ -20,15 +17,12 @@
 def ICP_matching(ppoints, cpoints):
     """
     Iterative Closest Point matching
-
     - input
     ppoints: 2D points in the previous frame
     cpoints: 2D points in the current frame
-
     - output
     R: Rotation matrix
     T: Translation vector
-
     """
     H = None  # homogeneraous transformation matrix
 
@@ -51,7 +45,7 @@ def ICP_matching(ppoints, cpoints):
         Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints)
 
         # update current points
-        cpoints = (Rt * cpoints) + Tt
+        cpoints = (Rt.dot(cpoints)) + Tt[:,np.newaxis]
 
         H = update_homogenerous_matrix(H, Rt, Tt)
 
@@ -66,24 +60,24 @@ def ICP_matching(ppoints, cpoints):
             print("Not Converge...", error, dError, count)
             break
 
-    R = np.matrix(H[0:2, 0:2])
-    T = np.matrix(H[0:2, 2])
+    R = np.array(H[0:2, 0:2])
+    T = np.array(H[0:2, 2])
 
     return R, T
 
 
 def update_homogenerous_matrix(Hin, R, T):
 
-    H = np.matrix(np.zeros((3, 3)))
+    H = np.zeros((3, 3))
 
     H[0, 0] = R[0, 0]
     H[1, 0] = R[1, 0]
     H[0, 1] = R[0, 1]
     H[1, 1] = R[1, 1]
     H[2, 2] = 1.0
 
-    H[0, 2] = T[0, 0]
-    H[1, 2] = T[1, 0]
+    H[0, 2] = T[0]
+    H[1, 2] = T[1]
 
     if Hin is None:
         return H
@@ -117,17 +111,18 @@ def nearest_neighbor_assosiation(ppoints, cpoints):
 
 def SVD_motion_estimation(ppoints, cpoints):
 
-    pm = np.matrix(np.mean(ppoints, axis=1))
-    cm = np.matrix(np.mean(cpoints, axis=1))
+    pm = np.asarray(np.mean(ppoints, axis=1))
+    cm = np.asarray(np.mean(cpoints, axis=1))
+    print(cm)
 
-    pshift = np.matrix(ppoints - pm)
-    cshift = np.matrix(cpoints - cm)
+    pshift = np.array(ppoints - pm[:,np.newaxis])
+    cshift = np.array(cpoints - cm[:,np.newaxis])
 
-    W = cshift * pshift.T
+    W = cshift.dot(pshift.T)
     u, s, vh = np.linalg.svd(W)
 
-    R = (u * vh).T
-    t = pm - R * cm
+    R = (u.dot(vh)).T
+    t = pm - R.dot(cm)
 
     return R, t
 
@@ -147,17 +142,18 @@ def main():
         # previous points
         px = (np.random.rand(nPoint) - 0.5) * fieldLength
         py = (np.random.rand(nPoint) - 0.5) * fieldLength
-        ppoints = np.matrix(np.vstack((px, py)))
+        ppoints = np.vstack((px, py))
 
         # current points
         cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0]
               for (x, y) in zip(px, py)]
         cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
               for (x, y) in zip(px, py)]
-        cpoints = np.matrix(np.vstack((cx, cy)))
+        cpoints = np.vstack((cx, cy))
+        print(cpoints)
 
         R, T = ICP_matching(ppoints, cpoints)
 
 
 if __name__ == '__main__':
-    main()
+    main()