From 96fe31ecb982e90a0ed0925a6bee9563a021f2c6 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Mon, 25 Jan 2021 17:03:20 +0100
Subject: [PATCH 01/12] 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
---
.../iterative_closest_point.py | 18 +++++++-----------
1 file changed, 7 insertions(+), 11 deletions(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index db252247a8..0635274aef 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -68,24 +68,20 @@ def icp_matching(previous_points, current_points):
print("Not Converge...", error, dError, count)
break
- R = np.array(H[0:2, 0:2])
- T = np.array(H[0:2, 2])
+ R = np.array(H[0:-1, 0:-1])
+ T = np.array(H[0:-1, -1])
return R, T
def update_homogeneous_matrix(Hin, R, T):
- H = np.zeros((3, 3))
+ r_size = R.shape[0]
+ H = np.zeros((r_size + 1, r_size + 1))
- H[0, 0] = R[0, 0]
- H[1, 0] = R[1, 0]
- H[0, 1] = R[0, 1]
- H[1, 1] = R[1, 1]
- H[2, 2] = 1.0
-
- H[0, 2] = T[0]
- H[1, 2] = T[1]
+ H[0:r_size, 0:r_size] = R
+ H[0:r_size, r_size] = T
+ H[r_size, r_size] = 1.0
if Hin is None:
return H
From 2c59fb9f1069f028ebb23e76c1e1d09f17947082 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Mon, 25 Jan 2021 17:03:57 +0100
Subject: [PATCH 02/12] Add test for 3d point cloud (with 2d visualization)
---
.../iterative_closest_point.py | 23 +++++++++++++++++++
1 file changed, 23 insertions(+)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 0635274aef..db04ffe0bf 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -148,6 +148,29 @@ def main():
print("R:", R)
print("T:", T)
+ # test for 3d point set
+ motion = [0.5, 2.0, -5, np.deg2rad(-10.0)] # [x[m],y[m],z[m],roll[deg]]
+
+ for _ in range(nsim):
+
+ # previous points
+ px = (np.random.rand(nPoint) - 0.5) * fieldLength
+ py = (np.random.rand(nPoint) - 0.5) * fieldLength
+ pz = (np.random.rand(nPoint) - 0.5) * fieldLength
+ previous_points = np.vstack((px, py, pz))
+
+ # current points
+ cx = [math.cos(motion[3]) * x - math.sin(motion[3]) * z + motion[0]
+ for (x, z) in zip(px, pz)]
+ cy = [y + motion[1] for y in py]
+ cz = [math.sin(motion[3]) * x + math.cos(motion[3]) * z + motion[2]
+ for (x, z) in zip(px, pz)]
+ current_points = np.vstack((cx, cy, cz))
+
+ R, T = icp_matching(previous_points, current_points)
+ print("R:", R)
+ print("T:", T)
+
if __name__ == '__main__':
main()
From 7aded2fe17e550edc0f65cc0367762d13ded327d Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Tue, 2 Feb 2021 16:41:08 +0100
Subject: [PATCH 03/12] Separate test for 3d points to main_3d_points
---
.../iterative_closest_point.py | 11 ++++++++++-
1 file changed, 10 insertions(+), 1 deletion(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index db04ffe0bf..346db38f36 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -148,9 +148,17 @@ def main():
print("R:", R)
print("T:", T)
- # test for 3d point set
+
+def main_3d_points():
+ print(__file__ + " start!!")
+
+ # simulation parameters for 3d point set
+ nPoint = 1000
+ fieldLength = 50.0
motion = [0.5, 2.0, -5, np.deg2rad(-10.0)] # [x[m],y[m],z[m],roll[deg]]
+ nsim = 3 # number of simulation
+
for _ in range(nsim):
# previous points
@@ -174,3 +182,4 @@ def main():
if __name__ == '__main__':
main()
+ main_3d_points()
From 3715a07a7c70b70b591d1566e23cf82b4f714d50 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Thu, 11 Feb 2021 16:15:39 +0100
Subject: [PATCH 04/12] Add test for ICP 3d
---
tests/test_iterative_closest_point.py | 3 +++
1 file changed, 3 insertions(+)
diff --git a/tests/test_iterative_closest_point.py b/tests/test_iterative_closest_point.py
index 3f212f7298..0375b04bb6 100644
--- a/tests/test_iterative_closest_point.py
+++ b/tests/test_iterative_closest_point.py
@@ -6,6 +6,9 @@ def test_1():
m.show_animation = False
m.main()
+def test_2():
+ m.show_animation = False
+ m.main_3d_points()
if __name__ == '__main__':
conftest.run_this_test(__file__)
From 5437994727b37863efe33e3846596adf211e0428 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Thu, 11 Feb 2021 16:56:10 +0100
Subject: [PATCH 05/12] Correct style
* Add space
---
tests/test_iterative_closest_point.py | 1 +
1 file changed, 1 insertion(+)
diff --git a/tests/test_iterative_closest_point.py b/tests/test_iterative_closest_point.py
index 0375b04bb6..86f547a05c 100644
--- a/tests/test_iterative_closest_point.py
+++ b/tests/test_iterative_closest_point.py
@@ -10,5 +10,6 @@ def test_2():
m.show_animation = False
m.main_3d_points()
+
if __name__ == '__main__':
conftest.run_this_test(__file__)
From 6287b9a3a8c6d703f7a4bbf721c9a2cdeefedd18 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Thu, 11 Feb 2021 17:08:36 +0100
Subject: [PATCH 06/12] Style correction
* Add more spaces
---
tests/test_iterative_closest_point.py | 1 +
1 file changed, 1 insertion(+)
diff --git a/tests/test_iterative_closest_point.py b/tests/test_iterative_closest_point.py
index 86f547a05c..3e726b5649 100644
--- a/tests/test_iterative_closest_point.py
+++ b/tests/test_iterative_closest_point.py
@@ -6,6 +6,7 @@ def test_1():
m.show_animation = False
m.main()
+
def test_2():
m.show_animation = False
m.main_3d_points()
From 74162e02b9cc4d3f54a9b449f32d15ea3c96d71d Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Thu, 11 Feb 2021 17:14:26 +0100
Subject: [PATCH 07/12] Add 3d visualizing for ICP
---
.../iterative_closest_point.py | 24 +++++++++++++------
1 file changed, 17 insertions(+), 7 deletions(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 346db38f36..5e4641c930 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -5,6 +5,7 @@
import math
+from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
import matplotlib.pyplot as plt
import numpy as np
@@ -19,8 +20,8 @@ def icp_matching(previous_points, current_points):
"""
Iterative Closest Point matching
- input
- previous_points: 2D points in the previous frame
- current_points: 2D points in the current frame
+ previous_points: 2D or 3D points in the previous frame
+ current_points: 2D or 3D points in the current frame
- output
R: Rotation matrix
T: Translation vector
@@ -35,15 +36,24 @@ def icp_matching(previous_points, current_points):
count += 1
if show_animation: # pragma: no cover
- plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
- plt.plot(previous_points[0, :], previous_points[1, :], ".r")
- plt.plot(current_points[0, :], current_points[1, :], ".b")
- plt.plot(0.0, 0.0, "xr")
- plt.axis("equal")
+ if previous_points.shape[0] == 3:
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ ax.scatter(previous_points[0, :], previous_points[1, :],
+ previous_points[2, :], c = "r", marker = ".")
+ ax.scatter(current_points[0, :], current_points[1, :],
+ current_points[2, :], c = "b", marker = ".")
+ ax.scatter(0.0, 0.0, 0.0, c = "r", marker = "x")
+ else:
+ plt.cla()
+ plt.plot(previous_points[0, :], previous_points[1, :], ".r")
+ plt.plot(current_points[0, :], current_points[1, :], ".b")
+ plt.plot(0.0, 0.0, "xr")
+ plt.axis("equal")
plt.pause(0.1)
indexes, error = nearest_neighbor_association(previous_points, current_points)
From bf50a065994759ddc8a1d77409f321b9e5a73144 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Thu, 11 Feb 2021 17:25:05 +0100
Subject: [PATCH 08/12] Style corrections
* Delete spaces
---
.../iterative_closest_point/iterative_closest_point.py | 10 +++++-----
1 file changed, 5 insertions(+), 5 deletions(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 5e4641c930..26d5dedfd9 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -43,11 +43,11 @@ def icp_matching(previous_points, current_points):
if previous_points.shape[0] == 3:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
- ax.scatter(previous_points[0, :], previous_points[1, :],
- previous_points[2, :], c = "r", marker = ".")
- ax.scatter(current_points[0, :], current_points[1, :],
- current_points[2, :], c = "b", marker = ".")
- ax.scatter(0.0, 0.0, 0.0, c = "r", marker = "x")
+ ax.scatter(previous_points[0, :], previous_points[1, :],
+ previous_points[2, :], c="r", marker=".")
+ ax.scatter(current_points[0, :], current_points[1, :],
+ current_points[2, :], c="b", marker=".")
+ ax.scatter(0.0, 0.0, 0.0, c="r", marker="x")
else:
plt.cla()
plt.plot(previous_points[0, :], previous_points[1, :], ".r")
From 56b319c7144a8ec8b16bd35ba7dc1d6e999ad535 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Thu, 11 Feb 2021 17:34:25 +0100
Subject: [PATCH 09/12] Style correction
* remove space
---
SLAM/iterative_closest_point/iterative_closest_point.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 26d5dedfd9..68c81cc85a 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -44,7 +44,7 @@ def icp_matching(previous_points, current_points):
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(previous_points[0, :], previous_points[1, :],
- previous_points[2, :], c="r", marker=".")
+ previous_points[2, :], c="r", marker=".")
ax.scatter(current_points[0, :], current_points[1, :],
current_points[2, :], c="b", marker=".")
ax.scatter(0.0, 0.0, 0.0, c="r", marker="x")
From e3d23a2d6ad3e79df5f0518ea39c80c441c6fa6e Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Fri, 19 Feb 2021 17:10:46 +0100
Subject: [PATCH 10/12] Separate plot drawing
* plot drawing in a separate function for both 2D and 3D versions
* figure creating before while loop
---
.../iterative_closest_point.py | 51 ++++++++++++-------
1 file changed, 32 insertions(+), 19 deletions(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 68c81cc85a..f4035f98db 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -1,6 +1,6 @@
"""
Iterative Closest Point (ICP) SLAM example
-author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
+author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı, Shamil Gemuev
"""
import math
@@ -32,28 +32,16 @@ def icp_matching(previous_points, current_points):
preError = np.inf
count = 0
+ if show_animation:
+ fig = plt.figure()
+ if previous_points.shape[0] == 3:
+ fig.add_subplot(111, projection='3d')
+
while dError >= EPS:
count += 1
if show_animation: # pragma: no cover
- # for stopping simulation with the esc key.
- plt.gcf().canvas.mpl_connect(
- 'key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
- if previous_points.shape[0] == 3:
- fig = plt.figure()
- ax = fig.add_subplot(111, projection='3d')
- ax.scatter(previous_points[0, :], previous_points[1, :],
- previous_points[2, :], c="r", marker=".")
- ax.scatter(current_points[0, :], current_points[1, :],
- current_points[2, :], c="b", marker=".")
- ax.scatter(0.0, 0.0, 0.0, c="r", marker="x")
- else:
- plt.cla()
- plt.plot(previous_points[0, :], previous_points[1, :], ".r")
- plt.plot(current_points[0, :], current_points[1, :], ".b")
- plt.plot(0.0, 0.0, "xr")
- plt.axis("equal")
+ plot_points(previous_points, current_points, fig)
plt.pause(0.1)
indexes, error = nearest_neighbor_association(previous_points, current_points)
@@ -130,6 +118,31 @@ def svd_motion_estimation(previous_points, current_points):
return R, t
+def plot_points(previous_points, current_points, figure):
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ if previous_points.shape[0] == 3:
+ # axes = figure.axes[0]
+ # axes.clear()
+ figure = plt.figure()
+ axes = figure.add_subplot(111, projection='3d')
+ axes.scatter(previous_points[0, :], previous_points[1, :],
+ previous_points[2, :], c="r", marker=".")
+ axes.scatter(current_points[0, :], current_points[1, :],
+ current_points[2, :], c="b", marker=".")
+ axes.scatter(0.0, 0.0, 0.0, c="r", marker="x")
+ # figure.canvas.draw()
+ # figure.canvas.flush_events()
+ else:
+ plt.cla()
+ plt.plot(previous_points[0, :], previous_points[1, :], ".r")
+ plt.plot(current_points[0, :], current_points[1, :], ".b")
+ plt.plot(0.0, 0.0, "xr")
+ plt.axis("equal")
+
+
def main():
print(__file__ + " start!!")
From 7aa502c2070ce725c150df9bc1658793b0136ae1 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Fri, 19 Feb 2021 17:25:18 +0100
Subject: [PATCH 11/12] Style correction
---
SLAM/iterative_closest_point/iterative_closest_point.py | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index f4035f98db..4a8fda10d6 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -129,9 +129,9 @@ def plot_points(previous_points, current_points, figure):
figure = plt.figure()
axes = figure.add_subplot(111, projection='3d')
axes.scatter(previous_points[0, :], previous_points[1, :],
- previous_points[2, :], c="r", marker=".")
+ previous_points[2, :], c="r", marker=".")
axes.scatter(current_points[0, :], current_points[1, :],
- current_points[2, :], c="b", marker=".")
+ current_points[2, :], c="b", marker=".")
axes.scatter(0.0, 0.0, 0.0, c="r", marker="x")
# figure.canvas.draw()
# figure.canvas.flush_events()
From b72b6f529eb11b183175380a1273e5bd72cb1e45 Mon Sep 17 00:00:00 2001
From: Shamil GEMUEV
Date: Wed, 31 Mar 2021 17:59:08 +0200
Subject: [PATCH 12/12] Comment 3d plot drawing
---
.../iterative_closest_point.py | 39 +++++++++----------
1 file changed, 18 insertions(+), 21 deletions(-)
diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py
index 4a8fda10d6..5ed01fa383 100644
--- a/SLAM/iterative_closest_point/iterative_closest_point.py
+++ b/SLAM/iterative_closest_point/iterative_closest_point.py
@@ -5,7 +5,7 @@
import math
-from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
+# from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
import matplotlib.pyplot as plt
import numpy as np
@@ -34,8 +34,8 @@ def icp_matching(previous_points, current_points):
if show_animation:
fig = plt.figure()
- if previous_points.shape[0] == 3:
- fig.add_subplot(111, projection='3d')
+ # if previous_points.shape[0] == 3:
+ # fig.add_subplot(111, projection='3d')
while dError >= EPS:
count += 1
@@ -123,24 +123,21 @@ def plot_points(previous_points, current_points, figure):
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
- if previous_points.shape[0] == 3:
- # axes = figure.axes[0]
- # axes.clear()
- figure = plt.figure()
- axes = figure.add_subplot(111, projection='3d')
- axes.scatter(previous_points[0, :], previous_points[1, :],
- previous_points[2, :], c="r", marker=".")
- axes.scatter(current_points[0, :], current_points[1, :],
- current_points[2, :], c="b", marker=".")
- axes.scatter(0.0, 0.0, 0.0, c="r", marker="x")
- # figure.canvas.draw()
- # figure.canvas.flush_events()
- else:
- plt.cla()
- plt.plot(previous_points[0, :], previous_points[1, :], ".r")
- plt.plot(current_points[0, :], current_points[1, :], ".b")
- plt.plot(0.0, 0.0, "xr")
- plt.axis("equal")
+ # if previous_points.shape[0] == 3:
+ # plt.clf()
+ # axes = figure.add_subplot(111, projection='3d')
+ # axes.scatter(previous_points[0, :], previous_points[1, :],
+ # previous_points[2, :], c="r", marker=".")
+ # axes.scatter(current_points[0, :], current_points[1, :],
+ # current_points[2, :], c="b", marker=".")
+ # axes.scatter(0.0, 0.0, 0.0, c="r", marker="x")
+ # figure.canvas.draw()
+ # else:
+ plt.cla()
+ plt.plot(previous_points[0, :], previous_points[1, :], ".r")
+ plt.plot(current_points[0, :], current_points[1, :], ".b")
+ plt.plot(0.0, 0.0, "xr")
+ plt.axis("equal")
def main():