From fd4923f2d4f499089ee495ee20300851093f86e2 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Thu, 22 Feb 2018 13:29:21 -0800 Subject: [PATCH] remove exit() --- PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py | 1 - SLAM/iterative_closest_point/iterative_closest_point.py | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 581deacc..f840330a 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -446,7 +446,6 @@ def main(): if not flag: print("cannot find feasible path") - exit() # flg, ax = plt.subplots(1) # Draw final path diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index 49343a9b..0b14a899 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -103,7 +103,7 @@ def ICP_matching(pdata, data): error = nearest_neighbor_assosiation(pdata, data) Rt, Tt = SVD_motion_estimation(pdata, data) # print(count) - print(error) + # print(error) data = (Rt * data) + Tt R = R * Rt @@ -134,8 +134,8 @@ def nearest_neighbor_assosiation(pdata, data): def SVD_motion_estimation(pdata, data): - pm = np.mean(pdata, axis=1) - cm = np.mean(data, axis=1) + pm = np.matrix(np.mean(pdata, axis=1)) + cm = np.matrix(np.mean(data, axis=1)) pshift = np.matrix(pdata - pm) cshift = np.matrix(data - cm)