mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-02-11 11:57:42 -05:00
154 lines
3.9 KiB
Python
154 lines
3.9 KiB
Python
"""
|
||
Iterative Closest Point (ICP) SLAM example
|
||
author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı
|
||
"""
|
||
|
||
import math
|
||
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
|
||
# ICP parameters
|
||
EPS = 0.0001
|
||
MAX_ITER = 100
|
||
|
||
show_animation = True
|
||
|
||
|
||
def icp_matching(previous_points, current_points):
|
||
"""
|
||
Iterative Closest Point matching
|
||
- input
|
||
previous_points: 2D points in the previous frame
|
||
current_points: 2D points in the current frame
|
||
- output
|
||
R: Rotation matrix
|
||
T: Translation vector
|
||
"""
|
||
H = None # homogeneous transformation matrix
|
||
|
||
dError = 1000.0
|
||
preError = 1000.0
|
||
count = 0
|
||
|
||
while dError >= EPS:
|
||
count += 1
|
||
|
||
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.plot(previous_points[0, :], previous_points[1, :], ".r")
|
||
plt.plot(current_points[0, :], current_points[1, :], ".b")
|
||
plt.plot(0.0, 0.0, "xr")
|
||
plt.axis("equal")
|
||
plt.pause(0.1)
|
||
|
||
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 = abs(preError - error)
|
||
preError = error
|
||
print("Residual:", error)
|
||
|
||
if dError <= EPS:
|
||
print("Converge", error, dError, count)
|
||
break
|
||
elif MAX_ITER <= count:
|
||
print("Not Converge...", error, dError, count)
|
||
break
|
||
|
||
R = np.array(H[0:2, 0:2])
|
||
T = np.array(H[0:2, 2])
|
||
|
||
return R, T
|
||
|
||
|
||
def update_homogeneous_matrix(Hin, R, T):
|
||
|
||
H = np.zeros((3, 3))
|
||
|
||
H[0, 0] = R[0, 0]
|
||
H[1, 0] = R[1, 0]
|
||
H[0, 1] = R[0, 1]
|
||
H[1, 1] = R[1, 1]
|
||
H[2, 2] = 1.0
|
||
|
||
H[0, 2] = T[0]
|
||
H[1, 2] = T[1]
|
||
|
||
if Hin is None:
|
||
return H
|
||
else:
|
||
return Hin @ H
|
||
|
||
|
||
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)
|
||
|
||
return indexes, error
|
||
|
||
|
||
def svd_motion_estimation(previous_points, current_points):
|
||
pm = np.mean(previous_points, axis=1)
|
||
cm = np.mean(current_points, axis=1)
|
||
|
||
p_shift = previous_points - pm[:, np.newaxis]
|
||
c_shift = current_points - cm[:, np.newaxis]
|
||
|
||
W = c_shift @ p_shift.T
|
||
u, s, vh = np.linalg.svd(W)
|
||
|
||
R = (u @ vh).T
|
||
t = pm - (R @ cm)
|
||
|
||
return R, t
|
||
|
||
|
||
def main():
|
||
print(__file__ + " start!!")
|
||
|
||
# simulation parameters
|
||
nPoint = 1000
|
||
fieldLength = 50.0
|
||
motion = [0.5, 2.0, np.deg2rad(-10.0)] # movement [x[m],y[m],yaw[deg]]
|
||
|
||
nsim = 3 # number of simulation
|
||
|
||
for _ in range(nsim):
|
||
|
||
# previous points
|
||
px = (np.random.rand(nPoint) - 0.5) * fieldLength
|
||
py = (np.random.rand(nPoint) - 0.5) * fieldLength
|
||
previous_points = np.vstack((px, py))
|
||
|
||
# current points
|
||
cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0]
|
||
for (x, y) in zip(px, py)]
|
||
cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
|
||
for (x, y) in zip(px, py)]
|
||
current_points = np.vstack((cx, cy))
|
||
|
||
R, T = icp_matching(previous_points, current_points)
|
||
print("R:", R)
|
||
print("T:", T)
|
||
|
||
|
||
if __name__ == '__main__':
|
||
main()
|