def create_grid(brf,tlb,resolution,pos_list,scan_list,l1,l2, display_flag=False,show_pts=True,rotation_angle=0., occupancy_threshold=1): ''' rotation angle - about the Z axis. ''' max_dist = np.linalg.norm(tlb) + 0.2 min_dist = brf[0,0] min_angle,max_angle=math.radians(-60),math.radians(60) all_pts = generate_pointcloud(pos_list, scan_list, min_angle, max_angle, l1, l2, max_dist=max_dist,min_dist=min_dist) rot_mat = tr.Rz(rotation_angle) all_pts_rot = rot_mat*all_pts gr = og3d.occupancy_grid_3d(brf,tlb,resolution,rotation_z=rotation_angle) gr.fill_grid(all_pts_rot) gr.to_binary(occupancy_threshold) if display_flag == True: if show_pts: d3m.plot_points(all_pts,color=(0.,0.,0.)) cube_tups = gr.grid_lines(rotation_angle=rotation_angle) d3m.plot_cuboid(cube_tups) return gr
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 create_grid(brf, tlb, resolution, pos_list, scan_list, l1, l2, display_flag=False, show_pts=True, rotation_angle=0., occupancy_threshold=1): ''' rotation angle - about the Z axis. ''' max_dist = np.linalg.norm(tlb) + 0.2 min_dist = brf[0, 0] min_angle, max_angle = math.radians(-60), math.radians(60) all_pts = generate_pointcloud(pos_list, scan_list, min_angle, max_angle, l1, l2, max_dist=max_dist, min_dist=min_dist) rot_mat = tr.Rz(rotation_angle) all_pts_rot = rot_mat * all_pts gr = og3d.occupancy_grid_3d(brf, tlb, resolution, rotation_z=rotation_angle) gr.fill_grid(all_pts_rot) gr.to_binary(occupancy_threshold) if display_flag == True: if show_pts: d3m.plot_points(all_pts, color=(0., 0., 0.)) cube_tups = gr.grid_lines(rotation_angle=rotation_angle) d3m.plot_cuboid(cube_tups) return gr