Skip to content

fix ICP negative dError. Issue #446 #447

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 7 commits into from
Jan 10, 2021
Merged

fix ICP negative dError. Issue #446 #447

Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
f3d28d0
fix ICP negative dError
Dec 21, 2020
7a8c035
add 2 spaces before inline comment
Dec 21, 2020
2f8aae9
reduce comment
Dec 21, 2020
1d59520
Fix indention
Dec 31, 2020
d728970
ICP negative dError mistakes correction
Dec 31, 2020
ce59891
Make code pretty
Jan 4, 2021
0c8b4bf
Correct whitespace
Jan 4, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 12 additions & 8 deletions SLAM/iterative_closest_point/iterative_closest_point.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ def icp_matching(previous_points, current_points):
"""
H = None # homogeneous transformation matrix

dError = 1000.0
preError = 1000.0
dError = np.inf
preError = np.inf
count = 0

while dError >= EPS:
Expand All @@ -37,8 +37,9 @@ def icp_matching(previous_points, current_points):
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.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")
Expand All @@ -47,15 +48,18 @@ def icp_matching(previous_points, current_points):

indexes, error = nearest_neighbor_association(previous_points, current_points)
Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points)

# update current points
current_points = (Rt @ current_points) + Tt[:, np.newaxis]

H = update_homogeneous_matrix(H, Rt, Tt)
dError = preError - error
print("Residual:", error)

if dError < 0: # prevent matrix H changing, exit loop
print("Not Converge...", preError, dError, count)
break

dError = abs(preError - error)
preError = error
print("Residual:", error)
H = update_homogeneous_matrix(H, Rt, Tt)

if dError <= EPS:
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We can merge these break statements to line 55 like:

if 0 <= dError <= EPS:
    print("Converge", error, dError, count)
    break
elif dError < 0:
    print("Not Converge... Error increased. ", preError, dError, count)
    break
elif MAX_ITER <= count:
     print("Not Converge... Reached max iterations", error, dError, count)
     break

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I cannot see how we can merge break statements. Because it's important for H = update_homogeneous_matrix to be after dError < 0 (as at this break we take H status from previous iteration) and before dError <= EPS (as we need to update H before possible breaking for these 2 cases).

P.S. I thought it over, it is possible to move up 2 last break statements, but there will always be something between dError < 0 and the others. Moving them up, it is necessary to do dError = preError - error after them but before dError < 0. To my mind it becomes a mess in algorithm logic, so I suggest to keep code as is.
I share your wish to make things beautiful. But here I don't see a way to make it better.

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK. Make sense. Thanks.

print("Converge", error, dError, count)
Expand Down