github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 04:28:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix name typo and update code

Browse Source
This commit is contained in:
Atsushi Sakai
2019-02-26 20:31:02 +09:00
parent f3a07514ae
commit 963ec11858
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
SLAM/iterative_closest_point/iterative_closest_point.py
View File

@@ -24,7 +24,7 @@ def ICP_matching(ppoints, cpoints):
R: Rotation matrix
T: Translation vector
"""
H = None # homogeneraous transformation matrix
H = None # homogeneous transformation matrix
dError = 1000.0
preError = 1000.0
@@ -47,7 +47,7 @@ def ICP_matching(ppoints, cpoints):
# update current points
cpoints = (Rt @ cpoints) + Tt[:, np.newaxis]
H = update_homogenerous_matrix(H, Rt, Tt)
H = update_homogeneous_matrix(H, Rt, Tt)
dError = abs(preError - error)
preError = error
@@ -66,7 +66,7 @@ def ICP_matching(ppoints, cpoints):
return R, T
def update_homogenerous_matrix(Hin, R, T):
def update_homogeneous_matrix(Hin, R, T):
H = np.zeros((3, 3))
@@ -82,7 +82,7 @@ def update_homogenerous_matrix(Hin, R, T):
if Hin is None:
return H
else:
return Hin * H
return Hin @ H
def nearest_neighbor_assosiation(ppoints, cpoints):