最近傍探索無しで対応点がわかっている前提でICPアルゴリズムを実装させたいです

##目的
２つの画像から抽出できる特徴点のマッチングをし, 対応するとわかった点同士の重ね合わせがしたいです.そのために, 最近傍探索なしのICPアルゴリズムを実装したいと考えています.

##参考にしたプログラム
最近傍探索有りの参考にしたプログラムを次に示します.

Python
1# -*- coding: utf-8 -*-
2"""
3Iterative Closest Point (ICP) SLAM example
4author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı, Shamil Gemuev
5"""
6
7import math
8
9from mpl_toolkits.mplot3d import Axes3D  # noqa: F401 unused import
10import matplotlib.pyplot as plt
11import numpy as np
12
13#  ICP parameters
14EPS = 0.0001
15MAX_ITER = 100
16
17show_animation = True
18
19
20def icp_matching(previous_points, current_points):
21    """
22    Iterative Closest Point matching
23    - input
24    previous_points: 2D or 3D points in the previous frame
25    current_points: 2D or 3D points in the current frame
26    - output
27    R: Rotation matrix
28    T: Translation vector
29    """
30    H = None  # homogeneous transformation matrix
31    #np.infは無限大
32    dError = np.inf
33    preError = np.inf
34    count = 0
35
36    if show_animation:
37        #描画領域の確保
38        fig = plt.figure()
39        if previous_points.shape[0] == 3:
40            #グラフを用意する　111は1✖︎1の一つ目をグラフにする
41           fig.add_subplot(111, projection='3d')
42
43    while dError >= EPS:
44        count += 1
45
46        if show_animation:  # pragma: no cover
47            plot_points(previous_points, current_points, fig)
48            #リアルタイム描画
49            plt.pause(0.1)
50
51        indexes, error = nearest_neighbor_association(previous_points, current_points)
52        Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points)
53        # update current points
54        current_points = np.dot(Rt, current_points) + Tt[:, np.newaxis]
55        dError = preError - error
56        print("Residual:", error)
57
58        if dError < 0:  # prevent matrix H changing, exit loop
59            print("Not Converge...", preError, dError, count)
60            break
61
62        preError = error
63        H = update_homogeneous_matrix(H, Rt, Tt)
64
65        if dError <= EPS:
66            print("Converge", error, dError, count)
67            break
68        elif MAX_ITER <= count:
69            print("Not Converge...", error, dError, count)
70            break
71
72    #要素数がnならn-1番目
73    R = np.array(H[0:-1, 0:-1])
74    T = np.array(H[0:-1, -1])
75
76    return R, T
77
78
79def update_homogeneous_matrix(Hin, R, T):
80
81    r_size = R.shape[0]
82    #行列の初期化
83    H = np.zeros((r_size + 1, r_size + 1))
84
85    H[0:r_size, 0:r_size] = R
86    H[0:r_size, r_size] = T
87    H[r_size, r_size] = 1.0
88
89    if Hin is None:
90        return H
91    else:
92        return np.dot(Hin, H)
93
94
95def nearest_neighbor_association(previous_points, current_points):
96
97    # calc the sum of residual errors
98    delta_points = previous_points - current_points
99    #列毎のベクトルノルムを取り行ベクトルを求める
100    d = np.linalg.norm(delta_points, axis=0)
101    error = sum(d)
102
103    # calc index with nearest neighbor assosiation
104    #np.repeatは一つ目の引数を二つ目の引数分繰り返す(列方向に)
105    #np.tileは一つ目の引数を二つ目の引数分行方向に、三つ目の引数を列方向に繰り返す
106    d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1)
107                       - np.tile(previous_points, (1, current_points.shape[1])), axis=0)
108    indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1)
109
110    return indexes, error
111
112
113def svd_motion_estimation(previous_points, current_points):
114    #特異値分解
115    #1. 各点群の重心を計算して各点の座標を重心中心に変換
116    #2. その座標行列を互いに掛け合わせたWという行列を特異値分解する　W=UΣV.T
117    #3. 特異値分解によって得られたUとVを使って各点のノルム誤差を最小にするtとRは　R=UV.T, t=ux-Rup(ux,upは点群の重心座標)
118    #平均
119    pm = np.mean(previous_points, axis=1)
120    cm = np.mean(current_points, axis=1)
121
122    #サイズが一の新たな次元追加
123    p_shift = previous_points - pm[:, np.newaxis]
124    c_shift = current_points - cm[:, np.newaxis]
125
126    #積
127    W = np.dot(c_shift, p_shift.T)
128    #SVD
129    u, s, vh = np.linalg.svd(W)
130
131    R = np.dot(u, vh).T
132    t = pm - np.dot(R, cm)
133
134    return R, t
135
136
137def plot_points(previous_points, current_points, figure):
138    # for stopping simulation with the esc key.
139    plt.gcf().canvas.mpl_connect(
140        'key_release_event',
141        lambda event: [exit(0) if event.key == 'escape' else None])
142    if previous_points.shape[0] == 3:
143        #現在の図形のクリア
144        plt.clf()
145        axes = figure.add_subplot(111, projection='3d')
146        #散布図の描画 x,y,z,color,shape
147        axes.scatter(previous_points[0, :], previous_points[1, :],
148                    previous_points[2, :], c="r", marker=".")
149        axes.scatter(current_points[0, :], current_points[1, :],
150                    current_points[2, :], c="b", marker=".")
151        axes.scatter(0.0, 0.0, 0.0, c="r", marker="x")
152        figure.canvas.draw()
153    else:
154        #軸のクリア
155        plt.cla()
156        plt.plot(previous_points[0, :], previous_points[1, :], ".r")
157        plt.plot(current_points[0, :], current_points[1, :], ".b")
158        plt.plot(0.0, 0.0, "xr")
159        #軸が等しい正方形で
160        plt.axis("equal")
161
162
163def main():
164    print(__file__ + " start!!")
165
166    # simulation parameters
167    nPoint = 1000
168    fieldLength = 50.0
169    motion = [0.5, 2.0, np.deg2rad(-10.0)]  # movement [x[m],y[m],yaw[deg]]
170
171    nsim = 3  # number of simulation
172
173    for _ in range(nsim):
174
175        # previous points
176        px = (np.random.rand(nPoint) - 0.5) * fieldLength
177        py = (np.random.rand(nPoint) - 0.5) * fieldLength
178        previous_points = np.vstack((px, py))
179
180        # current points
181        cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0]
182              for (x, y) in zip(px, py)]
183        cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
184              for (x, y) in zip(px, py)]
185        current_points = np.vstack((cx, cy))
186
187        R, T = icp_matching(previous_points, current_points)
188        print("R:", R)
189        print("T:", T)
190
191
192def main_3d_points():
193    print(__file__ + " start!!")
194
195    # simulation parameters for 3d point set
196    nPoint = 1000
197    fieldLength = 50.0
198    motion = [0.5, 2.0, -5, np.deg2rad(-10.0)]  # [x[m],y[m],z[m],roll[deg]]
199
200    nsim = 3  # number of simulation
201
202    for _ in range(nsim):
203
204        # previous points
205        px = (np.random.rand(nPoint) - 0.5) * fieldLength
206        py = (np.random.rand(nPoint) - 0.5) * fieldLength
207        pz = (np.random.rand(nPoint) - 0.5) * fieldLength
208        previous_points = np.vstack((px, py, pz))
209
210        # current points
211        cx = [math.cos(motion[3]) * x - math.sin(motion[3]) * z + motion[0]
212              for (x, z) in zip(px, pz)]
213        cy = [y + motion[1] for y in py]
214        cz = [math.sin(motion[3]) * x + math.cos(motion[3]) * z + motion[2]
215              for (x, z) in zip(px, pz)]
216        current_points = np.vstack((cx, cy, cz))
217
218        R, T = icp_matching(previous_points, current_points)
219        print("R:", R)
220        print("T:", T)
221
222
223if __name__ == '__main__':
224    main()
225    main_3d_points()
226

##知りたい部分
最近傍探索なしで対応する特徴点同士を重ね合わせるとしたら, この上記のコードの"nearest_neighbor_association()"を変えると思うのですが, そもそもこの関数の中身がよくわかっていないので書き換えができません.中身で何をしているのか教えていただきたいです.
また, 特徴点座標情報を与える必要があると思うのですがどの時点で与えるべきでしょうか.