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

it works but R,t update method is not good...

Browse Source
This commit is contained in:
Atsushi Sakai
2018-02-22 13:41:11 -08:00
parent fd4923f2d4
commit ea8f35af48
1 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
SLAM/iterative_closest_point/iterative_closest_point.py
View File

@@ -102,8 +102,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
@@ -113,6 +112,8 @@ def ICP_matching(pdata, data):
preError = error
if dError <= EPS:
print("Converge", dError)
plt.plot(data[0, :], data[1, :], "*k")
break
elif maxIter <= count:
break
@@ -140,7 +141,7 @@ def SVD_motion_estimation(pdata, data):
pshift = np.matrix(pdata - pm)
cshift = np.matrix(data - cm)
W = pshift * cshift.T
W = cshift * pshift.T
u, s, vh = np.linalg.svd(W)
R = (u * vh).T
@@ -174,12 +175,12 @@ def main():
# print(data)
R, T = ICP_matching(pdata, data)
print(motion)
print(R)
print(T)
fdata = (R * data) + T
plt.plot(px, py, ".b")
plt.plot(cx, cy, ".r")
plt.plot(fdata[0, :], fdata[1, :], "xk")
plt.plot(0.0, 0.0, "xr")
plt.axis("equal")
plt.show()