From 49ce57d6f8de1265eb1e578dd0f069c6b7a56ca6 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 27 Oct 2019 17:59:08 +0900 Subject: [PATCH] Code cleanup. --- .../iterative_closest_point.py | 57 +++++++++---------- tests/test_LQR_planner.py | 4 +- 2 files changed, 30 insertions(+), 31 deletions(-) diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index ec81bb12..3512ef97 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -4,22 +4,23 @@ author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı """ 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) + 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 inds, error + return indexes, error -def SVD_motion_estimation(ppoints, cpoints): +def svd_motion_estimation(previous_points, current_points): + pm = np.mean(previous_points, axis=1) + cm = np.mean(current_points, axis=1) - pm = np.mean(ppoints, axis=1) - cm = np.mean(cpoints, axis=1) + p_shift = previous_points - pm[:, np.newaxis] + c_shift = current_points - cm[:, np.newaxis] - pshift = ppoints - pm[:, np.newaxis] - cshift = cpoints - 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) diff --git a/tests/test_LQR_planner.py b/tests/test_LQR_planner.py index 5ece27f8..2bcf828c 100644 --- a/tests/test_LQR_planner.py +++ b/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 @@ print(__file__) class Test(TestCase): def test1(self): - m.show_animation = False + m.SHOW_ANIMATION = False m.main()