1
1
"""
2
2
Iterative Closest Point (ICP) SLAM example
3
- author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
3
+ author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı, Shamil Gemuev
4
4
"""
5
5
6
6
import math
7
7
8
+ # from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
8
9
import matplotlib .pyplot as plt
9
10
import numpy as np
10
11
@@ -19,8 +20,8 @@ def icp_matching(previous_points, current_points):
19
20
"""
20
21
Iterative Closest Point matching
21
22
- input
22
- previous_points: 2D points in the previous frame
23
- current_points: 2D points in the current frame
23
+ previous_points: 2D or 3D points in the previous frame
24
+ current_points: 2D or 3D points in the current frame
24
25
- output
25
26
R: Rotation matrix
26
27
T: Translation vector
@@ -31,19 +32,16 @@ def icp_matching(previous_points, current_points):
31
32
preError = np .inf
32
33
count = 0
33
34
35
+ if show_animation :
36
+ fig = plt .figure ()
37
+ # if previous_points.shape[0] == 3:
38
+ # fig.add_subplot(111, projection='3d')
39
+
34
40
while dError >= EPS :
35
41
count += 1
36
42
37
43
if show_animation : # pragma: no cover
38
- plt .cla ()
39
- # for stopping simulation with the esc key.
40
- plt .gcf ().canvas .mpl_connect (
41
- 'key_release_event' ,
42
- lambda event : [exit (0 ) if event .key == 'escape' else None ])
43
- plt .plot (previous_points [0 , :], previous_points [1 , :], ".r" )
44
- plt .plot (current_points [0 , :], current_points [1 , :], ".b" )
45
- plt .plot (0.0 , 0.0 , "xr" )
46
- plt .axis ("equal" )
44
+ plot_points (previous_points , current_points , fig )
47
45
plt .pause (0.1 )
48
46
49
47
indexes , error = nearest_neighbor_association (previous_points , current_points )
@@ -68,24 +66,20 @@ def icp_matching(previous_points, current_points):
68
66
print ("Not Converge..." , error , dError , count )
69
67
break
70
68
71
- R = np .array (H [0 :2 , 0 :2 ])
72
- T = np .array (H [0 :2 , 2 ])
69
+ R = np .array (H [0 :- 1 , 0 :- 1 ])
70
+ T = np .array (H [0 :- 1 , - 1 ])
73
71
74
72
return R , T
75
73
76
74
77
75
def update_homogeneous_matrix (Hin , R , T ):
78
76
79
- H = np .zeros ((3 , 3 ))
80
-
81
- H [0 , 0 ] = R [0 , 0 ]
82
- H [1 , 0 ] = R [1 , 0 ]
83
- H [0 , 1 ] = R [0 , 1 ]
84
- H [1 , 1 ] = R [1 , 1 ]
85
- H [2 , 2 ] = 1.0
77
+ r_size = R .shape [0 ]
78
+ H = np .zeros ((r_size + 1 , r_size + 1 ))
86
79
87
- H [0 , 2 ] = T [0 ]
88
- H [1 , 2 ] = T [1 ]
80
+ H [0 :r_size , 0 :r_size ] = R
81
+ H [0 :r_size , r_size ] = T
82
+ H [r_size , r_size ] = 1.0
89
83
90
84
if Hin is None :
91
85
return H
@@ -124,6 +118,28 @@ def svd_motion_estimation(previous_points, current_points):
124
118
return R , t
125
119
126
120
121
+ def plot_points (previous_points , current_points , figure ):
122
+ # for stopping simulation with the esc key.
123
+ plt .gcf ().canvas .mpl_connect (
124
+ 'key_release_event' ,
125
+ lambda event : [exit (0 ) if event .key == 'escape' else None ])
126
+ # if previous_points.shape[0] == 3:
127
+ # plt.clf()
128
+ # axes = figure.add_subplot(111, projection='3d')
129
+ # axes.scatter(previous_points[0, :], previous_points[1, :],
130
+ # previous_points[2, :], c="r", marker=".")
131
+ # axes.scatter(current_points[0, :], current_points[1, :],
132
+ # current_points[2, :], c="b", marker=".")
133
+ # axes.scatter(0.0, 0.0, 0.0, c="r", marker="x")
134
+ # figure.canvas.draw()
135
+ # else:
136
+ plt .cla ()
137
+ plt .plot (previous_points [0 , :], previous_points [1 , :], ".r" )
138
+ plt .plot (current_points [0 , :], current_points [1 , :], ".b" )
139
+ plt .plot (0.0 , 0.0 , "xr" )
140
+ plt .axis ("equal" )
141
+
142
+
127
143
def main ():
128
144
print (__file__ + " start!!" )
129
145
@@ -153,5 +169,37 @@ def main():
153
169
print ("T:" , T )
154
170
155
171
172
+ def main_3d_points ():
173
+ print (__file__ + " start!!" )
174
+
175
+ # simulation parameters for 3d point set
176
+ nPoint = 1000
177
+ fieldLength = 50.0
178
+ motion = [0.5 , 2.0 , - 5 , np .deg2rad (- 10.0 )] # [x[m],y[m],z[m],roll[deg]]
179
+
180
+ nsim = 3 # number of simulation
181
+
182
+ for _ in range (nsim ):
183
+
184
+ # previous points
185
+ px = (np .random .rand (nPoint ) - 0.5 ) * fieldLength
186
+ py = (np .random .rand (nPoint ) - 0.5 ) * fieldLength
187
+ pz = (np .random .rand (nPoint ) - 0.5 ) * fieldLength
188
+ previous_points = np .vstack ((px , py , pz ))
189
+
190
+ # current points
191
+ cx = [math .cos (motion [3 ]) * x - math .sin (motion [3 ]) * z + motion [0 ]
192
+ for (x , z ) in zip (px , pz )]
193
+ cy = [y + motion [1 ] for y in py ]
194
+ cz = [math .sin (motion [3 ]) * x + math .cos (motion [3 ]) * z + motion [2 ]
195
+ for (x , z ) in zip (px , pz )]
196
+ current_points = np .vstack ((cx , cy , cz ))
197
+
198
+ R , T = icp_matching (previous_points , current_points )
199
+ print ("R:" , R )
200
+ print ("T:" , T )
201
+
202
+
156
203
if __name__ == '__main__' :
157
204
main ()
205
+ main_3d_points ()
0 commit comments