From ea8f35af483f584d60bf93a058dfdcecd0696c7d Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Thu, 22 Feb 2018 13:41:11 -0800
Subject: [PATCH] it works but R,t update method is not good...

---
 .../iterative_closest_point.py                      | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 0b14a899..15069496 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -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()