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 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_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_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()