def test_segmentation(): gr = create_segmentation_grid(pt, pos_list, scan_list, l1, l2, display_flag=True) obj_pts_list = segment_objects_points(gr) if obj_pts_list == None: print 'There is no plane' obj_pts_list = [] pts = gr.grid_to_points() d3m.plot_points(pts, color=(1., 1., 1.)) d3m.plot_points(pt, color=(0, 1, 0.), mode='sphere') for i, obj_pts in enumerate(obj_pts_list): size = gr.resolution.A1.tolist() size[2] = size[2] * 2 d3m.plot_points(obj_pts, color=color_list[i % len(color_list)]) # display_list.append(pu.CubeCloud(obj_pts,color=color_list[i%len(color_list)],size=size)) #display_list.insert(0,pu.PointCloud(obj_pts,color=color_list[i%len(color_list)])) print 'mean:', obj_pts.mean(1).T d3m.show()
def test_plane_finding(): ''' visualize plane finding. ''' # brf = pt + np.matrix([-0.4,-0.2,-0.3]).T # brf[0,0] = max(brf[0,0],0.05) # print 'brf:', brf.T # # tlb = pt + np.matrix([0.3, 0.2,0.3]).T # resolution = np.matrix([0.01,0.01,0.0025]).T brf = pt + np.matrix([-0.15, -0.25, -0.2]).T brf[0, 0] = max(0.07, brf[0, 0]) tlb = pt + np.matrix([0.25, 0.25, 0.2]).T resolution = np.matrix([0.01, 0.01, 0.0025]).T max_dist = np.linalg.norm(tlb) + 0.2 min_dist = brf[0, 0] all_pts = generate_pointcloud(pos_list, scan_list, min_angle, max_angle, l1, l2, save_scan=False, max_dist=max_dist, min_dist=min_dist) #max_dist=2.0,min_dist=min_dist) gr = og3d.occupancy_grid_3d(brf, tlb, resolution) gr.fill_grid(all_pts) gr.to_binary(1) l = gr.find_plane_indices(assume_plane=True) z_min = min(l) * gr.resolution[2, 0] + gr.brf[2, 0] z_max = max(l) * gr.resolution[2, 0] + gr.brf[2, 0] print 'height of plane:', (z_max + z_min) / 2 pts = gr.grid_to_points() plane_pts_bool = np.multiply(pts[2, :] >= z_min, pts[2, :] <= z_max) plane_pts = pts[:, np.where(plane_pts_bool)[1].A1.tolist()] above_pts = pts[:, np.where(pts[2, :] > z_max)[1].A1.tolist()] below_pts = pts[:, np.where(pts[2, :] < z_min)[1].A1.tolist()] d3m.plot_points(pt, color=(0, 1, 0.), mode='sphere') d3m.plot_points(plane_pts, color=(0, 0, 1.)) d3m.plot_points(above_pts, color=(1.0, 1.0, 1.0)) d3m.plot_points(below_pts, color=(1., 0., 0.)) cube_tups = gr.grid_lines() d3m.plot_cuboid(cube_tups) d3m.show()
def test_plane_finding(): ''' visualize plane finding. ''' # brf = pt + np.matrix([-0.4,-0.2,-0.3]).T # brf[0,0] = max(brf[0,0],0.05) # print 'brf:', brf.T # # tlb = pt + np.matrix([0.3, 0.2,0.3]).T # resolution = np.matrix([0.01,0.01,0.0025]).T brf = pt+np.matrix([-0.15,-0.25,-0.2]).T brf[0,0] = max(0.07,brf[0,0]) tlb = pt+np.matrix([0.25, 0.25,0.2]).T resolution = np.matrix([0.01,0.01,0.0025]).T max_dist = np.linalg.norm(tlb) + 0.2 min_dist = brf[0,0] all_pts = generate_pointcloud(pos_list, scan_list, min_angle, max_angle, l1, l2,save_scan=False, max_dist=max_dist,min_dist=min_dist) #max_dist=2.0,min_dist=min_dist) gr = og3d.occupancy_grid_3d(brf,tlb,resolution) gr.fill_grid(all_pts) gr.to_binary(1) l = gr.find_plane_indices(assume_plane=True) z_min = min(l)*gr.resolution[2,0]+gr.brf[2,0] z_max = max(l)*gr.resolution[2,0]+gr.brf[2,0] print 'height of plane:', (z_max+z_min)/2 pts = gr.grid_to_points() plane_pts_bool = np.multiply(pts[2,:]>=z_min,pts[2,:]<=z_max) plane_pts = pts[:,np.where(plane_pts_bool)[1].A1.tolist()] above_pts =pts[:,np.where(pts[2,:]>z_max)[1].A1.tolist()] below_pts =pts[:,np.where(pts[2,:]<z_min)[1].A1.tolist()] d3m.plot_points(pt,color=(0,1,0.),mode='sphere') d3m.plot_points(plane_pts,color=(0,0,1.)) d3m.plot_points(above_pts,color=(1.0,1.0,1.0)) d3m.plot_points(below_pts,color=(1.,0.,0.)) cube_tups = gr.grid_lines() d3m.plot_cuboid(cube_tups) d3m.show()
def test_segmentation(): gr = create_segmentation_grid(pt,pos_list,scan_list,l1,l2, display_flag=True) obj_pts_list = segment_objects_points(gr) if obj_pts_list == None: print 'There is no plane' obj_pts_list = [] pts = gr.grid_to_points() d3m.plot_points(pts,color=(1.,1.,1.)) d3m.plot_points(pt,color=(0,1,0.),mode='sphere') for i,obj_pts in enumerate(obj_pts_list): size=gr.resolution.A1.tolist() size[2] = size[2]*2 d3m.plot_points(obj_pts,color=color_list[i%len(color_list)]) # display_list.append(pu.CubeCloud(obj_pts,color=color_list[i%len(color_list)],size=size)) #display_list.insert(0,pu.PointCloud(obj_pts,color=color_list[i%len(color_list)])) print 'mean:', obj_pts.mean(1).T d3m.show()