Skip to content

Commit 3abc9c6

Browse files
committed
enable 3d plot
1 parent bf2d9df commit 3abc9c6

File tree

1 file changed

+18
-18
lines changed

1 file changed

+18
-18
lines changed

SLAM/iterative_closest_point/iterative_closest_point.py

Lines changed: 18 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55

66
import math
77

8-
# from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
8+
from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import
99
import matplotlib.pyplot as plt
1010
import numpy as np
1111

@@ -34,8 +34,8 @@ def icp_matching(previous_points, current_points):
3434

3535
if show_animation:
3636
fig = plt.figure()
37-
# if previous_points.shape[0] == 3:
38-
# fig.add_subplot(111, projection='3d')
37+
if previous_points.shape[0] == 3:
38+
fig.add_subplot(111, projection='3d')
3939

4040
while dError >= EPS:
4141
count += 1
@@ -123,21 +123,21 @@ def plot_points(previous_points, current_points, figure):
123123
plt.gcf().canvas.mpl_connect(
124124
'key_release_event',
125125
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")
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")
141141

142142

143143
def main():

0 commit comments

Comments
 (0)