Skip to content

Commit bf2d9df

Browse files
Add ICP support for 3d point clouds (AtsushiSakai#465)
* Add 3d support ICP * icp_matching function returns R,T corresponding to 2D or 3D set of points * update_homogeneuous_matrix - general operations for translation and rotation matrixes * Add test for 3d point cloud (with 2d visualization) * Separate test for 3d points to main_3d_points * Add test for ICP 3d * Correct style * Add space * Style correction * Add more spaces * Add 3d visualizing for ICP * Style corrections * Delete spaces * Style correction * remove space * Separate plot drawing * plot drawing in a separate function for both 2D and 3D versions * figure creating before while loop * Style correction * Comment 3d plot drawing Co-authored-by: Shamil GEMUEV <https://github.maf-roda.com/>
1 parent f17c991 commit bf2d9df

File tree

2 files changed

+76
-23
lines changed

2 files changed

+76
-23
lines changed

SLAM/iterative_closest_point/iterative_closest_point.py

Lines changed: 71 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
11
"""
22
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
44
"""
55

66
import math
77

8+
# from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
89
import matplotlib.pyplot as plt
910
import numpy as np
1011

@@ -19,8 +20,8 @@ def icp_matching(previous_points, current_points):
1920
"""
2021
Iterative Closest Point matching
2122
- 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
2425
- output
2526
R: Rotation matrix
2627
T: Translation vector
@@ -31,19 +32,16 @@ def icp_matching(previous_points, current_points):
3132
preError = np.inf
3233
count = 0
3334

35+
if show_animation:
36+
fig = plt.figure()
37+
# if previous_points.shape[0] == 3:
38+
# fig.add_subplot(111, projection='3d')
39+
3440
while dError >= EPS:
3541
count += 1
3642

3743
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)
4745
plt.pause(0.1)
4846

4947
indexes, error = nearest_neighbor_association(previous_points, current_points)
@@ -68,24 +66,20 @@ def icp_matching(previous_points, current_points):
6866
print("Not Converge...", error, dError, count)
6967
break
7068

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])
7371

7472
return R, T
7573

7674

7775
def update_homogeneous_matrix(Hin, R, T):
7876

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))
8679

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
8983

9084
if Hin is None:
9185
return H
@@ -124,6 +118,28 @@ def svd_motion_estimation(previous_points, current_points):
124118
return R, t
125119

126120

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+
127143
def main():
128144
print(__file__ + " start!!")
129145

@@ -153,5 +169,37 @@ def main():
153169
print("T:", T)
154170

155171

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+
156203
if __name__ == '__main__':
157204
main()
205+
main_3d_points()

tests/test_iterative_closest_point.py

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,5 +7,10 @@ def test_1():
77
m.main()
88

99

10+
def test_2():
11+
m.show_animation = False
12+
m.main_3d_points()
13+
14+
1015
if __name__ == '__main__':
1116
conftest.run_this_test(__file__)

0 commit comments

Comments
 (0)