import numpy as np
import matplotlib.pyplot as plt
1. Known data association
def generate_data(num, R = np.eye(2), t = np.zeros([2,1])):
data = np.zeros([2,num])
data[0,:] = range(num)
data[1,:] = 0.2 * data[0,:] * np.sin(0.5 * data[0,:])
data = R @ data + t
return data
def plot_data(data1, data2, data1_label = 'data1', data2_label = 'data2', pt_size1 = 6, pt_size2 = 6):
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(111)
ax.axis('equal')
ax.plot(data1[0], data1[1], color='blue', marker='o', linestyle=":", label= data1_label, markersize = pt_size1)
ax.plot(data2[0], data2[1], color='red', marker='o', linestyle=":", label= data2_label, markersize = pt_size2)
ax.legend()
return ax
# generate data
num = 30
angle = np.pi / 4
R = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
T = np.array([[2],[5]])
p = generate_data(num, R, T)
q = generate_data(num)
ax = plot_data(q, p, 'q', 'p')
plt.show()
# rotation and translation from p to q
true_R = R.transpose()
true_T = -T
![png](icp_example_files/icp_example_3_0.png)
# equal weights pn
pn = np.ones(num)
p0 = np.zeros([2,1])
p0[0, 0] = (p[0, :] * pn).sum() / pn.sum()
p0[1, 0] = (p[1, :] * pn).sum() / pn.sum()
q0 = np.zeros([2,1])
q0[0, 0] = (q[0, :] * pn).sum() / pn.sum()
q0[1, 0] = (q[1, :] * pn).sum() / pn.sum()
H = np.zeros([2,2])
for i in range(num):
b = q[:,i] - q0[:,0]
a = p[:,i] - p0[:,0]
H += np.outer(a, b) * pn[i]
u, s, vh = np.linalg.svd(H)
cal_R = (u @ vh).transpose()
cal_t = q0 - cal_R @ p0
# now shift p by cal_R and cal_t
cal_p = cal_R @ p + cal_t
ax = plot_data(q, cal_p, 'q', 'cal_p', 8, 4)
plt.show()
![png](icp_example_files/icp_example_4_0.png)
2. Unkown data association
Closest points (Vanilla ICP)
def centralize(p, q):
# get center
p0 = np.zeros([2,1])
p0[0,0] = p[0, :].sum() / p.shape[1]
p0[1,0] = p[1, :].sum() / p.shape[1]
q0 = np.zeros([2,1])
q0[0,0] = q[0, :].sum() / q.shape[1]
q0[1,0] = q[1, :].sum() / q.shape[1]
# move p to center of q
p = p - p0 + q0
return p
new_p = centralize(p,q)
ax = plot_data(q, new_p, 'q', "p'")
plt.show()
![png](icp_example_files/icp_example_8_0.png)
def get_l2Norm(p1, p2):
return np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
def find_closest_pair(p, q):
pair = []
for i in range(p.shape[1]):
min_norm = np.inf
correspond_id = -1
for j in range(q.shape[1]):
norm = get_l2Norm(p[:,i], q[:,j])
if norm < min_norm:
min_norm = norm
correspond_id = j
pair.append((i, correspond_id))
return pair
def draw_correspondencies(p, q, pair, ax):
for i, j in pair:
x = [p[0, i], q[0, j]]
y = [p[1, i], q[1, j]]
ax.plot(x, y, color = 'grey')
def compute_error(p,q, pair):
error = 0.0
for i, j in pair:
error += get_l2Norm(p[:,i], q[:,j])
return error
ax = plot_data(q, new_p, 'q', "p'")
pair = find_closest_pair(new_p, q)
draw_correspondencies(new_p, q, pair, ax)
plt.show()
![png](icp_example_files/icp_example_10_0.png)
def compute_R_t(p, q, pair, pn):
p0 = np.zeros([2,1])
p0[0, 0] = (p[0, :] * pn).sum() / pn.sum()
p0[1, 0] = (p[1, :] * pn).sum() / pn.sum()
q0 = np.zeros([2,1])
q0[0, 0] = (q[0, :] * pn).sum() / pn.sum()
q0[1, 0] = (q[1, :] * pn).sum() / pn.sum()
H = np.zeros([2,2])
for i, j in pair:
b = q[:,j] - q0[:,0]
a = p[:,i] - p0[:,0]
H += np.outer(a, b) * pn[i]
u, s, vh = np.linalg.svd(H)
cal_R = (u @ vh).transpose()
cal_t = q0 - cal_R @ p0
return cal_R, cal_t
# equal weights pn
pn = np.ones(num)
p = centralize(p,q)
pair = find_closest_pair(p, q)
cal_R, cal_t = compute_R_t(p, q, pair, pn)
# shift p to new location
p = cal_R @ p + cal_t
plot_data(q, p, 'q', "p'", 8, 4)
![png](icp_example_files/icp_example_13_1.png)
# generate data
num = 30
angle = np.pi / 4
R = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
T = np.array([[2],[5]])
p = generate_data(num, R, T)
q = generate_data(num)
show_frame = [1, 3, 5]
vanilla_error = []
# 5 is enough for convergence
for i in range(10):
central_p = centralize(p,q)
pair = find_closest_pair(central_p, q)
cal_R, cal_t = compute_R_t(p, q, pair, pn)
# shift p to new location
p = cal_R @ p + cal_t
vanilla_error.append(compute_error(p, q, pair))
if i+1 in show_frame:
ax = plot_data(q, p, 'q', "p'", 8, 4)
ax.set_title('iteration: {}'.format(i+1))
plt.show()
![png](icp_example_files/icp_example_15_0.png)
![png](icp_example_files/icp_example_15_1.png)
![png](icp_example_files/icp_example_15_2.png)
3. non-linear least square
def Jacobian(point, guess):
tx = guess[0,0]
ty = guess[1,0]
theta = guess[2,0]
J = np.zeros([2, 3])
J[0,0] = 1
J[1,1] = 1
J[0,2] = -np.sin(theta) * point[0] - np.cos(theta) * point[1]
J[1,2] = np.cos(theta) * point[0] - np.sin(theta) * point[1]
return J
def errorVector(p, q, guess):
tx = guess[0,0]
ty = guess[1,0]
T = np.array([[tx],[ty]])
theta = guess[2,0]
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
# match dimension
xn = p.reshape([2,1])
yn = q.reshape([2,1])
error = (R @ xn + T - yn)
return error
def generate_R_t(guess):
tx = guess[0,0]
ty = guess[1,0]
t = np.array([[tx],[ty]])
theta = guess[2,0]
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
return R, t
def point_to_point(p, q, pair, initial_guess, pn):
H = np.zeros([3,3])
b = np.zeros([1,3])
for i, j in pair:
J = Jacobian(p[:,i], initial_guess) * pn[i]
error = errorVector(p[:,i], q[:,j], initial_guess) * pn[i]
H += J.transpose() @ J
b += error.transpose() @ J
# -b' = H * delta_parameter
delta = (- b @ np.linalg.inv(H)).transpose()
update_guess = initial_guess + delta
return update_guess
# equal weights pn
pn = np.ones(num)
# generate data
num = 30
angle = np.pi / 4
R = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
T = np.array([[2],[5]])
p = generate_data(num, R, T)
q = generate_data(num)
initial_guess = np.array([[0.0],
[0.0],
[0.0]])
p2point_error = []
show_frame = [1, 3, 5, 7]
# 5 is enough for convergence
for i in range(10):
central_p = centralize(p,q)
pair = find_closest_pair(central_p, q)
initial_guess = point_to_point(p, q, pair, initial_guess, pn)
cal_R, cal_t = generate_R_t(initial_guess)
# shift p to new location
p = cal_R @ p + cal_t
p2point_error.append(compute_error(p, q, pair))
if i+1 in show_frame:
ax = plot_data(q, p, 'q', "p'", 8, 4)
ax.set_title('iteration: {}'.format(i+1))
plt.show()
![png](icp_example_files/icp_example_20_0.png)
![png](icp_example_files/icp_example_20_1.png)
![png](icp_example_files/icp_example_20_2.png)
![png](icp_example_files/icp_example_20_3.png)
def get_normal(point):
nx = - point[1]
ny = point[0]
if nx == 0 and ny == 0:
return np.array([[0],[1]])
return np.array([[nx],[ny]]) / np.sqrt(nx**2 + ny**2)
def Jacobian(point, guess, normal):
tx = guess[0,0]
ty = guess[1,0]
theta = guess[2,0]
dR = np.array([[-np.sin(theta), -np.cos(theta)],
[np.cos(theta), -np.sin(theta)]])
J = np.zeros([1, 3])
J[0,0] = normal[0,0]
J[0,1] = normal[1,0]
# match dimension
x = point.reshape([2,1])
J[0,2] = normal.transpose() @ dR @ x
return J
def errorVector(p, q, guess, normal):
tx = guess[0,0]
ty = guess[1,0]
T = np.array([[tx],[ty]])
theta = guess[2,0]
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
# match dimension
xn = p.reshape([2,1])
yn = q.reshape([2,1])
error = (normal.transpose() @ (R @ xn + T - yn))
return error
def point_to_plan(p, q, pair, initial_guess, pn):
H = np.zeros([3,3])
b = np.zeros([1,3])
for i, j in pair:
normal = get_normal(p[:,i])
J = Jacobian(p[:,i], initial_guess, normal) * pn[i]
error = errorVector(p[:,i], q[:,j], initial_guess, normal) * pn[i]
H += J.transpose() @ J
b += error.transpose() @ J
# -b' = H * delta_parameter
delta = (- b @ np.linalg.inv(H)).transpose()
update_guess = initial_guess + delta
return update_guess
# equal weights pn
pn = np.ones(num)
# generate data
num = 30
angle = np.pi / 4
R = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
T = np.array([[2],[5]])
p = generate_data(num, R, T)
q = generate_data(num)
initial_guess = np.array([[0.0],
[0.0],
[0.0]])
p2plane_error = []
show_frame = [1, 3, 5, 7]
# 5 is enough for convergence
for i in range(10):
central_p = centralize(p,q)
pair = find_closest_pair(central_p, q)
initial_guess = point_to_plan(p, q, pair, initial_guess, pn)
cal_R, cal_t = generate_R_t(initial_guess)
# shift p to new location
p = cal_R @ p + cal_t
p2plane_error.append(compute_error(p, q, pair))
if i+1 in show_frame:
ax = plot_data(q, p, 'q', "p'", 8, 4)
ax.set_title('iteration: {}'.format(i+1))
plt.show()
![png](icp_example_files/icp_example_25_0.png)
![png](icp_example_files/icp_example_25_1.png)
![png](icp_example_files/icp_example_25_2.png)
![png](icp_example_files/icp_example_25_3.png)
def Jacobian(point, guess, normal1, normal2):
tx = guess[0,0]
ty = guess[1,0]
theta = guess[2,0]
dR = np.array([[-np.sin(theta), -np.cos(theta)],
[np.cos(theta), -np.sin(theta)]])
J = np.zeros([1, 3])
J[0,0] = normal1[0,0] + normal2[0,0]
J[0,1] = normal1[1,0] + normal2[1,0]
# match dimension
x = point.reshape([2,1])
J[0,2] = (normal1.transpose() @ dR @ x) + (normal2.transpose() @ dR @ x)
return J
def errorVector(p, q, guess, normal1, normal2):
tx = guess[0,0]
ty = guess[1,0]
T = np.array([[tx],[ty]])
theta = guess[2,0]
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
# match dimension
xn = p.reshape([2,1])
yn = q.reshape([2,1])
error = ((normal1.transpose() + normal2.transpose()) @ (R @ xn + T - yn))
return error
def point_to_plan_sym(p, q, pair, initial_guess, pn):
H = np.zeros([3,3])
b = np.zeros([1,3])
for i, j in pair:
normal1 = get_normal(p[:,i])
normal2 = get_normal(q[:,j])
J = Jacobian(p[:,i], initial_guess, normal1, normal2) * pn[i]
error = errorVector(p[:,i], q[:,j], initial_guess, normal1, normal2) * pn[i]
H += J.transpose() @ J
b += error.transpose() @ J
# -b' = H * delta_parameter
delta = (- b @ np.linalg.inv(H)).transpose()
update_guess = initial_guess + delta
return update_guess
# equal weights pn
pn = np.ones(num)
# generate data
num = 30
angle = np.pi / 4
R = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
T = np.array([[2],[5]])
p = generate_data(num, R, T)
q = generate_data(num)
initial_guess = np.array([[0.0],
[0.0],
[0.0]])
p2plane_sym_error = []
show_frame = [1, 3, 5, 7]
# 5 is enough for convergence
for i in range(10):
central_p = centralize(p,q)
pair = find_closest_pair(central_p, q)
initial_guess = point_to_plan_sym(p, q, pair, initial_guess, pn)
cal_R, cal_t = generate_R_t(initial_guess)
# shift p to new location
p = cal_R @ p + cal_t
p2plane_sym_error.append(compute_error(p, q, pair))
if i+1 in show_frame:
ax = plot_data(q, p, 'q', "p'", 8, 4)
ax.set_title('iteration: {}'.format(i+1))
plt.show()
![png](icp_example_files/icp_example_29_0.png)
![png](icp_example_files/icp_example_29_1.png)
![png](icp_example_files/icp_example_29_2.png)
![png](icp_example_files/icp_example_29_3.png)
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(111)
iteration = range(1, 11)
ax.plot(iteration, vanilla_error, label = "vanilla ICP #svd", color = 'blue')
ax.plot(iteration, p2point_error, label = "point to point", color = 'red')
ax.plot(iteration, p2plane_error, label = "point to plane", color = 'orange')
ax.plot(iteration, p2plane_sym_error, label = "point to plane symmetrical", color = 'grey')
ax.legend()
ax.set_xlabel("Iteration")
ax.set_ylabel("Error")
plt.show()
![png](icp_example_files/icp_example_31_0.png)
use robust kernel to reject outlier (based on point to point)
# generate data
num = 30
angle = np.pi / 4
R = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
T = np.array([[2],[5]])
p = generate_data(num, R, T)
p[:,10] = np.array([-10, 30])
p[:,20] = np.array([0, 40])
q = generate_data(num)
show_frame = [1, 3, 5]
for i in range(5):
central_p = centralize(p,q)
pair = find_closest_pair(central_p, q)
cal_R, cal_t = compute_R_t(p, q, pair, pn)
# shift p to new location
p = cal_R @ p + cal_t
if i+1 in show_frame:
ax = plot_data(q, p, 'q', "p'", 8, 4)
ax.set_title('iteration: {}'.format(i+1))
plt.show()
![png](icp_example_files/icp_example_33_0.png)
![png](icp_example_files/icp_example_33_1.png)
![png](icp_example_files/icp_example_33_2.png)
def Jacobian(point, guess):
tx = guess[0,0]
ty = guess[1,0]
theta = guess[2,0]
J = np.zeros([2, 3])
J[0,0] = 1
J[1,1] = 1
J[0,2] = -np.sin(theta) * point[0] - np.cos(theta) * point[1]
J[1,2] = np.cos(theta) * point[0] - np.sin(theta) * point[1]
return J
def errorVector(p, q, guess):
tx = guess[0,0]
ty = guess[1,0]
T = np.array([[tx],[ty]])
theta = guess[2,0]
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
# match dimension
xn = p.reshape([2,1])
yn = q.reshape([2,1])
error = (R @ xn + T - yn)
return error
def generate_R_t(guess):
tx = guess[0,0]
ty = guess[1,0]
t = np.array([[tx],[ty]])
theta = guess[2,0]
R = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
return R, t
def centralize_with_weight(p, q, pn):
# get center
p0 = np.zeros([2,1])
p0[0,0] = (p[0, :] * pn).sum() / pn.sum()
p0[1,0] = (p[1, :] * pn).sum() / pn.sum()
q0 = np.zeros([2,1])
q0[0,0] = q[0, :].sum() / q.shape[1]
q0[1,0] = q[1, :].sum() / q.shape[1]
# move p to center of q
p = p - p0 + q0
return p
# set threshold for error
def point_to_point_threshold(p, q, pair, initial_guess, pn, threshold):
H = np.zeros([3,3])
b = np.zeros([1,3])
for i, j in pair:
error = errorVector(p[:,i], q[:,j], initial_guess)
if (np.linalg.norm(error) >= threshold):
pn[i] = 0.0
else:
pn[i] = 1.0
J = Jacobian(p[:,i], initial_guess) * pn[i]
H += J.transpose() @ J
b += (error.transpose() * pn[i]) @ J
# -b' = H * delta_parameter
delta = (- b @ np.linalg.inv(H)).transpose()
update_guess = initial_guess + delta
return update_guess
# equal weights pn
pn = np.ones(num)
# generate data
num = 30
angle = np.pi / 4
R = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
T = np.array([[2],[5]])
p = generate_data(num, R, T)
p[:,10] = np.array([-10, 30])
p[:,20] = np.array([0, 40])
q = generate_data(num)
initial_guess = np.array([[0.0],
[0.0],
[0.0]])
show_frame = [1, 3, 5, 7,30]
# 5 is enough for convergence
for i in range(10):
central_p = centralize_with_weight(p,q,pn)
pair = find_closest_pair(central_p, q)
initial_guess = point_to_point_threshold(p, q, pair, initial_guess, pn, 15)
cal_R, cal_t = generate_R_t(initial_guess)
# shift p to new location
p = cal_R @ p + cal_t
ax = plot_data(q, p, 'q', "p'", 8, 4)
ax.set_title('iteration: {}'.format(i+1))
plt.show()
![png](icp_example_files/icp_example_36_0.png)
![png](icp_example_files/icp_example_36_1.png)
![png](icp_example_files/icp_example_36_2.png)
![png](icp_example_files/icp_example_36_3.png)
![png](icp_example_files/icp_example_36_4.png)
![png](icp_example_files/icp_example_36_5.png)
![png](icp_example_files/icp_example_36_6.png)
![png](icp_example_files/icp_example_36_7.png)
![png](icp_example_files/icp_example_36_8.png)
![png](icp_example_files/icp_example_36_9.png)