first release ICP SLAM

SLAM/iterative_closest_point/iterative_closest_point.py

@@ -10,10 +10,59 @@ import math
 import numpy as np
 import matplotlib.pyplot as plt
 
+#  ICP parameters
+EPS = 0.0001
+MAXITER = 100
+
+show_animation = True
+
+
+def ICP_matching(pdata, data):
+    H = None  # homogeneraous transformation matrix
+
+    dError = 1000.0
+    preError = 1000.0
+    count = 0
+
+    while dError >= EPS:
+        count += 1
+
+        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", error, dError, count)
+            break
+        elif MAXITER <= count:
+            print("Not Converge...", error, dError, count)
+            break
+
+    R = np.matrix(H[0:2, 0:2])
+    T = np.matrix(H[0:2, 2])
+
+    return R, T
+
 
 def update_homogenerous_matrix(Hin, R, T):
 
-    H = np.matrix(np.zeros((3, 3)))  # translation vector
+    H = np.matrix(np.zeros((3, 3)))
 
     H[0, 0] = R[0, 0]
     H[1, 0] = R[1, 0]
@@ -30,55 +79,28 @@ def update_homogenerous_matrix(Hin, R, T):
         return Hin * H
 
 
-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)
-
-        data = (Rt * data) + Tt
-
-        H = update_homogenerous_matrix(H, Rt, Tt)
-
-        dError = abs(preError - error)
-        preError = error
-
-        if dError <= EPS:
-            print("Converge", dError, count)
-            break
-        elif maxIter <= count:
-            break
-
-    R = np.matrix(H[0:2, 0:2])
-    T = np.matrix(H[0:2, 2])
-
-    return R, T
-
-
 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])
 
-    return error
+            if mind >= d:
+                mind = d
+                minid = ii
+
+        inds.append(minid)
+
+    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]
+    motion = [0.5, 2.0, math.radians(-10.0)]  # movement [x[m],y[m],yaw[deg]]
 
-    # previous point data
-    px = (np.random.rand(nPoint) - 0.5) * fieldLength
-    py = (np.random.rand(nPoint) - 0.5) * fieldLength
+    nsim = 3  # number of simulation
 
-    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)]
+    for _ in range(nsim):
 
-    pdata = np.matrix(np.vstack((px, py)))
-    #  print(pdata)
-    data = np.matrix(np.vstack((cx, cy)))
-    #  print(data)
-    odata = data[:, :]
+        # 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)))
 
-    R, T = ICP_matching(pdata, data)
+        # 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)))
 
-    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()
+        R, T = ICP_matching(pdata, data)
 
 
 if __name__ == '__main__':