first release ICP SLAM

Author: AtsushiSakai

SLAM/iterative_closest_point/iterative_closest_point.py

@@ -10,54 +10,48 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
+#  ICP parameters
+EPS = 0.0001
+MAXITER = 100
 
-def update_homogenerous_matrix(Hin, R, T):
-
-    H = np.matrix(np.zeros((3, 3)))  # translation vector
-
-    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]
-
-    if Hin is None:
-        return H
-    else:
-        return Hin * H
+show_animation = True
 
 
 def ICP_matching(pdata, data):
     H = None  # homogeneraous transformation matrix
 
-    #  ICP
-    EPS = 0.0001
-    maxIter = 100
-
     dError = 1000.0
     preError = 1000.0
     count = 0
 
     while dError >= EPS:
         count += 1
 
-        error = nearest_neighbor_assosiation(pdata, data)
-        Rt, Tt = SVD_motion_estimation(pdata, data)
+        if show_animation:
+            plt.cla()
+            plt.plot(pdata[0, :], pdata[1, :], ".r")
+            plt.plot(data[0, :], data[1, :], ".b")
+            plt.plot(0.0, 0.0, "xr")
+            plt.axis("equal")
+            plt.pause(1.0)
+
+        inds, error = nearest_neighbor_assosiation(pdata, data)
+        Rt, Tt = SVD_motion_estimation(pdata[:, inds], data)
 
+        # update current points
         data = (Rt * data) + Tt
 
         H = update_homogenerous_matrix(H, Rt, Tt)
 
         dError = abs(preError - error)
         preError = error
+        print("Residual:", error)
 
         if dError <= EPS:
-            print("Converge", dError, count)
+            print("Converge", error, dError, count)
             break
-        elif maxIter <= count:
+        elif MAXITER <= count:
+            print("Not Converge...", error, dError, count)
             break
 
     R = np.matrix(H[0:2, 0:2])
@@ -66,19 +60,47 @@ def ICP_matching(pdata, data):
     return R, T
 
 
+def update_homogenerous_matrix(Hin, R, T):
+
+    H = np.matrix(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]
+
+    if Hin is None:
+        return H
+    else:
+        return Hin * H
+
+
 def nearest_neighbor_assosiation(pdata, data):
 
+    # calc the sum of residual errors
     ddata = pdata - data
-
     d = np.linalg.norm(ddata, axis=0)
-
     error = sum(d)
 
+    # calc index with nearest neighbor assosiation
+    inds = []
     for i in range(data.shape[1]):
-        for ii in range(data.shape[1]):
-            print(i)
+        minid = -1
+        mind = float("inf")
+        for ii in range(pdata.shape[1]):
+            d = np.linalg.norm(pdata[:, ii] - data[:, i])
+
+            if mind >= d:
+                mind = d
+                minid = ii
+
+        inds.append(minid)
 
-    return error
+    return inds, error
 
 
 def SVD_motion_estimation(pdata, data):
@@ -104,35 +126,25 @@ def main():
     # simulation parameters
     nPoint = 10
     fieldLength = 50.0
-    motion = [0.5, 1.0, math.radians(-40.0)]  # movement [x[m],y[m],yaw[deg]]
-    #  transitionSigma=0.01;%並進方向の移動誤差標準偏差[m]
-    #  thetaSigma=1;   %回転方向の誤差標準偏差[deg]
-
-    # previous point data
-    px = (np.random.rand(nPoint) - 0.5) * fieldLength
-    py = (np.random.rand(nPoint) - 0.5) * fieldLength
-
-    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)]
-
-    pdata = np.matrix(np.vstack((px, py)))
-    #  print(pdata)
-    data = np.matrix(np.vstack((cx, cy)))
-    #  print(data)
-    odata = data[:, :]
-
-    R, T = ICP_matching(pdata, data)
-
-    fdata = (R * odata) + T
-
-    plt.plot(px, py, ".b")
-    plt.plot(cx, cy, ".r")
-    plt.plot(fdata[0, :], fdata[1, :], "xk")
-    plt.plot(0.0, 0.0, "xr")
-    plt.axis("equal")
-    plt.show()
+    motion = [0.5, 2.0, math.radians(-10.0)]  # movement [x[m],y[m],yaw[deg]]
+
+    nsim = 3  # number of simulation
+
+    for _ in range(nsim):
+
+        # previous points
+        px = (np.random.rand(nPoint) - 0.5) * fieldLength
+        py = (np.random.rand(nPoint) - 0.5) * fieldLength
+        pdata = np.matrix(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)]
+        data = np.matrix(np.vstack((cx, cy)))
+
+        R, T = ICP_matching(pdata, data)
 
 
 if __name__ == '__main__':