AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

remove exit()

Browse Source
This commit is contained in:
Atsushi Sakai
2018-02-22 13:29:21 -08:00
parent 124fb91b71
commit fd4923f2d4
2 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
SLAM/iterative_closest_point/iterative_closest_point.py
View File

@@ -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)