keep code cleaning

Author: AtsushiSakai

SLAM/iterative_closest_point/iterative_closest_point.py

@@ -10,82 +10,8 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
-#  % 点をランダムでばら撒く(t-1の時の点群)
-#  data1=fieldLength*rand(2,nPoint)-fieldLength/2;
-
-#  % data2= data1を移動させる & ノイズ付加
-#  % 回転方向 ＆ ノイズ付加
-#  theta=toRadian(motion(3))+toRadian(thetaSigma)*rand(1);
-#  % 並進ベクトル ＆ ノイズ付加
-#  t=repmat(motion(1:2)',1,nPoint)+transitionSigma*randn(2,nPoint);
-#  % 回転行列の作成
-#  A=[cos(theta) sin(theta);-sin(theta) cos(theta)];
-#  % data1を移動させてdata2を作る
-#  data2=t+A*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]'
-
-
-#  while ~(dError < EPS)
-#  count=count+1;
-
-#  [ii, error]=FindNearestPoint(data1, data2);%最近傍点探索
-#  [R1, t1]=SVDMotionEstimation(data1, data2, ii);%特異値分解による移動量推定
-#  %計算したRとtで点群とRとtの値を更新
-#  data2=R1*data2;
-#  data2=[data2(1,:)+t1(1) ; data2(2,:)+t1(2)];
-#  R = R1*R;
-#  t = R1*t + t1;
-
-#  dError=abs(preError-error);%エラーの改善量
-#  preError=error;%一つ前のエラーの総和値を保存
-
-#  if count > maxIter %収束しなかった
-#  disp('Max Iteration');return;
-#  end
-#  end
-#  disp(['Convergence:',num2str(count)]);
-
-#  function [index, error]=FindNearestPoint(data1, data2)
-#  %data2に対するdata1の最近傍点のインデックスを計算する関数
-#  m1=size(data1,2);
-#  m2=size(data2,2);
-#  index=[];
-#  error=0;
-
-#  for i=1:m1
-#  dx=(data2-repmat(data1(:,i),1,m2));
-#  dist=sqrt(dx(1,:).^2+dx(2,:).^2);
-#  [dist, ii]=min(dist);
-#  index=[index; ii];
-#  error=error+dist;
-#  end
-
-#  function [R, t]=SVDMotionEstimation(data1, data2, index)
-#  %特異値分解法による並進ベクトルと、回転行列の計算
-
-#  %各点群の重心の計算
-#  M = data1;
-#  mm = mean(M,2);
-#  S = data2(:,index);
-#  ms = mean(S,2);
-
-#  %各点群を重心中心の座標系に変換
-#  Sshifted = [S(1,:)-ms(1); S(2,:)-ms(2);];
-#  Mshifted = [M(1,:)-mm(1); M(2,:)-mm(2);];
-
-#  W = Sshifted*Mshifted';
-#  [U,A,V] = svd(W);%特異値分解
-
-#  R = (U*V')';%回転行列の計算
-#  t = mm - R*ms;%並進ベクトルの計算
 
 def update_homogenerous_matrix(Hin, R, T):
-    print(R)
 
     H = np.matrix(np.zeros((3, 3)))  # translation vector
 
@@ -105,9 +31,7 @@ def update_homogenerous_matrix(Hin, R, T):
 
 
 def ICP_matching(pdata, data):
-    R = np.eye(2)  # rotation matrix
-    T = np.zeros((2, 1))  # translation vector
-    H = None  # translation vector
+    H = None  # homogeneraous transformation matrix
 
     #  ICP
     EPS = 0.0001
@@ -131,30 +55,29 @@ def ICP_matching(pdata, data):
         preError = error
 
         if dError <= EPS:
-            print("Converge", dError)
-            plt.plot(data[0, :], data[1, :], "*k")
+            print("Converge", dError, count)
             break
         elif maxIter <= count:
             break
 
     R = np.matrix(H[0:2, 0:2])
     T = np.matrix(H[0:2, 2])
-    print(H)
-    print(R)
-    print(T)
 
     return R, T
 
 
 def nearest_neighbor_assosiation(pdata, data):
 
     ddata = pdata - data
-    #  print(ddata)
 
     d = np.linalg.norm(ddata, axis=0)
 
     error = sum(d)
 
+    for i in range(data.shape[1]):
+        for ii in range(data.shape[1]):
+            print(i)
+
     return error