Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
13 changes: 4 additions & 9 deletions SLAM/ICPMatching/icp_matching.py
Original file line number Diff line number Diff line change
Expand Up @@ -89,15 +89,10 @@ def update_homogeneous_matrix(Hin, R, T):

def nearest_neighbor_association(previous_points, current_points):

# calc the sum of residual errors
delta_points = previous_points - current_points
d = np.linalg.norm(delta_points, axis=0)
error = sum(d)

# calc index with nearest neighbor assosiation
d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1)
- np.tile(previous_points, (1, current_points.shape[1])), axis=0)
indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1)
distances = np.linalg.norm(
current_points[:, :, np.newaxis] - previous_points[:, np.newaxis, :], axis=0)
indexes = np.argmin(distances, axis=1)
error = distances[np.arange(current_points.shape[1]), indexes].sum()

return indexes, error

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,10 @@ This is a 2D ICP matching example with singular value decomposition.
It can calculate a rotation matrix and a translation vector between
points to points.

Each current point is paired with its nearest point in the previous cloud.
The convergence residual sums distances over these pairs, so the scans
may have different point counts or storage order.

.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif

Code Link
Expand Down
11 changes: 11 additions & 0 deletions tests/test_iterative_closest_point.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
import numpy as np
import conftest
from SLAM.ICPMatching import icp_matching as m

Expand All @@ -12,5 +13,15 @@ def test_2():
m.main_3d_points()


def test_residual_uses_nearest_neighbors():
previous = np.array([[0., 3., 8.], [0., 0., 0.]])
for current, expected, residual in [
(previous[:, [2, 0, 1]], [2, 0, 1], 0.),
(np.array([[8., 1.], [0., 0.]]), [2, 0], 1.)]:
indexes, error = m.nearest_neighbor_association(previous, current)
np.testing.assert_array_equal(indexes, expected)
assert error == residual


if __name__ == '__main__':
conftest.run_this_test(__file__)