From e1af88be23f6c57713528ec1ca5a9aa90f97a1af Mon Sep 17 00:00:00 2001
From: Hajdu Csaba <kyberszittya@protonmail.ch>
Date: Sun, 18 Nov 2018 13:20:45 +0100
Subject: [PATCH] Changing operator DOT to @

---
 SLAM/EKFSLAM/ekf_slam.py                         | 16 ++++++++--------
 .../iterative_closest_point.py                   | 10 ++++------
 2 files changed, 12 insertions(+), 14 deletions(-)

diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
index d69a2f83..8f1d9976 100644
--- a/SLAM/EKFSLAM/ekf_slam.py
+++ b/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
 
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index daaeab2c..a1f6a9f5 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/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)
 
@@ -113,16 +113,15 @@ def SVD_motion_estimation(ppoints, cpoints):
 
     pm = np.asarray(np.mean(ppoints, axis=1))
     cm = np.asarray(np.mean(cpoints, axis=1))
-    print(cm)
 
     pshift = np.array(ppoints - pm[:,np.newaxis])
     cshift = np.array(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)