def preprocess_points(points): global FANCY_OUTPUT ## divide points ## wallpoints = array([p for p in points if floorseparator.is_wall(p)]) floorpoints = array([p for p in points if floorseparator.is_floor(p)]) ## check if enough points if len(wallpoints) < MIN_NUM_POINTS: return None if len(floorpoints) < MIN_NUM_POINTS: return None ## convert 3D points to 2D ## if wallpoints.shape[1] == 3: wallpoints = wallpoints[:, :2] floorpoints = floorpoints[:, :2] ## plot points if FANCY_OUTPUT: plot_points([p for p in wallpoints if norm(p) < 10e3]) pl.legend(['raw wall points'], 'best') pl.draw() ## filter outliers by clustering ## wallpoints = outlierfilterlib.filter_outliers(wallpoints, threshold=300, min_cluster_size=20) clusters = outlierfilterlib.filter_outliers(wallpoints, threshold=150, min_cluster_size=1, return_clusters=True) ## plot filtered points if FANCY_OUTPUT: pts = [] for c in clusters: pts += c.tolist() pl.clf() plot_points(pts, '.y') pl.legend(['after cluster filter'], 'best') pl.draw() ## filter outliers by floor points ## definite_points = prefilterlib.get_definite_points(floorpoints) envelope_points = prefilterlib.envelope_from_points(definite_points) wallpoints = prefilterlib.filter_clusters_with_envelope(envelope_points, clusters) ## plot filtered points if FANCY_OUTPUT: plot_points(wallpoints, '.b') pl.legend(['after cluster filter', 'after floor filter'], 'best') pl.draw() ## enlighten dense areas ## wallpoints = prefilterlib.enlighten_dense_areas(wallpoints) ## plot filtered points if FANCY_OUTPUT: pl.clf() plot_points(wallpoints) pl.legend(['final'], 'best') pl.draw() ## check if enough points if len(wallpoints) < MIN_NUM_POINTS: return None return wallpoints
def preprocess_points(points): global FANCY_OUTPUT ## divide points ## wallpoints = array([p for p in points if floorseparator.is_wall(p)]) floorpoints = array([p for p in points if floorseparator.is_floor(p)]) ## check if enough points if len(wallpoints) < MIN_NUM_POINTS: return None if len(floorpoints) < MIN_NUM_POINTS: return None ## convert 3D points to 2D ## if wallpoints.shape[1] == 3: wallpoints = wallpoints[:, :2] floorpoints = floorpoints[:, :2] ## plot points if FANCY_OUTPUT: plot_points([p for p in wallpoints if norm(p) < 10e3]) pl.legend(['raw wall points'], 'best') pl.draw() ## filter outliers by clustering ## wallpoints = outlierfilterlib.filter_outliers(wallpoints, threshold=300, min_cluster_size=20) clusters = outlierfilterlib.filter_outliers(wallpoints, threshold=150, min_cluster_size=1, return_clusters=True) ## plot filtered points if FANCY_OUTPUT: pts = [] for c in clusters: pts += c.tolist() pl.clf() plot_points(pts, '.y') pl.legend(['after cluster filter'], 'best') pl.draw() ## filter outliers by floor points ## definite_points = prefilterlib.get_definite_points(floorpoints) envelope_points = prefilterlib.envelope_from_points(definite_points) wallpoints = prefilterlib.filter_clusters_with_envelope( envelope_points, clusters) ## plot filtered points if FANCY_OUTPUT: plot_points(wallpoints, '.b') pl.legend(['after cluster filter', 'after floor filter'], 'best') pl.draw() ## enlighten dense areas ## wallpoints = prefilterlib.enlighten_dense_areas(wallpoints) ## plot filtered points if FANCY_OUTPUT: pl.clf() plot_points(wallpoints) pl.legend(['final'], 'best') pl.draw() ## check if enough points if len(wallpoints) < MIN_NUM_POINTS: return None return wallpoints
#plot(points[:,0]/1e3, points[:,1]/1e3, '.b') #legend(['rejected', 'accepted'], loc='best') #xlabel('x (m)') #ylabel('y (m)') #show() """ plot 3: accepted """ #axes().set_aspect('equal') #plot(points[:,0]/1e3, points[:,1]/1e3, '.b') #plot(rej[:,0]/1e3, rej[:,1]/1e3, '.w') #xlabel('x (m)') #ylabel('y (m)') #show() """ plot 4: after density filter """ sys.path.append('..') import prefilterlib fpoints = prefilterlib.enlighten_dense_areas(points) plot(points[:,0]/1e3, points[:,1]/1e3, 'oy', label="rejected") plot(fpoints[:,0]/1e3, fpoints[:,1]/1e3, 'ob', label="accepted") xlabel('x (m)') ylabel('y (m)') legend() show() """ check how long the algorithm takes """ if args.action == 'time': M = 50 tic = time.clock() for i in range(M): filter_outliers(points) toc = time.clock() print "done in ", (toc-tic)/M, "s"