|
5 | 5 |
|
6 | 6 | import math
|
7 | 7 |
|
8 |
| -# from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import |
| 8 | +from mpl_toolkits.mplot3d import Axes3D # noqa: F401 unused import |
9 | 9 | import matplotlib.pyplot as plt
|
10 | 10 | import numpy as np
|
11 | 11 |
|
@@ -34,8 +34,8 @@ def icp_matching(previous_points, current_points):
|
34 | 34 |
|
35 | 35 | if show_animation:
|
36 | 36 | 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') |
39 | 39 |
|
40 | 40 | while dError >= EPS:
|
41 | 41 | count += 1
|
@@ -123,21 +123,21 @@ def plot_points(previous_points, current_points, figure):
|
123 | 123 | plt.gcf().canvas.mpl_connect(
|
124 | 124 | 'key_release_event',
|
125 | 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") |
| 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 | 141 |
|
142 | 142 |
|
143 | 143 | def main():
|
|
0 commit comments