Code cleanup.

Author: AtsushiSakai

SLAM/iterative_closest_point/iterative_closest_point.py

@@ -4,22 +4,23 @@
 """
 
 import math
-import numpy as np
+
 import matplotlib.pyplot as plt
+import numpy as np
 
 #  ICP parameters
 EPS = 0.0001
-MAXITER = 100
+MAX_ITER = 100
 
 show_animation = True
 
 
-def ICP_matching(ppoints, cpoints):
+def icp_matching(previous_points, current_points):
     """
     Iterative Closest Point matching
     - input
-    ppoints: 2D points in the previous frame
-    cpoints: 2D points in the current frame
+    previous_points: 2D points in the previous frame
+    current_points: 2D points in the current frame
     - output
     R: Rotation matrix
     T: Translation vector
@@ -35,17 +36,17 @@ def ICP_matching(ppoints, cpoints):
 
         if show_animation:  # pragma: no cover
             plt.cla()
-            plt.plot(ppoints[0, :], ppoints[1, :], ".r")
-            plt.plot(cpoints[0, :], cpoints[1, :], ".b")
+            plt.plot(previous_points[0, :], previous_points[1, :], ".r")
+            plt.plot(current_points[0, :], current_points[1, :], ".b")
             plt.plot(0.0, 0.0, "xr")
             plt.axis("equal")
             plt.pause(0.1)
 
-        inds, error = nearest_neighbor_assosiation(ppoints, cpoints)
-        Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints)
+        indexes, error = nearest_neighbor_association(previous_points, current_points)
+        Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points)
 
         # update current points
-        cpoints = (Rt @ cpoints) + Tt[:, np.newaxis]
+        current_points = (Rt @ current_points) + Tt[:, np.newaxis]
 
         H = update_homogeneous_matrix(H, Rt, Tt)
 
@@ -56,7 +57,7 @@ def ICP_matching(ppoints, cpoints):
         if dError <= EPS:
             print("Converge", error, dError, count)
             break
-        elif MAXITER <= count:
+        elif MAX_ITER <= count:
             print("Not Converge...", error, dError, count)
             break
 
@@ -85,31 +86,29 @@ def update_homogeneous_matrix(Hin, R, T):
         return Hin @ H
 
 
-def nearest_neighbor_assosiation(ppoints, cpoints):
+def nearest_neighbor_association(previous_points, current_points):
 
     # calc the sum of residual errors
-    dcpoints = ppoints - cpoints
-    d = np.linalg.norm(dcpoints, axis=0)
+    delta_points = previous_points - current_points
+    d = np.linalg.norm(delta_points, axis=0)
     error = sum(d)
 
     # calc index with nearest neighbor assosiation
-    d = np.linalg.norm(
-            np.repeat(cpoints, ppoints.shape[1], axis=1) - np.tile(ppoints, (1,
-                cpoints.shape[1])), axis=0)
-    inds = np.argmin(d.reshape(cpoints.shape[1], ppoints.shape[1]), axis=1)
-
-    return inds, error
+    d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1)
+                       - np.tile(previous_points, (1, current_points.shape[1])), axis=0)
+    indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1)
 
+    return indexes, error
 
-def SVD_motion_estimation(ppoints, cpoints):
 
-    pm = np.mean(ppoints, axis=1)
-    cm = np.mean(cpoints, axis=1)
+def svd_motion_estimation(previous_points, current_points):
+    pm = np.mean(previous_points, axis=1)
+    cm = np.mean(current_points, axis=1)
 
-    pshift = ppoints - pm[:, np.newaxis]
-    cshift = cpoints - cm[:, np.newaxis]
+    p_shift = previous_points - pm[:, np.newaxis]
+    c_shift = current_points - cm[:, np.newaxis]
 
-    W = cshift @ pshift.T
+    W = c_shift @ p_shift.T
     u, s, vh = np.linalg.svd(W)
 
     R = (u @ vh).T
@@ -133,16 +132,16 @@ def main():
         # previous points
         px = (np.random.rand(nPoint) - 0.5) * fieldLength
         py = (np.random.rand(nPoint) - 0.5) * fieldLength
-        ppoints = np.vstack((px, py))
+        previous_points = 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.vstack((cx, cy))
+        current_points = np.vstack((cx, cy))
 
-        R, T = ICP_matching(ppoints, cpoints)
+        R, T = icp_matching(previous_points, current_points)
         print("R:", R)
         print("T:", T)
 

tests/test_LQR_planner.py

@@ -1,6 +1,6 @@
+import sys
 from unittest import TestCase
 
-import sys
 sys.path.append("./PathPlanning/LQRPlanner")
 
 from PathPlanning.LQRPlanner import LQRplanner as m
@@ -11,5 +11,5 @@
 class Test(TestCase):
 
     def test1(self):
-        m.show_animation = False
+        m.SHOW_ANIMATION = False
         m.main()