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 get_transform(P, Q):
p = utils.convert_pc_to_matrix(P)
q = utils.convert_pc_to_matrix(Q)
p_mean = numpy.mean(p, 1)
q_mean = numpy.mean(q, 1)
p = p - p_mean
q = q - q_mean
S = p * q.T
u, s, v = numpy.linalg.svd(S)
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'
d = numpy.diag([1, 1, numpy.linalg.det(v * u.T)])
R = v * d * u.T
t = q_mean - R * p_mean
return R, t
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 pca_plane_fit(pc, delta):
start = time.clock()
#Rotate the points to align with the XY plane
n = len(pc) #number of data in the point cloud
x_raw = utils.convert_pc_to_matrix(pc)
pt = numpy.mean(x_raw, 1)
x = x_raw - pt
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'
# fit plane to original pc
variances = numpy.multiply(s, s)
normal = v[:, 2]
plane = normal / (normal.T.dot(pt))
#generate inliers and outliers
inliers = []
outliers = []
for point in pc:
if error([point], plane) < delta:
inliers.append(point)
else:
outliers.append(point)
rss_err = error_rss(inliers, plane)
end = time.clock()
#plot
fig = utils.view_pc([outliers])
utils.view_pc([inliers], fig, 'r', 'o')
pt = numpy.matrix([[0], [0], [1 / plane[2, 0]]])
utils.draw_plane(fig,
plane,
pt, (0.1, 0.7, 0.1, 0.5),
length=[-0.5, 1],
width=[-0.5, 1])
plt.title("Plane fitting result of PCA", fontsize=16)
#Draw the fitted plane
plt.show()
return end - start, rss_err
def main():
#Import the cloud
pc = utils.load_pc('cloud_pca.csv')
num_tests = 10
fig = None
error_list_pca = []
error_list_ransac = []
pc_pca_outliner_list_global = []
pc_ransac_outliner_list_global = []
pca_time_list = []
ransac_time_list = []
for i in range(0,num_tests):
pc = add_some_outliers(pc,10) #adding 10 new outliers for each test
fig = utils.view_pc([pc])
###YOUR CODE HERE###
#Compute Mean Point
pc2 = pc
pc2_mat = utils.convert_pc_to_matrix(pc2)
mean_mat = mean(pc2_mat, axis=1)
'''
Start PCA
'''
pca_start_time = time.clock()
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_pca_mat = pc2_mat - minus_mat
q_mat = cov(pc2_pca_mat)
u_mat, s_mat, v_mat_t = linalg.svd(q_mat)
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])
A_para = float(normal_vctr_mat[0])
B_para = float(normal_vctr_mat[1])
C_para = float(normal_vctr_mat[2])
D_para = -A_para*float(mean_mat[0]) - B_para*float(mean_mat[1]) - C_para*float(mean_mat[2])
pca_para_list = [-A_para/C_para, -B_para/C_para, -D_para/C_para]
pc_pca_inline_list_it = []
pc_pca_out_list_it = []
for mat in pc:
if DistancePointPlane(mat, pca_para_list) < 0.1:
# print 'found pca inliners'
pc_pca_inline_list_it.append(mat)
else:
pc_pca_out_list_it.append(mat)
error_pca = ErrorPointListPlane(pc_pca_inline_list_it, pca_para_list)
error_list_pca.append(error_pca)
pc_pca_outliner_list_global.append(len(pc_pca_out_list_it))
pca_end_time = time.clock()
pca_time = pca_end_time - pca_start_time
pca_time_list.append(pca_time)
'''
Start Ransac
'''
ransac_start_time = time.clock()
ans = RANSACsolve(pc, 500, 100, 0.15)
[a, b, c] = ans[0]
inliers_mat_list = ans[2]
mean_point_ransac = matrix([[float(mean_mat[0])], [float(mean_mat[1])], [a * float(mean_mat[0]) + b * float(mean_mat[1]) + c]])
normal_ransac = matrix([[a], [b], [-1]])
utils.draw_plane(fig, normal_ransac, mean_point_ransac, color=(1.0, 0.0, 0.0, 0.5), length=[-0.7, 0.7],
width=[-0.8, 0.8])
error_ransac = ErrorPointListPlane(inliers_mat_list, ans[0])
error_list_ransac.append(error_ransac)
#raw_input("Press enter for next test:")
#time.sleep(5)
matplotlib.pyplot.close(fig)
pc_ransac_out_list_it = []
for mat in pc:
result = IfPointMatInList(mat, inliers_mat_list)
if result[1] == False:
pc_ransac_out_list_it.append(mat)
pc_ransac_outliner_list_global.append(len(pc_ransac_out_list_it))
ransac_end_time = time.clock()
ransac_time_list.append(ransac_end_time - ransac_start_time)
'''
compute the inliners for PCA
'''
if i == (num_tests - 1):
pc_pca_inline_list = []
pc_pca_out_list = []
for mat in pc:
if DistancePointPlane(mat, pca_para_list) < 0.15:
#print 'found pca inliners'
pc_pca_inline_list.append(mat)
else:
pc_pca_out_list.append(mat)
#print 'pc_pca_inline_list = ', pc_pca_inline_list
#print 'len(pc_pca_inline_list) =', len(pc_pca_inline_list)
fig_pca = utils.view_pc([pc_pca_inline_list], color='r')
fig_pca = utils.view_pc([pc_pca_out_list], fig_pca, color='b')
fig_pca.set_label('pca')
utils.draw_plane(fig_pca, normal_vctr_mat, mean_mat, color=(0.0, 1, 0.0, 0.5), length=[-0.7, 0.7],
width=[-0.8, 0.8])
'''
compute the outliners for RANSAC
'''
pc_ransac_inline_list = ans[2]
pc_ransanc_out_list = []
for mat in pc:
result = IfPointMatInList(mat, pc_ransac_inline_list)
if result[1] == False:
pc_ransanc_out_list.append(mat)
fig_ransac = utils.view_pc([pc_ransac_inline_list], color='r')
fig_ransac = utils.view_pc([pc_ransanc_out_list], fig_ransac, color='b')
fig_ransac.set_label('ransac')
utils.draw_plane(fig_ransac, normal_ransac, mean_point_ransac, color=(1.0, 0.0, 0.0, 0.5), length=[-0.7, 0.7],
width=[-0.8, 0.8])
#fig_ransac.add_title('')
print 'len(pc_pca_inline_list) =', len(pc_pca_inline_list)
print 'len( pc_ransac_inline_list) =', len( pc_ransac_inline_list)
###YOUR CODE HERE###
print 'error_list_pca =', error_list_pca
print 'error_list_ransac =', error_list_ransac
plt.figure(3)
plt.title("Error vs outliners depends on Iteration Red-RANSAC Green-PCA")
xvals = arange(0, 100, 10)
yvals = array(error_list_pca)
yvals_ransac = array(error_list_ransac)
plt.plot(xvals, yvals, c='g')
#plt.title("Error of PCA vs outliners")
plt.plot(xvals, yvals_ransac, c='r')
#plt.title("Error of RANSAC vs outliners")
plt.figure(4)
plt.title("Error vs outliners numbers Red-RANSAC Green-PCA")
xval_srd_pca = array(pc_pca_outliner_list_global)
xval_srd_ransac = array(pc_ransac_outliner_list_global)
yval_srd_pca = array(error_list_pca)
yval_srd_ransac = array(error_list_ransac)
plt.scatter(xval_srd_pca, yval_srd_pca, c='g')
plt.scatter(xval_srd_ransac, yval_srd_ransac, c='r')
plt.plot()
print 'ransac_time_list =', ransac_time_list
print 'pca_time_list =', pca_time_list
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_ransac.csv')
###YOUR CODE HERE###
pc2 = pc[0:10]
pc3 = pc[20:60]
# print pc2
# print len(pc2)
# print type(pc2)
#
# for one in pc2:
# print one
# print type(one)
#
# test = (pc2[0].T).tolist()
# print 'pc2[0][0] =', float(pc2[0][0])
# print 'type(pc2[0][0]) =', type(pc2[0][0])
#
# print 'test =',test
#
# print type(test)
#
# lst_sq_result = LeastSquare(pc2)
# print 'LeastSquare(pc2) =', lst_sq_result
# Show the input point cloud
#utils.view_pc([pc])
#PlotThreeDPointClouds(pc2, pc3)
#Fit a plane to the data using ransac
#judge_test = IfPointMatInList(pc[10],pc)
#print 'judge_test =', judge_test
ans = RANSACsolve(pc, 1000, 100, 0.15)
[a, b, c] = ans[0]
inliers_mat_list = ans[2]
#print 'inliers_mat_list =', inliers_mat_list
#print 'len(inliers_mat_list) =', len(inliers_mat_list)
print '( z = ax+by+c ) a,b,c = ', ans[0]
pc2_mat = utils.convert_pc_to_matrix(pc)
mean_mat = mean(pc2_mat, axis=1)
out_pc_list = []
for mat_one in pc:
result = IfPointMatInList(mat_one, inliers_mat_list)
if result[1] == False:
out_pc_list.append(mat_one)
# print 'len(out_pc_list) =', len(out_pc_list)
mean_point = matrix([[float(mean_mat[0])], [float(mean_mat[1])],
[a * float(mean_mat[0]) + b * float(mean_mat[1]) + c]
])
normal = matrix([[a], [b], [-1]])
#Show the resulting point cloud
fig = view_pc([ans[2]], color='r')
fig = view_pc([out_pc_list], fig, color='b')
#Draw the fitted plane
fig = draw_plane(fig,
normal,
mean_point,
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:")