Miscellaneous files for experimentation

Iterative Closest Point (ICP) SLAM example

author: Atsushi Sakai (@Atsushi_twi)

misc.iterative_closest_point.ICP_matching(ppoints, cpoints, time)

Iterative Closest Point matching

• input

ppoints: 2D points in the previous frame cpoints: 2D points in the current frame

• output

R: Rotation matrix T: Translation vector

misc.iterative_closest_point.SVD_motion_estimation(ppoints, cpoints)

performs single value decomposition to determine rotation and translation

• input

ppoints: 2D points in the previous frame cpoints: 2D points in the current frame

• output

R: rotation angle t: translation

misc.iterative_closest_point.nearest_neighbor_assosiation(ppoints, cpoints)

Associates new LIDAR points with previous LIDAR points

• input

ppoints: 2D points in the previous frame cpoints: 2D points in the current frame

• output

inds: indices error: normalized difference in current and previous 2D points

misc.iterative_closest_point.update_homogeneous_matrix(Hin, R, T)

Update the homogeneous matrix (translation and rotation matrices combined)

• input

Hin: initial/previous homogeneous matrix R: rotation matrix T: Translation matrix

• output

H: updated homogeneous matrix

H

The updated homogeneous matrix.

Hin

The initial/previous homogeneous matrix.

R

The rotation matrix.

T

The translation matrix.