From 883df8ff6649dd9c8829552e9460899e82d1bb19 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Wed, 21 Feb 2018 22:47:15 -0800
Subject: [PATCH] keep codeing

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

diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 325f5c64..1a7c2fac 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -23,19 +23,12 @@ import matplotlib.pyplot as plt
 #  % data1を移動させてdata2を作る
 #  data2=t+A*data1;
 
-#  % ICPアルゴリズム data2とdata1のMatching
-#  % R:回転行列　t:併進ベクトル
-#  % [R,T]=icp(data1,data2)
-#  [R,T] = ICPMatching(data2,data1);
-
 #  function [R, t]=ICPMatching(data1, data2)
 #  % ICPアルゴリズムによる、並進ベクトルと回転行列の計算を実施する関数
 #  % data1 = [x(t)1 x(t)2 x(t)3 ...]
 #  % data2 = [x(t+1)1 x(t+1)2 x(t+1)3 ...]
 #  % x=[x y z]'
 
-#  R=eye(2);%回転行列
-#  t=zeros(2,1);%並進ベクトル
 
 #  while ~(dError < EPS)
 #  count=count+1;
@@ -93,20 +86,50 @@ import matplotlib.pyplot as plt
 
 
 def ICP_matching(pdata, data):
-    R = 0.0  # rotation matrix
-    T = 0.0  # translation vector
+    R = np.eye(2)  # rotation matrix
+    T = np.zeros((2, 1))  # translation vector
 
     #  ICP
-    #  EPS = 0.0001
-    #  maxIter = 100
+    EPS = 0.0001
+    maxIter = 100
+
+    dError = 1000.0
+    count = 0
+
+    while dError >= EPS:
+        count += 1
+
+        Rt, Tt = SVD_motion_estimation(pdata, data)
+        #  print(count)
+
+        R = R * Rt
+        T = R * T + Tt
+
+        if maxIter <= count:
+            break
 
     #  preError = 0
-    #  loopcount = 0
-    #  dError = 1000.0
 
     return R, T
 
 
+def SVD_motion_estimation(pdata, data):
+
+    pm = np.mean(pdata, axis=1)
+    cm = np.mean(data, axis=1)
+
+    pshift = pdata - pm
+    cshift = data - cm
+
+    W = pshift * cshift.T
+    u, s, vh = np.linalg.svd(W)
+
+    R = (u * vh.T).T
+    t = pm - R * cm
+
+    return R, t
+
+
 def main():
     print(__file__ + " start!!")
 
@@ -126,6 +149,16 @@ def main():
     cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
           for (x, y) in zip(px, py)]
 
+    pdata = np.matrix(np.vstack((px, py)))
+    #  print(pdata)
+    data = np.matrix(np.vstack((cx, cy)))
+    #  print(data)
+
+    R, T = ICP_matching(pdata, data)
+    print(motion)
+    print(R)
+    print(T)
+
     plt.plot(px, py, ".b")
     plt.plot(cx, cy, ".r")
     plt.plot(0.0, 0.0, "xr")