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 = 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_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 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')
###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 = 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
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:")
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:")