Esempio n. 1
0
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
Esempio n. 2
0
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
Esempio n. 3
0
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()
Esempio n. 4
0
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()
Esempio n. 5
0
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()
Esempio n. 6
0
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()