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_ransac.csv')
###YOUR CODE HERE###
# Show the input point cloud
#fig = utils.view_pc([pc])
theta = 0.08
num_thred = 140
shape_pc = numpy.shape(pc)
num_point = shape_pc[0]
data = numpy.matrix(numpy.reshape(pc, (shape_pc[0], shape_pc[1])))
#Fit a plane to the data using ransac
num_iteration = 2000
min_err = 100
flag = 0
for i in range(num_iteration):
index1, index2, index3 = random.sample(range(num_point), 3)
reference_point = data[index1][:]
v1 = data[index2][:] - data[index1][:]
v2 = data[index3][:] - data[index1][:]
dir_plane = numpy.cross(v1, v2)
if numpy.linalg.norm(dir_plane) == 0:
continue
else:
dir_plane = dir_plane / numpy.linalg.norm(dir_plane)
temp = numpy.absolute(dir_plane * (data - reference_point).T)
index_inlier = numpy.where(temp < theta)[1]
num_inlier = len(index_inlier)
if num_inlier > num_thred:
data_inlier = data[index_inlier, :]
mean_data_in = numpy.mean(data_inlier, 0)
data_in_zero = data_inlier - mean_data_in
u, s, vh = numpy.linalg.svd(
numpy.matrix(data_in_zero).T * numpy.matrix(data_in_zero) /
(num_inlier - 1))
if s[2] / num_inlier < min_err:
min_err = s[2] / num_inlier
print min_err
plane_dir = vh[:][2]
plane_point = mean_data_in
flag = 1
#Show the resulting point cloud
temp = numpy.absolute(plane_dir * (data - plane_point).T)
index_inlier = numpy.where(temp < theta)[1]
index_outlier = range(num_point)
index_outlier = numpy.delete(index_outlier, index_inlier)
data_inlier_dis = copy.deepcopy(itemgetter(*index_inlier)(pc))
data_outlier_dis = copy.deepcopy(itemgetter(*index_outlier)(pc))
fig = utils.view_pc([data_outlier_dis])
fig = utils.view_pc([data_inlier_dis], fig, ['r'])
#Draw the fitted plane
if flag == 1:
print 'Dir', plane_dir
print 'Point', plane_point
fig = utils.draw_plane(fig, plane_dir.T, plane_point.T,
(0.1, 1, 0.1, 0.5), [-0.7, 1.2], [-0.5, 1.3])
###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###
test = 2 # which pc_target to use, 0, 1, 2, 3
pc_target = utils.load_pc('cloud_icp_target{}.csv'.format(str(test)))
# Change this to load in a different target
fig = utils.view_pc([pc_source, pc_target], None, ['b', 'r'], ['o', '^'])
plt.axis([-0.15, 0.15, -0.15, 0.15])
plt.show()
error_threshold = 0.05 #0.0605 for target 0, 0.002 for target 1, 0.05 for target 2
# 1.45 for target 3 not working
err = [] #list of all errors
i = 0
while True:
i = i + 1
print i
Cp = []
Cq = []
#list of correspondence points
#Assign correspondence points
for p in pc_source:
q = find_min(p, pc_target)
Cp.append(p)
Cq.append(q)
#compute rigid transform
R, t = get_transform(Cp, Cq)
#apply transform
for id, p in enumerate(pc_source):
pc_source[id] = R * p + t
cur_err = error(pc_source, pc_target)
print "error: ", cur_err
err.append(cur_err)
if error(pc_source, pc_target) < error_threshold:
break
utils.view_pc([pc_source, pc_target], None, ['b', 'r'], ['o', '^'])
plt.axis([-0.15, 0.15, -0.15, 0.15])
plt.figure()
title_font = 16
label_font = 14
plt.plot(range(1, len(err) + 1), err, 'bo-')
plt.title('ICP Error vs iteration (target{})'.format(test),
fontsize=title_font)
plt.xlabel('Iteration', fontsize=label_font)
plt.ylabel('error', fontsize=label_font)
plt.xticks(range(1, len(err) + 1, 3))
plt.xticks(fontsize=label_font)
plt.yticks(fontsize=label_font)
plt.show()
###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_target2.csv') # Change this to load in a different target
icp_result = ICPfunction(pc_source, pc_target, 0.2)
result_pc = icp_result[2]
#print 'pc_source =', pc_source
#print 'pc_target =', pc_target
#print 'result_pc =', result_pc
#print 'r_mat_list = ', icp_result[0]
#print 't_mat_list = ', icp_result[1]
r_final_mat = matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
for one in icp_result[0]:
r_final_mat = one * r_final_mat
#t_final_mat = matrix([[0],[0],[0]])
i = 0
t_final_mat = icp_result[1][0]
for i in range(1, len(icp_result[1])):
t_final_mat = icp_result[0][i] * t_final_mat + icp_result[1][i]
print 'r_final_mat = ', r_final_mat
print 't_final_mat = ', t_final_mat
pc_verify = []
# for one in pc_source:
# temp = r_final_mat*one + t_final_mat
# pc_verify.append(temp)
utils.view_pc([pc_source, pc_target, result_pc, pc_verify], None,
['b', 'r', 'g', 'y'], ['o', '^', 'o', 'o'])
#plt.axis([-0.15, 0.15, -0.15, 0.15]) #origin
plt.axis([-0.1, 0.1, -0.1, 0.1]) # target0
#plt.axis([-0.15, 0.15, -0.15, 0.15]) # origin
#plt.axis([-0.15, 0.15, -0.15, 0.15]) # origin
#plt.axis([-0.15, 0.15, -0.15, 0.15]) # origin
x_array = array(range(1, len(icp_result[3]) + 1))
y_error_ary = array(icp_result[3])
fig = plot.figure()
plot.plot(x_array, y_error_ary)
plot.xlabel('Iteration')
plot.ylabel('Error')
plot.scatter(x_array, y_error_ary)
plot.title('Error vs iterations')
raw_input("Press enter to end:")
def ransac(pc, iteration=50, delta=0.15, N=200):
e_best = 10000
start = time.clock()
l = len(pc)
for i in range(iteration):
r = [] #r is the set of hypothetical inliers
c = [] #c is the set of consensus set
c_ids = []
r_ids = []
# randomly sample three points from point cloud
r_ids = random.sample(range(0, l), 3)
for r_id in r_ids:
r.append(pc[r_id])
# fit plane to hypothetical inliers
plane = fit_plane(r)
# generate c-consensus set
for id, pt in enumerate(pc):
if (id not in r_ids) and error([pc[id]], plane) < delta:
c.append(pc[id])
c_ids.append(id)
# refit model if consensus set is sufficiently big
if len(c) > N:
plane_refit = fit_plane(r + c)
e_new = error_rss(r + c, plane_refit)
if e_new < e_best:
e_best = e_new
plane_best = plane_refit
r_ids_best = r_ids
c_ids_best = c_ids
end = time.clock()
#Show the resulting point cloud
outliers = [] #plot outliers
inliers = [] #inliers
for id, pt in enumerate(pc):
if id not in r_ids_best + c_ids_best:
outliers.append(pt)
else:
inliers.append(pt)
fig = utils.view_pc([outliers])
#plot inliers
utils.view_pc([inliers], fig, 'r', 'o')
#Draw the fitted plane
pt = numpy.matrix([[0], [0], [1 / plane_best[2, 0]]])
utils.draw_plane(fig,
plane_best,
pt, (0.1, 0.7, 0.1, 0.5),
length=[-0.5, 1],
width=[-0.5, 1])
plt.show()
plt.title("Plane fitting result of RANSAC algorithm", fontsize=16)
return end - start, e_best
def main():
#Import the cloud
pc_source = utils.load_pc('cloud_icp_source.csv')
###YOUR CODE HERE###
pc_target = utils.load_pc(
'cloud_icp_target0.csv') # Change this to load in a different target
shape_p = numpy.shape(pc_source)
shape_q = numpy.shape(pc_target)
pc_p = numpy.reshape(copy.deepcopy(pc_source), (shape_p[0], shape_p[1])).T
pc_q = numpy.reshape(copy.deepcopy(pc_target), (shape_q[0], shape_q[1])).T
pc_p_origin = copy.deepcopy(pc_p)
thre = 100
ite_num = 0
error = 2
error_all = []
#while ite_num < thre:
t1 = time.clock()
while error > 0.001:
pc_q = icp_fun.correspondence(pc_p, pc_q)
pc_p, error, temp1, temp2 = icp_fun.icp_cal(pc_p, pc_q, ite_num)
ite_num += 1
if ite_num % 25 == 0:
print 'Iteration number: \n', ite_num
error_all.append(error)
t2 = time.clock()
print 'Time cost: ', t2 - t1
#the final R and T
temp1, temp2, R, t = icp_fun.icp_cal(pc_p_origin, pc_q, 0)
print R, '\n', t
#plot for error and iteration
ite_all = numpy.reshape(range(ite_num), (ite_num, ))
plt.plot(ite_all, error_all, 'o-')
plt.title('Error vs Iteration')
plt.xlabel('Iteration')
plt.ylabel('Error')
plt.show()
#plot the final point cloud
pc_result = numpy.reshape(pc_p.T, shape_p)
result = []
for i in range(len(pc_source)):
result.append(pc_result[i, :].T)
utils.view_pc([result, pc_target], None, ['b', 'r'],
['o', '^']) #after icp
plt.axis([-0.15, 0.15, -0.15, 0.15])
utils.view_pc([pc_source, pc_target], None, ['b', 'r'],
['o', '^']) #before icp
plt.axis([-0.15, 0.15, -0.15, 0.15])
###YOUR CODE HERE###
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')
pca_outlier=[]
ransac_outlier=[]
pca_error = []
ransca_error = []
num_tests = 10
fig = None
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###
print i
theta = 0.08
t1 = time.clock()
error_pca, num_pca = pca_ransac_fun.pca(pc, theta, i, num_tests)
pca_error.append(error_pca)
pca_outlier.append(num_pca)
t2 = time.clock()
print 'Time_PCA ', t2-t1, '\n'
t1 = time.clock()
error_ransac, num_ransac = pca_ransac_fun.ransac(pc, theta, i, num_tests)
ransca_error.append(error_ransac)
ransac_outlier.append(num_ransac)
t2 = time.clock()
print 'Time_Ransac ', t2-t1, '\n'
#this code is just for viewing, you can remove or change it
#raw_input("Press enter for next test:")
matplotlib.pyplot.close(fig)
plt.subplot(2,1,1)
plt.plot(pca_outlier, pca_error, 'o-')
plt.title('PCA')
plt.title('PCA')
plt.xlabel('Number of outlier')
plt.ylabel('Error')
plt.subplot(2,1,2)
plt.plot(ransac_outlier, ransca_error, 'o-')
plt.title('Ransac')
plt.xlabel('Number of outlier')
plt.ylabel('Error')
plt.show()
###YOUR CODE HERE###
raw_input("Press 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
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():
#Import the cloud
pc = utils.load_pc('cloud_pca.csv')
num_tests = 10
fig = None
err_ransac = []
time_ransac = []
err_pca = []
time_pca = []
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###
iteration = 500
delta = 0.2
N = 180
time, error = ransac(pc, iteration, delta, N)
err_ransac.append(error)
time_ransac.append(time)
time, error = pca_plane_fit(pc, delta)
err_pca.append(error)
time_pca.append(time)
#this code is just for viewing, you can remove or change it
#raw_input("Press enter for next test:")
matplotlib.pyplot.close(fig)
plt.figure()
plt.plot(range(10, 10 * num_tests + 1, 10), err_ransac, 'bo-')
plt.plot(range(10, 10 * num_tests + 1, 10), err_pca, 'ro-')
plt.title("Error - number of outliers")
plt.xlabel("number of outliers")
plt.ylabel("residual sum of squares")
plt.legend(["RANSAC", "PCA"])
print "RANSAC time, mean: ", numpy.mean(
time_ransac), "variance: ", numpy.var(time_ransac)
print "PCA time, mean: ", numpy.mean(time_pca), "variance: ", numpy.var(
time_pca)
###YOUR CODE HERE###
raw_input("Press enter to end:")
def main():
#Import the cloud
pc = utils.load_pc('cloud_pca.csv')
###YOUR CODE HERE###
# Show the input point cloud
fig = utils.view_pc([pc])
#Rotate the points to align with the XY plane
thredhold = 0.01
pc = numpy.reshape(pc,(200,3))
pc_mean = numpy.mean(pc, 0).T
pc_present = pc - pc_mean
q = numpy.matrix(pc_present.T) * numpy.matrix(pc_present)/199
u, s ,vh = numpy.linalg.svd(q)
x_new = (vh.T * numpy.matrix(pc_present.T)).T
print 'w=\n', vh
#Show the resulting point cloud
pc_present_display = numpy.reshape(copy.deepcopy(pc_present), (200,3,1))
utils.view_pc([pc_present_display])
x_new_dis = numpy.reshape(numpy.asarray(copy.deepcopy(x_new)), (200,3,1))
utils.view_pc([x_new_dis])
#Rotate the points to align with the XY plane AND eliminate the noise
# Show the resulting point cloud
for i in range(len(s)):
if s[i] < thredhold:
vh_cut = (numpy.matrix(copy.deepcopy(vh)).T[:][0:i]).T
print vh_cut
x_new_cut = (vh_cut.T * numpy.matrix(pc_present.T)).T
extra_col = numpy.zeros((200, 1))
x_new_cut_dis = numpy.reshape(numpy.array(numpy.hstack((x_new_cut, extra_col))), (200,3,1))
utils.view_pc([x_new_cut_dis])
break
dir_plan = numpy.matrix(vh)[:][2]
fig = utils.draw_plane(fig, dir_plan.T, numpy.matrix(pc_mean).T, (0.1, 1, 0.1, 0.5), [-0.5, 1], [-0.5,1.3])
###YOUR CODE HERE###
raw_input("Press enter to end:")
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###
print 'please wait for about 10 seconds'
# Fit a plane to the data using ransac
error_best = 1000
for i in range(200):
pcp = pc[:]
# choose 3 samples
for j in range(3):
a = random.choice(range(len(pcp)))
if j == 0:
p1 = pcp[a]
pcp.pop(a)
else:
p1 = numpy.concatenate((p1, pcp[a]), axis=1)
pcp.pop(a)
# compute model plane and normal vector
if numpy.linalg.matrix_rank(p1.T) < 3:
u, s, w = numpy.linalg.svd(p1.T)
normal = w.T[:, 3]
else:
normal = numpy.linalg.inv(p1.T) * numpy.matrix([[1.], [1.], [1.]])
# build consensus set and outliers set
c = []
o = []
for k in range(len(pcp)):
if numpy.linalg.matrix_rank(p1.T) < 3:
error = -normal.T * pcp[k] / numpy.linalg.norm(normal)
else:
error = (1.0 - normal.T * pcp[k]) / numpy.linalg.norm(normal)
if abs(error.tolist()[0][0]) < 0.05:
c.append(pcp[k])
else:
o.append(pcp[k])
# re-fit model if applicable
if len(c) > 100:
for l in range(3):
c.append(p1[:, l])
# use pca to find the plane that fits inliers
for m in range(len(c)):
if m == 0:
sum = c[0]
else:
sum = sum + c[m]
mu = sum / (m + 1)
for n in range(len(c)):
if n == 0:
cx = c[n] - mu
else:
cx = numpy.concatenate((cx, c[n] - mu), axis=1)
u, s, w = numpy.linalg.svd(cx * cx.T / n)
cx_new = w * cx
error_new = 0
for p in range(len(cx_new.tolist()[2])):
error_new = error_new + cx_new[2, p]**2
if error_new < error_best:
error_best = error_new
vector = numpy.matrix([[0., 0., 1.]])
planev = numpy.linalg.inv(w) * vector.T
origin = numpy.matrix([[0., 0., 0.]])
planep = numpy.linalg.inv(w) * origin.T + mu
inliers = c[:]
outliers = o[:]
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'
fig = utils.view_pc([outliers])
fig = utils.view_pc([inliers], fig, ['r'], ['^'])
a = [planep]
fig = utils.view_pc([a], fig, ['r'], ['^'])
fig = utils.draw_plane(fig, planev, planep, (0.1, 0.7, 0.1, 0.5),
[-0.5, 1.0], [-0.4, 1.2])
# Show the resulting point cloud
# Draw the fitted plane
###YOUR CODE HERE###
raw_input("Press 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:")
def main():
#Import the cloud
print 'please wait for about 20 seconds'
pc = utils.load_pc('cloud_pca.csv')
error_pca = [0] * 10
error_ransac = [1000] * 10
num_tests = 10
t = [0] * 10
fig = None
for num 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###
print '\niteration', num + 1, 'begins:\n'
t[num] = (num + 1) * 10
startp = time.clock()
# use pca
for i in range(len(pc)):
if i == 0:
sum = pc[0]
else:
sum = sum + pc[i]
mu = sum / (len(pc))
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 / (len(pc) - 1))
pcx_new = w * pcx
c_p = []
o_p = []
for k in range(len(pcx_new.tolist()[2])):
if abs(pcx_new.tolist()[2][k]) < 0.05:
c_p.append(pc[k])
error_pca[num] = error_pca[num] + abs(
pcx_new.tolist()[2][k])**2
else:
o_p.append(pc[k])
endp = time.clock()
print 'pca runtime: ', endp - startp
# print 'pca inliers: ', len(c_p)
if num == num_tests - 1:
fig2 = utils.view_pc([o_p])
fig2 = utils.view_pc([c_p], fig2, ['r'], ['^'])
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
fig2 = utils.draw_plane(fig2, planev, planep, (0.1, 0.7, 0.1, 0.5),
[-0.4, 0.9], [-0.4, 1])
matplotlib.pyplot.title('pca')
print '\npca fitting plane 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'
# use ransac
startr = time.clock()
for i in range(200):
pcp = pc[:]
# choose 3 samples
for j in range(3):
a = random.choice(range(len(pcp)))
if j == 0:
p1 = pcp[a]
pcp.pop(a)
else:
p1 = numpy.concatenate((p1, pcp[a]), axis=1)
pcp.pop(a)
# compute model plane and normal vector
if numpy.linalg.matrix_rank(p1.T) < 3:
u, s, w = numpy.linalg.svd(p1.T)
normal = w.T[:, 3]
else:
normal = numpy.linalg.inv(p1.T) * numpy.matrix([[1.], [1.],
[1.]])
# build consensus set and outliers set
c = []
o = []
for k in range(len(pcp)):
if numpy.linalg.matrix_rank(p1.T) < 3:
error = -normal.T * pcp[k] / numpy.linalg.norm(normal)
else:
error = (1.0 -
normal.T * pcp[k]) / numpy.linalg.norm(normal)
if abs(error.tolist()[0][0]) < 0.05:
c.append(pcp[k])
else:
o.append(pcp[k])
# re-fit model if applicable
if len(c) > 100:
for l in range(3):
c.append(p1[:, l])
# use pca to find the plane that fits inliers
for m in range(len(c)):
if m == 0:
sum = c[0]
else:
sum = sum + c[m]
mu = sum / (m + 1)
for n in range(len(c)):
if n == 0:
cx = c[n] - mu
else:
cx = numpy.concatenate((cx, c[n] - mu), axis=1)
u, s, w = numpy.linalg.svd(cx * cx.T / n)
cx_new = w * cx
error_new = 0
for p in range(len(cx_new.tolist()[2])):
error_new = error_new + abs(cx_new[2, p])**2
if error_new < error_ransac[num]:
error_ransac[num] = error_new
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
inliers = c[:]
outliers = o[:]
endr = time.clock()
print '\nransac runtime: ', endr - startr
if num == num_tests - 1:
fig3 = utils.view_pc([outliers])
fig3 = utils.view_pc([inliers], fig3, ['r'], ['^'])
a = [planep]
fig3 = utils.view_pc([a], fig3, ['r'], ['^'])
fig3 = utils.draw_plane(fig3, planev, planep, (0.1, 0.7, 0.1, 0.5),
[-0.4, 0.9], [-0.4, 1])
matplotlib.pyplot.title('ransac')
print '\nransac fitting plane 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'
#print '\nransac inliers: ', len(inliers)
#this code is just for viewing, you can remove or change it
#raw_input("Press enter for next test:")
#matplotlib.pyplot.close(fig)
#matplotlib.pyplot.close(fig2)
#matplotlib.pyplot.close(fig3)
matplotlib.pyplot.figure()
matplotlib.pyplot.title('pca')
matplotlib.pyplot.plot(t, error_pca, 'ro-')
matplotlib.pyplot.xlabel('Number of Outliers')
matplotlib.pyplot.ylabel('least squares error')
matplotlib.pyplot.ylim(0, 0.2)
matplotlib.pyplot.figure()
matplotlib.pyplot.title('ransac')
matplotlib.pyplot.plot(t, error_ransac, 'ro-')
matplotlib.pyplot.xlabel('Number of Outliers')
matplotlib.pyplot.ylabel('least squares error')
matplotlib.pyplot.ylim(0, 0.2)
###YOUR CODE HERE###
raw_input("Press enter to end:")