def main():
#Import the cloud
pc = utils.load_pc('cloud_pca.csv')
###YOUR CODE HERE###
# Show the input point cloud
utils.view_pc([pc])
#Rotate the points to align with the XY plane
n = len(pc) #number of data in the point cloud
x = utils.convert_pc_to_matrix(pc)
x = x - numpy.mean(x, 1)
Q = x * x.T / (n - 1)
u, s, v = numpy.linalg.svd(Q)
v = v.T #Computes svd of covariance matrix Q=u*s*v'
#output of v from numpy.linalg.svd is actualy V'
#from Q = U S V'
y = v.T * x
pc_y = utils.convert_matrix_to_pc(y)
#Show the resulting point cloud
fig = utils.view_pc([pc_y])
#plt.gca().set_aspect('equal', adjustable='box')
#Rotate the points to align with the XY plane AND eliminate the noise
v_s = v.copy()
variances = numpy.multiply(s, s)
e = 0.001 # threshold of variance, below which that dimension will be eliminated
for id, variance in enumerate(variances):
if variance < e:
v_s[:, id] = numpy.zeros((v.shape[0], 1))
y_s = v_s.T * x
pc_ys = utils.convert_matrix_to_pc(y_s)
# Show the resulting point cloud
utils.view_pc([pc_ys])
# fit plane to original pc
x = utils.convert_pc_to_matrix(pc)
pt = numpy.mean(x, 1)
normal = v[:, variances < e]
fig = utils.view_pc([pc])
utils.draw_plane(fig,
normal,
pt, (0.1, 0.7, 0.1, 0.5),
length=[-0.5, 1],
width=[-0.5, 1])
###YOUR CODE HERE###
raw_input("Press enter to end:")
def main():
#load in the point cloud
pc = utils.load_pc('cloud_pca.csv')
#convert the cloud to a 3 x N matrix
mpc = utils.convert_pc_to_matrix(pc)
#add some stuff to the matrix (move the points by (1,1,1))
mpc = mpc + numpy.ones(mpc.shape)
#convert back to point cloud for plotting
pc2 = utils.convert_matrix_to_pc(mpc)
#plot the original point cloud
fig = utils.view_pc([pc])
#plot the moved point cloud in red ^ markers
fig = utils.view_pc([pc2], fig, ['r'], ['^'])
#draw a plane
fig = utils.draw_plane(fig,
numpy.matrix([0, 0, 1]).T,
numpy.matrix([0.5, 0.5, 0.5]).T,
(0.1, 0.7, 0.1, 0.5), [-1, 1], [-1, 1])
#utils.view_pc([mug_pc, pc_noise], ['b', 'r'], ['o', '^'])
#plt.show()
raw_input("Press enter to end:")
def main():
#Import the cloud
pc = utils.load_pc('cloud_pca.csv')
pc2 = pc
###YOUR CODE HERE###
# Show the input point cloud
fig = utils.view_pc([pc])
# Rotate the points to align with the XY plane
pc2_mat = utils.convert_pc_to_matrix(pc2)
mean_mat = mean(pc2_mat, axis=1)
size = pc2_mat.shape
minus_mat = zeros((size[0], size[1]))
minus_mat[0, :] = mean_mat[0]
minus_mat[1, :] = mean_mat[1]
minus_mat[2, :] = mean_mat[2]
pc2_mat = pc2_mat - minus_mat
q_mat = cov(pc2_mat)
u_mat, s_mat, v_mat_t = linalg.svd(q_mat)
v_mat = v_mat_t.T
new_pc2_mat = v_mat * pc2_mat
new_pc2_pc = utils.convert_matrix_to_pc(new_pc2_mat)
print '(w) V_mat =', v_mat.T
# Show the resulting point cloud
fig = utils.view_pc([new_pc2_pc], fig, color='r')
# Rotate the points to align with the XY plane AND eliminate the noise
v_elim_mat = diag(s_mat) * diag(s_mat)
for i in range(0, len(v_elim_mat)):
if v_elim_mat[i][i] < 0.0001:
elim_index = i
v_mat_elim = v_mat
v_mat_elim[elim_index, :] = 0
print '(W)v_mat = ', v_mat_elim.T
new_pc2_mat_elim = v_mat_elim * pc2_mat
new_pc2_pc_elim = utils.convert_matrix_to_pc(new_pc2_mat_elim)
# Show the resulting point cloud
fig = utils.view_pc([new_pc2_pc_elim], fig, color='y')
#fit a plane to the cloud
u_mat_mat = matrix(u_mat)
normal_vctr_mat = u_mat_mat[:, 2]
utils.draw_plane(fig,
normal_vctr_mat,
mean_mat,
color=(0.0, 1, 0.0, 0.5),
length=[-0.7, 0.7],
width=[-0.8, 0.8])
###YOUR CODE HERE###
raw_input("Press enter to end:")
def main():
#Import the cloud
pc_source = utils.load_pc('cloud_icp_source.csv')
###YOUR CODE HERE###
#pc_target = utils.load_pc('cloud_icp_target3.csv') # Change this to load in a different target
for tg in range(4):
if tg == 0:
pc_target = utils.load_pc('cloud_icp_target0.csv')
utils.view_pc([pc_source, pc_target], None, ['b', 'r'], ['o', '^'])
print 'test target 0:\n\n'
elif tg == 1:
pc_source = utils.load_pc('cloud_icp_source.csv')
pc_target = utils.load_pc('cloud_icp_target1.csv')
utils.view_pc([pc_source, pc_target], None, ['b', 'r'], ['o', '^'])
print 'test target 1:\n\n'
elif tg == 2:
pc_source = utils.load_pc('cloud_icp_source.csv')
pc_target = utils.load_pc('cloud_icp_target2.csv')
utils.view_pc([pc_source, pc_target], None, ['b', 'r'], ['o', '^'])
print 'test target 2:\n\n'
elif tg == 3:
pc_source = utils.load_pc('cloud_icp_source.csv')
pc_target = utils.load_pc('cloud_icp_target3.csv')
utils.view_pc([pc_source, pc_target], None, ['b', 'r'], ['o', '^'])
print 'test target 3:\n\n'
p = utils.convert_pc_to_matrix(pc_source)
q = utils.convert_pc_to_matrix(pc_target)
T_list = []
iteration = []
error_all = []
success = 0
print 'stop criterion: distance error converges to the threshold or not able to converge within 2000 iterations. So please wait for at most 2000 iterations, which takes only a few minutes'
raw_input('\npress enter to start\n')
for num in range(2000):
print 'iteration', num + 1, ':\n'
iteration.append(num + 1)
pf = numpy.matrix([[], [], []])
qf = numpy.matrix([[], [], []])
while p.shape[1] > 0:
i = random.choice(range(p.shape[1]))
j = numpy.argmin(numpy.linalg.norm(q - p[:, i], axis=0))
pf = numpy.hstack((pf, p[:, i]))
p = numpy.delete(p, i, 1)
qf = numpy.hstack((qf, q[:, j]))
q = numpy.delete(q, j, 1)
p = pf.copy()
q = qf.copy()
p_avg = p.sum(axis=1) / (p.shape[1] * 1.0)
q_avg = q.sum(axis=1) / (q.shape[1] * 1.0)
X = numpy.subtract(p, p_avg)
Y = numpy.subtract(q, q_avg)
u, s, w = numpy.linalg.svd(X * Y.T)
m = numpy.matrix([[1., 0., 0.], [0., 1., 0.],
[0., 0., numpy.linalg.det(w.T * u.T)]])
R = w.T * m * u.T
t = q_avg - R * p_avg
T = numpy.concatenate((R, t), axis=1)
T = numpy.concatenate((T, numpy.matrix([[0., 0., 0., 1.]])))
T_list.append(T)
fit_error = numpy.add(R * p, t) - q
error_all.append(numpy.linalg.norm(fit_error)**2)
print 'distance least square error:', numpy.linalg.norm(
fit_error)**2, '\n\n'
p = R * p + t
if tg == 3 and random.randint(
1, 20) == 1 and numpy.linalg.norm(fit_error)**2 > 0.1:
R_random = random_walk.random_walk()
p = R_random * (p - p_avg) + p_avg
R = R_random
t = p_avg - R_random * p_avg
T = numpy.concatenate((R, t), axis=1)
T = numpy.concatenate((T, numpy.matrix([[0., 0., 0., 1.]])))
T_list.append(T)
if numpy.linalg.norm(fit_error) < 0.1:
for i in range(len(T_list)):
if i == 0:
T_final = T_list[i]
else:
T_final = T_list[i] * T_final
success = 1
break
pc = utils.convert_pc_to_matrix(pc_source)
if success == 0:
for i in range(len(T_list)):
if i == 0:
T_final = T_list[i]
else:
T_final = T_list[i] * T_final
print 'transformation from source to target point cloud:\n'
print 'R =\n', T_final[:3, :3], '\n\nt =\n', T_final[:3, 3]
pc = T_final[:3, :3] * pc + T_final[:3, 3]
pc_source = utils.convert_matrix_to_pc(pc)
utils.view_pc([pc_source], None, ['b'], ['o'])
plt.axis([-0.15, 0.15, -0.15, 0.15])
plt.figure()
plt.title('ICP Error vs Iteration')
plt.plot(iteration, error_all, 'ro-')
plt.xlabel('Iteration')
plt.ylabel('Least squares error')
raw_input('press enter and test the next target\n')
plt.close()
plt.close()
plt.close()
###YOUR CODE HERE###
raw_input("\nPress enter to end:")
def main():
#Import the cloud
pc = utils.load_pc('cloud_pca.csv')
###YOUR CODE HERE###
# Show the input point cloud
utils.view_pc([pc])
#Rotate the points to align with the XY plane
for i in range(len(pc)):
if i == 0:
sum = pc[0]
else:
sum = sum + pc[i]
mu = sum / (i + 1)
for j in range(len(pc)):
if j == 0:
pcx = pc[j] - mu
else:
pcx = numpy.concatenate((pcx, pc[j] - mu), axis=1)
u, s, w = numpy.linalg.svd(pcx * pcx.T / j)
pcx_new = w * pcx
print 'W:\n', w
# Show the resulting point cloud
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
pc1 = utils.convert_matrix_to_pc(pcx_new)
utils.view_pc([pc1], fig)
ax.set_xlim(-1.0, 1.0)
ax.set_zlim(-1.0, 1.0)
plt.title('pca')
# Rotate the points to align with the XY plane AND eliminate the noise
wv = w.copy()
wk = w.copy()
for i in range(len(s)):
if s[i] <= 0.01:
wv = numpy.delete(wv, i, 0)
wk[i] = [0., 0., 0.]
pcx_new_2 = wk * pcx
print '\nWv:\n', wv
fig2 = plt.figure()
ax = fig2.add_subplot(111, projection='3d')
pc2 = utils.convert_matrix_to_pc(pcx_new_2)
utils.view_pc([pc2], fig2)
ax.set_xlim(-1.0, 1.0)
ax.set_zlim(-1.0, 1.0)
plt.title('pca and noise filtering')
# plot plane
fig = utils.view_pc([pc])
vector = numpy.matrix([[0., 0., 1.]])
planev = numpy.linalg.inv(w) * vector.T
planev = planev / numpy.linalg.norm(planev)
origin = numpy.matrix([[0., 0., 0.]])
planep = numpy.linalg.inv(w) * origin.T + mu
fig = utils.draw_plane(fig, planev, planep, (0.1, 0.7, 0.1, 0.5),
[-0.4, 0.9], [-0.4, 1])
plt.title('fitting plane with pca')
print '\nplane function is: \n', planev.tolist(
)[0][0], '(x-', planep.tolist()[0][0], ')+', planev.tolist(
)[1][0], '(y-', planep.tolist()[1][0], ')+', planev.tolist(
)[2][0], '(z-', planep.tolist()[2][0], ')=0'
###YOUR CODE HERE###
raw_input("Press enter to end:")